File content as of revision 83:b67e4bb6a087:

/* BlueCrutch. mbed Microcontroller Library yada */
// Cloned from mbed-os-example-ble-HeartRate (after main.cpp was reverted to 14 Dec 2018), because for example where's ble/BLE.h
// Taking stuff across from Niall's project bit by bit, starting with the includes.
#include <events/mbed_events.h>
#include <mbed.h>
#include "mbed.h"
#include "ble/BLE.h"
#include "ble/Gap.h"
#include "ble/services/HeartRateService.h"
#include "max32630fthr.h"
#include "HX711.h"
#include "bmi160.h"
//#include "SDFileSystem.h" // Error: Class "FATFileSystem"  has no member "_ffs" in "SDFileSystem/FATFileSystem/ChaN/diskio.cpp", Line: 25, Col: 37. Compile error fixed by removing the SDFileSystem library from BlueCrutch. Compile was immediately successful. 24/10/19
// I have another SD card library, under FTHR_USBMSD_BD, which at least compiled - 24/10/19
#include "SDBlockDevice.h"
#include "USBMSD_BD.h"
#include "FATFileSystem.h" // Put in, aping what was in the FTHDR_USBMSD_BD example

Thread ble_thread;

DigitalOut led1(LED1, 1);

const static char     DEVICE_NAME[] = "HRM";
static const uint16_t uuid16_list[] = {GattService::UUID_HEART_RATE_SERVICE};

static uint8_t hrmCounter = 100; // init HRM to 100bps
static HeartRateService *hrServicePtr;

static EventQueue eventQueue(/* event count */ 16 * EVENTS_EVENT_SIZE);

/* These are all the initializations from the crutch project  - about the next 50 lines */
void dumpImuRegisters(BMI160 &imu);
void printRegister(BMI160 &imu, BMI160::Registers reg);
void printBlock(BMI160 &imu, BMI160::Registers startReg, BMI160::Registers stopReg);
MAX32630FTHR pegasus(MAX32630FTHR::VIO_3V3);

InterruptIn button(P2_3);
DigitalOut rLED(LED1, LED_OFF);
DigitalOut gLED(LED2, LED_OFF);
DigitalOut bLED(LED3, LED_OFF);
void button_callback(void);

I2C i2cBus(P5_7, P6_0);

BMI160_I2C imu(i2cBus, BMI160_I2C::I2C_ADRS_SDO_LO);
BMI160::SensorData accData;
BMI160::SensorData gyroData;
BMI160::SensorTime sensorTime;
BMI160::SensorTime startTime;
float imuTemperature;

BMI160::AccConfig accConfig;
BMI160::GyroConfig gyroConfig;

//SDFileSystem sd(P0_5, P0_6, P0_4, P0_7, "sd");  // mosi, miso, sclk, cs
// SDFileSystem sd(P0_5, P0_6, P0_4, P0_7, "sd", P2_2, SDFileSystem::SWITCH_NEG_NO, 400000); // Had to comment this out because the SDFileSystem library gave compile time errors in BlueCrutch
// Trying this as a replacement:
// Physical block device, can be any device that supports the BlockDevice API
// HeapBlockDevice bd(512*BLOCK_SIZE, BLOCK_SIZE);
SDBlockDevice bd(P0_5, P0_6, P0_4, P0_7); // Compiled into the program 24/10/19, so success to that extent at least.
FATFileSystem fs("fs"); // File system declaration 
USBMSD_BD msd(&bd);  // USB MSD

//DigitalIn sd_detect(P2_2);

Serial pc(USBTX, USBRX);    // USB Serial Terminal
int calibration_factor = 9500; //-7050 worked for my 440lb max scale setup
int epoch_time = 1570547100; // 1570547100 = 3:05 on 08/10/19. Will be overwritten
float zero_factor = 2.0781, weight;
int averageSamples = 100;
int seconds_to_blink_if_sd_mount_fails = 5;
char epoch_string[11];
char topline[200];
HX711 scale(P3_4, P3_5);
unsigned char button_pressed=0; 
bool waittimeexpired=false, debugging=false, ble_call, blecallcontinuously /* Idea is to return BLE operation to original if true */= false;
Ticker tick;

void tick_timer() {
    waittimeexpired = true;
    //pc.putc('x');
}
/* End of the initializations from the crutch project */

void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
{
    BLE::Instance().gap().startAdvertising(); // restart advertising
}

void updateSensorValue() {
    // Do blocking calls or whatever is necessary for sensor polling.
    // In our case, we simply update the HRM measurement.
    hrmCounter++;

    //  100 <= HRM bps <=175
    if (hrmCounter == 175) {
        hrmCounter = 100;
    }

    hrServicePtr->updateHeartRate(hrmCounter);
}

