File content as of revision 3:4dcadafb89a2:

/* mbed Microcontroller Library
 * Copyright (c) 2019 ARM Limited
 * SPDX-License-Identifier: Apache-2.0
 */

#include "mbed.h"
#include "platform/mbed_thread.h"
#include "ltc_message.h"
#include <SPI.h>
#include "EthernetInterface.h"
#include "bmp3.h"

/*---------------------- Initial setup ----------------------*/
// Serial port
Serial       pc(USBTX, USBRX);
// Ticker for logging at a fixed frequency
Ticker datalog;
bool tickerActivated = false;
void log_data(){
    tickerActivated = true;
} 

/*---------------------- Ethernet setup ----------------------*/
const int SERVER_PORT = 2;
const char* SERVER_ADDRESS = "192.168.1.2";    // Adress of the other Mbed (Mbed B)
const int LOCAL_PORT = 1;
const char* ip = "192.168.1.1";     // Mbed A = 1; Mbed B = 2
const char* mask = "255.255.255.0";
const char* gateway = "192.168.1.10";
EthernetInterface eth; // network stack
SocketAddress local; // local address
SocketAddress client; // client address (connection to other board)
UDPSocket server; // UDP socket (peripheral on this board)

// Messages to fill and send over ethernet
// buffer length is 6*6 + 11*8 + 14 = 138 
char send_buf[138];

/*---------------------- IMU setup ----------------------*/
// TODO: add this eventually


/*---------------------- LTC chip setup ----------------------*/
#define LTC_MOSI    PB_2
#define LTC_MISO    PC_11
#define LTC_CLK     PC_10
#define LTC_CS      PA_15
#define BSY         PB_5
#define CNV         PB_6

#define BUFF_SIZE   18 //24   // 24, 8-bit words will come through
#define NUM_CHANNELS   6 //8   // 8 total channels
#define CH_SIZE   3   // 3 words / channel

SPI LTC_chip(LTC_MOSI, LTC_MISO, LTC_CLK);
DigitalOut cs_LTC(LTC_CS);
DigitalIn bsy_LTC(BSY);
DigitalOut CNV_PIN(CNV);

float CONV_FACTOR = 0.00031294782f; //--> for -10.24 --> 10.24V
// receive and store data from LTC chip
uint8_t rx_buff[BUFF_SIZE]; //each is an 8-bit word 
ltc_spi adc_data;
int16_t ati_data[6];

// calibration matrix for ATI sensor
float Fxc[6] = {-0.1971322934773, -0.04349257334311, 2.298051028435, -80.35044049387, 1.362983909976, 78.23673392118};
float Fyc[6] = {-0.855555082028, 90.04004739944, -0.2236363056212, -46.22515556189, 0.4634720862657, -45.33866366008};
float Fzc[6] = {126.0118743229, -3.400673797001, 125.6239720415, -3.58428375801, 124.6128824882, -3.121863244239};
float Mxc[6] = {-0.03257086475743, 1.078228404461, -4.281073433774, -0.4388170286617, 4.26206973335, 7 - 0.6391561102933}; // check that these rows are x,y,z too
float Myc[6] = {5.013689449541, -0.1348267445261, -2.487858919058, 1.036624778844, -2.465023328927, -0.8776820303935};
float Mzc[6] = {0.03045090196646, -2.681788264229, 0.06994993822276, -2.787067635975, -0.04822780843519, -2.696991001959};
// bias terms
float bias[6];
// convert forces and torques
float ft_data[6];

// send 6 configuration words with the 6 channel numbers, still the default softspan 7 conversion
void config_LTC(){
    uint8_t discard;
    cs_LTC=0;
    // set sampling order for channels 0-5
    discard = LTC_chip.write(0b10000111); // byte is 7:V, 6:0, 5-3:ID, 2-0:mode
    discard = LTC_chip.write(0b10001111);
    discard = LTC_chip.write(0b10010111);
    discard = LTC_chip.write(0b10011111);
    discard = LTC_chip.write(0b10100111);
    discard = LTC_chip.write(0b10101111);   
    cs_LTC=1;
}

