BLE Bicycle Speed Sensor

#include "Particle.h"

SerialLogHandler logHandler(LOG_LEVEL_INFO);

const unsigned long UPDATE_INTERVAL_MS = 2000;
unsigned long lastUpdate = 0;
unsigned long lastwheelUpdate = 0;
unsigned long previouswheelUpdate = 0;
int led2 = D7;                                      //onboard LED connected to D7
volatile int swCount;
uint16_t wheel_rev_period;
uint16_t crank_rev_period;
const int debouncePeriod = 150;                      //interrupt debounce period, per magnet passby

//float measurements_update();
void measurements_update(void);

//global definitions
#define 	SPEED_AND_CADENCE_MEAS_INTERVAL         1000
#define 	WHEEL_CIRCUMFERENCE_MM                  2105
#define 	KPH_TO_MM_PER_SEC                       278
#define 	MIN_SPEED_KPH                           1
#define 	MAX_SPEED_KPH                           50
#define 	SPEED_KPH_INCREMENT                     1
#define 	DEGREES_PER_REVOLUTION                  360
#define 	RPM_TO_DEGREES_PER_SEC                  6
#define 	MIN_CRANK_RPM                           20
#define 	MAX_CRANK_RPM                           100
#define 	CRANK_RPM_INCREMENT                     3

/* Cycling Speed and Cadence configuration */
#define     GATT_CSC_UUID                           0x1816
#define     GATT_CSC_MEASUREMENT_UUID               0x2A5B
#define     GATT_CSC_FEATURE_UUID                   0x2A5C
#define     GATT_SENSOR_LOCATION_UUID               0x2A5D
#define     GATT_SC_CONTROL_POINT_UUID              0x2A55
/* Device Information configuration */
#define     GATT_DEVICE_INFO_UUID                   0x180A
#define     GATT_MANUFACTURER_NAME_UUID             0x2A29
#define     GATT_MODEL_NUMBER_UUID                  0x2A24

/*CSC Measurement flags*/
#define     CSC_MEASUREMENT_WHEEL_REV_PRESENT       0x01
#define     CSC_MEASUREMENT_CRANK_REV_PRESENT       0x02

/* CSC feature flags */
#define     CSC_FEATURE_WHEEL_REV_DATA              0x01
#define     CSC_FEATURE_CRANK_REV_DATA              0x02
#define     CSC_FEATURE_MULTIPLE_SENSOR_LOC         0x04

/* Sensor location enum */
#define     SENSOR_LOCATION_OTHER                   0
#define     SENSOR_LOCATION_TOP_OF_SHOE             1
#define     SENSOR_LOCATION_IN_SHOE                 2
#define     SENSOR_LOCATION_HIP                     3
#define     SENSOR_LOCATION_FRONT_WHEEL             4
#define     SENSOR_LOCATION_LEFT_CRANK              5
#define     SENSOR_LOCATION_RIGHT_CRANK             6
#define     SENSOR_LOCATION_LEFT_PEDAL              7
#define     SENSOR_LOCATION_RIGHT_PEDAL             8
#define     SENSOR_LOCATION_FROT_HUB                9
#define     SENSOR_LOCATION_REAR_DROPOUT            10
#define     SENSOR_LOCATION_CHAINSTAY               11
#define     SENSOR_LOCATION_REAR_WHEEL              12
#define     SENSOR_LOCATION_REAR_HUB                13
#define     SENSOR_LOCATION_CHEST                   14
#define     SENSOR_LOCATION_SPIDER                  15
#define     SENSOR_LOCATION_CHAIN_RING              16

/* SC Control Point op codes */
#define     SC_CP_OP_SET_CUMULATIVE_VALUE           1
#define     SC_CP_OP_START_SENSOR_CALIBRATION       2
#define     SC_CP_OP_UPDATE_SENSOR_LOCATION         3
#define     SC_CP_OP_REQ_SUPPORTED_SENSOR_LOCATIONS 4
#define     SC_CP_OP_RESPONSE                       16

/*SC Control Point response values */
#define     SC_CP_RESPONSE_SUCCESS                  1
#define     SC_CP_RESPONSE_OP_NOT_SUPPORTED         2
#define     SC_CP_RESPONSE_INVALID_PARAM            3
#define     SC_CP_RESPONSE_OP_FAILED                4

/* CSC simulation configuration */
#define     CSC_FEATURES                            (CSC_FEATURE_WHEEL_REV_DATA | \
                                                    CSC_FEATURE_CRANK_REV_DATA |\
                                                    CSC_FEATURE_MULTIPLE_SENSOR_LOC)

//static uint16_t csc_sim_speed_kph = MIN_SPEED_KPH;      /* Variable holds simulted speed (kilometers per hour) */
//static uint8_t csc_sim_crank_rpm = MIN_CRANK_RPM;       /* Variable holds simulated cadence (RPM) */