void periodicCallback(void) {
    if ((ble_call) || (blecallcontinuously)) {
        led1 = /* !led1 */ LED_OFF; /* Do blinky on LED1 while we're waiting for BLE events. It's set to LED_OFF at the moment (24/10/19) because the BLE blinking can get in the way of the lights for writing data, no SD card inserted and calibration and BLE is not yet doing anything useful */

        if (BLE::Instance().getGapState().connected) {
            eventQueue.call(updateSensorValue);
        }
    } else {
        led1 = LED_OFF; // Don't leave the lights on
    }
}

void onBleInitError(BLE &ble, ble_error_t error)
{
    (void)ble;
    (void)error;
   /* Initialization error handling should go here */
}

void printMacAddress()
{
    /* Print out device MAC address to the console*/
    Gap::AddressType_t addr_type;
    Gap::Address_t address;
    BLE::Instance().gap().getAddress(&addr_type, address);
    printf("DEVICE MAC ADDRESS: ");
    for (int i = 5; i >= 1; i--){
        printf("%02x:", address[i]);
    }
    printf("%02x\r\n", address[0]);
}

void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
{
    BLE&        ble   = params->ble;
    ble_error_t error = params->error;

    if (error != BLE_ERROR_NONE) {
        onBleInitError(ble, error);
        return;
    }

    if (ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
        return;
    }

    ble.gap().onDisconnection(disconnectionCallback);

    /* Setup primary service. */
    hrServicePtr = new HeartRateService(ble, hrmCounter, HeartRateService::LOCATION_FINGER);

    /* Setting up GAP mostly has to do with configuring connectability and the payload contained in the advertisement packets.*/
    /* Setup advertising. */
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::GENERIC_HEART_RATE_SENSOR);
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
    ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
    ble.gap().setAdvertisingInterval(1000); /* 1000ms */
    ble.gap().startAdvertising();
    /* The first line (above) is mandatory for Bluetooth Smart, and says that this device only supports Bluetooth low energy. The 'general discoverable'
    is the typical value to set when you want your device to be seen by other devices in order to connect. Next comes the ID for the heart rate sensor
    service and the name of the device. After the payload is set the code sets the advertising type and the advertising interval. In Bluetooth Smart,
    timing values are typically multiples of 625 us. */
    printMacAddress();
}

void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* context) {
    BLE &ble = BLE::Instance();
    eventQueue.call(Callback<void()>(&ble, &BLE::processEvents));
}

void bleheartrate() {
    eventQueue.call_every(500, periodicCallback);
    BLE &ble = BLE::Instance();
    ble.onEventsToProcess(scheduleBleEventsProcessing);
    ble.init(bleInitComplete);
    eventQueue.dispatch_forever(); // https://os.mbed.com/docs/mbed-os/v5.14/tutorials/the-eventqueue-api.html
    // have to do break_dispatch somewhere to stop it. Maybe inside the loop not to have BLE on all the time
}

