391 lines
14 KiB
C
391 lines
14 KiB
C
/**
|
|
* @author Rémi Heredero
|
|
* @version 1.0.0
|
|
* @date August 2023
|
|
* @file can_message.c
|
|
*/
|
|
|
|
#include "can_message.h"
|
|
#include "car.h"
|
|
#include "../app/factory/factory.h"
|
|
#include "../middleware/can_interface.h"
|
|
#include "kartculator.h"
|
|
|
|
typedef union {
|
|
struct {
|
|
uint8_t byte0;
|
|
uint8_t byte1;
|
|
uint8_t byte2;
|
|
uint8_t byte3;
|
|
} separate;
|
|
uint32_t full;
|
|
} BYTES_4;
|
|
|
|
typedef union {
|
|
struct {
|
|
uint8_t byte0;
|
|
uint8_t byte1;
|
|
} separate;
|
|
uint16_t full;
|
|
} BYTES_2;
|
|
|
|
void CM_processIncome(uint8_t idSender, uint8_t idMsg, bool rtr, uint32_t data){
|
|
BYTES_4 incomeData;
|
|
incomeData.full = data;
|
|
BYTES_4 revertData;
|
|
revertData.separate.byte0 = incomeData.separate.byte3;
|
|
revertData.separate.byte1 = incomeData.separate.byte2;
|
|
revertData.separate.byte2 = incomeData.separate.byte1;
|
|
revertData.separate.byte3 = incomeData.separate.byte0;
|
|
|
|
switch(idSender){
|
|
|
|
/*********************
|
|
* BROADCAST / DEBUG *
|
|
*********************/
|
|
case 0:
|
|
if(idMsg == 0x0) { // CONTROL_SETUP
|
|
// steeringMode eraseMemory - controlAliveTime
|
|
if (incomeData.separate.byte1) {
|
|
MEM_reset();
|
|
}
|
|
KART_CST.CONTROL_STEERING_MODE = incomeData.separate.byte0;
|
|
KART_CST.CONTROL_ALIVE_TIME = incomeData.separate.byte3;
|
|
MEM_write_1_byte(MEMADD_CONTROL_STEERING_MODE, KART_CST.CONTROL_STEERING_MODE);
|
|
MEM_write_1_byte(MEMADD_CONTROL_ALIVE_TIME, KART_CST.CONTROL_ALIVE_TIME);
|
|
ALIVE_setAliveTime(ALcontroller(), KART_CST.CONTROL_ALIVE_TIME);
|
|
ALIVE_emitStart(ALcontroller(), 0, 0);
|
|
}
|
|
|
|
if(idMsg == 0x1) { // CONTROL_SPEED_FACTOR
|
|
// valHH valH valL valLL
|
|
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 = 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 = revertData.full;
|
|
MEM_write_4_byte(MEMADD_CONTROL_STEERING_FACTOR, KART_CST.CONTROL_STEERING_FACTOR);
|
|
|
|
}
|
|
|
|
if(idMsg == 0x4) { // CONTROL_SECURITY_PARAM
|
|
// maxSpeedFw maxSpeedBw - -
|
|
KART_CST.CONTROL_MAX_SPEED_FW = incomeData.separate.byte0;
|
|
KART_CST.CONTROL_MAX_SPEED_BW = incomeData.separate.byte1;
|
|
MEM_write_1_byte(MEMADD_CONTROL_MAX_SPEED_FW, KART_CST.CONTROL_MAX_SPEED_FW);
|
|
MEM_write_1_byte(MEMADD_CONTROL_MAX_SPEED_BW, KART_CST.CONTROL_MAX_SPEED_BW);
|
|
|
|
}
|
|
|
|
if(idMsg == 0x5) { // CONTROL_SETUP_PARAM
|
|
// displayAliveTime steeringAliveTime - -
|
|
KART_CST.DISPLAY_ALIVE_TIME = incomeData.separate.byte0;
|
|
KART_CST.STEERING_ALIVE_TIME = incomeData.separate.byte1;
|
|
MEM_write_1_byte(MEMADD_DISPLAY_ALIVE_TIME, KART_CST.DISPLAY_ALIVE_TIME);
|
|
MEM_write_1_byte(MEMADD_STEERING_ALIVE_TIME, KART_CST.STEERING_ALIVE_TIME);
|
|
|
|
// TODO set alive times
|
|
// start alives
|
|
CM_DISPLAY_SETUP(NULL);
|
|
CM_STEERING_SETUP(&ALWAYSFALSE);
|
|
|
|
}
|
|
|
|
if(idMsg == 0x6) { // CONTROL_SETUP_PARAM_JOY
|
|
// joystickMode joystickParam1 joystickParam2 joystickAliveTime
|
|
KART_CST.JOYSTICK_MODE = incomeData.separate.byte0;
|
|
KART_CST.JOYSTICK_PARAM1 = incomeData.separate.byte1;
|
|
KART_CST.JOYSTICK_PARAM2 = incomeData.separate.byte2;
|
|
KART_CST.JOYSTICK_ALIVE_TIME = incomeData.separate.byte3;
|
|
MEM_write_1_byte(MEMADD_JOYSTICK_MODE, KART_CST.JOYSTICK_MODE);
|
|
MEM_write_1_byte(MEMADD_JOYSTICK_PARAM1, KART_CST.JOYSTICK_PARAM1);
|
|
MEM_write_1_byte(MEMADD_JOYSTICK_PARAM2, KART_CST.JOYSTICK_PARAM2);
|
|
MEM_write_1_byte(MEMADD_JOYSTICK_ALIVE_TIME, KART_CST.JOYSTICK_ALIVE_TIME);
|
|
|
|
ALIVE_setAliveTime(ALjoy(), KART_CST.JOYSTICK_ALIVE_TIME);
|
|
ALIVE_emitStart(ALjoy(), 0, 0);
|
|
CM_JOY_SETUP(NULL);
|
|
|
|
}
|
|
|
|
if(idMsg == 0x7) { // CONTROL_SETUP_PARAM_DRIVE
|
|
// driveAliveTime driveSpeedTime driveStopTime -
|
|
KART_CST.DRIVE_ALIVE_TIME = incomeData.separate.byte0;
|
|
KART_CST.DRIVE_SPEED_TIME = incomeData.separate.byte1;
|
|
KART_CST.DRIVE_STOP_TIME = incomeData.separate.byte2;
|
|
MEM_write_1_byte(MEMADD_DRIVE_ALIVE_TIME, KART_CST.DRIVE_ALIVE_TIME);
|
|
MEM_write_1_byte(MEMADD_DRIVE_SPEED_TIME, KART_CST.DRIVE_SPEED_TIME);
|
|
MEM_write_1_byte(MEMADD_DRIVE_STOP_TIME, KART_CST.DRIVE_STOP_TIME);
|
|
|
|
ALIVE_setAliveTime(&drive()->myChecker, KART_CST.DRIVE_ALIVE_TIME);
|
|
ALIVE_emitStart(&drive()->myChecker, 0, 0);
|
|
CM_DRIVE_SETUP(&ALWAYSFALSE);
|
|
|
|
}
|
|
|
|
if(idMsg == 0x8) { // CONTROL_SETUP_PARAM_BATTERY
|
|
// batteryVoltTime batteryCurrentTime batteryEnergyTime batteryAliveTime
|
|
KART_CST.BATTERY_VOLT_TIME = incomeData.separate.byte0;
|
|
KART_CST.BATTERY_CURRENT_TIME = incomeData.separate.byte1;
|
|
KART_CST.BATTERY_ENERGY_TIME = incomeData.separate.byte2;
|
|
KART_CST.BATTERY_ALIVE_TIME = incomeData.separate.byte3;
|
|
MEM_write_1_byte(MEMADD_BATTERY_VOLT_TIME, KART_CST.BATTERY_VOLT_TIME);
|
|
MEM_write_1_byte(MEMADD_BATTERY_CURRENT_TIME, KART_CST.BATTERY_CURRENT_TIME);
|
|
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);
|
|
|
|
// TODO set alive time
|
|
// TODO start alive
|
|
CM_SUPPLY_SETUP(NULL);
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
|
|
/************
|
|
* JOYSTICK *
|
|
************/
|
|
case 2:
|
|
if(idMsg == 0x1) { // JOY_MESURE
|
|
// posX posY button -
|
|
calcTorque(incomeData.separate.byte1);
|
|
calcPosition(incomeData.separate.byte0);
|
|
|
|
}
|
|
|
|
if(idMsg == 0xF) { // JOY_ALIVE
|
|
// - - - -
|
|
ALIVE_ISALIVE(ALjoy());
|
|
if(ALjoy()->state == STAL_DEAD){
|
|
KART_CST.JOYSTICK_MODE = MEM_read_1_byte(MEMADD_JOYSTICK_MODE);
|
|
KART_CST.JOYSTICK_PARAM1 = MEM_read_1_byte(MEMADD_JOYSTICK_PARAM1);
|
|
KART_CST.JOYSTICK_PARAM2 = MEM_read_1_byte(MEMADD_JOYSTICK_PARAM2);
|
|
KART_CST.JOYSTICK_ALIVE_TIME = MEM_read_1_byte(MEMADD_JOYSTICK_ALIVE_TIME);
|
|
ALIVE_setAliveTime(ALjoy(), KART_CST.JOYSTICK_ALIVE_TIME);
|
|
ALIVE_emitResurrect(ALjoy(), 0, 0);
|
|
ALIVE_emitBorn(ALjoy(), 0, 0);
|
|
ALIVE_emitReady(ALjoy(), 0, 0);
|
|
}
|
|
}
|
|
break;
|
|
|
|
|
|
/***********
|
|
* DISPLAY *
|
|
***********/
|
|
case 3:
|
|
if(idMsg == 0xF) { // DISPLAY_ALIVE
|
|
// powerMode - - -
|
|
// TODO display say ALIVE
|
|
|
|
}
|
|
break;
|
|
|
|
|
|
/*********
|
|
* DRIVE *
|
|
*********/
|
|
case 4:
|
|
if(idMsg == 0x0) { // DRIVE_SPEED
|
|
// speedHH speedH speedL speedLL
|
|
calcSpeed(revertData.full);
|
|
DRIVE_emitPollSpeed(drive());
|
|
}
|
|
|
|
if(idMsg == 0xF) { // DRIVE_ALIVE
|
|
// statusH statusL - -
|
|
ALIVE_ISALIVE(&drive()->myChecker);
|
|
if(drive()->myChecker.state == STAL_DEAD){
|
|
KART_CST.DRIVE_SPEED_TIME = MEM_read_1_byte(MEMADD_DRIVE_SPEED_TIME);
|
|
KART_CST.DRIVE_STOP_TIME = MEM_read_1_byte(MEMADD_DRIVE_STOP_TIME);
|
|
KART_CST.DRIVE_ALIVE_TIME = MEM_read_1_byte(MEMADD_DRIVE_ALIVE_TIME);
|
|
ALIVE_emitResurrect(&drive()->myChecker, 0, 0);
|
|
ALIVE_emitBorn(&drive()->myChecker, 0, 0);
|
|
}
|
|
|
|
}
|
|
break;
|
|
|
|
|
|
/************
|
|
* STEERING *
|
|
************/
|
|
case 5:
|
|
if(idMsg == 0x1) { // STEERING_GET_CENTER
|
|
// valHH valH valL valLL
|
|
eKart.center = revertData.full;
|
|
}
|
|
|
|
if(idMsg == 0x2) { // STEERING_GET_POSITION
|
|
// valHH valH valL valLL
|
|
|
|
}
|
|
|
|
if(idMsg == 0x3) { // STEERING_READY
|
|
// - - - -
|
|
|
|
}
|
|
|
|
if(idMsg == 0xF) { // STEERING_ALIVE
|
|
// statusH statusL - -
|
|
ALIVE_ISALIVE(&steering()->myChecker);
|
|
if(steering()->myChecker.state == STAL_DEAD) {
|
|
KART_CST.STEERING_ALIVE_TIME = MEM_read_1_byte(MEMADD_STEERING_ALIVE_TIME);
|
|
ALIVE_emitResurrect(&steering()->myChecker, 500, 0);
|
|
ALIVE_emitBorn(&steering()->myChecker, 1000, 0);
|
|
}
|
|
|
|
}
|
|
break;
|
|
|
|
|
|
/**********
|
|
* SUPPLY *
|
|
**********/
|
|
case 6:
|
|
if(idMsg == 0xF) { // BATTERY_ALIVE
|
|
// - - - -
|
|
// TODO battery say ALIVE
|
|
|
|
}
|
|
break;
|
|
|
|
|
|
/*************
|
|
* UNDEFINED *
|
|
*************/
|
|
case 7:
|
|
break;
|
|
}
|
|
}
|
|
|
|
void CM_CONTROLLER_ALIVE(void* p) {
|
|
// - - - -
|
|
CAN_Send(0x0, 0xF, 0);
|
|
}
|
|
|
|
void CM_JOY_SETUP(void* p) {
|
|
// mode parm1 param2 aliveTime
|
|
BYTES_4 joy;
|
|
joy.separate.byte0 = KART_CST.JOYSTICK_MODE;
|
|
joy.separate.byte1 = KART_CST.JOYSTICK_PARAM1;
|
|
joy.separate.byte2 = KART_CST.JOYSTICK_PARAM2;
|
|
joy.separate.byte3 = KART_CST.JOYSTICK_ALIVE_TIME;
|
|
CAN_Send(2, 0, joy.full);
|
|
}
|
|
void CM_DISPLAY_SETUP(void* p) {
|
|
// reset - - aliveTime
|
|
BYTES_4 display;
|
|
display.separate.byte0 = 0; // reset
|
|
display.separate.byte3 = KART_CST.DISPLAY_ALIVE_TIME;
|
|
CAN_Send(3, 0, display.full);
|
|
}
|
|
|
|
void CM_DISPLAY_SPEED(void* p) {
|
|
// valH valL - -
|
|
BYTES_4 tmpData;
|
|
// byte0 should be valH but isn't possible to go enough fast for use it
|
|
tmpData.separate.byte0 = 0;
|
|
tmpData.separate.byte1 = *((uint8_t*) p);
|
|
tmpData.separate.byte2 = 0;
|
|
tmpData.separate.byte3 = 0;
|
|
CAN_Send(3, 2, tmpData.full);
|
|
}
|
|
|
|
void CM_DISPLAY_DIRECTION(void* p) {
|
|
// direction - - -
|
|
BYTES_4 tmpData;
|
|
tmpData.separate.byte0 = (uint8_t) p;
|
|
tmpData.separate.byte1 = 0;
|
|
tmpData.separate.byte2 = 0;
|
|
tmpData.separate.byte3 = 0;
|
|
CAN_Send(3, 3, tmpData.full);
|
|
}
|
|
|
|
void CM_DRIVE_SETUP(void* p) {
|
|
// reset/init speedTime stopTime aliveTime
|
|
BYTES_4 tmpData;
|
|
tmpData.separate.byte0 = (uint8_t) p;
|
|
tmpData.separate.byte1 = KART_CST.DRIVE_SPEED_TIME;
|
|
tmpData.separate.byte2 = KART_CST.DRIVE_STOP_TIME;
|
|
tmpData.separate.byte3 = KART_CST.DRIVE_ALIVE_TIME;
|
|
CAN_Send(4, 0, tmpData.full);
|
|
|
|
}
|
|
|
|
void CM_DRIVE_POWER(void* p) {
|
|
// valH valL - -
|
|
BYTES_2 torque;
|
|
BYTES_4 tmpData;
|
|
torque.full = *((int16_t*) p);
|
|
tmpData.separate.byte0 = torque.separate.byte1;
|
|
tmpData.separate.byte1 = torque.separate.byte0;
|
|
tmpData.separate.byte2 = 0;
|
|
tmpData.separate.byte3 = 0;
|
|
CAN_Send(4, 1, tmpData.full);
|
|
}
|
|
|
|
void CM_STEERING_SETUP(void* p) {
|
|
// reset/init homing setCenter aliveTime
|
|
// TODO not working have to fix it
|
|
BYTES_4 tmpData;
|
|
uint8_t choice = (uint8_t) p;
|
|
switch (choice) {
|
|
case 1:
|
|
tmpData.separate.byte0 = 1;
|
|
tmpData.separate.byte1 = 0;
|
|
tmpData.separate.byte2 = 0;
|
|
break;
|
|
case 2:
|
|
tmpData.separate.byte0 = 0;
|
|
tmpData.separate.byte1 = 1;
|
|
tmpData.separate.byte2 = 0;
|
|
break;
|
|
case 3:
|
|
tmpData.separate.byte0 = 0;
|
|
tmpData.separate.byte1 = 0;
|
|
tmpData.separate.byte2 = 1;
|
|
break;
|
|
default:
|
|
tmpData.separate.byte0 = 0;
|
|
tmpData.separate.byte1 = 0;
|
|
tmpData.separate.byte2 = 0;
|
|
break;
|
|
}
|
|
tmpData.separate.byte3 = KART_CST.STEERING_ALIVE_TIME; // 0 -> no alive
|
|
CAN_Send(5, 0, tmpData.full);
|
|
}
|
|
|
|
void CM_STEERING_SET(void* p) {
|
|
// valHH valH valL valLL
|
|
BYTES_4 pPosition;
|
|
pPosition.full = *((uint32_t*) p);
|
|
BYTES_4 sPosition;
|
|
sPosition.separate.byte0 = pPosition.separate.byte3;
|
|
sPosition.separate.byte1 = pPosition.separate.byte2;
|
|
sPosition.separate.byte2 = pPosition.separate.byte1;
|
|
sPosition.separate.byte3 = pPosition.separate.byte0;
|
|
CAN_Send(5, 1, sPosition.full);
|
|
}
|
|
|
|
void CM_SUPPLY_SETUP(void* p) {
|
|
// batteryVoltTime batteryCurrentTime batteryEnergyTime aliveTime
|
|
BYTES_4 supply;
|
|
supply.separate.byte0 = KART_CST.BATTERY_VOLT_TIME;
|
|
supply.separate.byte1 = KART_CST.BATTERY_CURRENT_TIME;
|
|
supply.separate.byte2 = KART_CST.BATTERY_ENERGY_TIME;
|
|
supply.separate.byte3 = KART_CST.BATTERY_ALIVE_TIME;
|
|
CAN_Send(6, 0, supply.full);
|
|
}
|