void read_LTC_data() {
    
    // right now, programmed LTC chip to only sample 6 channels, starting at channel 0 on next conversion (in config_LTC)...also changed number of channels variables
    // TODO: could also decode channel ID from bits 4-6 of the info byte in message packet, assign to correct adc channel
    // TODO: could also include next channel for sampling in each conversion message
    
    // sample from all channels
    for (int i = 0; i<NUM_CHANNELS; i++) {
        //request conversion
        CNV_PIN=1;
        wait_ns(60); // wait for 60ns
        CNV_PIN=0;
        //WAIT FOR BSY --> bsy_LTC
        while(bsy_LTC==1){}
        //debugging ONLY
        wait_us(1);
        //then ask for data
        cs_LTC=0;   
        //read data
        int bytecount = CH_SIZE*i;
        while(bytecount < CH_SIZE*(1+i)){
            rx_buff[bytecount] = LTC_chip.write(0x00);
            bytecount++;
        }
        cs_LTC=1; //lift CS
    }
    // fill adc data struct with received bytes    
    for(int i = 0; i < BUFF_SIZE; i++){
        ((uint8_t*)(&adc_data))[i] = rx_buff[i];
    }
    // pull out voltage data here
    ati_data[0] = adc_data.channel[0].cnv_upper<<8 | adc_data.channel[0].cnv_lower;
    ati_data[1] = adc_data.channel[1].cnv_upper<<8 | adc_data.channel[1].cnv_lower;
    ati_data[2] = adc_data.channel[2].cnv_upper<<8 | adc_data.channel[2].cnv_lower;
    ati_data[3] = adc_data.channel[3].cnv_upper<<8 | adc_data.channel[3].cnv_lower;
    ati_data[4] = adc_data.channel[4].cnv_upper<<8 | adc_data.channel[4].cnv_lower;
    ati_data[5] = adc_data.channel[5].cnv_upper<<8 | adc_data.channel[5].cnv_lower;
}

// convert integer readings to voltages to f/t values
void convert_LTC_data(){
    // dummy buffer to store converted ADC vals
    float buff[6];
    for(int i=0; i<6; i++){
        buff[i] = CONV_FACTOR*((float)ati_data[i]-bias[i]); // bias[] is in same units as msg[]
        ft_data[i] = 0; // also zero out ft_data here
    }
    // convert each f/t value separately
    for(int i=0; i<6; i++){
        ft_data[0] += Fxc[i]*buff[i];   
        ft_data[1] += Fyc[i]*buff[i];  
        ft_data[2] += Fzc[i]*buff[i];  
        ft_data[3] += Mxc[i]*buff[i];  
        ft_data[4] += Myc[i]*buff[i];  
        ft_data[5] += Mzc[i]*buff[i];  
    }
}


/*---------------------- Force sensor setup ----------------------*/
#define SN_MOSI     PC_3
#define SN_MISO     PC_2
#define SN_CLK      PB_10
#define SN_CS       PB_12
#define SN_A        PG_15
#define SN_B        PG_10
#define SN_C        PG_12

SPI sn_spi(SN_MOSI, SN_MISO, SN_CLK); //Sensor SPI - mosi, miso, sclk
DigitalOut dec_enable(SN_CS);
DigitalOut dec_bit0(SN_A); 
DigitalOut dec_bit1(SN_B); 
DigitalOut dec_bit2(SN_C);

// structs for individual sensor devices
struct bmp3_dev s1; // sets up dev as a 'bmp3_dev structure' w/ associated variables
struct bmp3_dev s2;
struct bmp3_dev s3;
struct bmp3_dev s4;
struct bmp3_dev s5;
struct bmp3_dev s6;
struct bmp3_dev s7;
struct bmp3_dev s8;
// Structs for sensor data
struct bmp3_data sn_data1;
struct bmp3_data sn_data2;
struct bmp3_data sn_data3;
struct bmp3_data sn_data4;
struct bmp3_data sn_data5;
struct bmp3_data sn_data6;
struct bmp3_data sn_data7;
struct bmp3_data sn_data8;
// Store sensor output data (pressures)
int pr_data[8];
// Configure data from sensor
uint8_t sensor_comp = uint8_t(1)| uint8_t(1<<1); // sensor_comp = BMP3_PRESS | BMP3_TEMP;
    
void writeLow(uint8_t pin){ // modified for just 2 sensors
    dec_enable = 0;
    if (pin == 1){
        dec_bit0 = 0;
        dec_bit1 = 0;
        dec_bit2 = 0;
    }
    else if (pin == 2){
        dec_bit0 = 1;
        dec_bit1 = 0;
        dec_bit2 = 0;
    }
    else if (pin == 3){
        dec_bit0 = 0;
        dec_bit1 = 1;
        dec_bit2 = 0;
    }
    else if (pin == 4){
        dec_bit0 = 1;
        dec_bit1 = 1;
        dec_bit2 = 0;
    }
    else if (pin == 5){
        dec_bit0 = 0;
        dec_bit1 = 0;
        dec_bit2 = 1;
    }
    else if (pin == 6){
        dec_bit0 = 1;
        dec_bit1 = 0;
        dec_bit2 = 1;
    }
    else if (pin == 7){
        dec_bit0 = 0;
        dec_bit1 = 1;
        dec_bit2 = 1;
    }
    else if (pin == 8){
        dec_bit0 = 1;
        dec_bit1 = 1;
        dec_bit2 = 1;
    }
}

