main.cpp

00001 // FBR Logger 2012
00002 
00003 #include "SDFileSystem.h"
00004 #include "CANComms.h"
00005 #include "State.h"
00006 #include <stdint.h>
00007 #include <fstream>
00008 #include <sstream>
00009 #include <iomanip>
00010 #include <cstring>
00011 #include "pb.h"
00012 #include "pb_encode.h"
00013 #include "fbr.pb.h"
00014 #include "EthernetInterface.h"
00015 #include "rtos.h"
00016 #include "MSCANHeader.h"
00017 
00018 #define LOGGING_INTERVAL    10
00019 #define ANALOG_SCALE        3.3
00020 
00021 //potential divider scaling factor ~= 2/3
00022 //#define ACCEL_SCALE         22.0 / (22.0 + 10.0)
00023 #define ACCEL_SCALE     1
00024 //#define ACCEL_BIAS      0.5
00025 #define ACCEL_BIAS      2.5 / 3.3
00026 //312mV per g at 5V full scale
00027 #define ACCEL_SENSITIVITY   (0.312 * ACCEL_SCALE) / ANALOG_SCALE
00028 
00029 State car;
00030 
00031 CANComms can(&car, false, false, 0.0);
00032 SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
00033 
00034 EthernetInterface eth;
00035 TCPSocketServer server;
00036 TCPSocketConnection client;
00037 
00038 AnalogIn analogInputs[] = {p15, p16, p17, p18, p19, p20};
00039 
00040 Thread* write_thread;
00041 Thread* can_thread;
00042 
00043 Ticker sample;
00044 DigitalOut led1(LED1);
00045 DigitalOut led2(LED2);
00046 DigitalOut led3(LED3);
00047 
00048 char logFileName[50];
00049 FileHandle* logFile;
00050 char logIndex = 0;
00051 char write_buf[256];
00052 
00053 char net_buf[1024];
00054 int net_buf_len;
00055 
00056 bool file_exists(const char * filename)
00057 {
00058     if (FILE * file = fopen(filename, "r")) {
00059         fclose(file);
00060         return true;
00061     }
00062     return false;
00063 }
00064 
00065 void take_sample(void const * arg)
00066 {
00067     //float raw_x = analogInputs[5].read();
00068     //float norm_x = raw_x - ACCEL_BIAS;
00069 
00070     //printf("raw: %.4f norm: %.4f sens:%.4f", raw_x, norm_x, ACCEL_SENSITIVITY);
00071 
00072     //car.accel_x = (analogInputs[4].read() - ACCEL_BIAS) / ACCEL_SENSITIVITY;
00073     //car.accel_y = (analogInputs[5].read() - ACCEL_BIAS) / ACCEL_SENSITIVITY;
00074     car.accel_x = 0.23 + ((3.3 * analogInputs[5].read()) - 2.5) / 0.312;
00075     car.accel_y = 0.23 + ((3.3 * analogInputs[4].read()) - 2.5) / 0.312;
00076 
00077     //printf("%.4f %.4f\n", car.accel_x, car.accel_y);
00078 
00079     led1 = !led1;
00080     write_thread->signal_set(0x1);
00081 }
00082 
00083 void write(void const* args)
00084 {
00085     ofstream out;
00086     telemetry_message telemetry;
00087 
00088     printf("Starting write thread\n");
00089 
00090     int buf_msg_count = 0;
00091 
00092     net_buf_len = 0;
00093 
00094     telemetry.has_rpm = true;
00095     telemetry.has_throttle_pos = true;
00096     telemetry.has_manifold_pres = true;
00097     telemetry.has_air_temp = true;
00098     telemetry.has_coolant_temp = true;
00099     telemetry.has_lambda = true;
00100     telemetry.has_speed = true;
00101     telemetry.has_accel_x = true;
00102     telemetry.has_accel_y = true;
00103     telemetry.has_gear = true;
00104     telemetry.has_oil_temp = true;
00105     telemetry.has_warnings = true;
00106     telemetry.has_voltage = true;
00107 
00108     while(true) {
00109         led2 = true;
00110         Thread::signal_wait(0x1);
00111         led2 = false;
00112 
00113         telemetry.rpm = car.rpm;
00114         telemetry.throttle_pos = car.throttle_pos;
00115         telemetry.manifold_pres = car.manifold_pres;
00116         telemetry.air_temp = car.air_temp;
00117         telemetry.coolant_temp = car.coolant_temp;
00118         telemetry.lambda = car.lambda;
00119         telemetry.speed = car.speed;
00120         telemetry.gear = car.gear;
00121         telemetry.oil_temp = car.oil_temp;
00122         telemetry.warnings = car.warnings;
00123         telemetry.voltage = car.voltage;
00124         telemetry.accel_y = car.accel_y;
00125         telemetry.accel_x = car.accel_x;
00126 
00127         //printf("Getting Size\n");
00128         pb_ostream_t pb_size = {0};
00129         pb_encode(&pb_size, telemetry_message_fields, &telemetry);
00130 
00131         //printf("Sending %d bytes\n", pb_size.bytes_written);
00132 
00133         memcpy(write_buf, (char*)&pb_size.bytes_written, 4);
00134         pb_ostream_t pb_out = pb_ostream_from_buffer((uint8_t*)&write_buf[4], sizeof(write_buf) - 4);
00135 
00136         //printf("%2X%2X%2X%2X", write_buf[0], write_buf[1], write_buf[2], write_buf[3]);
00137 
00138         //printf("Encoding PB\n");
00139         if(!pb_encode(&pb_out, telemetry_message_fields, &telemetry)) {
00140             printf("Encoder Error\n");
00141         }
00142 
00143         if(net_buf_len + pb_out.bytes_written + 4 > sizeof(net_buf)) {
00144             //printf("Writing to Network\n");
00145             if(client.is_connected()) {
00146                 client.send_all(net_buf, net_buf_len);
00147             }
00148             led3 = !led3;
00149 
00150             //printf("Net Buf Emptied, %d messages\n", buf_msg_count);
00151             logFile->write(net_buf, net_buf_len);
00152             logFile->fsync();
00153 
00154             net_buf_len = 0;
00155             buf_msg_count = 0;
00156         }
00157 
00158         buf_msg_count++;
00159         memcpy(&net_buf[net_buf_len], write_buf, pb_out.bytes_written + 4);
00160         net_buf_len += (pb_out.bytes_written + 4);
00161 
00162         //printf("Sent\n");
00163     }
00164 }
00165 
00166 void can_thread_start(void const* args)
00167 {
00168     CANMessage msg;
00169     MSCANHeader header;
00170 
00171     printf("CAN Thread Running\n");
00172 
00173     while(true) {
00174         can.receive();
00175        
00176     }
00177 }
00178 
00179 int main()
00180 {
00181     RtosTimer sample_timer(take_sample, osTimerPeriodic);
00182 
00183     printf("FBR CAN Data Logger\n");
00184 
00185     mkdir("/sd/fbr", 0777);
00186 
00187     do {
00188         sprintf(&logFileName[0], "/sd/fbr/log.%d", logIndex);
00189         logIndex++;
00190     } while(file_exists(&logFileName[0]));
00191 
00192     sprintf(&logFileName[0], "fbr/log.%d", logIndex - 1);
00193     printf("Log File: %s\n", &logFileName[0]);
00194 
00195     logFile = sd.open(logFileName, O_WRONLY | O_CREAT);
00196     logFile->fsync();
00197     //fprintf(logFile, "FBR CANBUS Log File\n");
00198     //fclose(logFile);
00199 
00200     eth.init("192.168.0.2", "255.255.255.0", "0.0.0.0");
00201     eth.connect(1000);
00202 
00203     server.bind(8282);
00204     server.listen();
00205 
00206     printf("Listening Started\n");
00207 
00208     write_thread = new Thread(write, NULL, osPriorityLow, 10240);
00209     can_thread = new Thread(can_thread_start, NULL, osPriorityLow, 4096);
00210     sample_timer.start(LOGGING_INTERVAL);
00211 
00212 
00213    
00214 
00215     printf("Ready to Log\n");
00216 
00217     TCPSocketConnection temp;
00218 
00219     while (true) {                                   // accept net connection
00220         printf("\nWait for new connection...\n");
00221         server.accept(temp);
00222 
00223         client.close();
00224         client = temp;
00225         client.set_blocking(true, 1500); // Timeout after (1.5)s
00226 
00227         printf("Connection from: %s\n", client.get_address());
00228     }
00229 }