Einstein Filho / Mbed OS MANGUEBAJA2019_FRONT2

Mangue Baja team's code to frontal ECU

main.cpp@0:12fb9cbcabcc, 2019-07-24 (annotated)

Committer:
einsteingustavo
Date:
Wed Jul 24 20:03:52 2019 +0000
Revision:
0:12fb9cbcabcc
Mangue Baja team's code to frontal ECU

Who changed what in which revision?

UserRevisionLine numberNew contents of line
einsteingustavo 0:12fb9cbcabcc 1 #include "mbed.h"
einsteingustavo 0:12fb9cbcabcc 2 #include "stats_report.h"
einsteingustavo 0:12fb9cbcabcc 3 /* User libraries */
einsteingustavo 0:12fb9cbcabcc 4 #include "definitions.h"
einsteingustavo 0:12fb9cbcabcc 5 #include "frontdefs.h"
einsteingustavo 0:12fb9cbcabcc 6 #include "CANMsg.h"
einsteingustavo 0:12fb9cbcabcc 7 #include "LSM6DS3.h"
einsteingustavo 0:12fb9cbcabcc 8 #include "Kalman.h"
einsteingustavo 0:12fb9cbcabcc 9
einsteingustavo 0:12fb9cbcabcc 10 #define MB1
einsteingustavo 0:12fb9cbcabcc 11 //#define MB2
einsteingustavo 0:12fb9cbcabcc 12
einsteingustavo 0:12fb9cbcabcc 13 /* Communication protocols */
einsteingustavo 0:12fb9cbcabcc 14 CAN can(PB_8, PB_9, 1000000);
einsteingustavo 0:12fb9cbcabcc 15 Serial serial(PA_2, PA_3, 115200);
einsteingustavo 0:12fb9cbcabcc 16 LSM6DS3 LSM6DS3(PB_7, PB_6);
einsteingustavo 0:12fb9cbcabcc 17
einsteingustavo 0:12fb9cbcabcc 18 /* I/O pins */
einsteingustavo 0:12fb9cbcabcc 19 InterruptIn freq_sensor(PB_10, PullNone);
einsteingustavo 0:12fb9cbcabcc 20 InterruptIn choke_switch(PA_5, PullUp); // servomotor CHOKE mode
einsteingustavo 0:12fb9cbcabcc 21 InterruptIn run_switch(PA_7, PullUp); // servomotor RUN mode
einsteingustavo 0:12fb9cbcabcc 22 InterruptIn horn_button(PB_1, PullUp);
einsteingustavo 0:12fb9cbcabcc 23 InterruptIn headlight_switch(PB_0, PullUp);
einsteingustavo 0:12fb9cbcabcc 24 DigitalOut horn(PB_11);
einsteingustavo 0:12fb9cbcabcc 25 DigitalOut headlight(PA_1);
einsteingustavo 0:12fb9cbcabcc 26 /* Debug pins */
einsteingustavo 0:12fb9cbcabcc 27 DigitalOut led(PC_13);
einsteingustavo 0:12fb9cbcabcc 28 PwmOut signal(PA_6);
einsteingustavo 0:12fb9cbcabcc 29 DigitalOut dbg1(PC_14);
einsteingustavo 0:12fb9cbcabcc 30 DigitalOut dbg2(PC_15);
einsteingustavo 0:12fb9cbcabcc 31 DigitalOut dbg3(PA_4);
einsteingustavo 0:12fb9cbcabcc 32
einsteingustavo 0:12fb9cbcabcc 33 /* Interrupt services routine */
einsteingustavo 0:12fb9cbcabcc 34 void canISR();
einsteingustavo 0:12fb9cbcabcc 35 void servoSwitchISR();
einsteingustavo 0:12fb9cbcabcc 36 void ticker2HzISR();
einsteingustavo 0:12fb9cbcabcc 37 void ticker5HzISR();
einsteingustavo 0:12fb9cbcabcc 38 void ticker20HzISR();
einsteingustavo 0:12fb9cbcabcc 39 void tickerTrottleISR();
einsteingustavo 0:12fb9cbcabcc 40 void frequencyCounterISR();
einsteingustavo 0:12fb9cbcabcc 41 void hornISR();
einsteingustavo 0:12fb9cbcabcc 42 void headlightISR();
einsteingustavo 0:12fb9cbcabcc 43 /* Interrupt handlers */
einsteingustavo 0:12fb9cbcabcc 44 void canHandler();
einsteingustavo 0:12fb9cbcabcc 45 void throttleDebounceHandler();
einsteingustavo 0:12fb9cbcabcc 46 void hornDebounceHandler();
einsteingustavo 0:12fb9cbcabcc 47 void headlightDebounceHandler();
einsteingustavo 0:12fb9cbcabcc 48 /* General functions*/
einsteingustavo 0:12fb9cbcabcc 49 void setupInterrupts();
einsteingustavo 0:12fb9cbcabcc 50 void filterMessage(CANMsg msg);
einsteingustavo 0:12fb9cbcabcc 51 void calcAngles(int16_t accx, int16_t accy, int16_t accz, int16_t grx, int16_t gry, int16_t grz, int16_t dt);
einsteingustavo 0:12fb9cbcabcc 52 void displayData(uint16_t vel, uint16_t Hz, uint16_t temp, bool comb, \
einsteingustavo 0:12fb9cbcabcc 53 bool b, bool tl, bool fl, int16_t gp, int16_t gr, bool box);
einsteingustavo 0:12fb9cbcabcc 54
einsteingustavo 0:12fb9cbcabcc 55 /* Debug variables */
einsteingustavo 0:12fb9cbcabcc 56 Timer t;
einsteingustavo 0:12fb9cbcabcc 57 bool buffer_full = false;
einsteingustavo 0:12fb9cbcabcc 58 unsigned int t0, t1;
einsteingustavo 0:12fb9cbcabcc 59 /* Mbed OS tools */
einsteingustavo 0:12fb9cbcabcc 60 Thread eventThread;
einsteingustavo 0:12fb9cbcabcc 61 EventQueue queue(1024);
einsteingustavo 0:12fb9cbcabcc 62 Ticker ticker2Hz;
einsteingustavo 0:12fb9cbcabcc 63 Ticker ticker5Hz;
einsteingustavo 0:12fb9cbcabcc 64 Ticker ticker20Hz;
einsteingustavo 0:12fb9cbcabcc 65 Ticker tickerTrottle;
einsteingustavo 0:12fb9cbcabcc 66 Timeout debounce_throttle;
einsteingustavo 0:12fb9cbcabcc 67 Timeout debounce_horn;
einsteingustavo 0:12fb9cbcabcc 68 Timeout debounce_headlight;
einsteingustavo 0:12fb9cbcabcc 69 // Timeout horn_limiter; // stop sound of horn after a determined period
einsteingustavo 0:12fb9cbcabcc 70 CircularBuffer <state_t, BUFFER_SIZE> state_buffer;
einsteingustavo 0:12fb9cbcabcc 71 /* Global variables */
einsteingustavo 0:12fb9cbcabcc 72 bool switch_clicked = false;
einsteingustavo 0:12fb9cbcabcc 73 uint8_t switch_state = 0x00, flags = 0x00, pulse_counter = 0, temp_motor = 0;
einsteingustavo 0:12fb9cbcabcc 74 state_t current_state = IDLE_ST;
einsteingustavo 0:12fb9cbcabcc 75 uint64_t current_period = 0, last_count = 0, last_acq = 0;
einsteingustavo 0:12fb9cbcabcc 76 uint8_t imu_failed = 0; // number of times before a new connection attempt with imu
einsteingustavo 0:12fb9cbcabcc 77 float speed_hz = 0;
einsteingustavo 0:12fb9cbcabcc 78 uint16_t rpm_hz = 0, speed_display = 0, speed_radio = 0, dt = 0;
einsteingustavo 0:12fb9cbcabcc 79 int16_t angle_roll = 0, angle_pitch = 0;
einsteingustavo 0:12fb9cbcabcc 80 packet_t data;
einsteingustavo 0:12fb9cbcabcc 81
einsteingustavo 0:12fb9cbcabcc 82 int main()
einsteingustavo 0:12fb9cbcabcc 83 {
einsteingustavo 0:12fb9cbcabcc 84 /* Main variables */
einsteingustavo 0:12fb9cbcabcc 85 CANMsg txMsg;
einsteingustavo 0:12fb9cbcabcc 86 /* Initialization */
einsteingustavo 0:12fb9cbcabcc 87 t.start();
einsteingustavo 0:12fb9cbcabcc 88 horn = horn_button.read(); // horn OFF
einsteingustavo 0:12fb9cbcabcc 89 headlight = headlight_switch.read(); // headlight OFF
einsteingustavo 0:12fb9cbcabcc 90 led = 1; // led OFF
einsteingustavo 0:12fb9cbcabcc 91 eventThread.start(callback(&queue, &EventQueue::dispatch_forever));
einsteingustavo 0:12fb9cbcabcc 92 t0 = t.read_us();
einsteingustavo 0:12fb9cbcabcc 93 uint16_t lsm_addr = LSM6DS3.begin(LSM6DS3.G_SCALE_245DPS, LSM6DS3.A_SCALE_2G, LSM6DS3.G_ODR_26_BW_2, LSM6DS3.A_ODR_26);
einsteingustavo 0:12fb9cbcabcc 94 t1 = t.read_us();
einsteingustavo 0:12fb9cbcabcc 95 // serial.printf("%d\r\n", (t1 - t0));
einsteingustavo 0:12fb9cbcabcc 96 setupInterrupts();
einsteingustavo 0:12fb9cbcabcc 97
einsteingustavo 0:12fb9cbcabcc 98 while (true) {
einsteingustavo 0:12fb9cbcabcc 99 if (state_buffer.full())
einsteingustavo 0:12fb9cbcabcc 100 {
einsteingustavo 0:12fb9cbcabcc 101 buffer_full = true;
einsteingustavo 0:12fb9cbcabcc 102 led = 0;
einsteingustavo 0:12fb9cbcabcc 103 state_buffer.pop(current_state);
einsteingustavo 0:12fb9cbcabcc 104 }
einsteingustavo 0:12fb9cbcabcc 105 else
einsteingustavo 0:12fb9cbcabcc 106 {
einsteingustavo 0:12fb9cbcabcc 107 led = 1;
einsteingustavo 0:12fb9cbcabcc 108 buffer_full = false;
einsteingustavo 0:12fb9cbcabcc 109 if (!state_buffer.empty())
einsteingustavo 0:12fb9cbcabcc 110 state_buffer.pop(current_state);
einsteingustavo 0:12fb9cbcabcc 111 else
einsteingustavo 0:12fb9cbcabcc 112 current_state = IDLE_ST;
einsteingustavo 0:12fb9cbcabcc 113 }
einsteingustavo 0:12fb9cbcabcc 114
einsteingustavo 0:12fb9cbcabcc 115 switch (current_state)
einsteingustavo 0:12fb9cbcabcc 116 {
einsteingustavo 0:12fb9cbcabcc 117 case IDLE_ST:
einsteingustavo 0:12fb9cbcabcc 118 // Thread::wait(2);
einsteingustavo 0:12fb9cbcabcc 119 break;
einsteingustavo 0:12fb9cbcabcc 120 case SLOWACQ_ST:
einsteingustavo 0:12fb9cbcabcc 121 break;
einsteingustavo 0:12fb9cbcabcc 122 case IMU_ST:
einsteingustavo 0:12fb9cbcabcc 123 t0 = t.read_us();
einsteingustavo 0:12fb9cbcabcc 124 dbg1 = !dbg1;
einsteingustavo 0:12fb9cbcabcc 125 if (lsm_addr)
einsteingustavo 0:12fb9cbcabcc 126 {
einsteingustavo 0:12fb9cbcabcc 127 bool nack = LSM6DS3.readAccel(); // read accelerometer data into LSM6DS3.aN_raw
einsteingustavo 0:12fb9cbcabcc 128 if (!nack)
einsteingustavo 0:12fb9cbcabcc 129 nack = LSM6DS3.readGyro(); // " gyroscope data into LSM6DS3.gN_raw
einsteingustavo 0:12fb9cbcabcc 130
einsteingustavo 0:12fb9cbcabcc 131 if (nack)
einsteingustavo 0:12fb9cbcabcc 132 {
einsteingustavo 0:12fb9cbcabcc 133 lsm_addr = 0;
einsteingustavo 0:12fb9cbcabcc 134 LSM6DS3.ax_raw = 0;
einsteingustavo 0:12fb9cbcabcc 135 LSM6DS3.ay_raw = 0;
einsteingustavo 0:12fb9cbcabcc 136 LSM6DS3.az_raw = 0;
einsteingustavo 0:12fb9cbcabcc 137 LSM6DS3.gx_raw = 0;
einsteingustavo 0:12fb9cbcabcc 138 LSM6DS3.gy_raw = 0;
einsteingustavo 0:12fb9cbcabcc 139 LSM6DS3.gz_raw = 0;
einsteingustavo 0:12fb9cbcabcc 140 }
einsteingustavo 0:12fb9cbcabcc 141 }
einsteingustavo 0:12fb9cbcabcc 142 else if (imu_failed == IMU_TRIES)
einsteingustavo 0:12fb9cbcabcc 143 {
einsteingustavo 0:12fb9cbcabcc 144 lsm_addr = LSM6DS3.begin(LSM6DS3.G_SCALE_245DPS, LSM6DS3.A_SCALE_2G, LSM6DS3.G_ODR_26_BW_2, LSM6DS3.A_ODR_26);
einsteingustavo 0:12fb9cbcabcc 145 t1 = t.read_us();
einsteingustavo 0:12fb9cbcabcc 146 imu_failed = 0;
einsteingustavo 0:12fb9cbcabcc 147 // serial.printf("%d\r\n", (t1 - t0));
einsteingustavo 0:12fb9cbcabcc 148 }
einsteingustavo 0:12fb9cbcabcc 149 else
einsteingustavo 0:12fb9cbcabcc 150 {
einsteingustavo 0:12fb9cbcabcc 151 imu_failed++;
einsteingustavo 0:12fb9cbcabcc 152 }
einsteingustavo 0:12fb9cbcabcc 153
einsteingustavo 0:12fb9cbcabcc 154 last_acq = t.read_ms();
einsteingustavo 0:12fb9cbcabcc 155 // serial.printf("accz = %d\r\n", LSM6DS3.gz_raw);
einsteingustavo 0:12fb9cbcabcc 156 calcAngles(LSM6DS3.ax_raw, LSM6DS3.ay_raw, LSM6DS3.az_raw, LSM6DS3.gx_raw, LSM6DS3.gy_raw, LSM6DS3.gz_raw, dt);
einsteingustavo 0:12fb9cbcabcc 157 /* Send accelerometer data */
einsteingustavo 0:12fb9cbcabcc 158 txMsg.clear(IMU_ACC_ID);
einsteingustavo 0:12fb9cbcabcc 159 txMsg << LSM6DS3.ax_raw << LSM6DS3.ay_raw << LSM6DS3.az_raw;
einsteingustavo 0:12fb9cbcabcc 160 if(can.write(txMsg))
einsteingustavo 0:12fb9cbcabcc 161 {
einsteingustavo 0:12fb9cbcabcc 162 /* Send gyroscope data only if accelerometer data succeeds */
einsteingustavo 0:12fb9cbcabcc 163 txMsg.clear(IMU_DPS_ID);
einsteingustavo 0:12fb9cbcabcc 164 txMsg << LSM6DS3.gx_raw << LSM6DS3.gy_raw << LSM6DS3.gz_raw << dt;
einsteingustavo 0:12fb9cbcabcc 165 can.write(txMsg);
einsteingustavo 0:12fb9cbcabcc 166 }
einsteingustavo 0:12fb9cbcabcc 167 break;
einsteingustavo 0:12fb9cbcabcc 168 case SPEED_ST:
einsteingustavo 0:12fb9cbcabcc 169 // serial.printf("speed ok\r\n");
einsteingustavo 0:12fb9cbcabcc 170 dbg2 = !dbg2;
einsteingustavo 0:12fb9cbcabcc 171 freq_sensor.fall(NULL); // disable interrupt
einsteingustavo 0:12fb9cbcabcc 172 if (current_period != 0)
einsteingustavo 0:12fb9cbcabcc 173 {
einsteingustavo 0:12fb9cbcabcc 174 speed_hz = 1000000*((float)pulse_counter/current_period); //calculates frequency in Hz
einsteingustavo 0:12fb9cbcabcc 175 }
einsteingustavo 0:12fb9cbcabcc 176 else
einsteingustavo 0:12fb9cbcabcc 177 {
einsteingustavo 0:12fb9cbcabcc 178 speed_hz = 0;
einsteingustavo 0:12fb9cbcabcc 179 }
einsteingustavo 0:12fb9cbcabcc 180 speed_display = ((float)(PI*WHEEL_DIAMETER*speed_hz)/WHEEL_HOLES_NUMBER); // make conversion hz to km/h
einsteingustavo 0:12fb9cbcabcc 181 speed_radio = ((float)((speed_display)/60.0)*65535);
einsteingustavo 0:12fb9cbcabcc 182 pulse_counter = 0;
einsteingustavo 0:12fb9cbcabcc 183 current_period = 0; //|-> reset pulses related variables
einsteingustavo 0:12fb9cbcabcc 184 last_count = t.read_us();
einsteingustavo 0:12fb9cbcabcc 185 freq_sensor.fall(&frequencyCounterISR); // enable interrupt
einsteingustavo 0:12fb9cbcabcc 186 /* Send speed data */
einsteingustavo 0:12fb9cbcabcc 187 txMsg.clear(SPEED_ID);
einsteingustavo 0:12fb9cbcabcc 188 txMsg << speed_radio;
einsteingustavo 0:12fb9cbcabcc 189 can.write(txMsg);
einsteingustavo 0:12fb9cbcabcc 190 // state_buffer.push(DEBUG_ST); // debug
einsteingustavo 0:12fb9cbcabcc 191 break;
einsteingustavo 0:12fb9cbcabcc 192 case THROTTLE_ST:
einsteingustavo 0:12fb9cbcabcc 193 // serial.printf("throttle ok\r\n");
einsteingustavo 0:12fb9cbcabcc 194 dbg3 = !dbg3;
einsteingustavo 0:12fb9cbcabcc 195 if (switch_clicked)
einsteingustavo 0:12fb9cbcabcc 196 {
einsteingustavo 0:12fb9cbcabcc 197 switch_state = !choke_switch.read() << 1 | !run_switch.read() << 0;
einsteingustavo 0:12fb9cbcabcc 198 //serial.printf("switch_state = %d\r\n", switch_state);
einsteingustavo 0:12fb9cbcabcc 199 /* Send CAN message */
einsteingustavo 0:12fb9cbcabcc 200 txMsg.clear(THROTTLE_ID);
einsteingustavo 0:12fb9cbcabcc 201 txMsg << switch_state;
einsteingustavo 0:12fb9cbcabcc 202 // serial.printf("can ok\r/n"); // append data (8 bytes max)
einsteingustavo 0:12fb9cbcabcc 203 can.write(txMsg);
einsteingustavo 0:12fb9cbcabcc 204
einsteingustavo 0:12fb9cbcabcc 205 switch_clicked = false;
einsteingustavo 0:12fb9cbcabcc 206 }
einsteingustavo 0:12fb9cbcabcc 207 break;
einsteingustavo 0:12fb9cbcabcc 208 case DISPLAY_ST:
einsteingustavo 0:12fb9cbcabcc 209 // serial.printf("rpm_hz=%d\r\n", rpm_hz);
einsteingustavo 0:12fb9cbcabcc 210 displayData(speed_display, rpm_hz, temp_motor, (flags & 0x08), false, true, false, angle_pitch, angle_roll, (flags & 0x80));
einsteingustavo 0:12fb9cbcabcc 211 // state_buffer.push(DEBUG_ST);
einsteingustavo 0:12fb9cbcabcc 212 break;
einsteingustavo 0:12fb9cbcabcc 213 case DEBUG_ST:
einsteingustavo 0:12fb9cbcabcc 214 // serial.printf("r= %d, p=%d\r\n", angle_roll, angle_pitch);
einsteingustavo 0:12fb9cbcabcc 215 // serial.printf("imu acc x =%d\r\n", LSM6DS3.ax_raw);
einsteingustavo 0:12fb9cbcabcc 216 // serial.printf("imu acc y =%d\r\n", LSM6DS3.ay_raw);
einsteingustavo 0:12fb9cbcabcc 217 // serial.printf("imu acc z =%d\r\n", LSM6DS3.az_raw);
einsteingustavo 0:12fb9cbcabcc 218 // serial.printf("imu dps x =%d\r\n", LSM6DS3.gx_raw);
einsteingustavo 0:12fb9cbcabcc 219 // serial.printf("imu dps y =%d\r\n", LSM6DS3.gy_raw);
einsteingustavo 0:12fb9cbcabcc 220 // serial.printf("imu dps z =%d\r\n", LSM6DS3.gz_raw);
einsteingustavo 0:12fb9cbcabcc 221 break;
einsteingustavo 0:12fb9cbcabcc 222 default:
einsteingustavo 0:12fb9cbcabcc 223 break;
einsteingustavo 0:12fb9cbcabcc 224 }
einsteingustavo 0:12fb9cbcabcc 225 }
einsteingustavo 0:12fb9cbcabcc 226 }
einsteingustavo 0:12fb9cbcabcc 227
einsteingustavo 0:12fb9cbcabcc 228 /* Interrupt services routine */
einsteingustavo 0:12fb9cbcabcc 229 void canISR()
einsteingustavo 0:12fb9cbcabcc 230 {
einsteingustavo 0:12fb9cbcabcc 231 CAN_IER &= ~CAN_IER_FMPIE0; // disable RX interrupt
einsteingustavo 0:12fb9cbcabcc 232 queue.call(&canHandler); // add canHandler() to events queue
einsteingustavo 0:12fb9cbcabcc 233 }
einsteingustavo 0:12fb9cbcabcc 234
einsteingustavo 0:12fb9cbcabcc 235 void servoSwitchISR()
einsteingustavo 0:12fb9cbcabcc 236 {
einsteingustavo 0:12fb9cbcabcc 237 choke_switch.rise(NULL); // throttle interrupt in both edges dettach
einsteingustavo 0:12fb9cbcabcc 238 run_switch.rise(NULL); // throttle interrupt in both edges dettach
einsteingustavo 0:12fb9cbcabcc 239 choke_switch.fall(NULL); // throttle interrupt in both edges dettach
einsteingustavo 0:12fb9cbcabcc 240 run_switch.fall(NULL); // throttle interrupt in both edges dettach
einsteingustavo 0:12fb9cbcabcc 241 switch_clicked = true;
einsteingustavo 0:12fb9cbcabcc 242 debounce_throttle.attach(&throttleDebounceHandler, 0.1);
einsteingustavo 0:12fb9cbcabcc 243 }
einsteingustavo 0:12fb9cbcabcc 244
einsteingustavo 0:12fb9cbcabcc 245 void ticker2HzISR()
einsteingustavo 0:12fb9cbcabcc 246 {
einsteingustavo 0:12fb9cbcabcc 247 state_buffer.push(DISPLAY_ST);
einsteingustavo 0:12fb9cbcabcc 248 }
einsteingustavo 0:12fb9cbcabcc 249
einsteingustavo 0:12fb9cbcabcc 250 void ticker5HzISR()
einsteingustavo 0:12fb9cbcabcc 251 {
einsteingustavo 0:12fb9cbcabcc 252 state_buffer.push(SPEED_ST);
einsteingustavo 0:12fb9cbcabcc 253 state_buffer.push(DISPLAY_ST);
einsteingustavo 0:12fb9cbcabcc 254 }
einsteingustavo 0:12fb9cbcabcc 255
einsteingustavo 0:12fb9cbcabcc 256 void ticker20HzISR()
einsteingustavo 0:12fb9cbcabcc 257 {
einsteingustavo 0:12fb9cbcabcc 258 state_buffer.push(IMU_ST);
einsteingustavo 0:12fb9cbcabcc 259 }
einsteingustavo 0:12fb9cbcabcc 260
einsteingustavo 0:12fb9cbcabcc 261 void frequencyCounterISR()
einsteingustavo 0:12fb9cbcabcc 262 {
einsteingustavo 0:12fb9cbcabcc 263 pulse_counter++;
einsteingustavo 0:12fb9cbcabcc 264 current_period += t.read_us() - last_count;
einsteingustavo 0:12fb9cbcabcc 265 last_count = t.read_us();
einsteingustavo 0:12fb9cbcabcc 266 }
einsteingustavo 0:12fb9cbcabcc 267
einsteingustavo 0:12fb9cbcabcc 268 //void hornISR()
einsteingustavo 0:12fb9cbcabcc 269 //{
einsteingustavo 0:12fb9cbcabcc 270 // debounce_horn.attach(&hornDebounceHandler, DEBOUNCE_TIME);
einsteingustavo 0:12fb9cbcabcc 271 //}
einsteingustavo 0:12fb9cbcabcc 272
einsteingustavo 0:12fb9cbcabcc 273 void headlightISR()
einsteingustavo 0:12fb9cbcabcc 274 {
einsteingustavo 0:12fb9cbcabcc 275 debounce_headlight.attach(&headlightDebounceHandler, DEBOUNCE_TIME);
einsteingustavo 0:12fb9cbcabcc 276 }
einsteingustavo 0:12fb9cbcabcc 277
einsteingustavo 0:12fb9cbcabcc 278 /* Interrupt handlers */
einsteingustavo 0:12fb9cbcabcc 279 void canHandler()
einsteingustavo 0:12fb9cbcabcc 280 {
einsteingustavo 0:12fb9cbcabcc 281 CANMsg rxMsg;
einsteingustavo 0:12fb9cbcabcc 282
einsteingustavo 0:12fb9cbcabcc 283 can.read(rxMsg);
einsteingustavo 0:12fb9cbcabcc 284 filterMessage(rxMsg);
einsteingustavo 0:12fb9cbcabcc 285 CAN_IER |= CAN_IER_FMPIE0; // enable RX interrupt
einsteingustavo 0:12fb9cbcabcc 286 }
einsteingustavo 0:12fb9cbcabcc 287
einsteingustavo 0:12fb9cbcabcc 288 void throttleDebounceHandler()
einsteingustavo 0:12fb9cbcabcc 289 {
einsteingustavo 0:12fb9cbcabcc 290 state_buffer.push(THROTTLE_ST);
einsteingustavo 0:12fb9cbcabcc 291 choke_switch.rise(&servoSwitchISR); // trigger throttle interrupt in both edges
einsteingustavo 0:12fb9cbcabcc 292 run_switch.rise(&servoSwitchISR); // trigger throttle interrupt in both edges
einsteingustavo 0:12fb9cbcabcc 293 choke_switch.fall(&servoSwitchISR); // trigger throttle interrupt in both edges
einsteingustavo 0:12fb9cbcabcc 294 run_switch.fall(&servoSwitchISR); // trigger throttle interrupt in both edges
einsteingustavo 0:12fb9cbcabcc 295 }
einsteingustavo 0:12fb9cbcabcc 296
einsteingustavo 0:12fb9cbcabcc 297 //void hornDebounceHandler()
einsteingustavo 0:12fb9cbcabcc 298 //{
einsteingustavo 0:12fb9cbcabcc 299 // horn = horn_button.read();
einsteingustavo 0:12fb9cbcabcc 300 //}
einsteingustavo 0:12fb9cbcabcc 301
einsteingustavo 0:12fb9cbcabcc 302 void headlightDebounceHandler()
einsteingustavo 0:12fb9cbcabcc 303 {
einsteingustavo 0:12fb9cbcabcc 304 headlight = headlight_switch.read();
einsteingustavo 0:12fb9cbcabcc 305 }
einsteingustavo 0:12fb9cbcabcc 306
einsteingustavo 0:12fb9cbcabcc 307 /* General functions */
einsteingustavo 0:12fb9cbcabcc 308 void setupInterrupts()
einsteingustavo 0:12fb9cbcabcc 309 {
einsteingustavo 0:12fb9cbcabcc 310 can.attach(&canISR, CAN::RxIrq);
einsteingustavo 0:12fb9cbcabcc 311 choke_switch.rise(&servoSwitchISR); // trigger throttle interrupt in both edges
einsteingustavo 0:12fb9cbcabcc 312 choke_switch.fall(&servoSwitchISR); // trigger throttle interrupt in both edges
einsteingustavo 0:12fb9cbcabcc 313 run_switch.rise(&servoSwitchISR); // trigger throttle interrupt in both edges
einsteingustavo 0:12fb9cbcabcc 314 run_switch.fall(&servoSwitchISR); // trigger throttle interrupt in both edges
einsteingustavo 0:12fb9cbcabcc 315 // horn_button.rise(&hornISR);
einsteingustavo 0:12fb9cbcabcc 316 // horn_button.fall(&hornISR);
einsteingustavo 0:12fb9cbcabcc 317 headlight_switch.rise(&headlightISR);
einsteingustavo 0:12fb9cbcabcc 318 headlight_switch.fall(&headlightISR);
einsteingustavo 0:12fb9cbcabcc 319 // ticker2Hz.attach(&ticker2HzISR, 0.4);
einsteingustavo 0:12fb9cbcabcc 320 ticker5Hz.attach(&ticker5HzISR, 0.2);
einsteingustavo 0:12fb9cbcabcc 321 ticker20Hz.attach(&ticker20HzISR, 0.05);
einsteingustavo 0:12fb9cbcabcc 322 signal.period_ms(2);
einsteingustavo 0:12fb9cbcabcc 323 signal.write(0.5f);
einsteingustavo 0:12fb9cbcabcc 324 }
einsteingustavo 0:12fb9cbcabcc 325
einsteingustavo 0:12fb9cbcabcc 326 void filterMessage(CANMsg msg)
einsteingustavo 0:12fb9cbcabcc 327 {
einsteingustavo 0:12fb9cbcabcc 328 // serial.printf("id: %d\r\n", msg.id);
einsteingustavo 0:12fb9cbcabcc 329
einsteingustavo 0:12fb9cbcabcc 330 if(msg.id == RPM_ID)
einsteingustavo 0:12fb9cbcabcc 331 {
einsteingustavo 0:12fb9cbcabcc 332 msg >> rpm_hz;
einsteingustavo 0:12fb9cbcabcc 333 }
einsteingustavo 0:12fb9cbcabcc 334
einsteingustavo 0:12fb9cbcabcc 335 else if(msg.id == TEMPERATURE_ID)
einsteingustavo 0:12fb9cbcabcc 336 {
einsteingustavo 0:12fb9cbcabcc 337 msg >> temp_motor;
einsteingustavo 0:12fb9cbcabcc 338 }
einsteingustavo 0:12fb9cbcabcc 339
einsteingustavo 0:12fb9cbcabcc 340 else if (msg.id == FLAGS_ID)
einsteingustavo 0:12fb9cbcabcc 341 {
einsteingustavo 0:12fb9cbcabcc 342 msg >> flags;
einsteingustavo 0:12fb9cbcabcc 343 }
einsteingustavo 0:12fb9cbcabcc 344 }
einsteingustavo 0:12fb9cbcabcc 345
einsteingustavo 0:12fb9cbcabcc 346 /* Function adapted from Kristian Lauszus library example, source: https://github.com/TKJElectronics/KalmanFilter*/
einsteingustavo 0:12fb9cbcabcc 347 void calcAngles(int16_t accx, int16_t accy, int16_t accz, int16_t grx, int16_t gry, int16_t grz, int16_t dt){
einsteingustavo 0:12fb9cbcabcc 348 static Kalman kalmanX, kalmanY;
einsteingustavo 0:12fb9cbcabcc 349 float kalAngleX, kalAngleY;
einsteingustavo 0:12fb9cbcabcc 350 float pitch, roll;
einsteingustavo 0:12fb9cbcabcc 351 float gyroXrate, gyroYrate;
einsteingustavo 0:12fb9cbcabcc 352 float ax, ay, az;
einsteingustavo 0:12fb9cbcabcc 353 static bool first_execution = true;
einsteingustavo 0:12fb9cbcabcc 354
einsteingustavo 0:12fb9cbcabcc 355 ax = (float) accx * TO_G;
einsteingustavo 0:12fb9cbcabcc 356 ay = (float) accy * TO_G;
einsteingustavo 0:12fb9cbcabcc 357 az = (float) accz * TO_G;
einsteingustavo 0:12fb9cbcabcc 358 pitch = atan2(ay, az) * RAD_TO_DEGREE;
einsteingustavo 0:12fb9cbcabcc 359 roll = atan(-ax / sqrt(ay * ay + az * az)) * RAD_TO_DEGREE;
einsteingustavo 0:12fb9cbcabcc 360 gyroXrate = grx / TO_DPS; // Convert to deg/s
einsteingustavo 0:12fb9cbcabcc 361 gyroYrate = gry / TO_DPS; // Convert to deg/s
einsteingustavo 0:12fb9cbcabcc 362
einsteingustavo 0:12fb9cbcabcc 363 if (first_execution)
einsteingustavo 0:12fb9cbcabcc 364 {
einsteingustavo 0:12fb9cbcabcc 365 // set starting angle if first execution
einsteingustavo 0:12fb9cbcabcc 366 first_execution = false;
einsteingustavo 0:12fb9cbcabcc 367 kalmanX.setAngle(roll);
einsteingustavo 0:12fb9cbcabcc 368 kalmanY.setAngle(pitch);
einsteingustavo 0:12fb9cbcabcc 369 }
einsteingustavo 0:12fb9cbcabcc 370 // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
einsteingustavo 0:12fb9cbcabcc 371 if((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90))
einsteingustavo 0:12fb9cbcabcc 372 {
einsteingustavo 0:12fb9cbcabcc 373 kalmanX.setAngle(roll);
einsteingustavo 0:12fb9cbcabcc 374 kalAngleX = roll;
einsteingustavo 0:12fb9cbcabcc 375 }
einsteingustavo 0:12fb9cbcabcc 376 else
einsteingustavo 0:12fb9cbcabcc 377 {
einsteingustavo 0:12fb9cbcabcc 378 kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
einsteingustavo 0:12fb9cbcabcc 379 }
einsteingustavo 0:12fb9cbcabcc 380
einsteingustavo 0:12fb9cbcabcc 381 if(abs(kalAngleX) > 90)
einsteingustavo 0:12fb9cbcabcc 382 {
einsteingustavo 0:12fb9cbcabcc 383 gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
einsteingustavo 0:12fb9cbcabcc 384 }
einsteingustavo 0:12fb9cbcabcc 385 kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
einsteingustavo 0:12fb9cbcabcc 386
einsteingustavo 0:12fb9cbcabcc 387 angle_roll = roll;
einsteingustavo 0:12fb9cbcabcc 388 angle_pitch = pitch;
einsteingustavo 0:12fb9cbcabcc 389 }
einsteingustavo 0:12fb9cbcabcc 390
einsteingustavo 0:12fb9cbcabcc 391 void displayData(uint16_t vel, uint16_t Hz, uint16_t temp, bool comb, \
einsteingustavo 0:12fb9cbcabcc 392 bool b, bool tl, bool fl, int16_t gp, int16_t gr, bool box)
einsteingustavo 0:12fb9cbcabcc 393 {
einsteingustavo 0:12fb9cbcabcc 394 static uint16_t vel2 = -1, Hz2 = -1, temp2 = -1;
einsteingustavo 0:12fb9cbcabcc 395 static int16_t gr2 = -1, gp2 = -1;
einsteingustavo 0:12fb9cbcabcc 396 static bool box2 = false;
einsteingustavo 0:12fb9cbcabcc 397 static bool comb2 = true, b2 = true, tl2 = false, fl2 = false;
einsteingustavo 0:12fb9cbcabcc 398
einsteingustavo 0:12fb9cbcabcc 399 char str[512];
einsteingustavo 0:12fb9cbcabcc 400 int aux=0;
einsteingustavo 0:12fb9cbcabcc 401 strcpy(str,"");
einsteingustavo 0:12fb9cbcabcc 402
einsteingustavo 0:12fb9cbcabcc 403 if(box!=box2)
einsteingustavo 0:12fb9cbcabcc 404 sprintf(str + strlen(str), "page1.box.val=%d%c%c%c",box,0xff,0xff,0xff);
einsteingustavo 0:12fb9cbcabcc 405 if(vel!=vel2)
einsteingustavo 0:12fb9cbcabcc 406 sprintf(str + strlen(str), "page1.v.val=%d%c%c%c",vel,0xff,0xff,0xff);
einsteingustavo 0:12fb9cbcabcc 407 if(Hz!=Hz2)
einsteingustavo 0:12fb9cbcabcc 408 {
einsteingustavo 0:12fb9cbcabcc 409 int r = (Hz*6000)/(5000.0);
einsteingustavo 0:12fb9cbcabcc 410 sprintf(str + strlen(str), "page1.r.val=%d%c%c%c",r,0xff,0xff,0xff);
einsteingustavo 0:12fb9cbcabcc 411 }
einsteingustavo 0:12fb9cbcabcc 412 if(comb!=comb2)
einsteingustavo 0:12fb9cbcabcc 413 {
einsteingustavo 0:12fb9cbcabcc 414 if (!comb)
einsteingustavo 0:12fb9cbcabcc 415 sprintf(str + strlen(str),"page1.c.val=%d%c%c%c",25,0xff,0xff,0xff);
einsteingustavo 0:12fb9cbcabcc 416 else
einsteingustavo 0:12fb9cbcabcc 417 sprintf(str + strlen(str),"page1.c.val=%d%c%c%c",100,0xff,0xff,0xff);
einsteingustavo 0:12fb9cbcabcc 418 }
einsteingustavo 0:12fb9cbcabcc 419 if(temp!=temp2)
einsteingustavo 0:12fb9cbcabcc 420 {
einsteingustavo 0:12fb9cbcabcc 421 int tp = (100*temp)/120;
einsteingustavo 0:12fb9cbcabcc 422 sprintf(str + strlen(str),"page1.tp.val=%d%c%c%c",tp,0xff,0xff,0xff);
einsteingustavo 0:12fb9cbcabcc 423 }
einsteingustavo 0:12fb9cbcabcc 424 if(b!=b2)
einsteingustavo 0:12fb9cbcabcc 425 sprintf(str + strlen(str),"page1.b.val=%d%c%c%c",b,0xff,0xff,0xff);
einsteingustavo 0:12fb9cbcabcc 426 if(tl!=tl2)
einsteingustavo 0:12fb9cbcabcc 427 sprintf(str + strlen(str),"page1.tl.val=%d%c%c%c",tl,0xff,0xff,0xff);
einsteingustavo 0:12fb9cbcabcc 428 if(fl!=fl2)
einsteingustavo 0:12fb9cbcabcc 429 sprintf(str + strlen(str),"page1.fl.val=%d%c%c%c",fl,0xff,0xff,0xff);
einsteingustavo 0:12fb9cbcabcc 430 if(gp!=gp2)
einsteingustavo 0:12fb9cbcabcc 431 {
einsteingustavo 0:12fb9cbcabcc 432 int gpn = 7;
einsteingustavo 0:12fb9cbcabcc 433 if(gp<=-35){gpn=0;}
einsteingustavo 0:12fb9cbcabcc 434 else if(gp>=-30&&gp<-25){gpn=1;}
einsteingustavo 0:12fb9cbcabcc 435 else if(gp>=-25&&gp<-20){gpn=2;}
einsteingustavo 0:12fb9cbcabcc 436 else if(gp>=-20&&gp<-15){gpn=3;}
einsteingustavo 0:12fb9cbcabcc 437 else if(gp>=-15&&gp<-10){gpn=4;}
einsteingustavo 0:12fb9cbcabcc 438 else if(gp>=-10&&gp<-5){gpn=5;}
einsteingustavo 0:12fb9cbcabcc 439 else if(gp>=-5&&gp<0){gpn=6;}
einsteingustavo 0:12fb9cbcabcc 440 else if(gp==0){gpn=7;}
einsteingustavo 0:12fb9cbcabcc 441 else if(gp>0&&gp<=5){gpn=8;}
einsteingustavo 0:12fb9cbcabcc 442 else if(gp>5&&gp<=10){gpn=9;}
einsteingustavo 0:12fb9cbcabcc 443 else if(gp>10&&gp<=15){gpn=10;}
einsteingustavo 0:12fb9cbcabcc 444 else if(gp>15&&gp<=20){gpn=11;}
einsteingustavo 0:12fb9cbcabcc 445 else if(gp>20&&gp<=25){gpn=12;}
einsteingustavo 0:12fb9cbcabcc 446 else if(gp>25&&gp<=30){gpn=13;}
einsteingustavo 0:12fb9cbcabcc 447 else if(gp>=35){gpn=14;}
einsteingustavo 0:12fb9cbcabcc 448 #ifdef MB2
einsteingustavo 0:12fb9cbcabcc 449 sprintf(str + strlen(str),"page3.gr.val=%d%c%c%c",gpn,0xff,0xff,0xff);
einsteingustavo 0:12fb9cbcabcc 450 sprintf(str + strlen(str),"page3.gr_n.val=%d%c%c%c",gp*(-1),0xff,0xff,0xff);
einsteingustavo 0:12fb9cbcabcc 451 #endif
einsteingustavo 0:12fb9cbcabcc 452 #ifdef MB1
einsteingustavo 0:12fb9cbcabcc 453 sprintf(str + strlen(str),"page2.gp.val=%d%c%c%c",gpn,0xff,0xff,0xff);
einsteingustavo 0:12fb9cbcabcc 454 sprintf(str + strlen(str),"page2.gp_n.val=%d%c%c%c",gp,0xff,0xff,0xff);
einsteingustavo 0:12fb9cbcabcc 455 #endif
einsteingustavo 0:12fb9cbcabcc 456 }
einsteingustavo 0:12fb9cbcabcc 457 if(gr!=gr2)
einsteingustavo 0:12fb9cbcabcc 458 {
einsteingustavo 0:12fb9cbcabcc 459 int grn = 7;
einsteingustavo 0:12fb9cbcabcc 460 if(gr<=-35){grn=0;}
einsteingustavo 0:12fb9cbcabcc 461 else if(gr>=-30&&gr<-25){grn=1;}
einsteingustavo 0:12fb9cbcabcc 462 else if(gr>=-25&&gr<-20){grn=2;}
einsteingustavo 0:12fb9cbcabcc 463 else if(gr>=-20&&gr<-15){grn=3;}
einsteingustavo 0:12fb9cbcabcc 464 else if(gr>=-15&&gr<-10){grn=4;}
einsteingustavo 0:12fb9cbcabcc 465 else if(gr>=-10&&gr<-5){grn=5;}
einsteingustavo 0:12fb9cbcabcc 466 else if(gr>=-5&&gr<0){grn=6;}
einsteingustavo 0:12fb9cbcabcc 467 else if(gr==0){grn=7;}
einsteingustavo 0:12fb9cbcabcc 468 else if(gr>0&&gr<=5){grn=8;}
einsteingustavo 0:12fb9cbcabcc 469 else if(gr>5&&gr<=10){grn=9;}
einsteingustavo 0:12fb9cbcabcc 470 else if(gr>10&&gr<=15){grn=10;}
einsteingustavo 0:12fb9cbcabcc 471 else if(gr>15&&gr<=20){grn=11;}
einsteingustavo 0:12fb9cbcabcc 472 else if(gr>20&&gr<=25){grn=12;}
einsteingustavo 0:12fb9cbcabcc 473 else if(gr>25&&gr<=30){grn=13;}
einsteingustavo 0:12fb9cbcabcc 474 else if(gr>=35){grn=14;}
einsteingustavo 0:12fb9cbcabcc 475 #ifdef MB2
einsteingustavo 0:12fb9cbcabcc 476 sprintf(str + strlen(str),"page3.gp.val=%d%c%c%c",grn,0xff,0xff,0xff);
einsteingustavo 0:12fb9cbcabcc 477 sprintf(str + strlen(str),"page3.gp_n.val=%d%c%c%c",gr,0xff,0xff,0xff);
einsteingustavo 0:12fb9cbcabcc 478 #endif
einsteingustavo 0:12fb9cbcabcc 479 #ifdef MB1
einsteingustavo 0:12fb9cbcabcc 480 sprintf(str + strlen(str),"page2.gr.val=%d%c%c%c",grn,0xff,0xff,0xff);
einsteingustavo 0:12fb9cbcabcc 481 sprintf(str + strlen(str),"page2.gr_n.val=%d%c%c%c",gr,0xff,0xff,0xff);
einsteingustavo 0:12fb9cbcabcc 482 #endif
einsteingustavo 0:12fb9cbcabcc 483 }
einsteingustavo 0:12fb9cbcabcc 484
einsteingustavo 0:12fb9cbcabcc 485 while(str[aux]!='\0')
einsteingustavo 0:12fb9cbcabcc 486 {
einsteingustavo 0:12fb9cbcabcc 487 serial.putc(str[aux]);
einsteingustavo 0:12fb9cbcabcc 488 aux++;
einsteingustavo 0:12fb9cbcabcc 489 }
einsteingustavo 0:12fb9cbcabcc 490 box2 = box;
einsteingustavo 0:12fb9cbcabcc 491 vel2 = vel;
einsteingustavo 0:12fb9cbcabcc 492 Hz2 = Hz;
einsteingustavo 0:12fb9cbcabcc 493 temp2 = temp;
einsteingustavo 0:12fb9cbcabcc 494 gr2=gr;
einsteingustavo 0:12fb9cbcabcc 495 gp2=gp;
einsteingustavo 0:12fb9cbcabcc 496 comb2 = comb;
einsteingustavo 0:12fb9cbcabcc 497 b2=b;
einsteingustavo 0:12fb9cbcabcc 498 tl2=tl;
einsteingustavo 0:12fb9cbcabcc 499 fl2=fl;
einsteingustavo 0:12fb9cbcabcc 500 }