BleUuid CyclingSpeedAndCadenceService(GATT_CSC_UUID);   //"Cycling Speed and Cadence"	org.bluetooth.service.cycling_speed_and_cadence	0x1816	GSS

BleCharacteristic cscc("CSC Measurement", BleCharacteristicProperty::NOTIFY, BleUuid(GATT_CSC_MEASUREMENT_UUID), CyclingSpeedAndCadenceService);
BleCharacteristic Sensor_Location("Location", BleCharacteristicProperty::READ, BleUuid(GATT_SENSOR_LOCATION_UUID), CyclingSpeedAndCadenceService);
BleCharacteristic SC_ControlPoint("SC Control Point", BleCharacteristicProperty::INDICATE, BleUuid(GATT_SC_CONTROL_POINT_UUID), CyclingSpeedAndCadenceService);
BleCharacteristic CSC_Feature("CSC Feature", BleCharacteristicProperty::INDICATE, BleUuid(GATT_CSC_FEATURE_UUID), CyclingSpeedAndCadenceService);

// The battery level service allows the battery level to be monitored
BleUuid batteryLevelService(BLE_SIG_UUID_BATTERY_SVC);

BleCharacteristic batteryLevelCharacteristic("Battery Service", BleCharacteristicProperty::NOTIFY, BleUuid(0x180F), batteryLevelService);
//BleCharacteristic batteryLevelCharacteristic("Battery Service", BleCharacteristicProperty::NOTIFY, BleUuid(0x2A19), batteryLevelService);

uint8_t lastBattery = 100;

//Cycling Variables
uint32_t cum_wheel_rev = 0;
uint16_t last_wheel_event = 0;
uint16_t cum_cranks = 0;
uint16_t last_crank_event = 0;
uint16_t csc_sim_speed_kph = 0;      
uint16_t csc_sim_crank_rpm = 0; 

void setup() {
    
    Serial.begin();
    Log.warn("Bluetooth Emulation");
    (void)logHandler; // Does nothing, just to eliminate the unused variable warning
    
    BLE.selectAntenna(BleAntennaType::EXTERNAL);
    BLE.on();
    
    BLE.addCharacteristic(cscc);
    BLE.addCharacteristic(batteryLevelCharacteristic);
    //batteryLevelCharacteristic.setValue(&lastBattery, 1);

    uint8_t buf[BLE_MAX_ADV_DATA_LEN];
    size_t offset = 0;

    buf[offset++] = 0x01;
    buf[offset++] = 0xFC;

    BleAdvertisingData advData;
    advData.appendCustomData(buf,offset);
    //advData.appendServiceUUID(batteryLevelService);
    advData.appendLocalName("WAHOO BLUESC");
    advData.appendServiceUUID(CyclingSpeedAndCadenceService);
    
    // Continuously advertise when not connected
    BLE.advertise(&advData);
    
    //set up pin modes for LED and input, interrupt for input
    pinMode(D11, INPUT_PULLDOWN);
    pinMode(led2, OUTPUT);
    attachInterrupt(D11, switchIsr, RISING);
}

void loop() {
    
    if (millis() - lastUpdate >= UPDATE_INTERVAL_MS) {
        lastUpdate = millis();
        
        if (BLE.connected()) {
            
            //blecsc_simulate_speed_and_cadence();
            //swCount++;
            measurements_update();
            
            uint8_t data_buf[11];
            uint8_t data_offset = 1;

            //The Cycle Measurement Characteristic data is defined here:
            //https://www.bluetooth.com/wp-content/uploads/Sitecore-Media-Library/Gatt/Xml/Characteristics/org.bluetooth.characteristic.csc_measurement.xml
            //Frist byte is flags. Wheel Revolution Data Present (0 = false, 1 = true) = 1, Crank Revolution Data Present (0 = false, 1 = true), so the flags are 0x03 (binary 11 converted to HEX).
            //buf[0]=0x03;
            
            data_buf[0] |= CSC_FEATURE_WHEEL_REV_DATA;
            data_offset += 6;
            
            // Setting values for cycling measures (from the Characteristic File)
            //Cumulative Wheel Revolutions (unitless)
            //Last Wheel Event Time (Unit has a resolution of 1/1024s)
            //Cumulative Crank Revolutions (unitless)
            //Last Crank Event Time (Unit has a resolution of 1/1024s)
            
            memcpy(&data_buf[1], &cum_wheel_rev, 4);
            memcpy(&data_buf[5], &last_wheel_event, 5);
            memcpy(&data_buf[7], &cum_cranks, 6);
            memcpy(&data_buf[9], &last_crank_event, 7);

            cscc.setValue(data_buf, sizeof(data_buf));

            // The battery starts at 100% and drops to 10% then will jump back up again
            batteryLevelCharacteristic.setValue(&lastBattery, 1);
            if (--lastBattery < 90) {
                lastBattery = 100;
            }
        }
        else {
            Log.info("Not yet connected");
            cum_wheel_rev = 0;                  //keep the cumulative wheel revolutions at 0 whilst not connected
            cum_cranks = 0;                      //keep the cumulative crank revolutions at 0 whilst not connected
            cum_wheel_rev = 0;
            last_wheel_event = 0;
        }
    }
}

