Einstein Filho / Mbed OS MANGUEBAJA2019_REAR2

Mangue Baja team's code to rear ECU

main.cpp@0:80950b84a6c4, 2019-07-24 (annotated)

Committer:
einsteingustavo
Date:
Wed Jul 24 20:04:55 2019 +0000
Revision:
0:80950b84a6c4
Mangue Baja team's code to rear ECU

Who changed what in which revision?

UserRevisionLine numberNew contents of line
einsteingustavo 0:80950b84a6c4 1 #include "mbed.h"
einsteingustavo 0:80950b84a6c4 2 #include "stats_report.h"
einsteingustavo 0:80950b84a6c4 3 /* User libraries */
einsteingustavo 0:80950b84a6c4 4 #include "definitions.h"
einsteingustavo 0:80950b84a6c4 5 #include "reardefs.h"
einsteingustavo 0:80950b84a6c4 6 #include "CANMsg.h"
einsteingustavo 0:80950b84a6c4 7 #include "RFM69.h"
einsteingustavo 0:80950b84a6c4 8
einsteingustavo 0:80950b84a6c4 9 #define MB1 // uncomment this line if MB1
einsteingustavo 0:80950b84a6c4 10 //#define MB2 // uncomment this line if MB2
einsteingustavo 0:80950b84a6c4 11
einsteingustavo 0:80950b84a6c4 12 #ifdef MB1
einsteingustavo 0:80950b84a6c4 13 #define NODE_ID MB1_ID
einsteingustavo 0:80950b84a6c4 14 #endif
einsteingustavo 0:80950b84a6c4 15 #ifdef MB2
einsteingustavo 0:80950b84a6c4 16 #define NODE_ID MB2_ID
einsteingustavo 0:80950b84a6c4 17 #endif
einsteingustavo 0:80950b84a6c4 18
einsteingustavo 0:80950b84a6c4 19 /* Communication protocols */
einsteingustavo 0:80950b84a6c4 20 CAN can(PB_8, PB_9, 1000000);
einsteingustavo 0:80950b84a6c4 21 Serial serial(PA_2, PA_3, 115200);
einsteingustavo 0:80950b84a6c4 22 RFM69 radio(PB_15, PB_14, PB_13, PB_12, PA_8); // RFM69::RFM69(PinName PinName mosi, PinName miso, PinName sclk,slaveSelectPin, PinName int)
einsteingustavo 0:80950b84a6c4 23 /* I/O pins */
einsteingustavo 0:80950b84a6c4 24 AnalogIn analog(PA_0);
einsteingustavo 0:80950b84a6c4 25 #ifdef MB1
einsteingustavo 0:80950b84a6c4 26 DigitalIn fuel_sensor(PB_5, PullNone); // MB2
einsteingustavo 0:80950b84a6c4 27 InterruptIn freq_sensor(PB_4, PullNone); // MB2
einsteingustavo 0:80950b84a6c4 28 #endif
einsteingustavo 0:80950b84a6c4 29 #ifdef MB2
einsteingustavo 0:80950b84a6c4 30 DigitalIn fuel_sensor(PB_6, PullNone); // MB2
einsteingustavo 0:80950b84a6c4 31 InterruptIn freq_sensor(PB_5, PullNone); // MB2
einsteingustavo 0:80950b84a6c4 32 #endif
einsteingustavo 0:80950b84a6c4 33 PwmOut servo(PA_6);
einsteingustavo 0:80950b84a6c4 34 /* Debug pins */
einsteingustavo 0:80950b84a6c4 35 PwmOut signal(PA_7);
einsteingustavo 0:80950b84a6c4 36 DigitalOut led(PC_13);
einsteingustavo 0:80950b84a6c4 37 DigitalOut dbg1(PC_14);
einsteingustavo 0:80950b84a6c4 38 DigitalOut dbg2(PC_15);
einsteingustavo 0:80950b84a6c4 39 DigitalOut dbg3(PA_1);
einsteingustavo 0:80950b84a6c4 40 DigitalOut dbg4(PA_4);
einsteingustavo 0:80950b84a6c4 41 /* Interrupt services routine */
einsteingustavo 0:80950b84a6c4 42 void canISR();
einsteingustavo 0:80950b84a6c4 43 void servoSwitchISR();
einsteingustavo 0:80950b84a6c4 44 void ticker1HzISR();
einsteingustavo 0:80950b84a6c4 45 void ticker5HzISR();
einsteingustavo 0:80950b84a6c4 46 void ticker100HzISR();
einsteingustavo 0:80950b84a6c4 47 void frequencyCounterISR();
einsteingustavo 0:80950b84a6c4 48 /* Interrupt handlers */
einsteingustavo 0:80950b84a6c4 49 void canHandler();
einsteingustavo 0:80950b84a6c4 50 /* General functions*/
einsteingustavo 0:80950b84a6c4 51 void initPWM();
einsteingustavo 0:80950b84a6c4 52 void initRadio();
einsteingustavo 0:80950b84a6c4 53 void setupInterrupts();
einsteingustavo 0:80950b84a6c4 54 void filterMessage(CANMsg msg);
einsteingustavo 0:80950b84a6c4 55 void writeServo(uint8_t state);
einsteingustavo 0:80950b84a6c4 56
einsteingustavo 0:80950b84a6c4 57 /* Debug variables */
einsteingustavo 0:80950b84a6c4 58 Timer t;
einsteingustavo 0:80950b84a6c4 59 Timer engine_counter;
einsteingustavo 0:80950b84a6c4 60 bool buffer_full = false;
einsteingustavo 0:80950b84a6c4 61 uint32_t tim1, tim2, imu_last_acq = 0;
einsteingustavo 0:80950b84a6c4 62
einsteingustavo 0:80950b84a6c4 63 /* Mbed OS tools */
einsteingustavo 0:80950b84a6c4 64 Thread eventThread;
einsteingustavo 0:80950b84a6c4 65 EventQueue queue(1024);
einsteingustavo 0:80950b84a6c4 66 Ticker ticker1Hz;
einsteingustavo 0:80950b84a6c4 67 Ticker ticker5Hz;
einsteingustavo 0:80950b84a6c4 68 Ticker ticker100Hz;
einsteingustavo 0:80950b84a6c4 69 Timeout fuel_timeout;
einsteingustavo 0:80950b84a6c4 70 CircularBuffer <state_t, 2*BUFFER_SIZE> state_buffer;
einsteingustavo 0:80950b84a6c4 71 CircularBuffer <imu_t*, 20> imu_buffer;
einsteingustavo 0:80950b84a6c4 72
einsteingustavo 0:80950b84a6c4 73 /* Global variables */
einsteingustavo 0:80950b84a6c4 74 bool switch_clicked = false;
einsteingustavo 0:80950b84a6c4 75 uint8_t switch_state = 0x00;
einsteingustavo 0:80950b84a6c4 76 state_t current_state = IDLE_ST;
einsteingustavo 0:80950b84a6c4 77 uint8_t pulse_counter = 0;
einsteingustavo 0:80950b84a6c4 78 uint64_t current_period = 0, last_count = 0;
einsteingustavo 0:80950b84a6c4 79 float rpm_hz, V_termistor = 0;
einsteingustavo 0:80950b84a6c4 80 uint8_t fuel_timer = 0;
einsteingustavo 0:80950b84a6c4 81 uint8_t fuel_counter = 0;
einsteingustavo 0:80950b84a6c4 82 bool box = false;
einsteingustavo 0:80950b84a6c4 83 packet_t data; // Create package for radio comunication
einsteingustavo 0:80950b84a6c4 84 uint8_t imu_counter;
einsteingustavo 0:80950b84a6c4 85
einsteingustavo 0:80950b84a6c4 86 int main()
einsteingustavo 0:80950b84a6c4 87 {
einsteingustavo 0:80950b84a6c4 88 /* Main variables */
einsteingustavo 0:80950b84a6c4 89 CANMsg txMsg;
einsteingustavo 0:80950b84a6c4 90 /* Initialization */
einsteingustavo 0:80950b84a6c4 91 t.start();
einsteingustavo 0:80950b84a6c4 92 eventThread.start(callback(&queue, &EventQueue::dispatch_forever));
einsteingustavo 0:80950b84a6c4 93 initPWM();
einsteingustavo 0:80950b84a6c4 94 initRadio();
einsteingustavo 0:80950b84a6c4 95 setupInterrupts();
einsteingustavo 0:80950b84a6c4 96
einsteingustavo 0:80950b84a6c4 97 while (true) {
einsteingustavo 0:80950b84a6c4 98 if (state_buffer.full())
einsteingustavo 0:80950b84a6c4 99 {
einsteingustavo 0:80950b84a6c4 100 buffer_full = true;
einsteingustavo 0:80950b84a6c4 101 led = 0;
einsteingustavo 0:80950b84a6c4 102 }
einsteingustavo 0:80950b84a6c4 103 else
einsteingustavo 0:80950b84a6c4 104 {
einsteingustavo 0:80950b84a6c4 105 led = 1;
einsteingustavo 0:80950b84a6c4 106 buffer_full = false;
einsteingustavo 0:80950b84a6c4 107 if (!state_buffer.empty())
einsteingustavo 0:80950b84a6c4 108 state_buffer.pop(current_state);
einsteingustavo 0:80950b84a6c4 109 else
einsteingustavo 0:80950b84a6c4 110 current_state = IDLE_ST;
einsteingustavo 0:80950b84a6c4 111 }
einsteingustavo 0:80950b84a6c4 112
einsteingustavo 0:80950b84a6c4 113 switch (current_state)
einsteingustavo 0:80950b84a6c4 114 {
einsteingustavo 0:80950b84a6c4 115 case IDLE_ST:
einsteingustavo 0:80950b84a6c4 116 // Thread::wait(1);
einsteingustavo 0:80950b84a6c4 117 break;
einsteingustavo 0:80950b84a6c4 118 case TEMP_ST:
einsteingustavo 0:80950b84a6c4 119 //serial.printf("t");
einsteingustavo 0:80950b84a6c4 120 dbg1 = 0;
einsteingustavo 0:80950b84a6c4 121 V_termistor = VCC*analog.read();
einsteingustavo 0:80950b84a6c4 122 data.temperature = ((float) (1.0/0.032)*log((1842.8*(VCC - V_termistor)/(V_termistor*R_TERM))));
einsteingustavo 0:80950b84a6c4 123 /* Send temperature data */
einsteingustavo 0:80950b84a6c4 124 txMsg.clear(TEMPERATURE_ID);
einsteingustavo 0:80950b84a6c4 125 txMsg << data.temperature;
einsteingustavo 0:80950b84a6c4 126 can.write(txMsg);
einsteingustavo 0:80950b84a6c4 127 break;
einsteingustavo 0:80950b84a6c4 128 case FUEL_ST:
einsteingustavo 0:80950b84a6c4 129 //serial.printf("f");
einsteingustavo 0:80950b84a6c4 130 data.flags &= ~(0x88); // clear fuel and box flags
einsteingustavo 0:80950b84a6c4 131 data.flags |= (fuel_counter > NORMAL_THRESHOLD) ? (0x01 << 3) : 0;
einsteingustavo 0:80950b84a6c4 132 data.flags |= box << 7;
einsteingustavo 0:80950b84a6c4 133
einsteingustavo 0:80950b84a6c4 134 txMsg.clear(FLAGS_ID);
einsteingustavo 0:80950b84a6c4 135 txMsg << data.flags;
einsteingustavo 0:80950b84a6c4 136 can.write(txMsg);
einsteingustavo 0:80950b84a6c4 137
einsteingustavo 0:80950b84a6c4 138 fuel_timer = 0;
einsteingustavo 0:80950b84a6c4 139 fuel_counter = 0;
einsteingustavo 0:80950b84a6c4 140 ticker100Hz.attach(&ticker100HzISR, 0.01);
einsteingustavo 0:80950b84a6c4 141 dbg1 = 0;
einsteingustavo 0:80950b84a6c4 142 break;
einsteingustavo 0:80950b84a6c4 143 case RPM_ST:
einsteingustavo 0:80950b84a6c4 144 //serial.printf("r");
einsteingustavo 0:80950b84a6c4 145 dbg2 = 1;
einsteingustavo 0:80950b84a6c4 146 freq_sensor.fall(NULL); // disable interrupt
einsteingustavo 0:80950b84a6c4 147 if (current_period != 0)
einsteingustavo 0:80950b84a6c4 148 {
einsteingustavo 0:80950b84a6c4 149 rpm_hz = ((float)pulse_counter/(current_period/1000000.0)); //calculates frequency in Hz
einsteingustavo 0:80950b84a6c4 150 if (switch_state != RUN_MODE)
einsteingustavo 0:80950b84a6c4 151 writeServo(RUN_MODE);
einsteingustavo 0:80950b84a6c4 152 // engine_counter.start();
einsteingustavo 0:80950b84a6c4 153 }
einsteingustavo 0:80950b84a6c4 154 else
einsteingustavo 0:80950b84a6c4 155 {
einsteingustavo 0:80950b84a6c4 156 rpm_hz = 0;
einsteingustavo 0:80950b84a6c4 157 writeServo(switch_state);
einsteingustavo 0:80950b84a6c4 158 // engine_counter.stop();
einsteingustavo 0:80950b84a6c4 159 }
einsteingustavo 0:80950b84a6c4 160 /* Send rpm data */
einsteingustavo 0:80950b84a6c4 161 txMsg.clear(RPM_ID);
einsteingustavo 0:80950b84a6c4 162 txMsg << ((uint16_t) rpm_hz);
einsteingustavo 0:80950b84a6c4 163 can.write(txMsg);
einsteingustavo 0:80950b84a6c4 164 /* prepare to re-init rpm counter */
einsteingustavo 0:80950b84a6c4 165 pulse_counter = 0;
einsteingustavo 0:80950b84a6c4 166 current_period = 0; // reset pulses related variables
einsteingustavo 0:80950b84a6c4 167 last_count = t.read_us();
einsteingustavo 0:80950b84a6c4 168 freq_sensor.fall(&frequencyCounterISR); // enable interrupt
einsteingustavo 0:80950b84a6c4 169
einsteingustavo 0:80950b84a6c4 170 dbg2 = 0;
einsteingustavo 0:80950b84a6c4 171 break;
einsteingustavo 0:80950b84a6c4 172 case THROTTLE_ST:
einsteingustavo 0:80950b84a6c4 173 if (switch_clicked)
einsteingustavo 0:80950b84a6c4 174 {
einsteingustavo 0:80950b84a6c4 175 writeServo(switch_state);
einsteingustavo 0:80950b84a6c4 176 switch_clicked = false;
einsteingustavo 0:80950b84a6c4 177 }
einsteingustavo 0:80950b84a6c4 178
einsteingustavo 0:80950b84a6c4 179 break;
einsteingustavo 0:80950b84a6c4 180 case RADIO_ST:
einsteingustavo 0:80950b84a6c4 181 imu_t* temp_imu;
einsteingustavo 0:80950b84a6c4 182 // dbg4 = 1;
einsteingustavo 0:80950b84a6c4 183 //if(radio.receiveDone())
einsteingustavo 0:80950b84a6c4 184 // {
einsteingustavo 0:80950b84a6c4 185 // if (radio.ACKRequested())
einsteingustavo 0:80950b84a6c4 186 // radio.sendACK();
einsteingustavo 0:80950b84a6c4 187 // led = 0;
einsteingustavo 0:80950b84a6c4 188 // box = (bool) radio.DATA;
einsteingustavo 0:80950b84a6c4 189 //// serial.printf("%d\r\n", (bool)radio.DATA);
einsteingustavo 0:80950b84a6c4 190 // }
einsteingustavo 0:80950b84a6c4 191
einsteingustavo 0:80950b84a6c4 192 // serial.printf("%d,%d,%d\r\n", (!imu_buffer.empty()), (!d10hz_buffer.empty()), (!temp_buffer.empty()));
einsteingustavo 0:80950b84a6c4 193 // if((!imu_buffer.empty()) && (!d10hz_buffer.empty()) && (!temp_buffer.empty()))
einsteingustavo 0:80950b84a6c4 194 // {
einsteingustavo 0:80950b84a6c4 195 if (!imu_buffer.empty())
einsteingustavo 0:80950b84a6c4 196 {
einsteingustavo 0:80950b84a6c4 197 imu_buffer.pop(temp_imu);
einsteingustavo 0:80950b84a6c4 198 memcpy(&data.imu, temp_imu, 4*sizeof(imu_t));
einsteingustavo 0:80950b84a6c4 199 data.rpm = ((uint16_t)rpm_hz * 60)*65536.0/5000.0;
einsteingustavo 0:80950b84a6c4 200 radio.send((uint8_t)BOXRADIO_ID, &data, sizeof(packet_t), true, false); // request ACK with 1 retry (waitTime = 40ms)
einsteingustavo 0:80950b84a6c4 201 }
einsteingustavo 0:80950b84a6c4 202 else if (t.read_ms() - imu_last_acq > 500)
einsteingustavo 0:80950b84a6c4 203 {
einsteingustavo 0:80950b84a6c4 204 memset(&data.imu, 0, 4*sizeof(imu_t));
einsteingustavo 0:80950b84a6c4 205 data.rpm = ((uint16_t)rpm_hz * 60)*65536.0/5000.0;
einsteingustavo 0:80950b84a6c4 206 radio.send((uint8_t)BOXRADIO_ID, &data, sizeof(packet_t), true, false); // request ACK with 1 retry (waitTime = 40ms)
einsteingustavo 0:80950b84a6c4 207 }
einsteingustavo 0:80950b84a6c4 208 // }
einsteingustavo 0:80950b84a6c4 209 // radio.receiveDone();
einsteingustavo 0:80950b84a6c4 210 dbg4 = 0;
einsteingustavo 0:80950b84a6c4 211 // tim2 = t.read_us() - tim1;
einsteingustavo 0:80950b84a6c4 212 // serial.printf("%d\r\n",tim2);
einsteingustavo 0:80950b84a6c4 213 break;
einsteingustavo 0:80950b84a6c4 214 case DEBUG_ST:
einsteingustavo 0:80950b84a6c4 215 // serial.printf("radio state pushed");
einsteingustavo 0:80950b84a6c4 216 // serial.printf("bf=%d, cr=%d\r\n", buffer_full, switch_state);
einsteingustavo 0:80950b84a6c4 217 // serial.printf("speed=%d\r\n", data.data_10hz[packet_counter[N_SPEED]].speed);
einsteingustavo 0:80950b84a6c4 218 // serial.printf("rpm=%d\r\n", data.data_10hz[packet_counter[N_RPM]].rpm);
einsteingustavo 0:80950b84a6c4 219 // serial.printf("imu acc x =%d\r\n", data.imu[imu_counter].acc_x);
einsteingustavo 0:80950b84a6c4 220 // serial.printf("imu acc y =%d\r\n", data.imu[imu_counter].acc_y);
einsteingustavo 0:80950b84a6c4 221 // serial.printf("imu acc z =%d\r\n", data.imu[imu_counter].acc_z);
einsteingustavo 0:80950b84a6c4 222 // serial.printf("imu dps x =%d\r\n", data.imu[imu_counter].dps_x);
einsteingustavo 0:80950b84a6c4 223 // serial.printf("imu dps y =%d\r\n", data.imu[imu_counter].dps_y);
einsteingustavo 0:80950b84a6c4 224 // serial.printf("imu dps z =%d\r\n", data.imu[imu_counter].dps_z);
einsteingustavo 0:80950b84a6c4 225 break;
einsteingustavo 0:80950b84a6c4 226 default:
einsteingustavo 0:80950b84a6c4 227 break;
einsteingustavo 0:80950b84a6c4 228 }
einsteingustavo 0:80950b84a6c4 229 }
einsteingustavo 0:80950b84a6c4 230 }
einsteingustavo 0:80950b84a6c4 231
einsteingustavo 0:80950b84a6c4 232 /* Interrupt services routine */
einsteingustavo 0:80950b84a6c4 233 void canISR()
einsteingustavo 0:80950b84a6c4 234 {
einsteingustavo 0:80950b84a6c4 235 CAN_IER &= ~CAN_IER_FMPIE0; // disable RX interrupt
einsteingustavo 0:80950b84a6c4 236 queue.call(&canHandler); // add canHandler() to events queue
einsteingustavo 0:80950b84a6c4 237 }
einsteingustavo 0:80950b84a6c4 238
einsteingustavo 0:80950b84a6c4 239 void ticker1HzISR()
einsteingustavo 0:80950b84a6c4 240 {
einsteingustavo 0:80950b84a6c4 241 state_buffer.push(TEMP_ST);
einsteingustavo 0:80950b84a6c4 242 }
einsteingustavo 0:80950b84a6c4 243
einsteingustavo 0:80950b84a6c4 244 void ticker5HzISR()
einsteingustavo 0:80950b84a6c4 245 {
einsteingustavo 0:80950b84a6c4 246 state_buffer.push(RPM_ST);
einsteingustavo 0:80950b84a6c4 247 state_buffer.push(RADIO_ST);
einsteingustavo 0:80950b84a6c4 248 }
einsteingustavo 0:80950b84a6c4 249
einsteingustavo 0:80950b84a6c4 250 void ticker100HzISR()
einsteingustavo 0:80950b84a6c4 251 {
einsteingustavo 0:80950b84a6c4 252 if (fuel_timer < 100)
einsteingustavo 0:80950b84a6c4 253 {
einsteingustavo 0:80950b84a6c4 254 fuel_timer++;
einsteingustavo 0:80950b84a6c4 255 fuel_counter += !fuel_sensor.read();
einsteingustavo 0:80950b84a6c4 256 }
einsteingustavo 0:80950b84a6c4 257 else
einsteingustavo 0:80950b84a6c4 258 {
einsteingustavo 0:80950b84a6c4 259 state_buffer.push(FUEL_ST);
einsteingustavo 0:80950b84a6c4 260 ticker100Hz.detach();
einsteingustavo 0:80950b84a6c4 261 }
einsteingustavo 0:80950b84a6c4 262 }
einsteingustavo 0:80950b84a6c4 263
einsteingustavo 0:80950b84a6c4 264 void frequencyCounterISR()
einsteingustavo 0:80950b84a6c4 265 {
einsteingustavo 0:80950b84a6c4 266 led = !led;
einsteingustavo 0:80950b84a6c4 267 pulse_counter++;
einsteingustavo 0:80950b84a6c4 268 current_period += t.read_us() - last_count;
einsteingustavo 0:80950b84a6c4 269 last_count = t.read_us();
einsteingustavo 0:80950b84a6c4 270 }
einsteingustavo 0:80950b84a6c4 271
einsteingustavo 0:80950b84a6c4 272 /* Interrupt handlers */
einsteingustavo 0:80950b84a6c4 273 void canHandler()
einsteingustavo 0:80950b84a6c4 274 {
einsteingustavo 0:80950b84a6c4 275 CANMsg rxMsg;
einsteingustavo 0:80950b84a6c4 276
einsteingustavo 0:80950b84a6c4 277 can.read(rxMsg);
einsteingustavo 0:80950b84a6c4 278 filterMessage(rxMsg);
einsteingustavo 0:80950b84a6c4 279 CAN_IER |= CAN_IER_FMPIE0; // enable RX interrupt
einsteingustavo 0:80950b84a6c4 280 }
einsteingustavo 0:80950b84a6c4 281
einsteingustavo 0:80950b84a6c4 282 /* General functions */
einsteingustavo 0:80950b84a6c4 283 void initPWM()
einsteingustavo 0:80950b84a6c4 284 {
einsteingustavo 0:80950b84a6c4 285 servo.period_ms(20); // set signal frequency to 50Hz
einsteingustavo 0:80950b84a6c4 286 servo.write(0); // disables servo
einsteingustavo 0:80950b84a6c4 287 signal.period_ms(32); // set signal frequency to 1/0.032Hz
einsteingustavo 0:80950b84a6c4 288 signal.write(0.5f); // dutycycle 50%
einsteingustavo 0:80950b84a6c4 289 }
einsteingustavo 0:80950b84a6c4 290
einsteingustavo 0:80950b84a6c4 291 void initRadio()
einsteingustavo 0:80950b84a6c4 292 {
einsteingustavo 0:80950b84a6c4 293 radio.initialize(FREQUENCY_915MHZ, NODE_ID, NETWORK_ID);
einsteingustavo 0:80950b84a6c4 294 radio.encrypt(0);
einsteingustavo 0:80950b84a6c4 295 radio.setPowerLevel(20);
einsteingustavo 0:80950b84a6c4 296 }
einsteingustavo 0:80950b84a6c4 297
einsteingustavo 0:80950b84a6c4 298 void setupInterrupts()
einsteingustavo 0:80950b84a6c4 299 {
einsteingustavo 0:80950b84a6c4 300 can.attach(&canISR, CAN::RxIrq);
einsteingustavo 0:80950b84a6c4 301 ticker1Hz.attach(&ticker1HzISR, 1.0);
einsteingustavo 0:80950b84a6c4 302 ticker5Hz.attach(&ticker5HzISR, 0.2);
einsteingustavo 0:80950b84a6c4 303 ticker100Hz.attach(&ticker100HzISR, 0.01);
einsteingustavo 0:80950b84a6c4 304 freq_sensor.fall(&frequencyCounterISR);
einsteingustavo 0:80950b84a6c4 305 }
einsteingustavo 0:80950b84a6c4 306
einsteingustavo 0:80950b84a6c4 307 void filterMessage(CANMsg msg)
einsteingustavo 0:80950b84a6c4 308 {
einsteingustavo 0:80950b84a6c4 309 led = !led;
einsteingustavo 0:80950b84a6c4 310
einsteingustavo 0:80950b84a6c4 311 if (msg.id == THROTTLE_ID)
einsteingustavo 0:80950b84a6c4 312 {
einsteingustavo 0:80950b84a6c4 313 switch_clicked = true;
einsteingustavo 0:80950b84a6c4 314 state_buffer.push(THROTTLE_ST);
einsteingustavo 0:80950b84a6c4 315 msg >> switch_state;
einsteingustavo 0:80950b84a6c4 316 }
einsteingustavo 0:80950b84a6c4 317 else if (msg.id == IMU_ACC_ID)
einsteingustavo 0:80950b84a6c4 318 {
einsteingustavo 0:80950b84a6c4 319 imu_last_acq = t.read_ms();
einsteingustavo 0:80950b84a6c4 320 msg >> data.imu[imu_counter].acc_x >> data.imu[imu_counter].acc_y >> data.imu[imu_counter].acc_z;
einsteingustavo 0:80950b84a6c4 321 }
einsteingustavo 0:80950b84a6c4 322 else if (msg.id == IMU_DPS_ID)
einsteingustavo 0:80950b84a6c4 323 {
einsteingustavo 0:80950b84a6c4 324 msg >> data.imu[imu_counter].dps_x >> data.imu[imu_counter].dps_y
einsteingustavo 0:80950b84a6c4 325 >> data.imu[imu_counter].dps_z;
einsteingustavo 0:80950b84a6c4 326 if (imu_counter < 3)
einsteingustavo 0:80950b84a6c4 327 {
einsteingustavo 0:80950b84a6c4 328 imu_counter++;
einsteingustavo 0:80950b84a6c4 329 }
einsteingustavo 0:80950b84a6c4 330 else if (imu_counter == 3)
einsteingustavo 0:80950b84a6c4 331 {
einsteingustavo 0:80950b84a6c4 332 imu_buffer.push(data.imu);
einsteingustavo 0:80950b84a6c4 333 imu_counter = 0;
einsteingustavo 0:80950b84a6c4 334 }
einsteingustavo 0:80950b84a6c4 335 }
einsteingustavo 0:80950b84a6c4 336 else if (msg.id == SPEED_ID)
einsteingustavo 0:80950b84a6c4 337 {
einsteingustavo 0:80950b84a6c4 338 msg >> data.speed;
einsteingustavo 0:80950b84a6c4 339 // serial.printf("\r\nspeed = %d\r\n",data.data_10hz[packet_counter[N_SPEED]].speed);
einsteingustavo 0:80950b84a6c4 340 // d10hz_buffer.push(data.data_10hz);
einsteingustavo 0:80950b84a6c4 341 }
einsteingustavo 0:80950b84a6c4 342 }
einsteingustavo 0:80950b84a6c4 343
einsteingustavo 0:80950b84a6c4 344 void writeServo(uint8_t state)
einsteingustavo 0:80950b84a6c4 345 {
einsteingustavo 0:80950b84a6c4 346 data.flags &= ~(0x07); // reset servo-related flags
einsteingustavo 0:80950b84a6c4 347
einsteingustavo 0:80950b84a6c4 348 switch (state)
einsteingustavo 0:80950b84a6c4 349 {
einsteingustavo 0:80950b84a6c4 350 case MID_MODE:
einsteingustavo 0:80950b84a6c4 351 dbg3 = !dbg3;
einsteingustavo 0:80950b84a6c4 352 servo.pulsewidth_us(SERVO_MID);
einsteingustavo 0:80950b84a6c4 353 data.flags &= ~(0x03); // reset run and choke flags
einsteingustavo 0:80950b84a6c4 354 break;
einsteingustavo 0:80950b84a6c4 355 case RUN_MODE:
einsteingustavo 0:80950b84a6c4 356 dbg3 = !dbg3;
einsteingustavo 0:80950b84a6c4 357 servo.pulsewidth_us(SERVO_RUN);
einsteingustavo 0:80950b84a6c4 358 data.flags |= RUN_MODE; // set run flag
einsteingustavo 0:80950b84a6c4 359 break;
einsteingustavo 0:80950b84a6c4 360 case CHOKE_MODE:
einsteingustavo 0:80950b84a6c4 361 dbg3 = !dbg3;
einsteingustavo 0:80950b84a6c4 362 servo.pulsewidth_us(SERVO_CHOKE);
einsteingustavo 0:80950b84a6c4 363 data.flags |= CHOKE_MODE; // set choke flag
einsteingustavo 0:80950b84a6c4 364 break;
einsteingustavo 0:80950b84a6c4 365 default:
einsteingustavo 0:80950b84a6c4 366 // serial.printf("Choke/run error\r\n");
einsteingustavo 0:80950b84a6c4 367 data.flags |= 0x04; // set servo error flag
einsteingustavo 0:80950b84a6c4 368 break;
einsteingustavo 0:80950b84a6c4 369 }
einsteingustavo 0:80950b84a6c4 370 }