int main() {
    pegasus.init(MAX32630FTHR::VIO_3V3);
    ble_thread.start(bleheartrate); // Tried to start and stop this in each iteration with break_dispatch. Now using a semaphore
    ble_call = false;     

    // set PWRSEQ_REG0.pwr_rtcen_run to 1, PWRSEQ_REG0.pwr_rtcen_slip to 1
    // MXC_PWRSEQ->reg0 |= MXC_F_PWRSEQ_REG0_PWR_RTCEN_RUN | MXC_F_PWRSEQ_REG0_PWR_RTCEN_SLP; // needs mbed-dev, which won't load 10/19
    if (debugging) pc.printf("Starting Main Program Thread ...\n");
    //bLED=LED_ON; // Just to verify program has loaded
    //wait(0.5);
    //bLED=LED_OFF;
    gLED = LED_ON;
    rLED = LED_ON;
    i2cBus.frequency(400000);
    scale.tare();
    scale.setScale(calibration_factor); //Adjust to default calibration factor

    uint32_t failures = 0;

    if(imu.setSensorPowerMode(BMI160::GYRO, BMI160::NORMAL) != BMI160::RTN_NO_ERROR) {
        //printf("Failed to set gyroscope power mode\n");
        failures++;
    }
    wait_ms(100);

    if(imu.setSensorPowerMode(BMI160::ACC, BMI160::NORMAL) != BMI160::RTN_NO_ERROR) {
        //printf("Failed to set accelerometer power mode\n");
        failures++;
    }
    wait_ms(100);

    //example of using getSensorConfig
    if(imu.getSensorConfig(accConfig) == BMI160::RTN_NO_ERROR) {
        //printf("ACC Range = %d\n", accConfig.range);
        //printf("ACC UnderSampling = %d\n", accConfig.us);
        //printf("ACC BandWidthParam = %d\n", accConfig.bwp);
        //printf("ACC OutputDataRate = %d\n\n", accConfig.odr);
    } else {
        //printf("Failed to get accelerometer configuration\n");
        failures++;
    }

    //example of setting user defined configuration
    accConfig.range = BMI160::SENS_4G;
    accConfig.us = BMI160::ACC_US_OFF;
    accConfig.bwp = BMI160::ACC_BWP_2;
    accConfig.odr = BMI160::ACC_ODR_8;
    if(imu.setSensorConfig(accConfig) == BMI160::RTN_NO_ERROR) {
        //printf("ACC Range = %d\n", accConfig.range);
        //printf("ACC UnderSampling = %d\n", accConfig.us);
        //printf("ACC BandWidthParam = %d\n", accConfig.bwp);
        //printf("ACC OutputDataRate = %d\n\n", accConfig.odr);
    } else {
        //printf("Failed to set accelerometer configuration\n");
        failures++;
    }

    if(imu.getSensorConfig(gyroConfig) == BMI160::RTN_NO_ERROR) {
        //printf("GYRO Range = %d\n", gyroConfig.range);
        //printf("GYRO BandWidthParam = %d\n", gyroConfig.bwp);
        //printf("GYRO OutputDataRate = %d\n\n", gyroConfig.odr);
    } else {
        //printf("Failed to get gyroscope configuration\n");
        failures++;
    }

    wait(1.0);
    //printf("\033[H");  //home
    //printf("\033[0J");  //erase from cursor to end of screen

    /*if(failures == 0)
    // FORMAT_CODE_END
    //{

        BMI160::SensorData accData;
        BMI160::SensorData gyroData;
        BMI160::SensorTime sensorTime;

    // }*/

    rLED = LED_OFF;
    gLED = LED_OFF;

    button.rise(&button_callback);  // attach the address of the flip function to the rising edge

    button_pressed=0;

    while (true) {
    
        if(button_pressed>=1) { // double-pressed if for calibration (ideally). Set to triple-pressed to reduce sensitivity (see below).
            tick.attach_us(&tick_timer, 1000);
            gLED = LED_ON;
            rLED = LED_OFF;
            //float raw = scaleRaw.read();
            // maybe calibrate against weighing scales reading here
            float timer=0;
            time_t seconds = time(NULL);

            char filename[40];
            if (debugging) pc.printf("%s\n", filename);
            rLED = LED_ON;
            if (fs.mount(&bd) == 0) { // SDBlock stuff. changed from "if (sd.mount() == 0)"
                if (debugging) pc.printf("Mounted SD card\n");
                FILE *ft = fopen("/fs/time.txt",  "r"); // changed from "/sd/"...
                if(ft != NULL) {
                    fscanf(ft, "%d", &epoch_time);
                    fclose(ft);
                    set_time(epoch_time);  // handy to have this, since it allows users to take out the SD card and reset the time from graphgen.xlsx
                    seconds = time(NULL);  // no idea why it seemed necessary to subtract 2^20 from the time to get the time stamping to work properly
                } else if (debugging) pc.printf("Failed to open time.txt!");
                strftime(filename, 40, "/fs/%a_%d_%m_%Y_%H_%M_%S.csv", localtime(&seconds)); // changed from "/sd/"...
                FILE *fc = fopen("/fs/calibration.txt",  "r");  // changed from "/sd/"...
                if(fc != NULL) {

                    while (fscanf(fc, "%d", &calibration_factor) != EOF) {
                       //i++;
                    }
                    scale.setScale(calibration_factor); //Adjust to this calibration factor
                    fclose(fc);
                }

                if (debugging) pc.printf("-!-");
                FILE *fp = fopen(filename, "w");
                if(fp != NULL) {
                    if (debugging) pc.printf("-+-");
                    ble_call = true; // Turn on calling. This is read in periodicCallback(void)
                    sprintf(topline, "Time,Weight,ACC xAxis,ACC yAxis,ACC zAxis,GYRO xAxis,GYRO yAxis,GYRO zAxis,Calibration factor=%d\n", calibration_factor);
                    fprintf(fp, topline);
                    rLED = LED_OFF;
                    imu.getSensorTime(startTime);
                    char my_string[256];
                  
                    while(timer<=20) {
                        imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range);

                        weight = scale.getGram();
                        waittimeexpired = false;
                        if (debugging) pc.printf("Going to wait");
                        while (waittimeexpired&waittimeexpired==false); // Loop until another ticker sets the variable
                        if (debugging) pc.printf("Finished waiting");
                        sprintf(my_string, "%.3f,%.3f,%4.3f,%4.3f,%4.3f,%5.1f,%5.1f,%5.1f,\n"
                                , sensorTime.seconds-startTime.seconds
                                , weight
                                , accData.xAxis.scaled
                                , accData.yAxis.scaled
                                , accData.zAxis.scaled
                                , gyroData.xAxis.scaled
                                , gyroData.yAxis.scaled
                                , gyroData.zAxis.scaled);

                        //printf(my_string);
                        fprintf(fp, my_string);
                        timer=sensorTime.seconds-startTime.seconds;
                            //wait(0.01f);

                    } // while (timer<=20)
                    fclose(fp);
                    seconds = time(NULL);
                    //pc.printf("seconds %d\n", seconds);
                    FILE *fs = fopen("/fs/time.txt",  "w");  // changed from "/sd/"...
                    char time_buf[11];
                    sprintf(time_buf, "%u", seconds);
                    //pc.printf("time_buf: %s\n", time_buf);
                    fprintf(fs, "%s", time_buf);
                    fclose(fs);

                } else {
                    if (debugging) pc.printf("Failed to open csv file!");
                    gLED = LED_OFF;
                    wait(2);
                    rLED=LED_OFF;
                }
                fs.unmount(); // changed from sd.unmount()
            } else { // If sd has not mounted
                gLED = LED_OFF;
                rLED = LED_OFF;
                wait(1.0); // make plenty of time for second press
                if (button_pressed > 2) { // If double[triple for desensitivity]-click (or more)
                // blink blue once and send out a reading on the com port
                    bLED = LED_ON;
                    wait(1.0);
                    bLED = LED_OFF;
                    wait(1.0);
                    pc.printf("%f %d", scale.getGram(), calibration_factor);
                    button_pressed = 0;
                } else { // indicate that there is no sd card
                    for (int isd = 0; isd < seconds_to_blink_if_sd_mount_fails; isd++) {
                        rLED = LED_ON;
                        wait(0.5);
                        rLED = LED_OFF;
                        wait(0.5);
                    }
                }
                rLED = LED_OFF;
                
            }// if sd.mount == 0 ... else
            gLED = LED_OFF;
            button_pressed = 0;
            tick.detach();
            // eventQueue.break_dispatch(); // basic attempt to stop of BLE. Result was flaky.
            // ble_thread.terminate();
            ble_call = false; // This is read in periodicCallback(void)
        } // if (button_pressed==1)
        // sleep(); // "Note: In most cases, you don't need to call sleep() directly. Mbed OS enters sleep mode automatically any time the system is idle. That is when all your threads are in a waiting state, for example waiting for an event or a timeout."
    } // while (1)
}

