Einstein Filho
Mangue Baja team's code to frontal ECU
Diff: main.cpp
- Revision:
- 0:12fb9cbcabcc
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Jul 24 20:03:52 2019 +0000
@@ -0,0 +1,500 @@
+#include "mbed.h"
+#include "stats_report.h"
+/* User libraries */
+#include "definitions.h"
+#include "frontdefs.h"
+#include "CANMsg.h"
+#include "LSM6DS3.h"
+#include "Kalman.h"
+
+#define MB1
+//#define MB2
+
+/* Communication protocols */
+CAN can(PB_8, PB_9, 1000000);
+Serial serial(PA_2, PA_3, 115200);
+LSM6DS3 LSM6DS3(PB_7, PB_6);
+
+/* I/O pins */
+InterruptIn freq_sensor(PB_10, PullNone);
+InterruptIn choke_switch(PA_5, PullUp); // servomotor CHOKE mode
+InterruptIn run_switch(PA_7, PullUp); // servomotor RUN mode
+InterruptIn horn_button(PB_1, PullUp);
+InterruptIn headlight_switch(PB_0, PullUp);
+DigitalOut horn(PB_11);
+DigitalOut headlight(PA_1);
+/* Debug pins */
+DigitalOut led(PC_13);
+PwmOut signal(PA_6);
+DigitalOut dbg1(PC_14);
+DigitalOut dbg2(PC_15);
+DigitalOut dbg3(PA_4);
+
+/* Interrupt services routine */
+void canISR();
+void servoSwitchISR();
+void ticker2HzISR();
+void ticker5HzISR();
+void ticker20HzISR();
+void tickerTrottleISR();
+void frequencyCounterISR();
+void hornISR();
+void headlightISR();
+/* Interrupt handlers */
+void canHandler();
+void throttleDebounceHandler();
+void hornDebounceHandler();
+void headlightDebounceHandler();
+/* General functions*/
+void setupInterrupts();
+void filterMessage(CANMsg msg);
+void calcAngles(int16_t accx, int16_t accy, int16_t accz, int16_t grx, int16_t gry, int16_t grz, int16_t dt);
+void displayData(uint16_t vel, uint16_t Hz, uint16_t temp, bool comb, \
+ bool b, bool tl, bool fl, int16_t gp, int16_t gr, bool box);
+
+/* Debug variables */
+Timer t;
+bool buffer_full = false;
+unsigned int t0, t1;
+/* Mbed OS tools */
+Thread eventThread;
+EventQueue queue(1024);
+Ticker ticker2Hz;
+Ticker ticker5Hz;
+Ticker ticker20Hz;
+Ticker tickerTrottle;
+Timeout debounce_throttle;
+Timeout debounce_horn;
+Timeout debounce_headlight;
+// Timeout horn_limiter; // stop sound of horn after a determined period
+CircularBuffer <state_t, BUFFER_SIZE> state_buffer;
+/* Global variables */
+bool switch_clicked = false;
+uint8_t switch_state = 0x00, flags = 0x00, pulse_counter = 0, temp_motor = 0;
+state_t current_state = IDLE_ST;
+uint64_t current_period = 0, last_count = 0, last_acq = 0;
+uint8_t imu_failed = 0; // number of times before a new connection attempt with imu
+float speed_hz = 0;
+uint16_t rpm_hz = 0, speed_display = 0, speed_radio = 0, dt = 0;
+int16_t angle_roll = 0, angle_pitch = 0;
+packet_t data;
+
+int main()
+{
+ /* Main variables */
+ CANMsg txMsg;
+ /* Initialization */
+ t.start();
+ horn = horn_button.read(); // horn OFF
+ headlight = headlight_switch.read(); // headlight OFF
+ led = 1; // led OFF
+ eventThread.start(callback(&queue, &EventQueue::dispatch_forever));
+ t0 = t.read_us();
+ uint16_t lsm_addr = LSM6DS3.begin(LSM6DS3.G_SCALE_245DPS, LSM6DS3.A_SCALE_2G, LSM6DS3.G_ODR_26_BW_2, LSM6DS3.A_ODR_26);
+ t1 = t.read_us();
+// serial.printf("%d\r\n", (t1 - t0));
+ setupInterrupts();
+
+ while (true) {
+ if (state_buffer.full())
+ {
+ buffer_full = true;
+ led = 0;
+ state_buffer.pop(current_state);
+ }
+ else
+ {
+ led = 1;
+ buffer_full = false;
+ if (!state_buffer.empty())
+ state_buffer.pop(current_state);
+ else
+ current_state = IDLE_ST;
+ }
+
+ switch (current_state)
+ {
+ case IDLE_ST:
+// Thread::wait(2);
+ break;
+ case SLOWACQ_ST:
+ break;
+ case IMU_ST:
+ t0 = t.read_us();
+ dbg1 = !dbg1;
+ if (lsm_addr)
+ {
+ bool nack = LSM6DS3.readAccel(); // read accelerometer data into LSM6DS3.aN_raw
+ if (!nack)
+ nack = LSM6DS3.readGyro(); // " gyroscope data into LSM6DS3.gN_raw
+
+ if (nack)
+ {
+ lsm_addr = 0;
+ LSM6DS3.ax_raw = 0;
+ LSM6DS3.ay_raw = 0;
+ LSM6DS3.az_raw = 0;
+ LSM6DS3.gx_raw = 0;
+ LSM6DS3.gy_raw = 0;
+ LSM6DS3.gz_raw = 0;
+ }
+ }
+ else if (imu_failed == IMU_TRIES)
+ {
+ lsm_addr = LSM6DS3.begin(LSM6DS3.G_SCALE_245DPS, LSM6DS3.A_SCALE_2G, LSM6DS3.G_ODR_26_BW_2, LSM6DS3.A_ODR_26);
+ t1 = t.read_us();
+ imu_failed = 0;
+// serial.printf("%d\r\n", (t1 - t0));
+ }
+ else
+ {
+ imu_failed++;
+ }
+
+ last_acq = t.read_ms();
+// serial.printf("accz = %d\r\n", LSM6DS3.gz_raw);
+ calcAngles(LSM6DS3.ax_raw, LSM6DS3.ay_raw, LSM6DS3.az_raw, LSM6DS3.gx_raw, LSM6DS3.gy_raw, LSM6DS3.gz_raw, dt);
+ /* Send accelerometer data */
+ txMsg.clear(IMU_ACC_ID);
+ txMsg << LSM6DS3.ax_raw << LSM6DS3.ay_raw << LSM6DS3.az_raw;
+ if(can.write(txMsg))
+ {
+ /* Send gyroscope data only if accelerometer data succeeds */
+ txMsg.clear(IMU_DPS_ID);
+ txMsg << LSM6DS3.gx_raw << LSM6DS3.gy_raw << LSM6DS3.gz_raw << dt;
+ can.write(txMsg);
+ }
+ break;
+ case SPEED_ST:
+// serial.printf("speed ok\r\n");
+ dbg2 = !dbg2;
+ freq_sensor.fall(NULL); // disable interrupt
+ if (current_period != 0)
+ {
+ speed_hz = 1000000*((float)pulse_counter/current_period); //calculates frequency in Hz
+ }
+ else
+ {
+ speed_hz = 0;
+ }
+ speed_display = ((float)(PI*WHEEL_DIAMETER*speed_hz)/WHEEL_HOLES_NUMBER); // make conversion hz to km/h
+ speed_radio = ((float)((speed_display)/60.0)*65535);
+ pulse_counter = 0;
+ current_period = 0; //|-> reset pulses related variables
+ last_count = t.read_us();
+ freq_sensor.fall(&frequencyCounterISR); // enable interrupt
+ /* Send speed data */
+ txMsg.clear(SPEED_ID);
+ txMsg << speed_radio;
+ can.write(txMsg);
+// state_buffer.push(DEBUG_ST); // debug
+ break;
+ case THROTTLE_ST:
+// serial.printf("throttle ok\r\n");
+ dbg3 = !dbg3;
+ if (switch_clicked)
+ {
+ switch_state = !choke_switch.read() << 1 | !run_switch.read() << 0;
+ //serial.printf("switch_state = %d\r\n", switch_state);
+ /* Send CAN message */
+ txMsg.clear(THROTTLE_ID);
+ txMsg << switch_state;
+// serial.printf("can ok\r/n"); // append data (8 bytes max)
+ can.write(txMsg);
+
+ switch_clicked = false;
+ }
+ break;
+ case DISPLAY_ST:
+// serial.printf("rpm_hz=%d\r\n", rpm_hz);
+ displayData(speed_display, rpm_hz, temp_motor, (flags & 0x08), false, true, false, angle_pitch, angle_roll, (flags & 0x80));
+// state_buffer.push(DEBUG_ST);
+ break;
+ case DEBUG_ST:
+// serial.printf("r= %d, p=%d\r\n", angle_roll, angle_pitch);
+// serial.printf("imu acc x =%d\r\n", LSM6DS3.ax_raw);
+// serial.printf("imu acc y =%d\r\n", LSM6DS3.ay_raw);
+// serial.printf("imu acc z =%d\r\n", LSM6DS3.az_raw);
+// serial.printf("imu dps x =%d\r\n", LSM6DS3.gx_raw);
+// serial.printf("imu dps y =%d\r\n", LSM6DS3.gy_raw);
+// serial.printf("imu dps z =%d\r\n", LSM6DS3.gz_raw);
+ break;
+ default:
+ break;
+ }
+ }
+}
+
+/* Interrupt services routine */
+void canISR()
+{
+ CAN_IER &= ~CAN_IER_FMPIE0; // disable RX interrupt
+ queue.call(&canHandler); // add canHandler() to events queue
+}
+
+void servoSwitchISR()
+{
+ choke_switch.rise(NULL); // throttle interrupt in both edges dettach
+ run_switch.rise(NULL); // throttle interrupt in both edges dettach
+ choke_switch.fall(NULL); // throttle interrupt in both edges dettach
+ run_switch.fall(NULL); // throttle interrupt in both edges dettach
+ switch_clicked = true;
+ debounce_throttle.attach(&throttleDebounceHandler, 0.1);
+}
+
+void ticker2HzISR()
+{
+ state_buffer.push(DISPLAY_ST);
+}
+
+void ticker5HzISR()
+{
+ state_buffer.push(SPEED_ST);
+ state_buffer.push(DISPLAY_ST);
+}
+
+void ticker20HzISR()
+{
+ state_buffer.push(IMU_ST);
+}
+
+void frequencyCounterISR()
+{
+ pulse_counter++;
+ current_period += t.read_us() - last_count;
+ last_count = t.read_us();
+}
+
+//void hornISR()
+//{
+// debounce_horn.attach(&hornDebounceHandler, DEBOUNCE_TIME);
+//}
+
+void headlightISR()
+{
+ debounce_headlight.attach(&headlightDebounceHandler, DEBOUNCE_TIME);
+}
+
+/* Interrupt handlers */
+void canHandler()
+{
+ CANMsg rxMsg;
+
+ can.read(rxMsg);
+ filterMessage(rxMsg);
+ CAN_IER |= CAN_IER_FMPIE0; // enable RX interrupt
+}
+
+void throttleDebounceHandler()
+{
+ state_buffer.push(THROTTLE_ST);
+ choke_switch.rise(&servoSwitchISR); // trigger throttle interrupt in both edges
+ run_switch.rise(&servoSwitchISR); // trigger throttle interrupt in both edges
+ choke_switch.fall(&servoSwitchISR); // trigger throttle interrupt in both edges
+ run_switch.fall(&servoSwitchISR); // trigger throttle interrupt in both edges
+}
+
+//void hornDebounceHandler()
+//{
+// horn = horn_button.read();
+//}
+
+void headlightDebounceHandler()
+{
+ headlight = headlight_switch.read();
+}
+
+/* General functions */
+void setupInterrupts()
+{
+ can.attach(&canISR, CAN::RxIrq);
+ choke_switch.rise(&servoSwitchISR); // trigger throttle interrupt in both edges
+ choke_switch.fall(&servoSwitchISR); // trigger throttle interrupt in both edges
+ run_switch.rise(&servoSwitchISR); // trigger throttle interrupt in both edges
+ run_switch.fall(&servoSwitchISR); // trigger throttle interrupt in both edges
+// horn_button.rise(&hornISR);
+// horn_button.fall(&hornISR);
+ headlight_switch.rise(&headlightISR);
+ headlight_switch.fall(&headlightISR);
+// ticker2Hz.attach(&ticker2HzISR, 0.4);
+ ticker5Hz.attach(&ticker5HzISR, 0.2);
+ ticker20Hz.attach(&ticker20HzISR, 0.05);
+ signal.period_ms(2);
+ signal.write(0.5f);
+}
+
+void filterMessage(CANMsg msg)
+{
+ // serial.printf("id: %d\r\n", msg.id);
+
+ if(msg.id == RPM_ID)
+ {
+ msg >> rpm_hz;
+ }
+
+ else if(msg.id == TEMPERATURE_ID)
+ {
+ msg >> temp_motor;
+ }
+
+ else if (msg.id == FLAGS_ID)
+ {
+ msg >> flags;
+ }
+}
+
+/* Function adapted from Kristian Lauszus library example, source: https://github.com/TKJElectronics/KalmanFilter*/
+void calcAngles(int16_t accx, int16_t accy, int16_t accz, int16_t grx, int16_t gry, int16_t grz, int16_t dt){
+ static Kalman kalmanX, kalmanY;
+ float kalAngleX, kalAngleY;
+ float pitch, roll;
+ float gyroXrate, gyroYrate;
+ float ax, ay, az;
+ static bool first_execution = true;
+
+ ax = (float) accx * TO_G;
+ ay = (float) accy * TO_G;
+ az = (float) accz * TO_G;
+ pitch = atan2(ay, az) * RAD_TO_DEGREE;
+ roll = atan(-ax / sqrt(ay * ay + az * az)) * RAD_TO_DEGREE;
+ gyroXrate = grx / TO_DPS; // Convert to deg/s
+ gyroYrate = gry / TO_DPS; // Convert to deg/s
+
+ if (first_execution)
+ {
+ // set starting angle if first execution
+ first_execution = false;
+ kalmanX.setAngle(roll);
+ kalmanY.setAngle(pitch);
+ }
+ // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
+ if((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90))
+ {
+ kalmanX.setAngle(roll);
+ kalAngleX = roll;
+ }
+ else
+ {
+ kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
+ }
+
+ if(abs(kalAngleX) > 90)
+ {
+ gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
+ }
+ kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
+
+ angle_roll = roll;
+ angle_pitch = pitch;
+}
+
+void displayData(uint16_t vel, uint16_t Hz, uint16_t temp, bool comb, \
+ bool b, bool tl, bool fl, int16_t gp, int16_t gr, bool box)
+{
+ static uint16_t vel2 = -1, Hz2 = -1, temp2 = -1;
+ static int16_t gr2 = -1, gp2 = -1;
+ static bool box2 = false;
+ static bool comb2 = true, b2 = true, tl2 = false, fl2 = false;
+
+ char str[512];
+ int aux=0;
+ strcpy(str,"");
+
+ if(box!=box2)
+ sprintf(str + strlen(str), "page1.box.val=%d%c%c%c",box,0xff,0xff,0xff);
+ if(vel!=vel2)
+ sprintf(str + strlen(str), "page1.v.val=%d%c%c%c",vel,0xff,0xff,0xff);
+ if(Hz!=Hz2)
+ {
+ int r = (Hz*6000)/(5000.0);
+ sprintf(str + strlen(str), "page1.r.val=%d%c%c%c",r,0xff,0xff,0xff);
+ }
+ if(comb!=comb2)
+ {
+ if (!comb)
+ sprintf(str + strlen(str),"page1.c.val=%d%c%c%c",25,0xff,0xff,0xff);
+ else
+ sprintf(str + strlen(str),"page1.c.val=%d%c%c%c",100,0xff,0xff,0xff);
+ }
+ if(temp!=temp2)
+ {
+ int tp = (100*temp)/120;
+ sprintf(str + strlen(str),"page1.tp.val=%d%c%c%c",tp,0xff,0xff,0xff);
+ }
+ if(b!=b2)
+ sprintf(str + strlen(str),"page1.b.val=%d%c%c%c",b,0xff,0xff,0xff);
+ if(tl!=tl2)
+ sprintf(str + strlen(str),"page1.tl.val=%d%c%c%c",tl,0xff,0xff,0xff);
+ if(fl!=fl2)
+ sprintf(str + strlen(str),"page1.fl.val=%d%c%c%c",fl,0xff,0xff,0xff);
+ if(gp!=gp2)
+ {
+ int gpn = 7;
+ if(gp<=-35){gpn=0;}
+ else if(gp>=-30&&gp<-25){gpn=1;}
+ else if(gp>=-25&&gp<-20){gpn=2;}
+ else if(gp>=-20&&gp<-15){gpn=3;}
+ else if(gp>=-15&&gp<-10){gpn=4;}
+ else if(gp>=-10&&gp<-5){gpn=5;}
+ else if(gp>=-5&&gp<0){gpn=6;}
+ else if(gp==0){gpn=7;}
+ else if(gp>0&&gp<=5){gpn=8;}
+ else if(gp>5&&gp<=10){gpn=9;}
+ else if(gp>10&&gp<=15){gpn=10;}
+ else if(gp>15&&gp<=20){gpn=11;}
+ else if(gp>20&&gp<=25){gpn=12;}
+ else if(gp>25&&gp<=30){gpn=13;}
+ else if(gp>=35){gpn=14;}
+ #ifdef MB2
+ sprintf(str + strlen(str),"page3.gr.val=%d%c%c%c",gpn,0xff,0xff,0xff);
+ sprintf(str + strlen(str),"page3.gr_n.val=%d%c%c%c",gp*(-1),0xff,0xff,0xff);
+ #endif
+ #ifdef MB1
+ sprintf(str + strlen(str),"page2.gp.val=%d%c%c%c",gpn,0xff,0xff,0xff);
+ sprintf(str + strlen(str),"page2.gp_n.val=%d%c%c%c",gp,0xff,0xff,0xff);
+ #endif
+ }
+ if(gr!=gr2)
+ {
+ int grn = 7;
+ if(gr<=-35){grn=0;}
+ else if(gr>=-30&&gr<-25){grn=1;}
+ else if(gr>=-25&&gr<-20){grn=2;}
+ else if(gr>=-20&&gr<-15){grn=3;}
+ else if(gr>=-15&&gr<-10){grn=4;}
+ else if(gr>=-10&&gr<-5){grn=5;}
+ else if(gr>=-5&&gr<0){grn=6;}
+ else if(gr==0){grn=7;}
+ else if(gr>0&&gr<=5){grn=8;}
+ else if(gr>5&&gr<=10){grn=9;}
+ else if(gr>10&&gr<=15){grn=10;}
+ else if(gr>15&&gr<=20){grn=11;}
+ else if(gr>20&&gr<=25){grn=12;}
+ else if(gr>25&&gr<=30){grn=13;}
+ else if(gr>=35){grn=14;}
+ #ifdef MB2
+ sprintf(str + strlen(str),"page3.gp.val=%d%c%c%c",grn,0xff,0xff,0xff);
+ sprintf(str + strlen(str),"page3.gp_n.val=%d%c%c%c",gr,0xff,0xff,0xff);
+ #endif
+ #ifdef MB1
+ sprintf(str + strlen(str),"page2.gr.val=%d%c%c%c",grn,0xff,0xff,0xff);
+ sprintf(str + strlen(str),"page2.gr_n.val=%d%c%c%c",gr,0xff,0xff,0xff);
+ #endif
+ }
+
+ while(str[aux]!='\0')
+ {
+ serial.putc(str[aux]);
+ aux++;
+ }
+ box2 = box;
+ vel2 = vel;
+ Hz2 = Hz;
+ temp2 = temp;
+ gr2=gr;
+ gp2=gp;
+ comb2 = comb;
+ b2=b;
+ tl2=tl;
+ fl2=fl;
+}
\ No newline at end of file