Impact-Echo
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
--- /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
--- /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
--- /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
--- /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;
+}
+
--- /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
--- /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
--- /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