test
Dependencies: BLE_API mbed nRF51822
main.cpp@1:88e3620627f9, 2016-02-26 (annotated)
- Committer:
- sk84life85
- Date:
- Fri Feb 26 23:27:37 2016 +0000
- Revision:
- 1:88e3620627f9
- Parent:
- 0:a394a04bd0b7
test
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
sk84life85 | 0:a394a04bd0b7 | 1 | /* |
sk84life85 | 0:a394a04bd0b7 | 2 | SAD: 0x72 |
sk84life85 | 0:a394a04bd0b7 | 3 | TASK: WRITE 0x12 to register 0x42 then start reading. |
sk84life85 | 0:a394a04bd0b7 | 4 | |
sk84life85 | 0:a394a04bd0b7 | 5 | |
sk84life85 | 0:a394a04bd0b7 | 6 | mbed Microcontroller Library |
sk84life85 | 0:a394a04bd0b7 | 7 | * Copyright (c) 2006-2013 ARM Limited |
sk84life85 | 0:a394a04bd0b7 | 8 | * |
sk84life85 | 0:a394a04bd0b7 | 9 | * Licensed under the Apache License, Version 2.0 (the "License"); |
sk84life85 | 0:a394a04bd0b7 | 10 | * you may not use this file except in compliance with the License. |
sk84life85 | 0:a394a04bd0b7 | 11 | * You may obtain a copy of the License at |
sk84life85 | 0:a394a04bd0b7 | 12 | * |
sk84life85 | 0:a394a04bd0b7 | 13 | * http://www.apache.org/licenses/LICENSE-2.0 |
sk84life85 | 0:a394a04bd0b7 | 14 | * |
sk84life85 | 0:a394a04bd0b7 | 15 | * Unless required by applicable law or agreed to in writing, software |
sk84life85 | 0:a394a04bd0b7 | 16 | * distributed under the License is distributed on an "AS IS" BASIS, |
sk84life85 | 0:a394a04bd0b7 | 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
sk84life85 | 0:a394a04bd0b7 | 18 | * See the License for the specific language governing permissions and |
sk84life85 | 0:a394a04bd0b7 | 19 | * limitations under the License. |
sk84life85 | 0:a394a04bd0b7 | 20 | */ |
sk84life85 | 0:a394a04bd0b7 | 21 | |
sk84life85 | 0:a394a04bd0b7 | 22 | //#define AnalogTemp //BDE0600 |
sk84life85 | 0:a394a04bd0b7 | 23 | //#define AnalogUV //ML8511 |
sk84life85 | 0:a394a04bd0b7 | 24 | //#define HallSensor //BU52011 //Change 1: Change to use GPIO for BU52014 |
sk84life85 | 0:a394a04bd0b7 | 25 | //#define RPR0521 //RPR0521 //Change 2: Remove This and add in the RPR-0521 |
sk84life85 | 0:a394a04bd0b7 | 26 | //#define KMX62 //Change 3: Add Code For BH1745, KX022, BM1383GLV, KMX62 |
sk84life85 | 0:a394a04bd0b7 | 27 | |
sk84life85 | 0:a394a04bd0b7 | 28 | #include "mbed.h" |
sk84life85 | 0:a394a04bd0b7 | 29 | #include "BLEDevice.h" |
sk84life85 | 0:a394a04bd0b7 | 30 | #include "UARTService.h" |
sk84life85 | 0:a394a04bd0b7 | 31 | #include "nrf_temp.h" |
sk84life85 | 0:a394a04bd0b7 | 32 | #include "I2C.h" |
sk84life85 | 0:a394a04bd0b7 | 33 | #include <string> |
sk84life85 | 0:a394a04bd0b7 | 34 | #include "stdlib.h" |
sk84life85 | 0:a394a04bd0b7 | 35 | //#include "C12832.h" |
sk84life85 | 0:a394a04bd0b7 | 36 | |
sk84life85 | 0:a394a04bd0b7 | 37 | #define MAX_REPLY_LEN (UARTService::BLE_UART_SERVICE_MAX_DATA_LEN) //Actually equal to 20 |
sk84life85 | 0:a394a04bd0b7 | 38 | #define SENSOR_READ_INTERVAL_S (0.2F) |
sk84life85 | 0:a394a04bd0b7 | 39 | #define ADV_INTERVAL_MS (1000UL) |
sk84life85 | 0:a394a04bd0b7 | 40 | #define UART_BAUD_RATE (19200UL) |
sk84life85 | 0:a394a04bd0b7 | 41 | #define DEVICE_NAME ("Rohm") // This can be read AFTER connecting to the device. |
sk84life85 | 0:a394a04bd0b7 | 42 | #define SHORT_NAME ("BH1745") // Keep this short: max 8 chars if a 128bit UUID is also advertised. |
sk84life85 | 0:a394a04bd0b7 | 43 | #define DEBUG(...) { m_serial_port.printf(__VA_ARGS__); } |
sk84life85 | 0:a394a04bd0b7 | 44 | |
sk84life85 | 0:a394a04bd0b7 | 45 | #define BH1745 //KX022, Accelerometer Sensor |
sk84life85 | 0:a394a04bd0b7 | 46 | //#define BatteryI2C |
sk84life85 | 0:a394a04bd0b7 | 47 | |
sk84life85 | 0:a394a04bd0b7 | 48 | // Function Prototypes |
sk84life85 | 0:a394a04bd0b7 | 49 | void PBTrigger(); //Interrupt function for PB4 |
sk84life85 | 0:a394a04bd0b7 | 50 | |
sk84life85 | 0:a394a04bd0b7 | 51 | // Global Variables |
sk84life85 | 0:a394a04bd0b7 | 52 | BLEDevice m_ble; |
sk84life85 | 0:a394a04bd0b7 | 53 | Serial m_serial_port(p9, p11); // TX pin, RX pin |
sk84life85 | 0:a394a04bd0b7 | 54 | DigitalOut m_cmd_led(LED1); //Original Declaration |
sk84life85 | 0:a394a04bd0b7 | 55 | //PwmOut m_cmd_led(LED1); //Original Declaration |
sk84life85 | 0:a394a04bd0b7 | 56 | PwmOut PWM(p4); |
sk84life85 | 0:a394a04bd0b7 | 57 | PwmOut hornFreq(p13); |
sk84life85 | 0:a394a04bd0b7 | 58 | DigitalOut m_error_led(LED2); |
sk84life85 | 0:a394a04bd0b7 | 59 | UARTService *m_uart_service_ptr; |
sk84life85 | 0:a394a04bd0b7 | 60 | //InterruptIn sw4Press(p18); |
sk84life85 | 0:a394a04bd0b7 | 61 | InterruptIn resetCruise(p21); |
sk84life85 | 0:a394a04bd0b7 | 62 | DigitalIn hornEnable(p18); |
sk84life85 | 0:a394a04bd0b7 | 63 | InterruptIn setCruise(p20); |
sk84life85 | 0:a394a04bd0b7 | 64 | I2C i2c(p7,p30); |
sk84life85 | 0:a394a04bd0b7 | 65 | |
sk84life85 | 0:a394a04bd0b7 | 66 | //C12832 lcd(p25, p29, p28, p1, p0); |
sk84life85 | 0:a394a04bd0b7 | 67 | //DigitalOut LCD_RESET(p24); |
sk84life85 | 0:a394a04bd0b7 | 68 | DigitalOut LED_Enable(p1); |
sk84life85 | 0:a394a04bd0b7 | 69 | //DigitalOut LED_EnableR(p1); |
sk84life85 | 0:a394a04bd0b7 | 70 | |
sk84life85 | 0:a394a04bd0b7 | 71 | AnalogIn brake(p3); //Haven't done anything with this yet! |
sk84life85 | 0:a394a04bd0b7 | 72 | char BrakeIn, isClimbStop; |
sk84life85 | 0:a394a04bd0b7 | 73 | |
sk84life85 | 0:a394a04bd0b7 | 74 | //General Debug |
sk84life85 | 0:a394a04bd0b7 | 75 | //DigitalOut Test1(p12); |
sk84life85 | 0:a394a04bd0b7 | 76 | //DigitalOut Test1(p13); |
sk84life85 | 0:a394a04bd0b7 | 77 | //DigitalOut Test2(p14); |
sk84life85 | 0:a394a04bd0b7 | 78 | //DigitalOut Test3(p15); |
sk84life85 | 0:a394a04bd0b7 | 79 | |
sk84life85 | 0:a394a04bd0b7 | 80 | |
sk84life85 | 0:a394a04bd0b7 | 81 | AnalogIn Hall(p5); //p3 on prior EVK, p5 on the shield |
sk84life85 | 0:a394a04bd0b7 | 82 | AnalogIn Speed(p2); //p3 on prior EVK, p5 on the shield |
sk84life85 | 0:a394a04bd0b7 | 83 | uint16_t Speed_value; |
sk84life85 | 0:a394a04bd0b7 | 84 | uint16_t Speed_Thres = 1000; |
sk84life85 | 0:a394a04bd0b7 | 85 | uint16_t VsetThresh = 1; |
sk84life85 | 0:a394a04bd0b7 | 86 | uint16_t ADC_value,isIncreasing; |
sk84life85 | 0:a394a04bd0b7 | 87 | uint16_t deltaStep = 5; |
sk84life85 | 0:a394a04bd0b7 | 88 | uint16_t OutputLimit = 20;//10000; //Maximum increase ADC Value per loop |
sk84life85 | 0:a394a04bd0b7 | 89 | uint16_t LoopDelay = 50; //Loop Time in ms |
sk84life85 | 0:a394a04bd0b7 | 90 | uint16_t HALL_value; |
sk84life85 | 0:a394a04bd0b7 | 91 | uint16_t ADC_value_old = 0; |
sk84life85 | 0:a394a04bd0b7 | 92 | uint16_t deltaT = 100; |
sk84life85 | 0:a394a04bd0b7 | 93 | uint16_t ampModeDecrease = 50; |
sk84life85 | 0:a394a04bd0b7 | 94 | uint16_t tick; |
sk84life85 | 0:a394a04bd0b7 | 95 | uint16_t pOffset = 54000; |
sk84life85 | 0:a394a04bd0b7 | 96 | uint16_t pGain = 150; |
sk84life85 | 0:a394a04bd0b7 | 97 | uint16_t ExtModeGain = 150; |
sk84life85 | 0:a394a04bd0b7 | 98 | uint16_t AmpModeGain = 250; |
sk84life85 | 0:a394a04bd0b7 | 99 | uint16_t isDecreasing, currSpeed,prevSpeed; |
sk84life85 | 0:a394a04bd0b7 | 100 | uint16_t climbCnt; |
sk84life85 | 0:a394a04bd0b7 | 101 | uint16_t climbPer = 100; |
sk84life85 | 0:a394a04bd0b7 | 102 | uint16_t autoClimbThresh = 110; |
sk84life85 | 0:a394a04bd0b7 | 103 | uint16_t isLocked = 0; |
sk84life85 | 0:a394a04bd0b7 | 104 | uint16_t timerX, timerY, timerZ, isLightOn; |
sk84life85 | 0:a394a04bd0b7 | 105 | float ADC_output; |
sk84life85 | 0:a394a04bd0b7 | 106 | uint16_t SpeedSampNum = 3; |
sk84life85 | 0:a394a04bd0b7 | 107 | uint16_t speedDelta = 24; |
sk84life85 | 0:a394a04bd0b7 | 108 | uint16_t Mode = 0; |
sk84life85 | 0:a394a04bd0b7 | 109 | //string OutSetting; |
sk84life85 | 0:a394a04bd0b7 | 110 | uint16_t isConnected = 0; |
sk84life85 | 0:a394a04bd0b7 | 111 | int32_t p, LoopID; |
sk84life85 | 0:a394a04bd0b7 | 112 | int16_t cruiseSet = 0; |
sk84life85 | 0:a394a04bd0b7 | 113 | int16_t cruiseFlag = 0; |
sk84life85 | 0:a394a04bd0b7 | 114 | int16_t SpeedSet=0; |
sk84life85 | 0:a394a04bd0b7 | 115 | int16_t speedStep = 10; |
sk84life85 | 0:a394a04bd0b7 | 116 | int16_t i,j; |
sk84life85 | 0:a394a04bd0b7 | 117 | int16_t tempSpeed; |
sk84life85 | 0:a394a04bd0b7 | 118 | int16_t isAmpMode = 0; |
sk84life85 | 0:a394a04bd0b7 | 119 | bool RepStart = true; |
sk84life85 | 0:a394a04bd0b7 | 120 | bool NoRepStart = false; |
sk84life85 | 0:a394a04bd0b7 | 121 | bool LED_Command = false; |
sk84life85 | 0:a394a04bd0b7 | 122 | double chromaX, chromaY, tempCIE; |
sk84life85 | 0:a394a04bd0b7 | 123 | double CIEx[60]; |
sk84life85 | 0:a394a04bd0b7 | 124 | double CIEy[60]; |
sk84life85 | 0:a394a04bd0b7 | 125 | |
sk84life85 | 1:88e3620627f9 | 126 | double xPPU9_5 = 0.05660; |
sk84life85 | 1:88e3620627f9 | 127 | double yPPU9_5 = 0.06549; |
sk84life85 | 1:88e3620627f9 | 128 | |
sk84life85 | 1:88e3620627f9 | 129 | |
sk84life85 | 1:88e3620627f9 | 130 | double xPPU9_4 = 0.05072; |
sk84life85 | 1:88e3620627f9 | 131 | double yPPU9_4 = 0.04818; |
sk84life85 | 1:88e3620627f9 | 132 | |
sk84life85 | 0:a394a04bd0b7 | 133 | double xPPU2_14 = 0.03221; |
sk84life85 | 0:a394a04bd0b7 | 134 | double yPPU2_14 = 0.02597; |
sk84life85 | 0:a394a04bd0b7 | 135 | |
sk84life85 | 0:a394a04bd0b7 | 136 | double xPPU18_5 = 0.07085; |
sk84life85 | 0:a394a04bd0b7 | 137 | double yPPU18_5 = 0.06684; |
sk84life85 | 0:a394a04bd0b7 | 138 | |
sk84life85 | 0:a394a04bd0b7 | 139 | double xPPU18_4 = 0.04707; |
sk84life85 | 0:a394a04bd0b7 | 140 | double yPPU18_4 = 0.04474; |
sk84life85 | 0:a394a04bd0b7 | 141 | |
sk84life85 | 0:a394a04bd0b7 | 142 | double xPPU18_3 = 0.03384; |
sk84life85 | 0:a394a04bd0b7 | 143 | double yPPU18_3 = 0.03223; |
sk84life85 | 0:a394a04bd0b7 | 144 | |
sk84life85 | 0:a394a04bd0b7 | 145 | double xPPU17_18 = 0.03440; |
sk84life85 | 0:a394a04bd0b7 | 146 | double yPPU17_18 = 0.03201; |
sk84life85 | 0:a394a04bd0b7 | 147 | |
sk84life85 | 0:a394a04bd0b7 | 148 | double xPPU5_6 = 0.04115; |
sk84life85 | 0:a394a04bd0b7 | 149 | double yPPU5_6 = 0.03835; |
sk84life85 | 0:a394a04bd0b7 | 150 | |
sk84life85 | 0:a394a04bd0b7 | 151 | double xPPU4_15 = 0.08183; |
sk84life85 | 0:a394a04bd0b7 | 152 | double yPPU4_15 = 0.07357; |
sk84life85 | 0:a394a04bd0b7 | 153 | |
sk84life85 | 0:a394a04bd0b7 | 154 | double xPPU4_14 = 0.07272; |
sk84life85 | 0:a394a04bd0b7 | 155 | double yPPU4_14 = 0.06621; |
sk84life85 | 0:a394a04bd0b7 | 156 | |
sk84life85 | 0:a394a04bd0b7 | 157 | double xPPU4_13 = 0.07797; |
sk84life85 | 0:a394a04bd0b7 | 158 | double yPPU4_13 = 0.07208; |
sk84life85 | 0:a394a04bd0b7 | 159 | |
sk84life85 | 0:a394a04bd0b7 | 160 | double xPPU4_10 = 0.09362; |
sk84life85 | 0:a394a04bd0b7 | 161 | double yPPU4_10 = 0.08590; |
sk84life85 | 0:a394a04bd0b7 | 162 | |
sk84life85 | 0:a394a04bd0b7 | 163 | double xPPU4_8 = 0.07635; |
sk84life85 | 0:a394a04bd0b7 | 164 | double yPPU4_8 = 0.07052; |
sk84life85 | 0:a394a04bd0b7 | 165 | |
sk84life85 | 0:a394a04bd0b7 | 166 | |
sk84life85 | 0:a394a04bd0b7 | 167 | double xThresh = 0.0004; |
sk84life85 | 0:a394a04bd0b7 | 168 | double yThresh = 0.0004; |
sk84life85 | 0:a394a04bd0b7 | 169 | |
sk84life85 | 0:a394a04bd0b7 | 170 | string dataReceived = "aa"; |
sk84life85 | 0:a394a04bd0b7 | 171 | uint8_t buf[19]; |
sk84life85 | 0:a394a04bd0b7 | 172 | uint32_t len = 0; |
sk84life85 | 0:a394a04bd0b7 | 173 | string OutSetting = ""; |
sk84life85 | 0:a394a04bd0b7 | 174 | |
sk84life85 | 0:a394a04bd0b7 | 175 | |
sk84life85 | 0:a394a04bd0b7 | 176 | #ifdef BH1745 |
sk84life85 | 0:a394a04bd0b7 | 177 | int BH1745_addr_w = 0x72; |
sk84life85 | 0:a394a04bd0b7 | 178 | int BH1745_addr_r = 0x73; |
sk84life85 | 0:a394a04bd0b7 | 179 | |
sk84life85 | 0:a394a04bd0b7 | 180 | char BH1745_CTRL2[2] = {0x42, 0x11}; |
sk84life85 | 0:a394a04bd0b7 | 181 | |
sk84life85 | 0:a394a04bd0b7 | 182 | char BH1745_Content_ReadData[8]; |
sk84life85 | 0:a394a04bd0b7 | 183 | |
sk84life85 | 0:a394a04bd0b7 | 184 | float BH1745_Red; |
sk84life85 | 0:a394a04bd0b7 | 185 | float BH1745_Green; |
sk84life85 | 0:a394a04bd0b7 | 186 | float BH1745_Blue; |
sk84life85 | 0:a394a04bd0b7 | 187 | float BH1745_Clear; |
sk84life85 | 0:a394a04bd0b7 | 188 | |
sk84life85 | 0:a394a04bd0b7 | 189 | float SUM_BH1745_Red[10]; |
sk84life85 | 0:a394a04bd0b7 | 190 | float SUM_BH1745_Green[10]; |
sk84life85 | 0:a394a04bd0b7 | 191 | float SUM_BH1745_Blue[10]; |
sk84life85 | 0:a394a04bd0b7 | 192 | float SUM_BH1745_Clear[10]; |
sk84life85 | 0:a394a04bd0b7 | 193 | |
sk84life85 | 0:a394a04bd0b7 | 194 | double colorSum; |
sk84life85 | 0:a394a04bd0b7 | 195 | int colorNum; |
sk84life85 | 0:a394a04bd0b7 | 196 | |
sk84life85 | 0:a394a04bd0b7 | 197 | short int BH1745_Raw_R = 0; |
sk84life85 | 0:a394a04bd0b7 | 198 | short int BH1745_Raw_G = 0; |
sk84life85 | 0:a394a04bd0b7 | 199 | short int BH1745_Raw_B = 0; |
sk84life85 | 0:a394a04bd0b7 | 200 | short int BH1745_Raw_C = 0; |
sk84life85 | 0:a394a04bd0b7 | 201 | |
sk84life85 | 0:a394a04bd0b7 | 202 | char BH1745_Red_Data_LSB = 0x50; |
sk84life85 | 0:a394a04bd0b7 | 203 | |
sk84life85 | 0:a394a04bd0b7 | 204 | |
sk84life85 | 0:a394a04bd0b7 | 205 | int sampleNum=0; |
sk84life85 | 0:a394a04bd0b7 | 206 | char message[20]; |
sk84life85 | 0:a394a04bd0b7 | 207 | float rValue, gValue, bValue, cValue; |
sk84life85 | 0:a394a04bd0b7 | 208 | |
sk84life85 | 0:a394a04bd0b7 | 209 | float colorError; |
sk84life85 | 0:a394a04bd0b7 | 210 | |
sk84life85 | 0:a394a04bd0b7 | 211 | #endif |
sk84life85 | 0:a394a04bd0b7 | 212 | |
sk84life85 | 0:a394a04bd0b7 | 213 | #ifdef BatteryI2C |
sk84life85 | 0:a394a04bd0b7 | 214 | int BMS_addr_w = 0x16; //0x2C; |
sk84life85 | 0:a394a04bd0b7 | 215 | int BMS_addr_r = 0x17; //0x2D; |
sk84life85 | 0:a394a04bd0b7 | 216 | |
sk84life85 | 0:a394a04bd0b7 | 217 | char BMS_Content_ReadData[9]; |
sk84life85 | 0:a394a04bd0b7 | 218 | char BMS_DeviceName = 0x21; |
sk84life85 | 0:a394a04bd0b7 | 219 | char BMS_Cycles = 0x17; |
sk84life85 | 0:a394a04bd0b7 | 220 | char BMS_SOC = 0x0D; |
sk84life85 | 0:a394a04bd0b7 | 221 | |
sk84life85 | 0:a394a04bd0b7 | 222 | uint16_t BMS_Password0; |
sk84life85 | 0:a394a04bd0b7 | 223 | uint16_t BMS_Password1; |
sk84life85 | 0:a394a04bd0b7 | 224 | uint16_t BMS_Password2; |
sk84life85 | 0:a394a04bd0b7 | 225 | uint16_t BMS_Password3; |
sk84life85 | 0:a394a04bd0b7 | 226 | uint16_t BMS_Password4; |
sk84life85 | 0:a394a04bd0b7 | 227 | uint16_t BMS_Password5; |
sk84life85 | 0:a394a04bd0b7 | 228 | uint16_t BMS_Password6; |
sk84life85 | 0:a394a04bd0b7 | 229 | uint16_t BMS_Password7; |
sk84life85 | 0:a394a04bd0b7 | 230 | uint16_t BMS_Password8; |
sk84life85 | 0:a394a04bd0b7 | 231 | uint16_t BMS_CycleOut; |
sk84life85 | 0:a394a04bd0b7 | 232 | uint16_t BMS_SOCOut; |
sk84life85 | 0:a394a04bd0b7 | 233 | |
sk84life85 | 0:a394a04bd0b7 | 234 | #endif |
sk84life85 | 0:a394a04bd0b7 | 235 | |
sk84life85 | 0:a394a04bd0b7 | 236 | /** |
sk84life85 | 0:a394a04bd0b7 | 237 | * This callback is used whenever a disconnection occurs. |
sk84life85 | 0:a394a04bd0b7 | 238 | */ |
sk84life85 | 0:a394a04bd0b7 | 239 | void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason) |
sk84life85 | 0:a394a04bd0b7 | 240 | { |
sk84life85 | 0:a394a04bd0b7 | 241 | switch (reason) { |
sk84life85 | 0:a394a04bd0b7 | 242 | case Gap::REMOTE_USER_TERMINATED_CONNECTION: |
sk84life85 | 0:a394a04bd0b7 | 243 | DEBUG("Disconnected (REMOTE_USER_TERMINATED_CONNECTION)\n\r"); |
sk84life85 | 0:a394a04bd0b7 | 244 | break; |
sk84life85 | 0:a394a04bd0b7 | 245 | case Gap::LOCAL_HOST_TERMINATED_CONNECTION: |
sk84life85 | 0:a394a04bd0b7 | 246 | DEBUG("Disconnected (LOCAL_HOST_TERMINATED_CONNECTION)\n\r"); |
sk84life85 | 0:a394a04bd0b7 | 247 | break; |
sk84life85 | 0:a394a04bd0b7 | 248 | case Gap::CONN_INTERVAL_UNACCEPTABLE: |
sk84life85 | 0:a394a04bd0b7 | 249 | DEBUG("Disconnected (CONN_INTERVAL_UNACCEPTABLE)\n\r"); |
sk84life85 | 0:a394a04bd0b7 | 250 | break; |
sk84life85 | 0:a394a04bd0b7 | 251 | } |
sk84life85 | 0:a394a04bd0b7 | 252 | isConnected = 0; |
sk84life85 | 0:a394a04bd0b7 | 253 | |
sk84life85 | 0:a394a04bd0b7 | 254 | LED_Enable = 1; |
sk84life85 | 0:a394a04bd0b7 | 255 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 256 | LED_Enable = 0; |
sk84life85 | 0:a394a04bd0b7 | 257 | |
sk84life85 | 0:a394a04bd0b7 | 258 | DEBUG("Restarting the advertising process\n\r"); |
sk84life85 | 0:a394a04bd0b7 | 259 | m_ble.startAdvertising(); |
sk84life85 | 0:a394a04bd0b7 | 260 | } |
sk84life85 | 0:a394a04bd0b7 | 261 | |
sk84life85 | 0:a394a04bd0b7 | 262 | /** |
sk84life85 | 0:a394a04bd0b7 | 263 | * This callback is used whenever the host writes data to one of our GATT characteristics. |
sk84life85 | 0:a394a04bd0b7 | 264 | */ |
sk84life85 | 0:a394a04bd0b7 | 265 | void dataWrittenCallback(const GattCharacteristicWriteCBParams *params) |
sk84life85 | 0:a394a04bd0b7 | 266 | { |
sk84life85 | 0:a394a04bd0b7 | 267 | // Ensure that initialization is finished and the host has written to the TX characteristic. |
sk84life85 | 0:a394a04bd0b7 | 268 | if ((m_uart_service_ptr != NULL) && (params->charHandle == m_uart_service_ptr->getTXCharacteristicHandle())) { |
sk84life85 | 0:a394a04bd0b7 | 269 | len = 0; |
sk84life85 | 0:a394a04bd0b7 | 270 | // uint16_t TempInt = 1; |
sk84life85 | 0:a394a04bd0b7 | 271 | |
sk84life85 | 0:a394a04bd0b7 | 272 | /* |
sk84life85 | 0:a394a04bd0b7 | 273 | j = 0; |
sk84life85 | 0:a394a04bd0b7 | 274 | for( int i = (params->len) - 1; i > 0; i-- ) |
sk84life85 | 0:a394a04bd0b7 | 275 | { |
sk84life85 | 0:a394a04bd0b7 | 276 | dataReceived[j] = params->data[j]; |
sk84life85 | 0:a394a04bd0b7 | 277 | j++; |
sk84life85 | 0:a394a04bd0b7 | 278 | } |
sk84life85 | 0:a394a04bd0b7 | 279 | dataReceived[j] = params->data[j]; |
sk84life85 | 0:a394a04bd0b7 | 280 | dataReceived[++j] = 0; |
sk84life85 | 0:a394a04bd0b7 | 281 | */ |
sk84life85 | 0:a394a04bd0b7 | 282 | |
sk84life85 | 0:a394a04bd0b7 | 283 | if (params->len == 1) { |
sk84life85 | 0:a394a04bd0b7 | 284 | switch (params->data[0]) { |
sk84life85 | 0:a394a04bd0b7 | 285 | #ifdef BH1745 // |
sk84life85 | 0:a394a04bd0b7 | 286 | case 'r': |
sk84life85 | 0:a394a04bd0b7 | 287 | len = snprintf((char*) buf, MAX_REPLY_LEN, "Red= %f", BH1745_Red); |
sk84life85 | 0:a394a04bd0b7 | 288 | break; |
sk84life85 | 0:a394a04bd0b7 | 289 | case 'g': |
sk84life85 | 0:a394a04bd0b7 | 290 | len = snprintf((char*) buf, MAX_REPLY_LEN, "Green= %f", BH1745_Green); |
sk84life85 | 0:a394a04bd0b7 | 291 | break; |
sk84life85 | 0:a394a04bd0b7 | 292 | case 'b': |
sk84life85 | 0:a394a04bd0b7 | 293 | len = snprintf((char*) buf, MAX_REPLY_LEN, "Blue= %f", BH1745_Blue); |
sk84life85 | 0:a394a04bd0b7 | 294 | break; |
sk84life85 | 0:a394a04bd0b7 | 295 | case 'c': |
sk84life85 | 0:a394a04bd0b7 | 296 | len = snprintf((char*) buf, MAX_REPLY_LEN, "Clear= %f", BH1745_Clear); |
sk84life85 | 0:a394a04bd0b7 | 297 | break; |
sk84life85 | 0:a394a04bd0b7 | 298 | case 'x': |
sk84life85 | 0:a394a04bd0b7 | 299 | len = snprintf((char*) buf, MAX_REPLY_LEN, "CIE#%6.5f,%6.5f", chromaX, chromaY); |
sk84life85 | 0:a394a04bd0b7 | 300 | break; |
sk84life85 | 0:a394a04bd0b7 | 301 | /* |
sk84life85 | 0:a394a04bd0b7 | 302 | case '?': |
sk84life85 | 0:a394a04bd0b7 | 303 | if( (0.03212-0.0005<chromaX) && (chromaX<0.03212+0.0005) && (0.02598-0.0005<chromaY) && (chromaY<0.02598+0.0005) ){ |
sk84life85 | 0:a394a04bd0b7 | 304 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU"); |
sk84life85 | 0:a394a04bd0b7 | 305 | m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len); |
sk84life85 | 0:a394a04bd0b7 | 306 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "2-14"); |
sk84life85 | 0:a394a04bd0b7 | 307 | } |
sk84life85 | 0:a394a04bd0b7 | 308 | else if( (0.07793-0.0005<chromaX) && (chromaX<0.07793+0.0005) && (0.07237-0.0005<chromaY) && (chromaY<0.07237+0.0005) ){ |
sk84life85 | 0:a394a04bd0b7 | 309 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU"); |
sk84life85 | 0:a394a04bd0b7 | 310 | m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len); |
sk84life85 | 0:a394a04bd0b7 | 311 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-13"); |
sk84life85 | 0:a394a04bd0b7 | 312 | } |
sk84life85 | 0:a394a04bd0b7 | 313 | else if( (0.07256-0.0005<chromaX) && (chromaX<0.07256+0.0005) && (0.06636-0.0005<chromaY) && (chromaY<0.06636+0.0005) ){ |
sk84life85 | 0:a394a04bd0b7 | 314 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU"); |
sk84life85 | 0:a394a04bd0b7 | 315 | m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len); |
sk84life85 | 0:a394a04bd0b7 | 316 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-14"); |
sk84life85 | 0:a394a04bd0b7 | 317 | } |
sk84life85 | 0:a394a04bd0b7 | 318 | else if( (0.07079-0.0005<chromaX) && (chromaX<0.07079+0.0005) && (0.06706-0.0005<chromaY) && (chromaY<0.06706+0.0005) ){ |
sk84life85 | 0:a394a04bd0b7 | 319 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU"); |
sk84life85 | 0:a394a04bd0b7 | 320 | m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len); |
sk84life85 | 0:a394a04bd0b7 | 321 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "18-5"); |
sk84life85 | 0:a394a04bd0b7 | 322 | } |
sk84life85 | 0:a394a04bd0b7 | 323 | else if( (0.04708-0.0005<chromaX) && (chromaX<0.04708+0.0005) && (0.04485-0.0005<chromaY) && (chromaY<0.04485+0.0005) ){ |
sk84life85 | 0:a394a04bd0b7 | 324 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU"); |
sk84life85 | 0:a394a04bd0b7 | 325 | m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len); |
sk84life85 | 0:a394a04bd0b7 | 326 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "18-4"); |
sk84life85 | 0:a394a04bd0b7 | 327 | } |
sk84life85 | 0:a394a04bd0b7 | 328 | else if( (0.03373-0.0005<chromaX) && (chromaX<0.03373+0.0005) && (0.03221-0.0005<chromaY) && (chromaY<0.03221+0.0005) ){ |
sk84life85 | 0:a394a04bd0b7 | 329 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU"); |
sk84life85 | 0:a394a04bd0b7 | 330 | m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len); |
sk84life85 | 0:a394a04bd0b7 | 331 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "18-3"); |
sk84life85 | 0:a394a04bd0b7 | 332 | } |
sk84life85 | 0:a394a04bd0b7 | 333 | else if( (0.07630-0.0005<chromaX) && (chromaX<0.07630+0.0005) && (0.07077-0.0005<chromaY) && (chromaY<0.07077+0.0005) ){ |
sk84life85 | 0:a394a04bd0b7 | 334 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU"); |
sk84life85 | 0:a394a04bd0b7 | 335 | m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len); |
sk84life85 | 0:a394a04bd0b7 | 336 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-8"); |
sk84life85 | 0:a394a04bd0b7 | 337 | } |
sk84life85 | 0:a394a04bd0b7 | 338 | else if( (0.09365-0.0005<chromaX) && (chromaX<0.09365+0.0005) && (0.08628-0.0005<chromaY) && (chromaY<0.08628+0.0005) ){ |
sk84life85 | 0:a394a04bd0b7 | 339 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU"); |
sk84life85 | 0:a394a04bd0b7 | 340 | m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len); |
sk84life85 | 0:a394a04bd0b7 | 341 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-10"); |
sk84life85 | 0:a394a04bd0b7 | 342 | } |
sk84life85 | 0:a394a04bd0b7 | 343 | else if( (0.09483-0.0005<chromaX) && (chromaX<0.09483+0.0005) && (0.08657-0.0005<chromaY) && (chromaY<0.08657+0.0005) ){ |
sk84life85 | 0:a394a04bd0b7 | 344 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU"); |
sk84life85 | 0:a394a04bd0b7 | 345 | m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len); |
sk84life85 | 0:a394a04bd0b7 | 346 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-11"); |
sk84life85 | 0:a394a04bd0b7 | 347 | } |
sk84life85 | 0:a394a04bd0b7 | 348 | else if( (0.08935-0.0005<chromaX) && (chromaX<0.08935+0.0005) && (0.08311-0.0005<chromaY) && (chromaY<0.08311+0.0005) ){ |
sk84life85 | 0:a394a04bd0b7 | 349 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU"); |
sk84life85 | 0:a394a04bd0b7 | 350 | m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len); |
sk84life85 | 0:a394a04bd0b7 | 351 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-12"); |
sk84life85 | 0:a394a04bd0b7 | 352 | } |
sk84life85 | 0:a394a04bd0b7 | 353 | else if( (0.08176-0.0005<chromaX) && (chromaX<0.08176+0.0005) && (0.07384-0.0005<chromaY) && (chromaY<0.07384+0.0005) ){ |
sk84life85 | 0:a394a04bd0b7 | 354 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU"); |
sk84life85 | 0:a394a04bd0b7 | 355 | m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len); |
sk84life85 | 0:a394a04bd0b7 | 356 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-15"); |
sk84life85 | 0:a394a04bd0b7 | 357 | } |
sk84life85 | 0:a394a04bd0b7 | 358 | else if( (0.04112-0.0005<chromaX) && (chromaX<0.04112+0.0005) && (0.03841-0.0005<chromaY) && (chromaY<0.03841+0.0005) ){ |
sk84life85 | 0:a394a04bd0b7 | 359 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU"); |
sk84life85 | 0:a394a04bd0b7 | 360 | m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len); |
sk84life85 | 0:a394a04bd0b7 | 361 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "5-6"); |
sk84life85 | 0:a394a04bd0b7 | 362 | } |
sk84life85 | 0:a394a04bd0b7 | 363 | else if( (0.03577-0.0005<chromaX) && (chromaX<0.03577+0.0005) && (0.03325-0.0005<chromaY) && (chromaY<0.03325+0.0005) ){ |
sk84life85 | 0:a394a04bd0b7 | 364 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU"); |
sk84life85 | 0:a394a04bd0b7 | 365 | m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len); |
sk84life85 | 0:a394a04bd0b7 | 366 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "5-5"); |
sk84life85 | 0:a394a04bd0b7 | 367 | } |
sk84life85 | 0:a394a04bd0b7 | 368 | else if( (0.03432-0.0005<chromaX) && (chromaX<0.03432+0.0005) && (0.03203-0.0005<chromaY) && (chromaY<0.03203+0.0005) ){ |
sk84life85 | 0:a394a04bd0b7 | 369 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU"); |
sk84life85 | 0:a394a04bd0b7 | 370 | m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len); |
sk84life85 | 0:a394a04bd0b7 | 371 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "17-18"); |
sk84life85 | 0:a394a04bd0b7 | 372 | } |
sk84life85 | 0:a394a04bd0b7 | 373 | else{ |
sk84life85 | 0:a394a04bd0b7 | 374 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "Unregistered"); |
sk84life85 | 0:a394a04bd0b7 | 375 | } |
sk84life85 | 0:a394a04bd0b7 | 376 | break; |
sk84life85 | 0:a394a04bd0b7 | 377 | */ |
sk84life85 | 0:a394a04bd0b7 | 378 | |
sk84life85 | 0:a394a04bd0b7 | 379 | case 'l': |
sk84life85 | 0:a394a04bd0b7 | 380 | if(LED_Enable==0){ |
sk84life85 | 0:a394a04bd0b7 | 381 | LED_Enable = 1; |
sk84life85 | 0:a394a04bd0b7 | 382 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 383 | len = snprintf((char*) buf, MAX_REPLY_LEN, "LED ON"); |
sk84life85 | 0:a394a04bd0b7 | 384 | } |
sk84life85 | 0:a394a04bd0b7 | 385 | else{ |
sk84life85 | 0:a394a04bd0b7 | 386 | LED_Enable = 0; |
sk84life85 | 0:a394a04bd0b7 | 387 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 388 | len = snprintf((char*) buf, MAX_REPLY_LEN, "LED OFF"); |
sk84life85 | 0:a394a04bd0b7 | 389 | } |
sk84life85 | 0:a394a04bd0b7 | 390 | |
sk84life85 | 0:a394a04bd0b7 | 391 | break; |
sk84life85 | 0:a394a04bd0b7 | 392 | #endif |
sk84life85 | 0:a394a04bd0b7 | 393 | |
sk84life85 | 0:a394a04bd0b7 | 394 | default: |
sk84life85 | 0:a394a04bd0b7 | 395 | len = snprintf((char*) buf, MAX_REPLY_LEN, "ERROR"); |
sk84life85 | 0:a394a04bd0b7 | 396 | break; |
sk84life85 | 0:a394a04bd0b7 | 397 | } |
sk84life85 | 0:a394a04bd0b7 | 398 | }/* |
sk84life85 | 0:a394a04bd0b7 | 399 | else if (params->len > 1) { |
sk84life85 | 0:a394a04bd0b7 | 400 | switch (params->data[0]) { |
sk84life85 | 0:a394a04bd0b7 | 401 | case 's': |
sk84life85 | 0:a394a04bd0b7 | 402 | |
sk84life85 | 0:a394a04bd0b7 | 403 | rValue = BH1745_Red; |
sk84life85 | 0:a394a04bd0b7 | 404 | gValue = BH1745_Green; |
sk84life85 | 0:a394a04bd0b7 | 405 | bValue = BH1745_Blue; |
sk84life85 | 0:a394a04bd0b7 | 406 | cValue = BH1745_Clear; |
sk84life85 | 0:a394a04bd0b7 | 407 | |
sk84life85 | 0:a394a04bd0b7 | 408 | break; |
sk84life85 | 0:a394a04bd0b7 | 409 | } |
sk84life85 | 0:a394a04bd0b7 | 410 | } */ |
sk84life85 | 0:a394a04bd0b7 | 411 | else |
sk84life85 | 0:a394a04bd0b7 | 412 | { |
sk84life85 | 0:a394a04bd0b7 | 413 | len = snprintf((char*) buf, MAX_REPLY_LEN, "ERROR"); |
sk84life85 | 0:a394a04bd0b7 | 414 | } |
sk84life85 | 0:a394a04bd0b7 | 415 | |
sk84life85 | 0:a394a04bd0b7 | 416 | m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len); |
sk84life85 | 0:a394a04bd0b7 | 417 | DEBUG("%d bytes received from host\n\r", params->len); |
sk84life85 | 0:a394a04bd0b7 | 418 | } |
sk84life85 | 0:a394a04bd0b7 | 419 | } |
sk84life85 | 0:a394a04bd0b7 | 420 | |
sk84life85 | 0:a394a04bd0b7 | 421 | |
sk84life85 | 0:a394a04bd0b7 | 422 | /** |
sk84life85 | 0:a394a04bd0b7 | 423 | * This callback is used whenever a write to a GATT characteristic causes data to be sent to the host. |
sk84life85 | 0:a394a04bd0b7 | 424 | */ |
sk84life85 | 0:a394a04bd0b7 | 425 | void dataSentCallback(unsigned count) |
sk84life85 | 0:a394a04bd0b7 | 426 | { |
sk84life85 | 0:a394a04bd0b7 | 427 | // NOTE: The count always seems to be 1 regardless of data. |
sk84life85 | 0:a394a04bd0b7 | 428 | DEBUG("%d bytes sent to host\n\r", count); |
sk84life85 | 0:a394a04bd0b7 | 429 | } |
sk84life85 | 0:a394a04bd0b7 | 430 | |
sk84life85 | 0:a394a04bd0b7 | 431 | |
sk84life85 | 0:a394a04bd0b7 | 432 | /** |
sk84life85 | 0:a394a04bd0b7 | 433 | * This callback is scheduled to be called periodically via a low-priority interrupt. |
sk84life85 | 0:a394a04bd0b7 | 434 | */ |
sk84life85 | 0:a394a04bd0b7 | 435 | void periodicCallback(void) |
sk84life85 | 0:a394a04bd0b7 | 436 | { |
sk84life85 | 0:a394a04bd0b7 | 437 | |
sk84life85 | 0:a394a04bd0b7 | 438 | // uint8_t buf[MAX_REPLY_LEN]; |
sk84life85 | 0:a394a04bd0b7 | 439 | // uint32_t len = 0; |
sk84life85 | 0:a394a04bd0b7 | 440 | |
sk84life85 | 0:a394a04bd0b7 | 441 | |
sk84life85 | 0:a394a04bd0b7 | 442 | |
sk84life85 | 0:a394a04bd0b7 | 443 | if (m_ble.getGapState().connected) { |
sk84life85 | 0:a394a04bd0b7 | 444 | if(isConnected==0){ |
sk84life85 | 0:a394a04bd0b7 | 445 | } |
sk84life85 | 0:a394a04bd0b7 | 446 | isConnected = 1; |
sk84life85 | 0:a394a04bd0b7 | 447 | } |
sk84life85 | 0:a394a04bd0b7 | 448 | else{ |
sk84life85 | 0:a394a04bd0b7 | 449 | isConnected = 0; |
sk84life85 | 0:a394a04bd0b7 | 450 | } |
sk84life85 | 0:a394a04bd0b7 | 451 | |
sk84life85 | 0:a394a04bd0b7 | 452 | } |
sk84life85 | 0:a394a04bd0b7 | 453 | |
sk84life85 | 0:a394a04bd0b7 | 454 | |
sk84life85 | 0:a394a04bd0b7 | 455 | void error(ble_error_t err, uint32_t line) |
sk84life85 | 0:a394a04bd0b7 | 456 | { |
sk84life85 | 0:a394a04bd0b7 | 457 | m_error_led = 1; |
sk84life85 | 0:a394a04bd0b7 | 458 | DEBUG("Error %d on line number %d\n\r", err, line); |
sk84life85 | 0:a394a04bd0b7 | 459 | } |
sk84life85 | 0:a394a04bd0b7 | 460 | |
sk84life85 | 0:a394a04bd0b7 | 461 | //Code for writing back to Console if a PB is pressed |
sk84life85 | 0:a394a04bd0b7 | 462 | void PBTrigger() |
sk84life85 | 0:a394a04bd0b7 | 463 | { |
sk84life85 | 0:a394a04bd0b7 | 464 | //uint8_t buf[MAX_REPLY_LEN]; |
sk84life85 | 0:a394a04bd0b7 | 465 | //uint32_t len = 0; |
sk84life85 | 0:a394a04bd0b7 | 466 | |
sk84life85 | 0:a394a04bd0b7 | 467 | m_cmd_led = !m_cmd_led; |
sk84life85 | 0:a394a04bd0b7 | 468 | |
sk84life85 | 0:a394a04bd0b7 | 469 | if(!setCruise){ |
sk84life85 | 0:a394a04bd0b7 | 470 | cruiseFlag = 1; |
sk84life85 | 0:a394a04bd0b7 | 471 | cruiseSet = Speed_value*2.75; |
sk84life85 | 0:a394a04bd0b7 | 472 | } |
sk84life85 | 0:a394a04bd0b7 | 473 | if(!resetCruise){ |
sk84life85 | 0:a394a04bd0b7 | 474 | cruiseFlag = 0; |
sk84life85 | 0:a394a04bd0b7 | 475 | } |
sk84life85 | 0:a394a04bd0b7 | 476 | m_cmd_led = cruiseFlag; |
sk84life85 | 0:a394a04bd0b7 | 477 | |
sk84life85 | 0:a394a04bd0b7 | 478 | |
sk84life85 | 0:a394a04bd0b7 | 479 | /* |
sk84life85 | 0:a394a04bd0b7 | 480 | if (m_ble.getGapState().connected) { |
sk84life85 | 0:a394a04bd0b7 | 481 | } |
sk84life85 | 0:a394a04bd0b7 | 482 | */ |
sk84life85 | 0:a394a04bd0b7 | 483 | } |
sk84life85 | 0:a394a04bd0b7 | 484 | |
sk84life85 | 0:a394a04bd0b7 | 485 | int main(void) |
sk84life85 | 0:a394a04bd0b7 | 486 | { |
sk84life85 | 0:a394a04bd0b7 | 487 | ble_error_t err; |
sk84life85 | 0:a394a04bd0b7 | 488 | Ticker ticker; |
sk84life85 | 0:a394a04bd0b7 | 489 | |
sk84life85 | 0:a394a04bd0b7 | 490 | m_serial_port.baud(UART_BAUD_RATE); |
sk84life85 | 0:a394a04bd0b7 | 491 | |
sk84life85 | 0:a394a04bd0b7 | 492 | DEBUG("Initialising...\n\r"); |
sk84life85 | 0:a394a04bd0b7 | 493 | |
sk84life85 | 0:a394a04bd0b7 | 494 | m_cmd_led = 0; |
sk84life85 | 0:a394a04bd0b7 | 495 | m_error_led = 0; |
sk84life85 | 0:a394a04bd0b7 | 496 | |
sk84life85 | 0:a394a04bd0b7 | 497 | //ticker.attach(periodicCallback, SENSOR_READ_INTERVAL_S); |
sk84life85 | 0:a394a04bd0b7 | 498 | |
sk84life85 | 0:a394a04bd0b7 | 499 | //sw4Press.fall(&PBTrigger); |
sk84life85 | 0:a394a04bd0b7 | 500 | |
sk84life85 | 0:a394a04bd0b7 | 501 | setCruise.fall(&PBTrigger); |
sk84life85 | 0:a394a04bd0b7 | 502 | resetCruise.fall(&PBTrigger); |
sk84life85 | 0:a394a04bd0b7 | 503 | |
sk84life85 | 0:a394a04bd0b7 | 504 | //Start BTLE Initialization Section |
sk84life85 | 0:a394a04bd0b7 | 505 | m_ble.init(); |
sk84life85 | 0:a394a04bd0b7 | 506 | m_ble.onDisconnection(disconnectionCallback); |
sk84life85 | 0:a394a04bd0b7 | 507 | m_ble.onDataWritten(dataWrittenCallback); |
sk84life85 | 0:a394a04bd0b7 | 508 | m_ble.onDataSent(dataSentCallback); |
sk84life85 | 0:a394a04bd0b7 | 509 | |
sk84life85 | 0:a394a04bd0b7 | 510 | // Set the TX power in dBm units. |
sk84life85 | 0:a394a04bd0b7 | 511 | // Possible values (in decreasing order): 4, 0, -4, -8, -12, -16, -20. |
sk84life85 | 0:a394a04bd0b7 | 512 | err = m_ble.setTxPower(4); |
sk84life85 | 0:a394a04bd0b7 | 513 | if (BLE_ERROR_NONE != err) { |
sk84life85 | 0:a394a04bd0b7 | 514 | error(err, __LINE__); |
sk84life85 | 0:a394a04bd0b7 | 515 | } |
sk84life85 | 0:a394a04bd0b7 | 516 | |
sk84life85 | 0:a394a04bd0b7 | 517 | // Setup advertising (GAP stuff). |
sk84life85 | 0:a394a04bd0b7 | 518 | err = m_ble.setDeviceName(DEVICE_NAME); |
sk84life85 | 0:a394a04bd0b7 | 519 | if (BLE_ERROR_NONE != err) { |
sk84life85 | 0:a394a04bd0b7 | 520 | error(err, __LINE__); |
sk84life85 | 0:a394a04bd0b7 | 521 | } |
sk84life85 | 0:a394a04bd0b7 | 522 | |
sk84life85 | 0:a394a04bd0b7 | 523 | err = m_ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED); |
sk84life85 | 0:a394a04bd0b7 | 524 | if (BLE_ERROR_NONE != err) { |
sk84life85 | 0:a394a04bd0b7 | 525 | error(err, __LINE__); |
sk84life85 | 0:a394a04bd0b7 | 526 | } |
sk84life85 | 0:a394a04bd0b7 | 527 | |
sk84life85 | 0:a394a04bd0b7 | 528 | m_ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED); |
sk84life85 | 0:a394a04bd0b7 | 529 | |
sk84life85 | 0:a394a04bd0b7 | 530 | err = m_ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME, |
sk84life85 | 0:a394a04bd0b7 | 531 | (const uint8_t *)SHORT_NAME, |
sk84life85 | 0:a394a04bd0b7 | 532 | (sizeof(SHORT_NAME) - 1)); |
sk84life85 | 0:a394a04bd0b7 | 533 | if (BLE_ERROR_NONE != err) { |
sk84life85 | 0:a394a04bd0b7 | 534 | error(err, __LINE__); |
sk84life85 | 0:a394a04bd0b7 | 535 | } |
sk84life85 | 0:a394a04bd0b7 | 536 | |
sk84life85 | 0:a394a04bd0b7 | 537 | err = m_ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS, |
sk84life85 | 0:a394a04bd0b7 | 538 | (const uint8_t *)UARTServiceUUID_reversed, |
sk84life85 | 0:a394a04bd0b7 | 539 | sizeof(UARTServiceUUID_reversed)); |
sk84life85 | 0:a394a04bd0b7 | 540 | if (BLE_ERROR_NONE != err) { |
sk84life85 | 0:a394a04bd0b7 | 541 | error(err, __LINE__); |
sk84life85 | 0:a394a04bd0b7 | 542 | } |
sk84life85 | 0:a394a04bd0b7 | 543 | |
sk84life85 | 0:a394a04bd0b7 | 544 | m_ble.setAdvertisingInterval(Gap::MSEC_TO_ADVERTISEMENT_DURATION_UNITS(ADV_INTERVAL_MS)); |
sk84life85 | 0:a394a04bd0b7 | 545 | m_ble.startAdvertising(); |
sk84life85 | 0:a394a04bd0b7 | 546 | |
sk84life85 | 0:a394a04bd0b7 | 547 | // Create a UARTService object (GATT stuff). |
sk84life85 | 0:a394a04bd0b7 | 548 | UARTService uartService(m_ble); |
sk84life85 | 0:a394a04bd0b7 | 549 | m_uart_service_ptr = &uartService; |
sk84life85 | 0:a394a04bd0b7 | 550 | |
sk84life85 | 0:a394a04bd0b7 | 551 | #ifdef BH1745 |
sk84life85 | 0:a394a04bd0b7 | 552 | i2c.write(BH1745_addr_w, &BH1745_CTRL2[0], 2, false); |
sk84life85 | 0:a394a04bd0b7 | 553 | #endif |
sk84life85 | 0:a394a04bd0b7 | 554 | |
sk84life85 | 0:a394a04bd0b7 | 555 | #ifdef BatteryI2C |
sk84life85 | 0:a394a04bd0b7 | 556 | i2c.write(BMS_addr_w, &BMS_DeviceName, 1, RepStart); |
sk84life85 | 0:a394a04bd0b7 | 557 | i2c.read(BMS_addr_r, &BMS_Content_ReadData[0], 8, NoRepStart); |
sk84life85 | 0:a394a04bd0b7 | 558 | |
sk84life85 | 0:a394a04bd0b7 | 559 | //BMS_Password = (BMS_Content_ReadData[1]<<8) | (BMS_Content_ReadData[0]); |
sk84life85 | 0:a394a04bd0b7 | 560 | BMS_Password0 = (BMS_Content_ReadData[0]); |
sk84life85 | 0:a394a04bd0b7 | 561 | BMS_Password1 = (BMS_Content_ReadData[1]); |
sk84life85 | 0:a394a04bd0b7 | 562 | BMS_Password2 = (BMS_Content_ReadData[2]); |
sk84life85 | 0:a394a04bd0b7 | 563 | BMS_Password3 = (BMS_Content_ReadData[3]); |
sk84life85 | 0:a394a04bd0b7 | 564 | BMS_Password4 = (BMS_Content_ReadData[4]); |
sk84life85 | 0:a394a04bd0b7 | 565 | BMS_Password5 = (BMS_Content_ReadData[5]); |
sk84life85 | 0:a394a04bd0b7 | 566 | BMS_Password6 = (BMS_Content_ReadData[6]); |
sk84life85 | 0:a394a04bd0b7 | 567 | BMS_Password7 = (BMS_Content_ReadData[7]); |
sk84life85 | 0:a394a04bd0b7 | 568 | BMS_Password8 = (BMS_Content_ReadData[8]); |
sk84life85 | 0:a394a04bd0b7 | 569 | i2c.write(BMS_addr_w, &BMS_Cycles, 1, RepStart); |
sk84life85 | 0:a394a04bd0b7 | 570 | i2c.read(BMS_addr_r, &BMS_Content_ReadData[0], 2, NoRepStart); |
sk84life85 | 0:a394a04bd0b7 | 571 | BMS_CycleOut = (BMS_Content_ReadData[1]<<8) | (BMS_Content_ReadData[0]); |
sk84life85 | 0:a394a04bd0b7 | 572 | |
sk84life85 | 0:a394a04bd0b7 | 573 | i2c.write(BMS_addr_w, &BMS_SOC, 1, RepStart); |
sk84life85 | 0:a394a04bd0b7 | 574 | i2c.read(BMS_addr_r, &BMS_Content_ReadData[0], 2, NoRepStart); |
sk84life85 | 0:a394a04bd0b7 | 575 | BMS_SOCOut = (BMS_Content_ReadData[1]<<8) | (BMS_Content_ReadData[0]); |
sk84life85 | 0:a394a04bd0b7 | 576 | #endif |
sk84life85 | 0:a394a04bd0b7 | 577 | |
sk84life85 | 0:a394a04bd0b7 | 578 | //Initialize PWM Output |
sk84life85 | 0:a394a04bd0b7 | 579 | PWM.period_us(1000); |
sk84life85 | 0:a394a04bd0b7 | 580 | hornFreq.period_us(1250); |
sk84life85 | 0:a394a04bd0b7 | 581 | //m_cmd_led.period_ms(10); |
sk84life85 | 0:a394a04bd0b7 | 582 | ADC_value_old = 0; |
sk84life85 | 0:a394a04bd0b7 | 583 | |
sk84life85 | 0:a394a04bd0b7 | 584 | p = 0; |
sk84life85 | 0:a394a04bd0b7 | 585 | SpeedSet = 1; |
sk84life85 | 0:a394a04bd0b7 | 586 | tick = deltaT; |
sk84life85 | 0:a394a04bd0b7 | 587 | SpeedSampNum = 5; |
sk84life85 | 0:a394a04bd0b7 | 588 | |
sk84life85 | 0:a394a04bd0b7 | 589 | LED_Enable = 1; |
sk84life85 | 0:a394a04bd0b7 | 590 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 591 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 592 | LED_Enable = 0; |
sk84life85 | 0:a394a04bd0b7 | 593 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 594 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 595 | LED_Enable = 1; |
sk84life85 | 0:a394a04bd0b7 | 596 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 597 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 598 | LED_Enable = 0; |
sk84life85 | 0:a394a04bd0b7 | 599 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 600 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 601 | LED_Enable = 1; |
sk84life85 | 0:a394a04bd0b7 | 602 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 603 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 604 | LED_Enable = 0; |
sk84life85 | 0:a394a04bd0b7 | 605 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 606 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 607 | |
sk84life85 | 0:a394a04bd0b7 | 608 | while (true) { |
sk84life85 | 0:a394a04bd0b7 | 609 | //m_ble.waitForEvent(); |
sk84life85 | 0:a394a04bd0b7 | 610 | if(m_ble.getGapState().connected){ |
sk84life85 | 0:a394a04bd0b7 | 611 | if(!isConnected){ |
sk84life85 | 0:a394a04bd0b7 | 612 | LED_Enable = 1; |
sk84life85 | 0:a394a04bd0b7 | 613 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 614 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 615 | LED_Enable = 0; |
sk84life85 | 0:a394a04bd0b7 | 616 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 617 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 618 | LED_Enable = 1; |
sk84life85 | 0:a394a04bd0b7 | 619 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 620 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 621 | LED_Enable = 0; |
sk84life85 | 0:a394a04bd0b7 | 622 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 623 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 624 | LED_Enable = 1; |
sk84life85 | 0:a394a04bd0b7 | 625 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 626 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 627 | LED_Enable = 0; |
sk84life85 | 0:a394a04bd0b7 | 628 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 629 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 630 | } |
sk84life85 | 0:a394a04bd0b7 | 631 | isConnected = 1; |
sk84life85 | 0:a394a04bd0b7 | 632 | |
sk84life85 | 0:a394a04bd0b7 | 633 | //Read KX022 Portion from the IC |
sk84life85 | 0:a394a04bd0b7 | 634 | i2c.write(BH1745_addr_w, &BH1745_Red_Data_LSB, 1, RepStart); |
sk84life85 | 0:a394a04bd0b7 | 635 | i2c.read(BH1745_addr_r, &BH1745_Content_ReadData[0], 8, NoRepStart); |
sk84life85 | 0:a394a04bd0b7 | 636 | |
sk84life85 | 0:a394a04bd0b7 | 637 | //reconfigure the data (taken from arduino code) |
sk84life85 | 0:a394a04bd0b7 | 638 | BH1745_Raw_R = (BH1745_Content_ReadData[1]<<8) | (BH1745_Content_ReadData[0]); |
sk84life85 | 0:a394a04bd0b7 | 639 | BH1745_Raw_G = (BH1745_Content_ReadData[3]<<8) | (BH1745_Content_ReadData[2]); |
sk84life85 | 0:a394a04bd0b7 | 640 | BH1745_Raw_B = (BH1745_Content_ReadData[5]<<8) | (BH1745_Content_ReadData[4]); |
sk84life85 | 0:a394a04bd0b7 | 641 | BH1745_Raw_C = (BH1745_Content_ReadData[7]<<8) | (BH1745_Content_ReadData[6]); |
sk84life85 | 0:a394a04bd0b7 | 642 | |
sk84life85 | 0:a394a04bd0b7 | 643 | BH1745_Red = (float)BH1745_Raw_R / 16384; |
sk84life85 | 0:a394a04bd0b7 | 644 | BH1745_Green = (float)BH1745_Raw_G / 16384; |
sk84life85 | 0:a394a04bd0b7 | 645 | BH1745_Blue = (float)BH1745_Raw_B / 16384; |
sk84life85 | 0:a394a04bd0b7 | 646 | BH1745_Clear = (float)BH1745_Raw_C / 16384; |
sk84life85 | 0:a394a04bd0b7 | 647 | |
sk84life85 | 0:a394a04bd0b7 | 648 | if(BH1745_Clear < 0.00001){ |
sk84life85 | 0:a394a04bd0b7 | 649 | |
sk84life85 | 0:a394a04bd0b7 | 650 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 651 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 652 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 653 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 654 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 655 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 656 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 657 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 658 | LED_Enable = 1; |
sk84life85 | 0:a394a04bd0b7 | 659 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 660 | wait_ms(100); |
sk84life85 | 0:a394a04bd0b7 | 661 | |
sk84life85 | 0:a394a04bd0b7 | 662 | for(i=0; i<60; i++){ |
sk84life85 | 0:a394a04bd0b7 | 663 | |
sk84life85 | 0:a394a04bd0b7 | 664 | //---------------------------- KX022 ---------------------------- |
sk84life85 | 0:a394a04bd0b7 | 665 | //Read KX022 Portion from the IC |
sk84life85 | 0:a394a04bd0b7 | 666 | i2c.write(BH1745_addr_w, &BH1745_Red_Data_LSB, 1, RepStart); |
sk84life85 | 0:a394a04bd0b7 | 667 | i2c.read(BH1745_addr_r, &BH1745_Content_ReadData[0], 8, NoRepStart); |
sk84life85 | 0:a394a04bd0b7 | 668 | |
sk84life85 | 0:a394a04bd0b7 | 669 | //reconfigure the data (taken from arduino code) |
sk84life85 | 0:a394a04bd0b7 | 670 | BH1745_Raw_R = (BH1745_Content_ReadData[1]<<8) | (BH1745_Content_ReadData[0]); |
sk84life85 | 0:a394a04bd0b7 | 671 | BH1745_Raw_G = (BH1745_Content_ReadData[3]<<8) | (BH1745_Content_ReadData[2]); |
sk84life85 | 0:a394a04bd0b7 | 672 | BH1745_Raw_B = (BH1745_Content_ReadData[5]<<8) | (BH1745_Content_ReadData[4]); |
sk84life85 | 0:a394a04bd0b7 | 673 | BH1745_Raw_C = (BH1745_Content_ReadData[7]<<8) | (BH1745_Content_ReadData[6]); |
sk84life85 | 0:a394a04bd0b7 | 674 | |
sk84life85 | 0:a394a04bd0b7 | 675 | LED_Enable = 0; |
sk84life85 | 0:a394a04bd0b7 | 676 | |
sk84life85 | 0:a394a04bd0b7 | 677 | BH1745_Red = (float)BH1745_Raw_R / 16384; |
sk84life85 | 0:a394a04bd0b7 | 678 | BH1745_Green = (float)BH1745_Raw_G / 16384; |
sk84life85 | 0:a394a04bd0b7 | 679 | BH1745_Blue = (float)BH1745_Raw_B / 16384; |
sk84life85 | 0:a394a04bd0b7 | 680 | BH1745_Clear = (float)BH1745_Raw_C / 16384; |
sk84life85 | 0:a394a04bd0b7 | 681 | |
sk84life85 | 0:a394a04bd0b7 | 682 | CIEx[i] = 0.232*BH1745_Red + 0.610*BH1745_Green + 0.055*BH1745_Blue; |
sk84life85 | 0:a394a04bd0b7 | 683 | if( BH1745_Clear < (0.2*BH1745_Green) ){ |
sk84life85 | 0:a394a04bd0b7 | 684 | if( BH1745_Blue < (0.45*BH1745_Green) ){ |
sk84life85 | 0:a394a04bd0b7 | 685 | CIEy[i] = 0.083*BH1745_Red + 0.860*BH1745_Green; |
sk84life85 | 0:a394a04bd0b7 | 686 | } |
sk84life85 | 0:a394a04bd0b7 | 687 | else{ |
sk84life85 | 0:a394a04bd0b7 | 688 | CIEy[i] = 0.070*BH1745_Red + 0.709*BH1745_Green; |
sk84life85 | 0:a394a04bd0b7 | 689 | } |
sk84life85 | 0:a394a04bd0b7 | 690 | } |
sk84life85 | 0:a394a04bd0b7 | 691 | else if( BH1745_Clear < (0.50*BH1745_Green) ){ |
sk84life85 | 0:a394a04bd0b7 | 692 | CIEy[i] = 0.110*BH1745_Red + 0.712*BH1745_Green; |
sk84life85 | 0:a394a04bd0b7 | 693 | } |
sk84life85 | 0:a394a04bd0b7 | 694 | else{ |
sk84life85 | 0:a394a04bd0b7 | 695 | CIEy[i] = 0.073*BH1745_Red + 0.864*BH1745_Green; |
sk84life85 | 0:a394a04bd0b7 | 696 | } |
sk84life85 | 0:a394a04bd0b7 | 697 | for(j=i; j>0; j--){ |
sk84life85 | 0:a394a04bd0b7 | 698 | tempCIE = CIEx[j]; |
sk84life85 | 0:a394a04bd0b7 | 699 | if(tempCIE<CIEx[j-1]){ |
sk84life85 | 0:a394a04bd0b7 | 700 | CIEx[j] = CIEx[j-1]; |
sk84life85 | 0:a394a04bd0b7 | 701 | CIEx[j-1] = tempCIE; |
sk84life85 | 0:a394a04bd0b7 | 702 | } |
sk84life85 | 0:a394a04bd0b7 | 703 | } |
sk84life85 | 0:a394a04bd0b7 | 704 | for(j=i; j>0; j--){ |
sk84life85 | 0:a394a04bd0b7 | 705 | tempCIE = CIEy[j]; |
sk84life85 | 0:a394a04bd0b7 | 706 | if(tempCIE<CIEy[j-1]){ |
sk84life85 | 0:a394a04bd0b7 | 707 | CIEy[j] = CIEy[j-1]; |
sk84life85 | 0:a394a04bd0b7 | 708 | CIEy[j-1] = tempCIE; |
sk84life85 | 0:a394a04bd0b7 | 709 | } |
sk84life85 | 0:a394a04bd0b7 | 710 | } |
sk84life85 | 0:a394a04bd0b7 | 711 | } |
sk84life85 | 0:a394a04bd0b7 | 712 | |
sk84life85 | 0:a394a04bd0b7 | 713 | |
sk84life85 | 0:a394a04bd0b7 | 714 | chromaX = (CIEx[28]+CIEx[29]+CIEx[30]+CIEx[31]+CIEx[32])/5.0; |
sk84life85 | 0:a394a04bd0b7 | 715 | chromaY = (CIEy[28]+CIEy[29]+CIEy[30]+CIEy[31]+CIEy[32])/5.0; |
sk84life85 | 0:a394a04bd0b7 | 716 | |
sk84life85 | 0:a394a04bd0b7 | 717 | if( (xPPU2_14-xThresh<chromaX) && (chromaX<xPPU2_14+xThresh) && (yPPU2_14-yThresh<chromaY) && (chromaY<yPPU2_14+yThresh) ){ |
sk84life85 | 0:a394a04bd0b7 | 718 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "2-14"); |
sk84life85 | 0:a394a04bd0b7 | 719 | m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len); |
sk84life85 | 0:a394a04bd0b7 | 720 | } |
sk84life85 | 0:a394a04bd0b7 | 721 | else if( (xPPU18_5-xThresh<chromaX) && (chromaX<xPPU18_5+xThresh) && (yPPU18_5-yThresh<chromaY) && (chromaY<yPPU18_5+yThresh) ){ |
sk84life85 | 0:a394a04bd0b7 | 722 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "18-5"); |
sk84life85 | 0:a394a04bd0b7 | 723 | m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len); |
sk84life85 | 0:a394a04bd0b7 | 724 | } |
sk84life85 | 0:a394a04bd0b7 | 725 | else if( (xPPU18_4-xThresh<chromaX) && (chromaX<xPPU18_4+xThresh) && (yPPU18_4-yThresh<chromaY) && (chromaY<yPPU18_4+yThresh) ){ |
sk84life85 | 0:a394a04bd0b7 | 726 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "18-4"); |
sk84life85 | 0:a394a04bd0b7 | 727 | m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len); |
sk84life85 | 0:a394a04bd0b7 | 728 | } |
sk84life85 | 0:a394a04bd0b7 | 729 | else if( (xPPU18_3-xThresh<chromaX) && (chromaX<xPPU18_3+xThresh) && (yPPU18_3-yThresh<chromaY) && (chromaY<yPPU18_3+yThresh) ){ |
sk84life85 | 0:a394a04bd0b7 | 730 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "18-3"); |
sk84life85 | 0:a394a04bd0b7 | 731 | m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len); |
sk84life85 | 0:a394a04bd0b7 | 732 | } |
sk84life85 | 0:a394a04bd0b7 | 733 | else if( (xPPU17_18-xThresh<chromaX) && (chromaX<xPPU17_18+xThresh) && (yPPU17_18-yThresh<chromaY) && (chromaY<yPPU17_18+yThresh) ){ |
sk84life85 | 0:a394a04bd0b7 | 734 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "17-18"); |
sk84life85 | 0:a394a04bd0b7 | 735 | m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len); |
sk84life85 | 0:a394a04bd0b7 | 736 | } |
sk84life85 | 0:a394a04bd0b7 | 737 | else if( (xPPU5_6-xThresh<chromaX) && (chromaX<xPPU5_6+xThresh) && (yPPU5_6-yThresh<chromaY) && (chromaY<yPPU5_6+yThresh) ){ |
sk84life85 | 0:a394a04bd0b7 | 738 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "5-6"); |
sk84life85 | 0:a394a04bd0b7 | 739 | m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len); |
sk84life85 | 0:a394a04bd0b7 | 740 | } |
sk84life85 | 0:a394a04bd0b7 | 741 | else if( (xPPU4_15-xThresh<chromaX) && (chromaX<xPPU4_15+xThresh) && (yPPU4_15-yThresh<chromaY) && (chromaY<yPPU4_15+yThresh) ){ |
sk84life85 | 0:a394a04bd0b7 | 742 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-15"); |
sk84life85 | 0:a394a04bd0b7 | 743 | m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len); |
sk84life85 | 0:a394a04bd0b7 | 744 | } |
sk84life85 | 0:a394a04bd0b7 | 745 | else if( (xPPU4_14-xThresh<chromaX) && (chromaX<xPPU4_14+xThresh) && (yPPU4_14-yThresh<chromaY) && (chromaY<yPPU4_14+yThresh) ){ |
sk84life85 | 0:a394a04bd0b7 | 746 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-14"); |
sk84life85 | 0:a394a04bd0b7 | 747 | m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len); |
sk84life85 | 0:a394a04bd0b7 | 748 | } |
sk84life85 | 0:a394a04bd0b7 | 749 | else if( (xPPU4_13-xThresh<chromaX) && (chromaX<xPPU4_13+xThresh) && (yPPU4_13-yThresh<chromaY) && (chromaY<yPPU4_13+yThresh) ){ |
sk84life85 | 0:a394a04bd0b7 | 750 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-13"); |
sk84life85 | 0:a394a04bd0b7 | 751 | m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len); |
sk84life85 | 0:a394a04bd0b7 | 752 | } |
sk84life85 | 0:a394a04bd0b7 | 753 | else if( (xPPU4_10-xThresh<chromaX) && (chromaX<xPPU4_10+xThresh) && (yPPU4_10-yThresh<chromaY) && (chromaY<yPPU4_10+yThresh) ){ |
sk84life85 | 0:a394a04bd0b7 | 754 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-10"); |
sk84life85 | 0:a394a04bd0b7 | 755 | m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len); |
sk84life85 | 0:a394a04bd0b7 | 756 | } |
sk84life85 | 0:a394a04bd0b7 | 757 | else if( (xPPU4_8-xThresh<chromaX) && (chromaX<xPPU4_8+xThresh) && (yPPU4_8-yThresh<chromaY) && (chromaY<yPPU4_8+yThresh) ){ |
sk84life85 | 0:a394a04bd0b7 | 758 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-8"); |
sk84life85 | 0:a394a04bd0b7 | 759 | m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len); |
sk84life85 | 0:a394a04bd0b7 | 760 | } |
sk84life85 | 1:88e3620627f9 | 761 | else if( (xPPU9_4-xThresh<chromaX) && (chromaX<xPPU9_4+xThresh) && (yPPU9_4-yThresh<chromaY) && (chromaY<yPPU9_4+yThresh) ){ |
sk84life85 | 1:88e3620627f9 | 762 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "9-4"); |
sk84life85 | 1:88e3620627f9 | 763 | m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len); |
sk84life85 | 1:88e3620627f9 | 764 | } |
sk84life85 | 1:88e3620627f9 | 765 | else if( (xPPU9_5-xThresh<chromaX) && (chromaX<xPPU9_5+xThresh) && (yPPU9_5-yThresh<chromaY) && (chromaY<yPPU9_5+yThresh) ){ |
sk84life85 | 1:88e3620627f9 | 766 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "9-5"); |
sk84life85 | 1:88e3620627f9 | 767 | m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len); |
sk84life85 | 1:88e3620627f9 | 768 | } |
sk84life85 | 0:a394a04bd0b7 | 769 | else{ |
sk84life85 | 0:a394a04bd0b7 | 770 | len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "Unregistered"); |
sk84life85 | 0:a394a04bd0b7 | 771 | m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len); |
sk84life85 | 0:a394a04bd0b7 | 772 | } |
sk84life85 | 0:a394a04bd0b7 | 773 | } |
sk84life85 | 0:a394a04bd0b7 | 774 | |
sk84life85 | 0:a394a04bd0b7 | 775 | } |
sk84life85 | 0:a394a04bd0b7 | 776 | else{ |
sk84life85 | 0:a394a04bd0b7 | 777 | isConnected = 0; |
sk84life85 | 0:a394a04bd0b7 | 778 | } |
sk84life85 | 0:a394a04bd0b7 | 779 | } |
sk84life85 | 0:a394a04bd0b7 | 780 | } |