FBRLogger final version
Dependencies: EthernetInterface MSCAN Nanopb SDFileSystem mbed-rtos mbed
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()); } }