void writeHigh(){
    dec_enable = 1; // write all pins high by disabling the decoder
}

// General Read and Write functions
// read function: |0x80 done in library, dummy byte taken care of in library
static int8_t user_spi_read(uint8_t cspin, uint8_t reg_addr, uint8_t *reg_data, uint16_t len) {
    writeLow(cspin);
    sn_spi.write(reg_addr); // send read command to chip_id register (reg 0x00)
    for(int i = 0; i < len; i++){
        *(reg_data+i) = sn_spi.write(0x00); // read in 2nd byte = chip_id
    }
    writeHigh();
    return 0;
}

static int8_t user_spi_write(uint8_t cspin, uint8_t reg_addr, uint8_t *reg_data, uint16_t len) {
    writeLow(cspin);
    sn_spi.write(reg_addr);
    if (len>1) {
        for(int i = 0; i < len-1; i++){
            sn_spi.write(*(reg_data+i)); // send alternating register address and register bytes in multi write
        }
    }
    else{
        sn_spi.write(reg_data[0]);
    }    
    writeHigh();
    return 0;
}

void user_delay_ms(uint32_t msec){ //delay in milliseconds
    wait_ms(msec); 
}

void config_bmp_dev(struct bmp3_dev *dev){
    int8_t rslt=0;//BMP3_OK; // get error with rslt = BMP3_OK;
    
    dev -> intf = BMP3_SPI_INTF;
    dev -> read = &user_spi_read; // what to set here??? should be pointer to read function? &spi_read
    dev -> write = &user_spi_write;// what to set here??? should be pointer to write function? &spi_write
    dev -> delay_ms = &user_delay_ms;// what to set here???  delay in ms
    rslt = bmp3_init(dev);
    pc.printf("* initialize sensor result = 0x%x *\r\n", rslt);
    wait(0.25);
    
    // ***** Configuring settings of sensor
    // Normal Mode - bmp3_set_op_mode
    // Temp En, Press En 
    // OSR = no oversampling temp, press
    // ODR = 200Hz temp, press
    // IRR = no IRR filter
    // ^^^all 4 above =  bmp3_set_sensor_settings
    
    // Set sensor settings (press en, temp en, OSR, ODR, IRR)
    dev -> settings.press_en = 0x01; // BMP3_ENABLE
    dev -> settings.temp_en = 0x01; //BMP3_ENABLE
    dev -> settings.odr_filter.press_os = 0x00; //BMP3_NO_OVERSAMPLING
    dev -> settings.odr_filter.temp_os = 0x00; //BMP3_NO_OVERSAMPLING
    dev -> settings.odr_filter.odr = 0x00; //BMP3_ODR_200_HZ
    dev -> settings.odr_filter.iir_filter = 0x00; //BMP3_IIR_Filter_disable
    
    uint16_t settings_sel;
    //settings_sel = BMP3_PRESS_EN_SEL | BMP3_TEMP_EN_SEL | BMP3_PRESS_OS_SEL | BMP3_TEMP_OS_SEL | BMP3_IIR_FILTER_SEL | BMP3_ODR_SEL;
    settings_sel = uint16_t(1 << 1) | uint16_t(1 << 2) | uint16_t(1 << 4) | uint16_t(1 << 5) | uint16_t(1 << 6) | uint16_t(1 << 7);
    //settings_sel = uint16_t(1 << 1) | uint16_t(1 << 2);
    rslt = bmp3_set_sensor_settings(settings_sel, dev);
    
    // Set operating (power) mode
    dev -> settings.op_mode = 0x03; /// normal mode = 0x03
    rslt = bmp3_set_op_mode(dev);
    
    // Check settings
    rslt = bmp3_get_sensor_settings(dev);  
}

void read_bmp_data(){
    // read data from sensors
    bmp3_get_sensor_data(sensor_comp, &sn_data1, &s1); // todo: combine into a read_data function        
    bmp3_get_sensor_data(sensor_comp, &sn_data2, &s2);
    bmp3_get_sensor_data(sensor_comp, &sn_data3, &s3);
    bmp3_get_sensor_data(sensor_comp, &sn_data4, &s4);
    bmp3_get_sensor_data(sensor_comp, &sn_data5, &s5);
    bmp3_get_sensor_data(sensor_comp, &sn_data6, &s6);
    bmp3_get_sensor_data(sensor_comp, &sn_data7, &s7);
    bmp3_get_sensor_data(sensor_comp, &sn_data8, &s8);
}



