Vesko Karadzhov / Mbed 2 deprecated FBRLogger

FBRLogger final version

Dependencies:   EthernetInterface MSCAN Nanopb SDFileSystem mbed-rtos mbed

main.cpp@11:594a9f668ccf, 2013-03-08 (annotated)

Committer:
veskokaradzhov
Date:
Fri Mar 08 17:00:04 2013 +0000
Revision:
11:594a9f668ccf
Parent:
9:634a74d5aa41
Child:
12:8ab0f26e65df
Logging code (final version)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
veskokaradzhov 11:594a9f668ccf 1 // FBR Logger 2012
veskokaradzhov 11:594a9f668ccf 2
intrinseca 6:9d9e7c9adc7c 3 #include "SDFileSystem.h"
intrinseca 2:2400fab06b33 4 #include "CANComms.h"
intrinseca 2:2400fab06b33 5 #include "State.h"
intrinseca 1:5f51069d5e09 6 #include <stdint.h>
intrinseca 3:32206cf84eb4 7 #include <fstream>
intrinseca 8:99cca8c964e6 8 #include <sstream>
intrinseca 3:32206cf84eb4 9 #include <iomanip>
intrinseca 8:99cca8c964e6 10 #include <cstring>
intrinseca 4:66928695da01 11 #include "pb.h"
intrinseca 4:66928695da01 12 #include "pb_encode.h"
intrinseca 4:66928695da01 13 #include "fbr.pb.h"
intrinseca 8:99cca8c964e6 14 #include "EthernetInterface.h"
intrinseca 8:99cca8c964e6 15 #include "rtos.h"
intrinseca 8:99cca8c964e6 16 #include "MSCANHeader.h"
intrinseca 3:32206cf84eb4 17
intrinseca 8:99cca8c964e6 18 #define LOGGING_INTERVAL 0.025
intrinseca 3:32206cf84eb4 19 #define ANALOG_SCALE 3.3
intrinseca 1:5f51069d5e09 20
intrinseca 8:99cca8c964e6 21 //potential divider scaling factor ~= 2/3
intrinseca 8:99cca8c964e6 22 //#define ACCEL_SCALE 22.0 / (22.0 + 10.0)
intrinseca 8:99cca8c964e6 23 #define ACCEL_SCALE 1
intrinseca 8:99cca8c964e6 24 //#define ACCEL_BIAS 0.5
intrinseca 8:99cca8c964e6 25 #define ACCEL_BIAS 2.5 / 3.3
intrinseca 5:3c4f35ea3cd9 26 //312mV per g at 5V full scale
intrinseca 8:99cca8c964e6 27 #define ACCEL_SENSITIVITY (0.312 * ACCEL_SCALE) / ANALOG_SCALE
intrinseca 5:3c4f35ea3cd9 28
intrinseca 2:2400fab06b33 29 State car;
veskokaradzhov 9:634a74d5aa41 30
veskokaradzhov 9:634a74d5aa41 31 CANComms can(&car, false, false, 0.0);
intrinseca 8:99cca8c964e6 32 SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
intrinseca 2:2400fab06b33 33
intrinseca 8:99cca8c964e6 34 EthernetInterface eth;
intrinseca 8:99cca8c964e6 35 TCPSocketServer server;
intrinseca 8:99cca8c964e6 36 TCPSocketConnection client;
intrinseca 2:2400fab06b33 37
intrinseca 3:32206cf84eb4 38 AnalogIn analogInputs[] = {p15, p16, p17, p18, p19, p20};
intrinseca 2:2400fab06b33 39
intrinseca 8:99cca8c964e6 40 Thread* write_thread;
intrinseca 8:99cca8c964e6 41 Thread* can_thread;
intrinseca 8:99cca8c964e6 42
intrinseca 2:2400fab06b33 43 Ticker sample;
intrinseca 8:99cca8c964e6 44 DigitalOut led1(LED1);
intrinseca 8:99cca8c964e6 45 DigitalOut led2(LED2);
intrinseca 8:99cca8c964e6 46 DigitalOut led3(LED3);
intrinseca 1:5f51069d5e09 47
intrinseca 1:5f51069d5e09 48 char logFileName[50];
intrinseca 8:99cca8c964e6 49 FileHandle* logFile;
intrinseca 8:99cca8c964e6 50 char logIndex = 0;
intrinseca 8:99cca8c964e6 51 char write_buf[256];
intrinseca 1:5f51069d5e09 52
intrinseca 8:99cca8c964e6 53 char net_buf[1024];
intrinseca 8:99cca8c964e6 54 int net_buf_len;
intrinseca 1:5f51069d5e09 55
intrinseca 1:5f51069d5e09 56 bool file_exists(const char * filename)
intrinseca 1:5f51069d5e09 57 {
intrinseca 3:32206cf84eb4 58 if (FILE * file = fopen(filename, "r")) {
intrinseca 1:5f51069d5e09 59 fclose(file);
intrinseca 1:5f51069d5e09 60 return true;
intrinseca 1:5f51069d5e09 61 }
intrinseca 1:5f51069d5e09 62 return false;
intrinseca 1:5f51069d5e09 63 }
intrinseca 1:5f51069d5e09 64
intrinseca 8:99cca8c964e6 65 void take_sample(void const * arg)
intrinseca 4:66928695da01 66 {
intrinseca 8:99cca8c964e6 67 //float raw_x = analogInputs[5].read();
intrinseca 8:99cca8c964e6 68 //float norm_x = raw_x - ACCEL_BIAS;
intrinseca 8:99cca8c964e6 69
intrinseca 8:99cca8c964e6 70 //printf("raw: %.4f norm: %.4f sens:%.4f", raw_x, norm_x, ACCEL_SENSITIVITY);
intrinseca 8:99cca8c964e6 71
intrinseca 8:99cca8c964e6 72 //car.accel_x = (analogInputs[4].read() - ACCEL_BIAS) / ACCEL_SENSITIVITY;
intrinseca 8:99cca8c964e6 73 //car.accel_y = (analogInputs[5].read() - ACCEL_BIAS) / ACCEL_SENSITIVITY;
intrinseca 8:99cca8c964e6 74 car.accel_x = 0.23 + ((3.3 * analogInputs[5].read()) - 2.5) / 0.312;
intrinseca 8:99cca8c964e6 75 car.accel_y = 0.23 + ((3.3 * analogInputs[4].read()) - 2.5) / 0.312;
intrinseca 8:99cca8c964e6 76
intrinseca 8:99cca8c964e6 77 //printf("%.4f %.4f\n", car.accel_x, car.accel_y);
intrinseca 8:99cca8c964e6 78
intrinseca 8:99cca8c964e6 79 led1 = !led1;
intrinseca 8:99cca8c964e6 80 write_thread->signal_set(0x1);
intrinseca 4:66928695da01 81 }
intrinseca 4:66928695da01 82
intrinseca 8:99cca8c964e6 83 void write(void const* args)
intrinseca 2:2400fab06b33 84 {
intrinseca 3:32206cf84eb4 85 ofstream out;
intrinseca 4:66928695da01 86 telemetry_message telemetry;
intrinseca 8:99cca8c964e6 87
intrinseca 8:99cca8c964e6 88 printf("Starting write thread\n");
intrinseca 8:99cca8c964e6 89
intrinseca 8:99cca8c964e6 90 int buf_msg_count = 0;
intrinseca 8:99cca8c964e6 91
intrinseca 8:99cca8c964e6 92 net_buf_len = 0;
intrinseca 8:99cca8c964e6 93
intrinseca 8:99cca8c964e6 94 telemetry.has_rpm = true;
intrinseca 8:99cca8c964e6 95 telemetry.has_throttle_pos = true;
intrinseca 8:99cca8c964e6 96 telemetry.has_manifold_pres = true;
intrinseca 8:99cca8c964e6 97 telemetry.has_air_temp = true;
intrinseca 8:99cca8c964e6 98 telemetry.has_coolant_temp = true;
intrinseca 8:99cca8c964e6 99 telemetry.has_lambda = true;
intrinseca 8:99cca8c964e6 100 telemetry.has_speed = true;
intrinseca 8:99cca8c964e6 101 telemetry.has_accel_x = true;
intrinseca 8:99cca8c964e6 102 telemetry.has_accel_y = true;
intrinseca 8:99cca8c964e6 103 telemetry.has_gear = true;
intrinseca 8:99cca8c964e6 104 telemetry.has_oil_temp = true;
intrinseca 8:99cca8c964e6 105 telemetry.has_warnings = true;
intrinseca 8:99cca8c964e6 106 telemetry.has_voltage = true;
intrinseca 8:99cca8c964e6 107
intrinseca 8:99cca8c964e6 108 while(true) {
intrinseca 8:99cca8c964e6 109 led2 = true;
intrinseca 8:99cca8c964e6 110 Thread::signal_wait(0x1);
intrinseca 8:99cca8c964e6 111 led2 = false;
intrinseca 8:99cca8c964e6 112
intrinseca 8:99cca8c964e6 113 telemetry.rpm = car.rpm;
intrinseca 8:99cca8c964e6 114 telemetry.throttle_pos = car.throttle_pos;
intrinseca 8:99cca8c964e6 115 telemetry.manifold_pres = car.manifold_pres;
intrinseca 8:99cca8c964e6 116 telemetry.air_temp = car.air_temp;
intrinseca 8:99cca8c964e6 117 telemetry.coolant_temp = car.coolant_temp;
intrinseca 8:99cca8c964e6 118 telemetry.lambda = car.lambda;
intrinseca 8:99cca8c964e6 119 telemetry.speed = car.speed;
intrinseca 8:99cca8c964e6 120 telemetry.gear = car.gear;
intrinseca 8:99cca8c964e6 121 telemetry.oil_temp = car.oil_temp;
intrinseca 8:99cca8c964e6 122 telemetry.warnings = car.warnings;
intrinseca 8:99cca8c964e6 123 telemetry.voltage = car.voltage;
intrinseca 8:99cca8c964e6 124 telemetry.accel_y = car.accel_y;
intrinseca 8:99cca8c964e6 125 telemetry.accel_x = car.accel_x;
intrinseca 8:99cca8c964e6 126
intrinseca 8:99cca8c964e6 127 //printf("Getting Size\n");
intrinseca 8:99cca8c964e6 128 pb_ostream_t pb_size = {0};
intrinseca 8:99cca8c964e6 129 pb_encode(&pb_size, telemetry_message_fields, &telemetry);
intrinseca 8:99cca8c964e6 130
intrinseca 8:99cca8c964e6 131 //printf("Sending %d bytes\n", pb_size.bytes_written);
intrinseca 8:99cca8c964e6 132
intrinseca 8:99cca8c964e6 133 memcpy(write_buf, (char*)&pb_size.bytes_written, 4);
intrinseca 8:99cca8c964e6 134 pb_ostream_t pb_out = pb_ostream_from_buffer((uint8_t*)&write_buf[4], sizeof(write_buf) - 4);
intrinseca 8:99cca8c964e6 135
intrinseca 8:99cca8c964e6 136 //printf("%2X%2X%2X%2X", write_buf[0], write_buf[1], write_buf[2], write_buf[3]);
intrinseca 8:99cca8c964e6 137
intrinseca 8:99cca8c964e6 138 //printf("Encoding PB\n");
intrinseca 8:99cca8c964e6 139 if(!pb_encode(&pb_out, telemetry_message_fields, &telemetry)) {
intrinseca 8:99cca8c964e6 140 printf("Encoder Error\n");
intrinseca 8:99cca8c964e6 141 }
intrinseca 8:99cca8c964e6 142
intrinseca 8:99cca8c964e6 143 if(net_buf_len + pb_out.bytes_written + 4 > sizeof(net_buf)) {
intrinseca 8:99cca8c964e6 144 //printf("Writing to Network\n");
intrinseca 8:99cca8c964e6 145 if(client.is_connected()) {
intrinseca 8:99cca8c964e6 146 client.send_all(net_buf, net_buf_len);
intrinseca 8:99cca8c964e6 147 }
intrinseca 8:99cca8c964e6 148 led3 = !led3;
veskokaradzhov 9:634a74d5aa41 149
intrinseca 8:99cca8c964e6 150 //printf("Net Buf Emptied, %d messages\n", buf_msg_count);
intrinseca 8:99cca8c964e6 151 logFile->write(net_buf, net_buf_len);
intrinseca 8:99cca8c964e6 152 logFile->fsync();
intrinseca 8:99cca8c964e6 153
intrinseca 8:99cca8c964e6 154 net_buf_len = 0;
intrinseca 8:99cca8c964e6 155 buf_msg_count = 0;
intrinseca 8:99cca8c964e6 156 }
intrinseca 8:99cca8c964e6 157
intrinseca 8:99cca8c964e6 158 buf_msg_count++;
intrinseca 8:99cca8c964e6 159 memcpy(&net_buf[net_buf_len], write_buf, pb_out.bytes_written + 4);
intrinseca 8:99cca8c964e6 160 net_buf_len += (pb_out.bytes_written + 4);
intrinseca 8:99cca8c964e6 161
intrinseca 8:99cca8c964e6 162 //printf("Sent\n");
intrinseca 8:99cca8c964e6 163 }
intrinseca 8:99cca8c964e6 164 }
intrinseca 8:99cca8c964e6 165
intrinseca 8:99cca8c964e6 166 void can_thread_start(void const* args)
intrinseca 8:99cca8c964e6 167 {
intrinseca 8:99cca8c964e6 168 CANMessage msg;
intrinseca 8:99cca8c964e6 169 MSCANHeader header;
intrinseca 8:99cca8c964e6 170
intrinseca 8:99cca8c964e6 171 printf("CAN Thread Running\n");
intrinseca 8:99cca8c964e6 172
intrinseca 8:99cca8c964e6 173 while(true) {
veskokaradzhov 9:634a74d5aa41 174 can.receive();
veskokaradzhov 9:634a74d5aa41 175
intrinseca 8:99cca8c964e6 176 }
intrinseca 2:2400fab06b33 177 }
intrinseca 2:2400fab06b33 178
intrinseca 3:32206cf84eb4 179 int main()
intrinseca 3:32206cf84eb4 180 {
intrinseca 8:99cca8c964e6 181 RtosTimer sample_timer(take_sample, osTimerPeriodic);
intrinseca 3:32206cf84eb4 182
intrinseca 1:5f51069d5e09 183 printf("FBR CAN Data Logger\n");
intrinseca 1:5f51069d5e09 184
intrinseca 2:2400fab06b33 185 mkdir("/sd/fbr", 0777);
intrinseca 3:32206cf84eb4 186
intrinseca 3:32206cf84eb4 187 do {
intrinseca 1:5f51069d5e09 188 sprintf(&logFileName[0], "/sd/fbr/log.%d", logIndex);
intrinseca 1:5f51069d5e09 189 logIndex++;
intrinseca 1:5f51069d5e09 190 } while(file_exists(&logFileName[0]));
intrinseca 3:32206cf84eb4 191
intrinseca 8:99cca8c964e6 192 sprintf(&logFileName[0], "fbr/log.%d", logIndex - 1);
intrinseca 2:2400fab06b33 193 printf("Log File: %s\n", &logFileName[0]);
veskokaradzhov 9:634a74d5aa41 194
intrinseca 8:99cca8c964e6 195 logFile = sd.open(logFileName, O_WRONLY | O_CREAT);
intrinseca 8:99cca8c964e6 196 logFile->fsync();
intrinseca 8:99cca8c964e6 197 //fprintf(logFile, "FBR CANBUS Log File\n");
intrinseca 8:99cca8c964e6 198 //fclose(logFile);
veskokaradzhov 9:634a74d5aa41 199
intrinseca 8:99cca8c964e6 200 eth.init("192.168.0.2", "255.255.255.0", "0.0.0.0");
intrinseca 8:99cca8c964e6 201 eth.connect(1000);
intrinseca 8:99cca8c964e6 202
intrinseca 8:99cca8c964e6 203 server.bind(8282);
intrinseca 8:99cca8c964e6 204 server.listen();
intrinseca 3:32206cf84eb4 205
intrinseca 1:5f51069d5e09 206 printf("Listening Started\n");
intrinseca 1:5f51069d5e09 207
intrinseca 8:99cca8c964e6 208 write_thread = new Thread(write, NULL, osPriorityLow, 10240);
intrinseca 8:99cca8c964e6 209 can_thread = new Thread(can_thread_start, NULL, osPriorityLow, 4096);
intrinseca 8:99cca8c964e6 210 sample_timer.start(10);
intrinseca 8:99cca8c964e6 211
veskokaradzhov 9:634a74d5aa41 212
veskokaradzhov 9:634a74d5aa41 213
intrinseca 8:99cca8c964e6 214
intrinseca 8:99cca8c964e6 215 printf("Ready to Log\n");
intrinseca 1:5f51069d5e09 216
intrinseca 8:99cca8c964e6 217 TCPSocketConnection temp;
intrinseca 8:99cca8c964e6 218
veskokaradzhov 9:634a74d5aa41 219 while (true) { // accept net connection
intrinseca 8:99cca8c964e6 220 printf("\nWait for new connection...\n");
intrinseca 8:99cca8c964e6 221 server.accept(temp);
intrinseca 8:99cca8c964e6 222
intrinseca 8:99cca8c964e6 223 client.close();
intrinseca 8:99cca8c964e6 224 client = temp;
intrinseca 8:99cca8c964e6 225 client.set_blocking(true, 1500); // Timeout after (1.5)s
intrinseca 8:99cca8c964e6 226
intrinseca 8:99cca8c964e6 227 printf("Connection from: %s\n", client.get_address());
intrinseca 1:5f51069d5e09 228 }
intrinseca 3:32206cf84eb4 229 }