Vesko Karadzhov / Mbed 2 deprecated FBRLogger

FBRLogger final version

Dependencies:   EthernetInterface MSCAN Nanopb SDFileSystem mbed-rtos mbed

main.cpp@8:99cca8c964e6, 2013-03-01 (annotated)

Committer:
intrinseca
Date:
Fri Mar 01 12:43:17 2013 +0000
Revision:
8:99cca8c964e6
Parent:
6:9d9e7c9adc7c
Child:
9:634a74d5aa41
buggy logging

Who changed what in which revision?

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