/*---------------------- Main function ----------------------*/
int main()
{
    // Initialise the digital pin LED1 as an output
    DigitalOut led(LED1);
    CNV_PIN=0;
    
    // Set up serial port
    pc.baud(115200);
    
    // Set up LTC chip
    LTC_chip.format(8, 0);
    LTC_chip.frequency(10000); //10MHz? //60Mhz clock frequency 
    wait_ms(1);
    config_LTC(); // programs sample order
    wait_ms(1);
    
    // Set up ethernet
    eth.set_network(ip, mask, gateway);
    eth.connect();
    wait_ms(1);
    client.set_port(SERVER_PORT);
    client.set_ip_address(SERVER_ADDRESS);
    local.set_port(LOCAL_PORT);
    local.set_ip_address(ip);
    int code = server.open(&eth);
    if(code!=0) { pc.printf("Error from opening server = %d\n\r",code); }    
    code = server.bind(local);
    if(code!=0) { pc.printf("Error from binding socket = %d\n\r",code); }
    
    // Set up force sensor and decoder
    dec_enable = 1;    
    // Initialize bmp device 1
    s1.dev_id = 1;  // tells which cs pin associated with device
    config_bmp_dev(&s1);
    //Initialize bmp device 2
    s2.dev_id = 2;  // tells which cs pin associated with device
    config_bmp_dev(&s2);
    //Initialize bmp device 3
    s3.dev_id = 3;  // tells which cs pin associated with device
    config_bmp_dev(&s3);
    //Initialize bmp device 4
    s4.dev_id = 4;  // tells which cs pin associated with device
    config_bmp_dev(&s4);
    //Initialize bmp device 5
    s5.dev_id = 5;  // tells which cs pin associated with device
    config_bmp_dev(&s5);
    //Initialize bmp device 6
    s6.dev_id = 6;  // tells which cs pin associated with device
    config_bmp_dev(&s6);
    //Initialize bmp device 7
    s7.dev_id = 7;  // tells which cs pin associated with device
    config_bmp_dev(&s7);
    //Initialize bmp device 8
    s8.dev_id = 8;  // tells which cs pin associated with device
    config_bmp_dev(&s8);
    
    // Calculate bias voltages for ATI sensor
    for(int i=0; i<100; i++){ // read 100 times and take average
        read_LTC_data();  
        for(int j=0; j<6; j++){
            bias[j] += 0.01*(float)ati_data[j];
        }
        wait_ms(1);
    }
    pc.printf("ATI sensor is calibrated with biases of: %.2f, %.2f, %.2f, %.2f, %.2f, %.2f \n\r", bias[0],bias[1],bias[2],bias[3],bias[4],bias[5]);
      
    // Attach sampling interrupt
//    datalog.attach_us(&log_data,50000); // 1000us = 1ms (10000 = 10 ms = 100 Hz)
    datalog.attach(&log_data,0.1); // 10Hz
    
    while (true) {
        
        if(tickerActivated == true) { 

            tickerActivated = false;
            
            // Sample from LTC chip 
            read_LTC_data();
            convert_LTC_data(); // TODO: remove this eventually, since we'll convert the data after sending over ethernet
            // Print received data
            //pc.printf("%6d,%6d,%6d,%6d,%6d,%6d\n\r", ati_data[0],ati_data[1],ati_data[2],ati_data[3],ati_data[4],ati_data[5]);
            // Print converted data
            pc.printf("%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,  ", ft_data[0],ft_data[1],ft_data[2],ft_data[3],ft_data[4],ft_data[5]);
            
            // Sample from force sensor
            read_bmp_data();
            // Convert to relative pressures
            pr_data[0] = int(sn_data1.pressure)-100000; // pressure is returned in Pa, could subtract actual sea level pressure here
            pr_data[1] = int(sn_data2.pressure)-100000;
            pr_data[2] = int(sn_data3.pressure)-100000;
            pr_data[3] = int(sn_data4.pressure)-100000;
            pr_data[4] = int(sn_data5.pressure)-100000;
            pr_data[5] = int(sn_data6.pressure)-100000;
            pr_data[6] = int(sn_data7.pressure)-100000;
            pr_data[7] = int(sn_data8.pressure)-100000;
            // Print received data
            pc.printf("%d,%d,%d,%d,%d,%d,%d,%d\n\r", pr_data[0],pr_data[1],pr_data[2],pr_data[3],pr_data[4],pr_data[5],pr_data[6],pr_data[7]);      
            
            // Pack and send the ethernet message
            sprintf(send_buf, "%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n", ati_data[0],ati_data[1],ati_data[2],ati_data[3],ati_data[4],ati_data[5], pr_data[0],pr_data[1],pr_data[2],pr_data[3],pr_data[4],pr_data[5],pr_data[6],pr_data[7]);       
            server.sendto(client, send_buf, sizeof(send_buf)); // send message, look for '\n' character when decoding the string
            
         }
        
    }
        
    // Terminate connection (if you want)
    server.close();
    eth.disconnect();
    
}