FBRLogger final version
Dependencies: EthernetInterface MSCAN Nanopb SDFileSystem mbed-rtos mbed
main.cpp
- Committer:
- veskokaradzhov
- Date:
- 2013-03-06
- Revision:
- 9:634a74d5aa41
- Parent:
- 8:99cca8c964e6
- Child:
- 11:594a9f668ccf
File content as of revision 9:634a74d5aa41:
#include "SDFileSystem.h" #include "CANComms.h" #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.025 #define ANALOG_SCALE 3.3 //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 (0.312 * ACCEL_SCALE) / ANALOG_SCALE State car; CANComms can(&car, false, false, 0.0); 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]; char net_buf[1024]; int net_buf_len; bool file_exists(const char * filename) { if (FILE * file = fopen(filename, "r")) { fclose(file); return true; } return false; } void take_sample(void const * arg) { //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 write(void const* args) { ofstream out; telemetry_message telemetry; 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) { can.receive(); } } int main() { RtosTimer sample_timer(take_sample, osTimerPeriodic); printf("FBR CAN Data Logger\n"); mkdir("/sd/fbr", 0777); do { sprintf(&logFileName[0], "/sd/fbr/log.%d", logIndex); 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"); write_thread = new Thread(write, NULL, osPriorityLow, 10240); can_thread = new Thread(can_thread_start, NULL, osPriorityLow, 4096); sample_timer.start(10); printf("Ready to Log\n"); TCPSocketConnection temp; while (true) { // accept net connection 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()); } }