Impact-Echo / Mbed 2 deprecated Lidar_Magnet

Working firmware for simultaneous LiDAR and Magnetometer data retrieval over ethernet.

Dependencies:   EthernetInterface FXOS8700Q LidarLite mbed-rtos mbed

main.cpp@0:ade62dde274b, 2015-12-10 (annotated)

Committer:
jakelarsen17
Date:
Thu Dec 10 17:45:30 2015 +0000
Revision:
0:ade62dde274b
Working firmware for simultaneous LiDAR and Magnetometer data retrieval over ethernet.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jakelarsen17 0:ade62dde274b 1 /*
jakelarsen17 0:ade62dde274b 2 #include "mbed.h"
jakelarsen17 0:ade62dde274b 3
jakelarsen17 0:ade62dde274b 4 DigitalOut gpo(D0);
jakelarsen17 0:ade62dde274b 5 DigitalOut led(LED_RED);
jakelarsen17 0:ade62dde274b 6
jakelarsen17 0:ade62dde274b 7 int main()
jakelarsen17 0:ade62dde274b 8 {
jakelarsen17 0:ade62dde274b 9 while (true) {
jakelarsen17 0:ade62dde274b 10 gpo = !gpo; // toggle pin
jakelarsen17 0:ade62dde274b 11 led = !led; // toggle led
jakelarsen17 0:ade62dde274b 12 wait(0.2f);
jakelarsen17 0:ade62dde274b 13 }
jakelarsen17 0:ade62dde274b 14 }
jakelarsen17 0:ade62dde274b 15 */
jakelarsen17 0:ade62dde274b 16
jakelarsen17 0:ade62dde274b 17 // MalletFirmware, this is the real deal version
jakelarsen17 0:ade62dde274b 18 /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
jakelarsen17 0:ade62dde274b 19 *
jakelarsen17 0:ade62dde274b 20 * Pinout for FRDM-k64f
jakelarsen17 0:ade62dde274b 21 * J2
jakelarsen17 0:ade62dde274b 22 * X X
jakelarsen17 0:ade62dde274b 23 * X X
jakelarsen17 0:ade62dde274b 24 * X X
jakelarsen17 0:ade62dde274b 25 * J3 X X GND
jakelarsen17 0:ade62dde274b 26 * X X X X SCLK
jakelarsen17 0:ade62dde274b 27 * X X X X MISO
jakelarsen17 0:ade62dde274b 28 * X X X X MOSI
jakelarsen17 0:ade62dde274b 29 * X X X X CS
jakelarsen17 0:ade62dde274b 30 * X X GND X X
jakelarsen17 0:ade62dde274b 31 * GND X X X X
jakelarsen17 0:ade62dde274b 32 * GND X X
jakelarsen17 0:ade62dde274b 33 * 5Vin X X J1
jakelarsen17 0:ade62dde274b 34 * X X
jakelarsen17 0:ade62dde274b 35 * J4 X X motorA
jakelarsen17 0:ade62dde274b 36 * X X X X motorB
jakelarsen17 0:ade62dde274b 37 * mic1 X X X X
jakelarsen17 0:ade62dde274b 38 * mic2 X X X X
jakelarsen17 0:ade62dde274b 39 * X X X X
jakelarsen17 0:ade62dde274b 40 * X X quadA X X
jakelarsen17 0:ade62dde274b 41 * X X quadB X X Painter 1
jakelarsen17 0:ade62dde274b 42 *
jakelarsen17 0:ade62dde274b 43 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
jakelarsen17 0:ade62dde274b 44
jakelarsen17 0:ade62dde274b 45 #include "mbed.h"
jakelarsen17 0:ade62dde274b 46 #include <string>
jakelarsen17 0:ade62dde274b 47
jakelarsen17 0:ade62dde274b 48 //LiDAR
jakelarsen17 0:ade62dde274b 49 #include "LidarLite.h"
jakelarsen17 0:ade62dde274b 50 #define LIDARLite1_SDA PTC11 //SDA pin
jakelarsen17 0:ade62dde274b 51 #define LIDARLite1_SCL PTC10 //SCL pin
jakelarsen17 0:ade62dde274b 52
jakelarsen17 0:ade62dde274b 53 //Magnetometer
jakelarsen17 0:ade62dde274b 54 #include "FXOS8700Q.h"
jakelarsen17 0:ade62dde274b 55 FXOS8700Q_mag mag( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1); // Proper Ports and I2C Address for K64F Freedom board
jakelarsen17 0:ade62dde274b 56 MotionSensorDataUnits mag_data;
jakelarsen17 0:ade62dde274b 57 MotionSensorDataCounts mag_raw;
jakelarsen17 0:ade62dde274b 58
jakelarsen17 0:ade62dde274b 59 // Hardware delays
jakelarsen17 0:ade62dde274b 60 #include "pause.cpp"
jakelarsen17 0:ade62dde274b 61
jakelarsen17 0:ade62dde274b 62 // Ethernet
jakelarsen17 0:ade62dde274b 63 #include "EthernetInterface.h"
jakelarsen17 0:ade62dde274b 64
jakelarsen17 0:ade62dde274b 65 #include <stdio.h>
jakelarsen17 0:ade62dde274b 66 #include "PeripheralNames.h"
jakelarsen17 0:ade62dde274b 67 #include "PeripheralPins.h"
jakelarsen17 0:ade62dde274b 68
jakelarsen17 0:ade62dde274b 69 // Settings for each mallet
jakelarsen17 0:ade62dde274b 70 #define NUM 9 // this number identifies the mallet and is a value between 1-7, (8 or 9 for the painters)
jakelarsen17 0:ade62dde274b 71 #define PORT (55000+NUM) // set to a random port number. All the mallets can use the same port number.
jakelarsen17 0:ade62dde274b 72 #define MAX_CLIENTS 2 // set the max number of clients to at least 2 (first client is MATLAB, second is the distance unit)
jakelarsen17 0:ade62dde274b 73 #define DEBUG 0 // '1' enables debug statements printed through serial port at baud 230400, '0' disables
jakelarsen17 0:ade62dde274b 74
jakelarsen17 0:ade62dde274b 75 // Ethernet
jakelarsen17 0:ade62dde274b 76 #define LEN_PACKET 1460
jakelarsen17 0:ade62dde274b 77 //#define N_PACKET (TOTAL_SAMPLES/LEN_PACKET)
jakelarsen17 0:ade62dde274b 78 #define GATEWAY "169.254.99.1" // set to match your computer
jakelarsen17 0:ade62dde274b 79 #define MASK "255.255.0.0" // set to match your computer (probably does already)
jakelarsen17 0:ade62dde274b 80 #define IP_LAPTOP "169.254.99.103"
jakelarsen17 0:ade62dde274b 81
jakelarsen17 0:ade62dde274b 82 #define IP "169.254.99.139"
jakelarsen17 0:ade62dde274b 83
jakelarsen17 0:ade62dde274b 84 // Status reporting
jakelarsen17 0:ade62dde274b 85
jakelarsen17 0:ade62dde274b 86 // data good flags?
jakelarsen17 0:ade62dde274b 87
jakelarsen17 0:ade62dde274b 88 // DMA status flag?
jakelarsen17 0:ade62dde274b 89
jakelarsen17 0:ade62dde274b 90 // for debug purposes
jakelarsen17 0:ade62dde274b 91 Serial pc(USBTX, USBRX);
jakelarsen17 0:ade62dde274b 92 #if DEBUG
jakelarsen17 0:ade62dde274b 93 #define DEBUG_PRINT(x) pc.printf(x)
jakelarsen17 0:ade62dde274b 94 #else
jakelarsen17 0:ade62dde274b 95 #define DEBUG_PRINT(x) do {} while(0)
jakelarsen17 0:ade62dde274b 96 #endif
jakelarsen17 0:ade62dde274b 97 DigitalOut led_red(LED_RED);
jakelarsen17 0:ade62dde274b 98 DigitalOut led_green(LED_GREEN);
jakelarsen17 0:ade62dde274b 99 DigitalOut led_blue(LED_BLUE);
jakelarsen17 0:ade62dde274b 100 DigitalOut paint(D0);
jakelarsen17 0:ade62dde274b 101
jakelarsen17 0:ade62dde274b 102 LidarLite sensor1(LIDARLite1_SDA, LIDARLite1_SCL); //Define LIDAR Lite sensor 1
jakelarsen17 0:ade62dde274b 103 Timer dt;
jakelarsen17 0:ade62dde274b 104 float distance;
jakelarsen17 0:ade62dde274b 105
jakelarsen17 0:ade62dde274b 106 float fmX, fmY, fmZ;
jakelarsen17 0:ade62dde274b 107 int16_t rmX, rmY, rmZ;
jakelarsen17 0:ade62dde274b 108
jakelarsen17 0:ade62dde274b 109 char buffer[LEN_PACKET]; // general purpose tx/rx buffer
jakelarsen17 0:ade62dde274b 110
jakelarsen17 0:ade62dde274b 111 // Declaration of functions
jakelarsen17 0:ade62dde274b 112 void clearBuffer();
jakelarsen17 0:ade62dde274b 113
jakelarsen17 0:ade62dde274b 114 using namespace std;
jakelarsen17 0:ade62dde274b 115
jakelarsen17 0:ade62dde274b 116 int main() {
jakelarsen17 0:ade62dde274b 117 led_blue = 1;
jakelarsen17 0:ade62dde274b 118 led_green = 1;
jakelarsen17 0:ade62dde274b 119 led_red = 0;
jakelarsen17 0:ade62dde274b 120
jakelarsen17 0:ade62dde274b 121 pc.baud(230400);
jakelarsen17 0:ade62dde274b 122 mag.enable();
jakelarsen17 0:ade62dde274b 123 dt.start();
jakelarsen17 0:ade62dde274b 124
jakelarsen17 0:ade62dde274b 125 pc.printf("Starting M%i\r\n",NUM);
jakelarsen17 0:ade62dde274b 126 DEBUG_PRINT("HELLO\r\n");
jakelarsen17 0:ade62dde274b 127 // Give everything lower priority
jakelarsen17 0:ade62dde274b 128 for(int i = 0; i < 86; i++)
jakelarsen17 0:ade62dde274b 129 {
jakelarsen17 0:ade62dde274b 130 if(NVIC_GetPriority((IRQn_Type) i) == 0) NVIC_SetPriority((IRQn_Type) i, 2);
jakelarsen17 0:ade62dde274b 131 }
jakelarsen17 0:ade62dde274b 132
jakelarsen17 0:ade62dde274b 133 // Give hardware associated with
jakelarsen17 0:ade62dde274b 134 // sampling the highest priority
jakelarsen17 0:ade62dde274b 135 NVIC_SetPriority(ADC1_IRQn,0);
jakelarsen17 0:ade62dde274b 136 NVIC_SetPriority(ADC0_IRQn,0);
jakelarsen17 0:ade62dde274b 137 NVIC_SetPriority(PDB0_IRQn,0);
jakelarsen17 0:ade62dde274b 138 NVIC_SetPriority(DMA0_IRQn,0);
jakelarsen17 0:ade62dde274b 139 NVIC_SetPriority(DMA1_IRQn,0);
jakelarsen17 0:ade62dde274b 140 NVIC_SetPriority(DMA2_IRQn,0);
jakelarsen17 0:ade62dde274b 141
jakelarsen17 0:ade62dde274b 142 NVIC_SetPriority(ENET_1588_Timer_IRQn,1);
jakelarsen17 0:ade62dde274b 143 NVIC_SetPriority(ENET_Transmit_IRQn,1);
jakelarsen17 0:ade62dde274b 144 NVIC_SetPriority(ENET_Receive_IRQn,1);
jakelarsen17 0:ade62dde274b 145 NVIC_SetPriority(ENET_Error_IRQn,1);
jakelarsen17 0:ade62dde274b 146
jakelarsen17 0:ade62dde274b 147
jakelarsen17 0:ade62dde274b 148 // The ethernet setup must be within the first few lines of code, otherwise the program hangs
jakelarsen17 0:ade62dde274b 149 EthernetInterface interface;
jakelarsen17 0:ade62dde274b 150 interface.init(IP, MASK, GATEWAY);
jakelarsen17 0:ade62dde274b 151 interface.connect();
jakelarsen17 0:ade62dde274b 152
jakelarsen17 0:ade62dde274b 153 led_green = 0;
jakelarsen17 0:ade62dde274b 154
jakelarsen17 0:ade62dde274b 155 pc.printf("IP Address is: %s\n\r", interface.getIPAddress());
jakelarsen17 0:ade62dde274b 156 pc.printf("Port is: %i\n\r", PORT);
jakelarsen17 0:ade62dde274b 157 ENET_TIPG = 0x08; // minimum time between TCP packets
jakelarsen17 0:ade62dde274b 158
jakelarsen17 0:ade62dde274b 159 // ethernet setup failed for some reason. Flash yellow light then uC resets itself
jakelarsen17 0:ade62dde274b 160 if(interface.getIPAddress() == 0)
jakelarsen17 0:ade62dde274b 161 {
jakelarsen17 0:ade62dde274b 162 for(int i = 0; i < 5; i++)
jakelarsen17 0:ade62dde274b 163 {
jakelarsen17 0:ade62dde274b 164 // flash yellow LED
jakelarsen17 0:ade62dde274b 165 led_red = 0;
jakelarsen17 0:ade62dde274b 166 led_green = 0;
jakelarsen17 0:ade62dde274b 167 pause_ms(500);
jakelarsen17 0:ade62dde274b 168 led_red = 1;
jakelarsen17 0:ade62dde274b 169 led_green = 1;
jakelarsen17 0:ade62dde274b 170 pause_ms(1000);
jakelarsen17 0:ade62dde274b 171 }
jakelarsen17 0:ade62dde274b 172 NVIC_SystemReset();
jakelarsen17 0:ade62dde274b 173 }
jakelarsen17 0:ade62dde274b 174
jakelarsen17 0:ade62dde274b 175
jakelarsen17 0:ade62dde274b 176 TCPSocketServer server;
jakelarsen17 0:ade62dde274b 177 server.bind(PORT);
jakelarsen17 0:ade62dde274b 178 server.listen(MAX_CLIENTS);
jakelarsen17 0:ade62dde274b 179
jakelarsen17 0:ade62dde274b 180 led_green = 1;
jakelarsen17 0:ade62dde274b 181 led_red = 1;
jakelarsen17 0:ade62dde274b 182 led_blue = 1;
jakelarsen17 0:ade62dde274b 183 pc.printf("Server started\r\n");
jakelarsen17 0:ade62dde274b 184
jakelarsen17 0:ade62dde274b 185 while(true) { // tcp connect/disconnect loop
jakelarsen17 0:ade62dde274b 186
jakelarsen17 0:ade62dde274b 187 TCPSocketConnection laptop;
jakelarsen17 0:ade62dde274b 188 server.accept(laptop);
jakelarsen17 0:ade62dde274b 189 laptop.set_blocking(false, 9000); // timeout after 9s
jakelarsen17 0:ade62dde274b 190
jakelarsen17 0:ade62dde274b 191 bool ipVerified = true;
jakelarsen17 0:ade62dde274b 192 string ipAddress = laptop.get_address();
jakelarsen17 0:ade62dde274b 193 if(ipAddress != IP_LAPTOP) ipVerified = false;
jakelarsen17 0:ade62dde274b 194 int n;
jakelarsen17 0:ade62dde274b 195 while(true) { // rx/tx loop
jakelarsen17 0:ade62dde274b 196 n = laptop.receive(buffer,LEN_PACKET); // this operation times out after 1.5s
jakelarsen17 0:ade62dde274b 197 if(n <= 0) break; // exit rx/tx loop
jakelarsen17 0:ade62dde274b 198
jakelarsen17 0:ade62dde274b 199 if(!ipVerified){
jakelarsen17 0:ade62dde274b 200 clearBuffer();
jakelarsen17 0:ade62dde274b 201 sprintf(buffer,"Incorrect IP address");
jakelarsen17 0:ade62dde274b 202 n = laptop.send(buffer,LEN_PACKET);
jakelarsen17 0:ade62dde274b 203 pc.printf("%s\r\n");
jakelarsen17 0:ade62dde274b 204 break; // exit rx/tx loop
jakelarsen17 0:ade62dde274b 205 }
jakelarsen17 0:ade62dde274b 206
jakelarsen17 0:ade62dde274b 207 if(buffer[0] == ':' && ipVerified) {
jakelarsen17 0:ade62dde274b 208 /*
jakelarsen17 0:ade62dde274b 209 if(n == 2){
jakelarsen17 0:ade62dde274b 210 if(buffer[1] == '.'){
jakelarsen17 0:ade62dde274b 211 paint = 1;
jakelarsen17 0:ade62dde274b 212 pause_ms(100);
jakelarsen17 0:ade62dde274b 213 paint = 0;
jakelarsen17 0:ade62dde274b 214 }
jakelarsen17 0:ade62dde274b 215 else if(buffer[1] == '-'){
jakelarsen17 0:ade62dde274b 216 paint = 1;
jakelarsen17 0:ade62dde274b 217 pause_ms(300);
jakelarsen17 0:ade62dde274b 218 paint = 0;
jakelarsen17 0:ade62dde274b 219 }
jakelarsen17 0:ade62dde274b 220 }
jakelarsen17 0:ade62dde274b 221 */
jakelarsen17 0:ade62dde274b 222
jakelarsen17 0:ade62dde274b 223 //dt.start();
jakelarsen17 0:ade62dde274b 224
jakelarsen17 0:ade62dde274b 225 /*
jakelarsen17 0:ade62dde274b 226 pc.printf("range: %d cm, velocity: %d cm/s, rate: %.2f Hz\n", sensor1.getRange_cm(), sensor1.getVelocity_cms(), 1/dt.read());
jakelarsen17 0:ade62dde274b 227 dt.reset();
jakelarsen17 0:ade62dde274b 228 */
jakelarsen17 0:ade62dde274b 229
jakelarsen17 0:ade62dde274b 230 if(n == 2){
jakelarsen17 0:ade62dde274b 231 if(buffer[1] == 'l'){
jakelarsen17 0:ade62dde274b 232 sensor1.refreshRangeVelocity();
jakelarsen17 0:ade62dde274b 233 //distance = double(sensor1.getRange_cm())*.393701;
jakelarsen17 0:ade62dde274b 234 //pc.printf("Distance: %f in\r\n", float(sensor1.getRange_cm())*.393701);
jakelarsen17 0:ade62dde274b 235 clearBuffer();
jakelarsen17 0:ade62dde274b 236 sprintf(buffer, "Distance: %f inches", float(sensor1.getRange_cm())*.393701);
jakelarsen17 0:ade62dde274b 237 n = laptop.send(buffer,LEN_PACKET);
jakelarsen17 0:ade62dde274b 238 }
jakelarsen17 0:ade62dde274b 239 else if(buffer[1] == 'm'){
jakelarsen17 0:ade62dde274b 240 clearBuffer();
jakelarsen17 0:ade62dde274b 241
jakelarsen17 0:ade62dde274b 242 mag.getAxis(mag_data);
jakelarsen17 0:ade62dde274b 243 //sprintf(buffer, "MAG: X=%4.1f Y=%4.1f Z=%4.1f\r\n", mag_data.x, mag_data.y, mag_data.z);
jakelarsen17 0:ade62dde274b 244
jakelarsen17 0:ade62dde274b 245 mag.getX(&fmX);
jakelarsen17 0:ade62dde274b 246 mag.getY(&fmY);
jakelarsen17 0:ade62dde274b 247 mag.getZ(&fmZ);
jakelarsen17 0:ade62dde274b 248 //sprintf(buffer, "MAG: X=%4.1f Y=%4.1f Z=%4.1f\r\n", fmX, fmY, fmZ);
jakelarsen17 0:ade62dde274b 249
jakelarsen17 0:ade62dde274b 250 mag.getAxis(mag_raw);
jakelarsen17 0:ade62dde274b 251 //sprintf(buffer, "MAG: X=%d Y=%d Z=%d\r\n", mag_raw.x, mag_raw.y, mag_raw.z);
jakelarsen17 0:ade62dde274b 252
jakelarsen17 0:ade62dde274b 253 mag.getX(&rmX);
jakelarsen17 0:ade62dde274b 254 mag.getY(&rmY);
jakelarsen17 0:ade62dde274b 255 mag.getZ(&rmZ);
jakelarsen17 0:ade62dde274b 256 //sprintf(buffer, "MAG: X=%d Y=%d Z=%d\r\n", rmX, rmY, rmZ);
jakelarsen17 0:ade62dde274b 257
jakelarsen17 0:ade62dde274b 258 sprintf(buffer, "MAG: X=%4.1f Y=%4.1f Z=%4.1f\r\nMAG: X=%4.1f Y=%4.1f Z=%4.1f\r\nMAG: X=%d Y=%d Z=%d\r\nMAG: X=%d Y=%d Z=%d\r\n\n", mag_data.x, mag_data.y, mag_data.z, fmX, fmY, fmZ, mag_raw.x, mag_raw.y, mag_raw.z, rmX, rmY, rmZ);
jakelarsen17 0:ade62dde274b 259 n = laptop.send(buffer,LEN_PACKET);
jakelarsen17 0:ade62dde274b 260 }
jakelarsen17 0:ade62dde274b 261 else{
jakelarsen17 0:ade62dde274b 262 clearBuffer();
jakelarsen17 0:ade62dde274b 263 sprintf(buffer, "Invalid Input");
jakelarsen17 0:ade62dde274b 264 n = laptop.send(buffer,LEN_PACKET);
jakelarsen17 0:ade62dde274b 265 }
jakelarsen17 0:ade62dde274b 266 }
jakelarsen17 0:ade62dde274b 267 else{
jakelarsen17 0:ade62dde274b 268 clearBuffer();
jakelarsen17 0:ade62dde274b 269 sprintf(buffer, "Invalid Input");
jakelarsen17 0:ade62dde274b 270 n = laptop.send(buffer,LEN_PACKET);
jakelarsen17 0:ade62dde274b 271 }
jakelarsen17 0:ade62dde274b 272 //pc.printf("%s\r\n",buffer);
jakelarsen17 0:ade62dde274b 273
jakelarsen17 0:ade62dde274b 274 } // end if(buffer[0] == ':')
jakelarsen17 0:ade62dde274b 275 if(n <= 0) break;
jakelarsen17 0:ade62dde274b 276 } // end while(true) rx/tx loop
jakelarsen17 0:ade62dde274b 277 } // end while(true) tcp connect/disconnect loop
jakelarsen17 0:ade62dde274b 278 } // end main
jakelarsen17 0:ade62dde274b 279
jakelarsen17 0:ade62dde274b 280 void clearBuffer() {
jakelarsen17 0:ade62dde274b 281 for(int i = 0; i < 100; i++) buffer[i] = 0;
jakelarsen17 0:ade62dde274b 282 }
jakelarsen17 0:ade62dde274b 283