void measurements_update() {
   
    Log.warn("Measurement Update Loop");
    
    if(swCount) {

        //unsigned long currentTime = millis();
        
        swCount = 0;
       
        Serial.printlnf("Wheel_Rev_Period : %d", wheel_rev_period);
        Serial.printlnf("LastwheelUpdte : %d", lastwheelUpdate);
        Serial.printlnf("Cum_Wheel_Rev : %d", cum_wheel_rev);
        Serial.printlnf("Speed (km/h) : %u", csc_sim_speed_kph);
        Serial.printlnf("Crank RPM : %u", csc_sim_crank_rpm);
        Serial.printlnf("Cum. Cranks : %u", cum_cranks);
        Serial.printlnf("Crank Period : %u", crank_rev_period);
    }
    Log.warn("Finished Measurement Update Loop");
}

void switchIsr()
{
    static volatile int lastInterruptMillis = 0;
    
    if(millis()-lastInterruptMillis>debouncePeriod) {
    
    swCount++;
    lastInterruptMillis = millis();
    
    wheel_rev_period = millis() - lastwheelUpdate;
    lastwheelUpdate = millis();
    cum_wheel_rev++;
    last_wheel_event += wheel_rev_period;
    csc_sim_speed_kph = (3.6*WHEEL_CIRCUMFERENCE_MM)/wheel_rev_period;
    csc_sim_crank_rpm = 0.32*60*1000/wheel_rev_period;
    //csc_sim_crank_rpm = (60/(wheel_rev_period/1000))*(16/50);                  //rear cog = 16 teeth, front cog = 50 teeth (fixed speed; no other gears)
    cum_cranks = cum_wheel_rev*0.32;
    //crank_rev_period = 360/csc_sim_crank_rpm;
    crank_rev_period = ((50/16)*1024*wheel_rev_period);
    last_crank_event += crank_rev_period;
    
    //csc_sim_speed_kph = ((60*WHEEL_CIRCUMFERENCE_MM*60)/(1000000*wheel_rev_period));
        
    //csc_sim_speed_kph = ((36*64*WHEEL_CIRCUMFERENCE_MM)/(625*wheel_rev_period));
    //wheel_rev_period = (1024*60*60*WHEEL_CIRCUMFERENCE_MM) / (1000000*csc_sim_speed_kph);
    //wheel_rev_period = wheel_rev_period*1024;

    }
}

static void blecsc_simulate_speed_and_cadence()
{
    uint16_t wheel_rev_period;
    uint16_t crank_rev_period;

    /* Update simulated crank and wheel rotation speed */
    csc_sim_speed_kph++;
    if (csc_sim_speed_kph >= MAX_SPEED_KPH) {
         csc_sim_speed_kph = MIN_SPEED_KPH;
    }
    
    csc_sim_crank_rpm++;
    if (csc_sim_crank_rpm >= MAX_CRANK_RPM) {
         csc_sim_crank_rpm = MIN_CRANK_RPM;
    }
    
    /* Calculate simulated measurement values */
    if (csc_sim_speed_kph > 0){
        wheel_rev_period = (36*64*WHEEL_CIRCUMFERENCE_MM) / 
                           (625*csc_sim_speed_kph);
        cum_wheel_rev++;
        last_wheel_event += wheel_rev_period;
    }
    
    if (csc_sim_crank_rpm > 0){
        crank_rev_period = (60*1024) / csc_sim_crank_rpm;
        cum_cranks++;
        last_crank_event += crank_rev_period; 
    }
}

    //if(digitalRead(D11) == HIGH) {
    //    wheel_rev_period = millis()-lastwheelUpdate;
    //}
    

    //digitalWrite(led2, LOW);
    
    //wheel_rev_period = (36*64*WHEEL_CIRCUMFERENCE_MM) / 
    //                       (625*csc_sim_speed_kph);
    
    //Update wheel measurements from magnet and update crank / wheel / speed values. 
    //---------------------------------------------------------------------------------------------
    /* Update simulated CSC measurements.
     * Each call increments wheel and crank revolution counters by one and
     * computes last event time in order to match simulated candence and speed.
     * Last event time is expressedd in 1/1024th of second units.
     *
     *                 60 * 1024
     * crank_dt =    --------------
     *                cadence[RPM]
     *
     *
     *                circumference[mm] * 1024 * 60 * 60
     * wheel_dt =    -------------------------------------
     *                         10^6 * speed [kph] 
     */
     //---------------------------------------------------------------------------------------------