Diff: main.cpp

Revision:

8:99cca8c964e6

Parent:

6:9d9e7c9adc7c

Child:

9:634a74d5aa41

--- a/main.cpp	Tue Feb 19 22:04:15 2013 +0000
+++ b/main.cpp	Fri Mar 01 12:43:17 2013 +0000
@@ -3,34 +3,52 @@
 #include "State.h"
 #include <stdint.h>
 #include <fstream>
+#include <sstream>
 #include <iomanip>
+#include <cstring>
 #include "pb.h"
 #include "pb_encode.h"
 #include "fbr.pb.h"
+#include "EthernetInterface.h"
+#include "rtos.h"
+#include "MSCANHeader.h"
 
-#define LOGGING_INTERVAL    0.1
+#define LOGGING_INTERVAL    0.025
 #define ANALOG_SCALE        3.3
 
-//potential divider scaling factor = 2/3
-#define ACCEL_SCALE         20.0 / (10.0 + 10.0)
-#define ACCEL_BIAS          0.5
+//potential divider scaling factor ~= 2/3
+//#define ACCEL_SCALE         22.0 / (22.0 + 10.0)
+#define ACCEL_SCALE     1
+//#define ACCEL_BIAS      0.5
+#define ACCEL_BIAS      2.5 / 3.3
 //312mV per g at 5V full scale
-#define ACCEL_SENSITIVITY   312 * ACCEL_SCALE
-
+#define ACCEL_SENSITIVITY   (0.312 * ACCEL_SCALE) / ANALOG_SCALE
 
 State car;
-CANComms* can;
+CAN* can;
+SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
 
-SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
+EthernetInterface eth;
+TCPSocketServer server;
+TCPSocketConnection client;
 
 AnalogIn analogInputs[] = {p15, p16, p17, p18, p19, p20};
 
+Thread* write_thread;
+Thread* can_thread;
+
 Ticker sample;
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
 
 char logFileName[50];
+FileHandle* logFile;
+char logIndex = 0;
+char write_buf[256];
 
-void writeLog_string(const char *data);
-void writeLog_data(const char *data, unsigned char length);
+char net_buf[1024];
+int net_buf_len;
 
 bool file_exists(const char * filename)
 {
@@ -41,56 +59,153 @@
     return false;
 }
 
-static bool write_callback(pb_ostream_t *stream, const uint8_t *buf, size_t count)
+void take_sample(void const * arg)
 {
-    ostream* out = (ostream*)stream->state;
-    out->write(reinterpret_cast<const char*>(buf), count);
-    return out->bad();
+    //float raw_x = analogInputs[5].read();
+    //float norm_x = raw_x - ACCEL_BIAS;
+
+    //printf("raw: %.4f norm: %.4f sens:%.4f", raw_x, norm_x, ACCEL_SENSITIVITY);
+
+    //car.accel_x = (analogInputs[4].read() - ACCEL_BIAS) / ACCEL_SENSITIVITY;
+    //car.accel_y = (analogInputs[5].read() - ACCEL_BIAS) / ACCEL_SENSITIVITY;
+    car.accel_x = 0.23 + ((3.3 * analogInputs[5].read()) - 2.5) / 0.312;
+    car.accel_y = 0.23 + ((3.3 * analogInputs[4].read()) - 2.5) / 0.312;
+
+    //printf("%.4f %.4f\n", car.accel_x, car.accel_y);
+
+    led1 = !led1;
+    write_thread->signal_set(0x1);
 }
 