//*****************************************************************************
void dumpImuRegisters(BMI160 &imu) {
    printRegister(imu, BMI160::CHIP_ID);
    printBlock(imu, BMI160::ERR_REG,BMI160::FIFO_DATA);
    printBlock(imu, BMI160::ACC_CONF, BMI160::FIFO_CONFIG_1);
    printBlock(imu, BMI160::MAG_IF_0, BMI160::SELF_TEST);
    printBlock(imu, BMI160::NV_CONF, BMI160::STEP_CONF_1);
    printRegister(imu, BMI160::CMD);
    //printf("\n");
}

//*****************************************************************************
void printRegister(BMI160 &imu, BMI160::Registers reg) {
    uint8_t data;
    if(imu.readRegister(reg, &data) == BMI160::RTN_NO_ERROR) {
        //printf("IMU Register 0x%02x = 0x%02x\n", reg, data);
    } else {
        //printf("Failed to read register\n");
    }
}


//*****************************************************************************
void printBlock(BMI160 &imu, BMI160::Registers startReg, BMI160::Registers stopReg) {
    uint8_t numBytes = ((stopReg - startReg) + 1);
    uint8_t buff[numBytes];
    uint8_t offset = static_cast<uint8_t>(startReg);

    if(imu.readBlock(startReg, stopReg, buff) == BMI160::RTN_NO_ERROR) {
        for(uint8_t idx = offset; idx < (numBytes + offset); idx++) {
            //printf("IMU Register 0x%02x = 0x%02x\n", idx, buff[idx - offset]);
        }
    } else {
        //printf("Failed to read block\n");
    }
}
void button_callback() {

    button_pressed++; // changed to allow detection of double presses

}