![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Working firmware for simultaneous LiDAR and Magnetometer data retrieval over ethernet.
Dependencies: EthernetInterface FXOS8700Q LidarLite mbed-rtos mbed
Revision 0:ade62dde274b, committed 2015-12-10
- Comitter:
- jakelarsen17
- Date:
- Thu Dec 10 17:45:30 2015 +0000
- Commit message:
- Working firmware for simultaneous LiDAR and Magnetometer data retrieval over ethernet.
Changed in this revision
diff -r 000000000000 -r ade62dde274b EthernetInterface.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/EthernetInterface.lib Thu Dec 10 17:45:30 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/EthernetInterface/#2fc406e2553f
diff -r 000000000000 -r ade62dde274b FXOS8700Q.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/FXOS8700Q.lib Thu Dec 10 17:45:30 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/JimCarver/code/FXOS8700Q/#c53dda05b8cf
diff -r 000000000000 -r ade62dde274b LidarLite.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LidarLite.lib Thu Dec 10 17:45:30 2015 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/akashvibhute/code/LidarLite/#8e6304ab38d2
diff -r 000000000000 -r ade62dde274b main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Dec 10 17:45:30 2015 +0000 @@ -0,0 +1,283 @@ +/* +#include "mbed.h" + +DigitalOut gpo(D0); +DigitalOut led(LED_RED); + +int main() +{ + while (true) { + gpo = !gpo; // toggle pin + led = !led; // toggle led + wait(0.2f); + } +} +*/ + +// MalletFirmware, this is the real deal version +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * Pinout for FRDM-k64f + * J2 + * X X + * X X + * X X + * J3 X X GND + * X X X X SCLK + * X X X X MISO + * X X X X MOSI + * X X X X CS + * X X GND X X + * GND X X X X + * GND X X + * 5Vin X X J1 + * X X + * J4 X X motorA + * X X X X motorB + * mic1 X X X X + * mic2 X X X X + * X X X X + * X X quadA X X + * X X quadB X X Painter 1 + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ + +#include "mbed.h" +#include <string> + +//LiDAR +#include "LidarLite.h" +#define LIDARLite1_SDA PTC11 //SDA pin +#define LIDARLite1_SCL PTC10 //SCL pin + +//Magnetometer +#include "FXOS8700Q.h" +FXOS8700Q_mag mag( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1); // Proper Ports and I2C Address for K64F Freedom board +MotionSensorDataUnits mag_data; +MotionSensorDataCounts mag_raw; + +// Hardware delays +#include "pause.cpp" + +// Ethernet +#include "EthernetInterface.h" + +#include <stdio.h> +#include "PeripheralNames.h" +#include "PeripheralPins.h" + +// Settings for each mallet +#define NUM 9 // this number identifies the mallet and is a value between 1-7, (8 or 9 for the painters) +#define PORT (55000+NUM) // set to a random port number. All the mallets can use the same port number. +#define MAX_CLIENTS 2 // set the max number of clients to at least 2 (first client is MATLAB, second is the distance unit) +#define DEBUG 0 // '1' enables debug statements printed through serial port at baud 230400, '0' disables + +// Ethernet +#define LEN_PACKET 1460 +//#define N_PACKET (TOTAL_SAMPLES/LEN_PACKET) +#define GATEWAY "169.254.99.1" // set to match your computer +#define MASK "255.255.0.0" // set to match your computer (probably does already) +#define IP_LAPTOP "169.254.99.103" + +#define IP "169.254.99.139" + +// Status reporting + +// data good flags? + +// DMA status flag? + +// for debug purposes +Serial pc(USBTX, USBRX); +#if DEBUG +#define DEBUG_PRINT(x) pc.printf(x) +#else +#define DEBUG_PRINT(x) do {} while(0) +#endif +DigitalOut led_red(LED_RED); +DigitalOut led_green(LED_GREEN); +DigitalOut led_blue(LED_BLUE); +DigitalOut paint(D0); + +LidarLite sensor1(LIDARLite1_SDA, LIDARLite1_SCL); //Define LIDAR Lite sensor 1 +Timer dt; +float distance; + +float fmX, fmY, fmZ; +int16_t rmX, rmY, rmZ; + +char buffer[LEN_PACKET]; // general purpose tx/rx buffer + +// Declaration of functions +void clearBuffer(); + +using namespace std; + +int main() { + led_blue = 1; + led_green = 1; + led_red = 0; + + pc.baud(230400); + mag.enable(); + dt.start(); + + pc.printf("Starting M%i\r\n",NUM); + DEBUG_PRINT("HELLO\r\n"); + // Give everything lower priority + for(int i = 0; i < 86; i++) + { + if(NVIC_GetPriority((IRQn_Type) i) == 0) NVIC_SetPriority((IRQn_Type) i, 2); + } + + // Give hardware associated with + // sampling the highest priority + NVIC_SetPriority(ADC1_IRQn,0); + NVIC_SetPriority(ADC0_IRQn,0); + NVIC_SetPriority(PDB0_IRQn,0); + NVIC_SetPriority(DMA0_IRQn,0); + NVIC_SetPriority(DMA1_IRQn,0); + NVIC_SetPriority(DMA2_IRQn,0); + + NVIC_SetPriority(ENET_1588_Timer_IRQn,1); + NVIC_SetPriority(ENET_Transmit_IRQn,1); + NVIC_SetPriority(ENET_Receive_IRQn,1); + NVIC_SetPriority(ENET_Error_IRQn,1); + + + // The ethernet setup must be within the first few lines of code, otherwise the program hangs + EthernetInterface interface; + interface.init(IP, MASK, GATEWAY); + interface.connect(); + + led_green = 0; + + pc.printf("IP Address is: %s\n\r", interface.getIPAddress()); + pc.printf("Port is: %i\n\r", PORT); + ENET_TIPG = 0x08; // minimum time between TCP packets + + // ethernet setup failed for some reason. Flash yellow light then uC resets itself + if(interface.getIPAddress() == 0) + { + for(int i = 0; i < 5; i++) + { + // flash yellow LED + led_red = 0; + led_green = 0; + pause_ms(500); + led_red = 1; + led_green = 1; + pause_ms(1000); + } + NVIC_SystemReset(); + } + + + TCPSocketServer server; + server.bind(PORT); + server.listen(MAX_CLIENTS); + + led_green = 1; + led_red = 1; + led_blue = 1; + pc.printf("Server started\r\n"); + + while(true) { // tcp connect/disconnect loop + + TCPSocketConnection laptop; + server.accept(laptop); + laptop.set_blocking(false, 9000); // timeout after 9s + + bool ipVerified = true; + string ipAddress = laptop.get_address(); + if(ipAddress != IP_LAPTOP) ipVerified = false; + int n; + while(true) { // rx/tx loop + n = laptop.receive(buffer,LEN_PACKET); // this operation times out after 1.5s + if(n <= 0) break; // exit rx/tx loop + + if(!ipVerified){ + clearBuffer(); + sprintf(buffer,"Incorrect IP address"); + n = laptop.send(buffer,LEN_PACKET); + pc.printf("%s\r\n"); + break; // exit rx/tx loop + } + + if(buffer[0] == ':' && ipVerified) { + /* + if(n == 2){ + if(buffer[1] == '.'){ + paint = 1; + pause_ms(100); + paint = 0; + } + else if(buffer[1] == '-'){ + paint = 1; + pause_ms(300); + paint = 0; + } + } + */ + + //dt.start(); + + /* + pc.printf("range: %d cm, velocity: %d cm/s, rate: %.2f Hz\n", sensor1.getRange_cm(), sensor1.getVelocity_cms(), 1/dt.read()); + dt.reset(); + */ + + if(n == 2){ + if(buffer[1] == 'l'){ + sensor1.refreshRangeVelocity(); + //distance = double(sensor1.getRange_cm())*.393701; + //pc.printf("Distance: %f in\r\n", float(sensor1.getRange_cm())*.393701); + clearBuffer(); + sprintf(buffer, "Distance: %f inches", float(sensor1.getRange_cm())*.393701); + n = laptop.send(buffer,LEN_PACKET); + } + else if(buffer[1] == 'm'){ + clearBuffer(); + + mag.getAxis(mag_data); + //sprintf(buffer, "MAG: X=%4.1f Y=%4.1f Z=%4.1f\r\n", mag_data.x, mag_data.y, mag_data.z); + + mag.getX(&fmX); + mag.getY(&fmY); + mag.getZ(&fmZ); + //sprintf(buffer, "MAG: X=%4.1f Y=%4.1f Z=%4.1f\r\n", fmX, fmY, fmZ); + + mag.getAxis(mag_raw); + //sprintf(buffer, "MAG: X=%d Y=%d Z=%d\r\n", mag_raw.x, mag_raw.y, mag_raw.z); + + mag.getX(&rmX); + mag.getY(&rmY); + mag.getZ(&rmZ); + //sprintf(buffer, "MAG: X=%d Y=%d Z=%d\r\n", rmX, rmY, rmZ); + + 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); + n = laptop.send(buffer,LEN_PACKET); + } + else{ + clearBuffer(); + sprintf(buffer, "Invalid Input"); + n = laptop.send(buffer,LEN_PACKET); + } + } + else{ + clearBuffer(); + sprintf(buffer, "Invalid Input"); + n = laptop.send(buffer,LEN_PACKET); + } + //pc.printf("%s\r\n",buffer); + + } // end if(buffer[0] == ':') + if(n <= 0) break; + } // end while(true) rx/tx loop + } // end while(true) tcp connect/disconnect loop +} // end main + +void clearBuffer() { + for(int i = 0; i < 100; i++) buffer[i] = 0; +} +
diff -r 000000000000 -r ade62dde274b mbed-rtos.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-rtos.lib Thu Dec 10 17:45:30 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed-rtos/#2db19f47c2ba
diff -r 000000000000 -r ade62dde274b mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Dec 10 17:45:30 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/8ed44a420e5c \ No newline at end of file
diff -r 000000000000 -r ade62dde274b pause.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/pause.cpp Thu Dec 10 17:45:30 2015 +0000 @@ -0,0 +1,23 @@ +#ifndef PAUSE_CPP +#define PAUSE_CPP +#include "mbed.h" + +// http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.dui0496b/BABDJCCI.html +#pragma diag_suppress 174 // suppress the warning "Expression has no effect". This warning is displayed for each of these functions. + + + +// these are various delays +inline void pause(uint32_t seconds) { + for(seconds; seconds > 0; seconds--) for(uint32_t i = 0x1ffffff; i > 0; i--) asm("nop"); +} + +inline void pause_ms(uint32_t milliseconds) { + for(milliseconds; milliseconds > 0; milliseconds--) for(uint32_t i = 0x8312; i > 0; i--) asm("nop"); +} + +inline void pause_us(uint32_t microseconds) { + for(microseconds; microseconds > 0; microseconds--) for(uint32_t i = 0x21; i > 0; i--) asm("nop"); +} + +#endif \ No newline at end of file