diff --git a/306-controller_interface.X/app/can_message.c b/306-controller_interface.X/app/can_message.c
index 72ff705..ba37fed 100644
--- a/306-controller_interface.X/app/can_message.c
+++ b/306-controller_interface.X/app/can_message.c
@@ -29,7 +29,7 @@ typedef union {
uint16_t full;
} BYTES_2;
-void CM_processIncome(uint8_t idSender, uint8_t idMsg, uint32_t data){
+void CM_processIncome(uint8_t idSender, uint8_t idMsg, bool rtr, uint32_t data){
BYTES_4 incomeData;
incomeData.full = data;
BYTES_4 revertData;
@@ -59,21 +59,21 @@ void CM_processIncome(uint8_t idSender, uint8_t idMsg, uint32_t data){
if(idMsg == 0x1) { // CONTROL_SPEED_FACTOR
// valHH valH valL valLL
- KART_CST.CONTROL_SPEED_FACTOR = data;
+ KART_CST.CONTROL_SPEED_FACTOR = revertData.full;
MEM_write_4_byte(MEMADD_CONTROL_SPEED_FACTOR, KART_CST.CONTROL_SPEED_FACTOR);
}
if(idMsg == 0x2) { // CONTROL_POWER_FACTOR
// valHH valH valL valLL
- KART_CST.CONTROL_POWER_FACTOR = data;
+ KART_CST.CONTROL_POWER_FACTOR = revertData.full;
MEM_write_4_byte(MEMADD_CONTROL_POWER_FACTOR, KART_CST.CONTROL_POWER_FACTOR);
-
+ CAN_Send(0, 5, KART_CST.CONTROL_POWER_FACTOR);
}
if(idMsg == 0x3) { // CONTROL_STEERING_FACTOR
// valHH valH valL valLL
- KART_CST.CONTROL_STEERING_FACTOR = data;
+ KART_CST.CONTROL_STEERING_FACTOR = revertData.full;
MEM_write_4_byte(MEMADD_CONTROL_STEERING_FACTOR, KART_CST.CONTROL_STEERING_FACTOR);
}
diff --git a/306-controller_interface.X/app/can_message.h b/306-controller_interface.X/app/can_message.h
index 0249c07..7a7a352 100644
--- a/306-controller_interface.X/app/can_message.h
+++ b/306-controller_interface.X/app/can_message.h
@@ -32,7 +32,7 @@ S R M
* @param idMsg is of the message
* @param data data of the message
*/
-void CM_processIncome(uint8_t idSender, uint8_t idMsg, uint32_t data);
+void CM_processIncome(uint8_t idSender, uint8_t idMsg, bool rtr, uint32_t data);
/**
* Send alive message from controller
diff --git a/306-controller_interface.X/app/eeprom.c b/306-controller_interface.X/app/eeprom.c
index 2e1ba5d..4efdbf8 100644
--- a/306-controller_interface.X/app/eeprom.c
+++ b/306-controller_interface.X/app/eeprom.c
@@ -7,6 +7,7 @@
#include "eeprom.h"
#include "../app/car.h"
+#include "../middleware/can_interface.h"
typedef union {
struct {
@@ -21,12 +22,12 @@ typedef union {
void MEM_init(){
uint8_t check = MEM_read_1_byte(0x0);
- if(check != 0x2A){
+ if(check != 0x42){
KART_CST.CONTROL_STEERING_MODE = 0;
KART_CST.CONTROL_ALIVE_TIME = 50;
- KART_CST.CONTROL_SPEED_FACTOR = 111111;
- KART_CST.CONTROL_POWER_FACTOR = 10000;
- KART_CST.CONTROL_STEERING_FACTOR = 400000000;
+ KART_CST.CONTROL_SPEED_FACTOR = 111111; // 111'111
+ KART_CST.CONTROL_POWER_FACTOR = 10000; // 10'000
+ KART_CST.CONTROL_STEERING_FACTOR = 400000000; // 400'000'000
KART_CST.CONTROL_MAX_SPEED_FW = 50;
KART_CST.CONTROL_MAX_SPEED_BW = 25;
@@ -74,13 +75,13 @@ void MEM_init(){
MEM_write_1_byte(MEMADD_BATTERY_ENERGY_TIME, KART_CST.BATTERY_ENERGY_TIME);
MEM_write_1_byte(MEMADD_BATTERY_ALIVE_TIME, KART_CST.BATTERY_ALIVE_TIME);
- MEM_write_1_byte(0x0, 0x2A);
+ MEM_write_1_byte(0x0, 0x42);
} else {
KART_CST.CONTROL_STEERING_MODE = MEM_read_1_byte(MEMADD_CONTROL_STEERING_MODE);
KART_CST.CONTROL_ALIVE_TIME = MEM_read_1_byte(MEMADD_CONTROL_ALIVE_TIME);
- KART_CST.CONTROL_SPEED_FACTOR = MEM_read_1_byte(MEMADD_CONTROL_SPEED_FACTOR);
- KART_CST.CONTROL_POWER_FACTOR = MEM_read_1_byte(MEMADD_CONTROL_POWER_FACTOR);
- KART_CST.CONTROL_STEERING_FACTOR = MEM_read_1_byte(MEMADD_CONTROL_STEERING_FACTOR);
+ KART_CST.CONTROL_SPEED_FACTOR = MEM_read_4_byte(MEMADD_CONTROL_SPEED_FACTOR);
+ KART_CST.CONTROL_POWER_FACTOR = MEM_read_4_byte(MEMADD_CONTROL_POWER_FACTOR);
+ KART_CST.CONTROL_STEERING_FACTOR = MEM_read_4_byte(MEMADD_CONTROL_STEERING_FACTOR);
KART_CST.CONTROL_MAX_SPEED_FW = MEM_read_1_byte(MEMADD_CONTROL_MAX_SPEED_FW);
KART_CST.CONTROL_MAX_SPEED_BW = MEM_read_1_byte(MEMADD_CONTROL_MAX_SPEED_BW);
@@ -103,6 +104,7 @@ void MEM_init(){
KART_CST.BATTERY_ALIVE_TIME = MEM_read_1_byte(MEMADD_BATTERY_ALIVE_TIME);
}
+ CAN_Send(0, 5, KART_CST.CONTROL_POWER_FACTOR);
}
void MEM_reset() {
diff --git a/306-controller_interface.X/middleware/can_interface.c b/306-controller_interface.X/middleware/can_interface.c
index 6c893fd..44568e0 100644
--- a/306-controller_interface.X/middleware/can_interface.c
+++ b/306-controller_interface.X/middleware/can_interface.c
@@ -1,5 +1,5 @@
/**
- * @author Rémi Heredero
+ * @author R�mi Heredero
* @version 1.0.0
* @date August 2023
* @file can_interface.c
@@ -34,9 +34,7 @@ typedef union {
uint8_t byte2;
uint8_t byte3;
} separate;
- struct {
- uint32_t bytes;
- } full;
+ uint32_t full;
} CAN_4_BYTES;
void CAN_init(){
@@ -55,6 +53,12 @@ bool CAN_processEvent(Event* ev) {
evIDT evid = Event_getId(ev);
uint64_t data = Event_getData(ev);
+ CAN_4_BYTES tmpData;
+ uCAN_MSG canMsg;
+ uint32_t canData = (uint32_t) data;
+ uint8_t idMsg;
+ uint8_t idRecipient;
+ uint8_t idSender;
switch (me->state) { // onState
@@ -69,26 +73,40 @@ bool CAN_processEvent(Event* ev) {
// New message arrive
if (ev->id == evCAnewMsg) {
if (me->receiveCan != NULL) {
- uint32_t canData = (uint32_t) data;
data = data>>32;
- CAN_4_BYTES tmpData;
- tmpData.full.bytes = data;
- uint8_t idMsg = tmpData.separate.byte0;
+ tmpData.full = data;
+ idMsg = tmpData.separate.byte0;
idMsg = idMsg >> 4;
idMsg = idMsg & 0xF;
- uint8_t idRecipient = tmpData.separate.byte1;
+ idRecipient = tmpData.separate.byte1;
idRecipient = idRecipient & 0xF;
- uint8_t idSender = tmpData.separate.byte1;
+ idSender = tmpData.separate.byte1;
idSender = idSender >> 4;
- me->receiveCan(idSender, idMsg, canData);
+ me->receiveCan(idSender, idMsg, false, canData);
+ }
+ }
+
+ if (ev->id == evCAnewRTR) {
+ if (me->receiveCan != NULL) {
+ data = data>>32;
+
+ tmpData.full = data;
+ idMsg = tmpData.separate.byte0;
+ idMsg = idMsg >> 4;
+ idMsg = idMsg & 0xF;
+ idRecipient = tmpData.separate.byte1;
+ idRecipient = idRecipient & 0xF;
+ idSender = tmpData.separate.byte1;
+ idSender = idSender >> 4;
+
+ me->receiveCan(idSender, idMsg, true, canData);
}
}
// Send a message
- if (ev->id == evCAsend) {
- uCAN_MSG canMsg;
+ if (ev->id == evCAsend4) {
canMsg.frame.idType = dSTANDARD_CAN_MSG_ID_2_0B; // standard
canMsg.frame.dlc = 4; // 4 bytes to send
canMsg.frame.rtr = 0; // no remote frame
@@ -103,6 +121,70 @@ bool CAN_processEvent(Event* ev) {
canMsg.frame.id = (uint32_t) data;
CAN_transmit(&canMsg);
}
+
+ if (ev->id == evCAsend2) {
+ canMsg.frame.idType = dSTANDARD_CAN_MSG_ID_2_0B; // standard
+ canMsg.frame.dlc = 2; // 4 bytes to send
+ canMsg.frame.rtr = 0; // no remote frame
+ canMsg.frame.data0 = (uint8_t) data;
+ data = data >> 8;
+ canMsg.frame.data1 = (uint8_t) data;
+ data = data >> 8;
+ canMsg.frame.data2 = (uint8_t) data;
+ data = data >> 8;
+ canMsg.frame.data3 = (uint8_t) data;
+ data = data >> 8;
+ canMsg.frame.id = (uint32_t) data;
+ CAN_transmit(&canMsg);
+ }
+
+ if (ev->id == evCAsend1) {
+ canMsg.frame.idType = dSTANDARD_CAN_MSG_ID_2_0B; // standard
+ canMsg.frame.dlc = 1; // 4 bytes to send
+ canMsg.frame.rtr = 0; // no remote frame
+ canMsg.frame.data0 = (uint8_t) data;
+ data = data >> 8;
+ canMsg.frame.data1 = (uint8_t) data;
+ data = data >> 8;
+ canMsg.frame.data2 = (uint8_t) data;
+ data = data >> 8;
+ canMsg.frame.data3 = (uint8_t) data;
+ data = data >> 8;
+ canMsg.frame.id = (uint32_t) data;
+ CAN_transmit(&canMsg);
+ }
+
+ if (ev->id == evCAsend0) {
+ canMsg.frame.idType = dSTANDARD_CAN_MSG_ID_2_0B; // standard
+ canMsg.frame.dlc = 0; // 4 bytes to send
+ canMsg.frame.rtr = 0; // no remote frame
+ canMsg.frame.data0 = (uint8_t) data;
+ data = data >> 8;
+ canMsg.frame.data1 = (uint8_t) data;
+ data = data >> 8;
+ canMsg.frame.data2 = (uint8_t) data;
+ data = data >> 8;
+ canMsg.frame.data3 = (uint8_t) data;
+ data = data >> 8;
+ canMsg.frame.id = (uint32_t) data;
+ CAN_transmit(&canMsg);
+ }
+
+ if (ev->id == evCAsendRTR) {
+ canMsg.frame.idType = dSTANDARD_CAN_MSG_ID_2_0B; // standard
+ canMsg.frame.dlc = 0; // 4 bytes to send
+ canMsg.frame.rtr = 1; // no remote frame
+ canMsg.frame.data0 = (uint8_t) data;
+ data = data >> 8;
+ canMsg.frame.data1 = (uint8_t) data;
+ data = data >> 8;
+ canMsg.frame.data2 = (uint8_t) data;
+ data = data >> 8;
+ canMsg.frame.data3 = (uint8_t) data;
+ data = data >> 8;
+ canMsg.frame.id = (uint32_t) data;
+ CAN_transmit(&canMsg);
+ }
break;
}
@@ -153,7 +235,11 @@ void CAN_newMsg() {
data = data | canMsg.frame.data1;
data = data<<8;
data = data | canMsg.frame.data0;
- POST(&CAN_myself, &CAN_processEvent, evCAnewMsg, 0, data);
+ if(canMsg.frame.rtr) {
+ POST(&CAN_myself, &CAN_processEvent, evCAnewRTR, 0, data);
+ } else {
+ POST(&CAN_myself, &CAN_processEvent, evCAnewMsg, 0, data);
+ }
}
void CAN_Send(uint8_t idRecipient, uint8_t idMsg, uint32_t data) {
@@ -161,7 +247,50 @@ void CAN_Send(uint8_t idRecipient, uint8_t idMsg, uint32_t data) {
tmpData = (tmpData<<4) | idRecipient;
tmpData = (tmpData<<4) | idMsg;
tmpData = (tmpData<<32) | data;
- POST(&CAN_myself, &CAN_processEvent, evCAsend, 0, tmpData);
+ POST(&CAN_myself, &CAN_processEvent, evCAsend4, 0, tmpData);
+}
+
+void CAN_send_4_bytes(uint8_t idRecipient, uint8_t idMsg, uint8_t byte0, uint8_t byte1, uint8_t byte2, uint8_t byte3) {
+ uint64_t tmpData = CAN_myself.sender;
+ tmpData = (tmpData<<4) | idRecipient;
+ tmpData = (tmpData<<4) | idMsg;
+ tmpData = (tmpData<<8) | byte3;
+ tmpData = (tmpData<<8) | byte2;
+ tmpData = (tmpData<<8) | byte1;
+ tmpData = (tmpData<<8) | byte0;
+ POST(&CAN_myself, &CAN_processEvent, evCAsend4, 0, tmpData);
+}
+
+void CAN_send_2_bytes(uint8_t idRecipient, uint8_t idMsg, uint16_t data) {
+ uint64_t tmpData = CAN_myself.sender;
+ tmpData = (tmpData<<4) | idRecipient;
+ tmpData = (tmpData<<4) | idMsg;
+ tmpData = (tmpData<<32) | data;
+ POST(&CAN_myself, &CAN_processEvent, evCAsend2, 0, tmpData);
+}
+
+void CAN_send_1_byte(uint8_t idRecipient, uint8_t idMsg, uint8_t data) {
+ uint64_t tmpData = CAN_myself.sender;
+ tmpData = (tmpData<<4) | idRecipient;
+ tmpData = (tmpData<<4) | idMsg;
+ tmpData = (tmpData<<32) | data;
+ POST(&CAN_myself, &CAN_processEvent, evCAsend1, 0, tmpData);
+}
+
+void CAN_send_0_byte(uint8_t idRecipient, uint8_t idMsg) {
+ uint64_t tmpData = CAN_myself.sender;
+ tmpData = (tmpData<<4) | idRecipient;
+ tmpData = (tmpData<<4) | idMsg;
+ tmpData = tmpData<<32;
+ POST(&CAN_myself, &CAN_processEvent, evCAsend0, 0, tmpData);
+}
+
+void CAN_send_rtr(uint8_t idRecipient, uint8_t idMsg) {
+ uint64_t tmpData = CAN_myself.sender;
+ tmpData = (tmpData<<4) | idRecipient;
+ tmpData = (tmpData<<4) | idMsg;
+ tmpData = tmpData<<32;
+ POST(&CAN_myself, &CAN_processEvent, evCAsendRTR, 0, tmpData);
}
/***********
diff --git a/306-controller_interface.X/middleware/can_interface.h b/306-controller_interface.X/middleware/can_interface.h
index d65266b..8d65ba9 100644
--- a/306-controller_interface.X/middleware/can_interface.h
+++ b/306-controller_interface.X/middleware/can_interface.h
@@ -17,10 +17,15 @@ typedef enum {
typedef enum {
evCAinit = 10,
evCAnewMsg,
- evCAsend
+ evCAnewRTR,
+ evCAsend4,
+ evCAsend2,
+ evCAsend1,
+ evCAsend0,
+ evCAsendRTR
} CAN_EVENTS;
-typedef void (*CAN_CALLBACK)(uint8_t, uint8_t, uint32_t);
+typedef void (*CAN_CALLBACK)(uint8_t, uint8_t, bool, uint32_t);
typedef struct {
CAN_STATES state;
@@ -78,6 +83,12 @@ void CAN_newMsg();
*/
void CAN_Send(uint8_t idRecipient, uint8_t idMsg, uint32_t data);
+void CAN_send_4_bytes(uint8_t idRecipient, uint8_t idMsg, uint8_t byte0, uint8_t byte1, uint8_t byte2, uint8_t byte3);
+void CAN_send_2_bytes(uint8_t idRecipient, uint8_t idMsg, uint16_t data);
+void CAN_send_1_byte(uint8_t idRecipient, uint8_t idMsg, uint8_t data);
+void CAN_send_0_byte(uint8_t idRecipient, uint8_t idMsg);
+void CAN_send_rtr(uint8_t idRecipient, uint8_t idMsg);
+
/***********
* SETTERS *
***********/
diff --git a/306-controller_interface.X/nbproject/configurations.xml b/306-controller_interface.X/nbproject/configurations.xml
index 2d248e4..ded209f 100644
--- a/306-controller_interface.X/nbproject/configurations.xml
+++ b/306-controller_interface.X/nbproject/configurations.xml
@@ -245,7 +245,7 @@
-
+
@@ -292,7 +292,7 @@
-
+
diff --git a/306-controller_interface.X/queuelogs/debugtool b/306-controller_interface.X/queuelogs/debugtool
new file mode 100644
index 0000000..e69de29
diff --git a/busmaster_config.cfx b/busmaster_config.cfx
index f5a4e23..d41b117 100644
--- a/busmaster_config.cfx
+++ b/busmaster_config.cfx
@@ -366,9 +366,9 @@
452
- 147
+ 94
94
- 139
+ 189
94
@@ -436,7 +436,7 @@
SHOWNORMAL
- SHOWNORMAL
+ HIDE
291
205
957
@@ -567,9 +567,9 @@
SHOWNORMAL
RESTORETOMAXIMIZED
- 173
+ 179
10
- 451
+ 457
359
@@ -577,8 +577,8 @@
1
- 0
- 0
+ 1
+ 19
Time
@@ -650,11 +650,11 @@
Data Byte(s)
13
1
- 720
+ 701
1
- 1
- 0
+ 0
+ 1
SYSTEM
SHOWNORMAL
@@ -667,9 +667,9 @@
SHOWNORMAL
RESTORETOMAXIMIZED
- 173
+ 179
10
- 451
+ 457
359
@@ -680,8 +680,8 @@
1
- 0
- 0
+ 1
+ 19
Time
@@ -735,11 +735,11 @@
Checksum
10
1
- 734
+ 715
1
- 1
- 0
+ 0
+ 1
SYSTEM
SHOWNORMAL
@@ -752,9 +752,9 @@
SHOWNORMAL
RESTORETOMAXIMIZED
- 173
+ 179
10
- 451
+ 457
359
@@ -798,7 +798,7 @@
FALSE
FALSE
4
- 0,0,0,0
+ 0,0,39,16
10
FALSE
a