Vesko Karadzhov / Mbed 2 deprecated FBRLogger

FBRLogger final version

Dependencies:   EthernetInterface MSCAN Nanopb SDFileSystem mbed-rtos mbed

main.cpp@9:634a74d5aa41, 2013-03-06 (annotated)

Committer:
veskokaradzhov
Date:
Wed Mar 06 17:34:00 2013 +0000
Revision:
9:634a74d5aa41
Parent:
8:99cca8c964e6
Child:
11:594a9f668ccf
bug fixed, function receive() moved to public

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