-void take_sample()
+void write(void const* args)
 {
     ofstream out;
     telemetry_message telemetry;
-    
-    pb_ostream_t pb_out = {&write_callback, out, 1000, 0};
-    uint8_t zero = 0;
-    
-    telemetry.rpm = car.rpm;
-    telemetry.throttle_pos = car.throttle_pos;
-    telemetry.manifold_pres = car.manifold_pres;
-    telemetry.air_temp = car.air_temp;
-    telemetry.coolant_temp = car.coolant_temp;
-    telemetry.lambda = car.lambda;
-    telemetry.speed = car.speed;
-    telemetry.gear = car.gear;
-    telemetry.oil_temp = car.oil_temp;
-    telemetry.warnings = car.warnings;
-    telemetry.voltage = car.voltage;
-    
-    //convert the accelerometer values to g
-    telemetry.accel_x = (analogInputs[4].read() - ACCEL_BIAS) / ACCEL_SENSITIVITY;
-    telemetry.accel_y = (analogInputs[5].read() - ACCEL_BIAS) / ACCEL_SENSITIVITY;;
-    
-    pb_encode(&pb_out, telemetry_message_fields, &telemetry);
-    pb_write(&pb_out, &zero, 1);
-    
-//    float value;
-//
-//    out.open(logFileName);
-//
-//    // Write the Analog Sensors
-//
-//    //Write the ECU data (in binary form)
-//    out.write(reinterpret_cast<char*>(&car), sizeof(State));
-//    
-//    out << endl;
-//    out.close();
+
+    printf("Starting write thread\n");
+
+    int buf_msg_count = 0;
+
+    net_buf_len = 0;
+
+    telemetry.has_rpm = true;
+    telemetry.has_throttle_pos = true;
+    telemetry.has_manifold_pres = true;
+    telemetry.has_air_temp = true;
+    telemetry.has_coolant_temp = true;
+    telemetry.has_lambda = true;
+    telemetry.has_speed = true;
+    telemetry.has_accel_x = true;
+    telemetry.has_accel_y = true;
+    telemetry.has_gear = true;
+    telemetry.has_oil_temp = true;
+    telemetry.has_warnings = true;
+    telemetry.has_voltage = true;
+
+    while(true) {
+        led2 = true;
+        Thread::signal_wait(0x1);
+        led2 = false;
+
+        telemetry.rpm = car.rpm;
+        telemetry.throttle_pos = car.throttle_pos;
+        telemetry.manifold_pres = car.manifold_pres;
+        telemetry.air_temp = car.air_temp;
+        telemetry.coolant_temp = car.coolant_temp;
+        telemetry.lambda = car.lambda;
+        telemetry.speed = car.speed;
+        telemetry.gear = car.gear;
+        telemetry.oil_temp = car.oil_temp;
+        telemetry.warnings = car.warnings;
+        telemetry.voltage = car.voltage;
+        telemetry.accel_y = car.accel_y;
+        telemetry.accel_x = car.accel_x;
+
+        //printf("Getting Size\n");
+        pb_ostream_t pb_size = {0};
+        pb_encode(&pb_size, telemetry_message_fields, &telemetry);
+
+        //printf("Sending %d bytes\n", pb_size.bytes_written);
+
+        memcpy(write_buf, (char*)&pb_size.bytes_written, 4);
+        pb_ostream_t pb_out = pb_ostream_from_buffer((uint8_t*)&write_buf[4], sizeof(write_buf) - 4);
+
+        //printf("%2X%2X%2X%2X", write_buf[0], write_buf[1], write_buf[2], write_buf[3]);
+
+        //printf("Encoding PB\n");
+        if(!pb_encode(&pb_out, telemetry_message_fields, &telemetry)) {
+            printf("Encoder Error\n");
+        }
+
+        if(net_buf_len + pb_out.bytes_written + 4 > sizeof(net_buf)) {
+            //printf("Writing to Network\n");
+            if(client.is_connected()) {
+                client.send_all(net_buf, net_buf_len);
+            }
+            led3 = !led3;
+            
+            //printf("Net Buf Emptied, %d messages\n", buf_msg_count);
+            logFile->write(net_buf, net_buf_len);
+            logFile->fsync();
+
+            net_buf_len = 0;
+            buf_msg_count = 0;
+        }
+
+        buf_msg_count++;
+        memcpy(&net_buf[net_buf_len], write_buf, pb_out.bytes_written + 4);
+        net_buf_len += (pb_out.bytes_written + 4);
+
+        //printf("Sent\n");
+    }
+}
+
+void can_thread_start(void const* args)
+{
+    CANMessage msg;
+    MSCANHeader header;
+
+    printf("CAN Thread Running\n");
+
+    while(true) {
+        while(can->read(msg)) {
+            /*printf("CAN Message %08X %d %02X%02X%02X%02X%02X%02X%02X%02X\n", msg.id, msg.len,
+                msg.data[0],
+                msg.data[1],
+                msg.data[2],
+                msg.data[3],
+                msg.data[4],
+                msg.data[5],
+                msg.data[6],
+                msg.data[7]
+                );*/
+
+            header.parse(msg.id);
+
+            //printf("Processing data, block %d\n", block);
+
+            if(header.from_id == MSCAN_ID_MS) {
+                if(header.var_blk == 0) {
+                    car.rpm = (msg.data[0] << 8) | msg.data[1];
+                } else if(header.var_blk == 1) {
+                    car.manifold_pres = ((msg.data[2] << 8) | msg.data[3]) / 10.0;
+                    car.air_temp = (((msg.data[4] << 8) | msg.data[5]) - 320.0) * 0.05555;
+                    car.coolant_temp = (((msg.data[6] << 8) | msg.data[7]) - 320.0) * 0.05555;
+                }
+                else {
+                    car.throttle_pos = ((msg.data[0] << 8) | msg.data[1]) / 10.0;
+                    car.voltage = ((msg.data[2] << 8) | msg.data[3]) / 10.0;
+                }
+            }
+        }
+        
+        Thread::wait(1);
+    }
 }
 
 int main()
 {
-    char logIndex = 0;
+    RtosTimer sample_timer(take_sample, osTimerPeriodic);
 
     printf("FBR CAN Data Logger\n");
 
@@ -101,14 +216,44 @@
         logIndex++;
     } while(file_exists(&logFileName[0]));
 
+    sprintf(&logFileName[0], "fbr/log.%d", logIndex - 1);
     printf("Log File: %s\n", &logFileName[0]);
+    
+    logFile = sd.open(logFileName, O_WRONLY | O_CREAT);
+    logFile->fsync();
+    //fprintf(logFile, "FBR CANBUS Log File\n");
+    //fclose(logFile);
+            
+    eth.init("192.168.0.2", "255.255.255.0", "0.0.0.0");
+    eth.connect(1000);
+
+    server.bind(8282);
+    server.listen();
 
     printf("Listening Started\n");
 
-    can = new CANComms(&car, true, LOGGING_INTERVAL);
-    sample.attach(&take_sample, LOGGING_INTERVAL);
+    write_thread = new Thread(write, NULL, osPriorityLow, 10240);
+    can_thread = new Thread(can_thread_start, NULL, osPriorityLow, 4096);
+    sample_timer.start(10);
+
+    
+    can = new CAN(p30, p29);
+    can->frequency(500000);
+    //can->attach(&can_receive_isr); 
+    printf("Can ISR Attached");
+
+    printf("Ready to Log\n");
 
-    while(true) {
-        __wfi();
+    TCPSocketConnection temp;
+
+    while (true) {
+        printf("\nWait for new connection...\n");
+        server.accept(temp);
+
+        client.close();
+        client = temp;
+        client.set_blocking(true, 1500); // Timeout after (1.5)s
+
+        printf("Connection from: %s\n", client.get_address());
     }
 }