275 lines
8.3 KiB
C
Executable File
275 lines
8.3 KiB
C
Executable File
|
|
#include <can/can_client.h>
|
|
#include <io/io.h>
|
|
#include <mqtt/mqtt_client.h>
|
|
|
|
struct MOTOR_CONTROL_DATA motctrl[MOTOR_COUNT];
|
|
struct CAN_INTERFACE_DATA intf_data[MOTOR_COUNT];
|
|
|
|
int iBusTimeoutCounter = 0;
|
|
void IncBusTimeoutCounter(int iMotorIndex)
|
|
{
|
|
if (iBusTimeoutCounter < 2000)
|
|
{
|
|
iBusTimeoutCounter++;
|
|
if (iBusTimeoutCounter >= 2000)
|
|
{
|
|
motctrl[iMotorIndex].nSwitchState = 0;
|
|
MqttClient_Publish_MotorSwitchState(iMotorIndex, motctrl[iMotorIndex].nSwitchState);
|
|
}
|
|
}
|
|
}
|
|
/// @brief Open socket of CAN interface for the given motor
|
|
/// @param iMotorIndex
|
|
/// @param ifacename
|
|
/// @return
|
|
int Can_OpenInterface(int iMotorIndex, const char * ifacename)
|
|
{
|
|
// Init control data
|
|
motctrl[iMotorIndex].iActualMotorPowerW = 0;
|
|
motctrl[iMotorIndex].iMotorGear = MOTOR_GEAR_NEUTRAL;
|
|
motctrl[iMotorIndex].iMotorPower = 0;
|
|
motctrl[iMotorIndex].iMotorPowerPct = 0;
|
|
motctrl[iMotorIndex].nSwitchState = 0;
|
|
|
|
strcpy(intf_data[iMotorIndex].iface_name, ifacename);
|
|
Can_SetMotorGear(iMotorIndex, MOTOR_GEAR_NEUTRAL);
|
|
Can_SetMotorPower(iMotorIndex, 0);
|
|
|
|
// first we have to create a socket
|
|
if ((intf_data[iMotorIndex].socket = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0)
|
|
{
|
|
printf("Could not create socket for motor %d!\n", iMotorIndex);
|
|
return 1;
|
|
}
|
|
|
|
// retrieve the interface index
|
|
strcpy(intf_data[iMotorIndex].ifr.ifr_name, intf_data[iMotorIndex].iface_name);
|
|
if (ioctl(intf_data[iMotorIndex].socket, SIOCGIFINDEX, &intf_data[iMotorIndex].ifr) < 0)
|
|
{
|
|
printf("Could not get interface index for motor %d!\n", iMotorIndex);
|
|
return 2;
|
|
}
|
|
|
|
// bind the socket to the CAN interface
|
|
memset(&intf_data[iMotorIndex].addr, 0, sizeof(intf_data[iMotorIndex].addr));
|
|
intf_data[iMotorIndex].addr.can_family = AF_CAN;
|
|
intf_data[iMotorIndex].addr.can_ifindex = intf_data[iMotorIndex].ifr.ifr_ifindex;
|
|
if (bind(intf_data[iMotorIndex].socket, (struct sockaddr *)&intf_data[iMotorIndex].addr, sizeof(intf_data[iMotorIndex].addr)) < 0)
|
|
{
|
|
printf("Could not bind socket to inteface for motor %d!\n", iMotorIndex);
|
|
return 3;
|
|
}
|
|
|
|
// socket auf nicht-blockierend umstellen
|
|
int fcntl_flags = fcntl(intf_data[iMotorIndex].socket, F_GETFL, 0);
|
|
if (fcntl_flags < 0)
|
|
{
|
|
printf("Could not get file descriptor flags!\n");
|
|
return 4;
|
|
}
|
|
fcntl_flags |= O_NONBLOCK;
|
|
if (fcntl(intf_data[iMotorIndex].socket, F_SETFL, fcntl_flags) < 0)
|
|
{
|
|
printf("Could not set file descriptor flags (set socket none-blocking)!\n");
|
|
return 5;
|
|
}
|
|
|
|
printf("Interface %s (motor %d) opened!\n", ifacename, iMotorIndex);
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
/// @brief Close socket of CAN interface for the given motor
|
|
/// @param iMotorIndex
|
|
void Can_CloseInterface(int iMotorIndex)
|
|
{
|
|
if (close(intf_data[iMotorIndex].socket) < 0)
|
|
{
|
|
printf("Could not close socket of motor %d!\n", iMotorIndex);
|
|
}
|
|
else
|
|
{
|
|
printf("Interface %s (motor %d) closed.\n", intf_data[iMotorIndex].iface_name, iMotorIndex);
|
|
}
|
|
}
|
|
|
|
|
|
/// @brief Set gear for the given motor
|
|
/// @param iMotorIndex
|
|
/// @param iGear (-1=reverse, 0=neutral, 1=forward)
|
|
void Can_SetMotorGear(int iMotorIndex, int iGear)
|
|
{
|
|
if (iGear > 0)
|
|
{
|
|
if (motctrl[iMotorIndex].iMotorGear != MOTOR_GEAR_FORWARD)
|
|
{
|
|
MqttClient_Publish_MotorGear(iMotorIndex, iGear);
|
|
motctrl[iMotorIndex].iMotorGear = MOTOR_GEAR_FORWARD;
|
|
}
|
|
WriteOutputPin(GPIO_LED_MOTRUN, HIGH);
|
|
printf("Motor[%d]: Set gear forward.\n", iMotorIndex);
|
|
}
|
|
else if (iGear < 0)
|
|
{
|
|
if (motctrl[iMotorIndex].iMotorGear != MOTOR_GEAR_REVERSE)
|
|
{
|
|
MqttClient_Publish_MotorGear(iMotorIndex, iGear);
|
|
motctrl[iMotorIndex].iMotorGear = MOTOR_GEAR_REVERSE;
|
|
}
|
|
WriteOutputPin(GPIO_LED_MOTRUN, HIGH);
|
|
printf("Motor[%d]: Set gear reverse.\n", iMotorIndex);
|
|
}
|
|
else
|
|
{
|
|
if (motctrl[iMotorIndex].iMotorGear != MOTOR_GEAR_NEUTRAL)
|
|
{
|
|
MqttClient_Publish_MotorGear(iMotorIndex, iGear);
|
|
motctrl[iMotorIndex].iMotorGear = MOTOR_GEAR_NEUTRAL;
|
|
}
|
|
Can_SetMotorPower(iMotorIndex, MOTOR_PWR_MIN_PCT);
|
|
WriteOutputPin(GPIO_LED_MOTRUN, LOW);
|
|
printf("Motor[%d]: Set gear neutral.\n", iMotorIndex);
|
|
}
|
|
}
|
|
|
|
|
|
/// @brief Set power for the given motor
|
|
/// @param iMotorIndex
|
|
/// @param iPower (Range: 0..100)
|
|
void Can_SetMotorPower(int iMotorIndex, int iPower)
|
|
{
|
|
if (iPower <= MOTOR_PWR_MIN_PCT)
|
|
{
|
|
motctrl[iMotorIndex].iMotorPowerPct = MOTOR_PWR_MIN_PCT;
|
|
}
|
|
else if (iPower >= MOTOR_PWR_MAX_PCT)
|
|
{
|
|
motctrl[iMotorIndex].iMotorPowerPct = MOTOR_PWR_MAX_PCT;
|
|
}
|
|
else
|
|
{
|
|
motctrl[iMotorIndex].iMotorPowerPct = iPower;
|
|
}
|
|
MqttClient_Publish_MotorPower(iMotorIndex, motctrl[iMotorIndex].iMotorPowerPct);
|
|
|
|
motctrl[iMotorIndex].iMotorPower = 250 * motctrl[iMotorIndex].iMotorPowerPct / 100;
|
|
|
|
printf("Motor[%d]: Set power to %d%% -> %d\n",
|
|
iMotorIndex, motctrl[iMotorIndex].iMotorPowerPct, motctrl[iMotorIndex].iMotorPower);
|
|
}
|
|
|
|
|
|
/// @brief Send CAN protocol for motor gear for the given motor
|
|
/// @param iMotorIndex
|
|
void Can_TransmitMotorGear(int iMotorIndex)
|
|
{
|
|
//if (motctrl[iMotorIndex].nSwitchState > 0)
|
|
{
|
|
// Transmission rate: 100ms
|
|
struct can_frame frame;
|
|
|
|
frame.can_id = 0x18F005D0;
|
|
frame.can_id |= CAN_EFF_FLAG;
|
|
frame.can_dlc = 8;
|
|
frame.data[0] = motctrl[iMotorIndex].iMotorGear;
|
|
frame.data[1] = 0xFF;
|
|
frame.data[2] = 0xFF;
|
|
frame.data[3] = 0xFF;
|
|
frame.data[4] = 0xFF;
|
|
frame.data[5] = 0xFF;
|
|
frame.data[6] = 0xFF;
|
|
frame.data[7] = 0xFF;
|
|
|
|
if (write(intf_data[iMotorIndex].socket, &frame, sizeof(frame)) != sizeof(frame))
|
|
{
|
|
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
/// @brief Send CAN protocol for motor power for the given motor
|
|
/// @param iMotorIndex
|
|
void Can_TransmitMotorPower(int iMotorIndex)
|
|
{
|
|
//if (motctrl[iMotorIndex].nSwitchState > 0)
|
|
{
|
|
// Transmission rate: 50ms
|
|
struct can_frame frame;
|
|
|
|
frame.can_id = 0x0CF003D0;
|
|
frame.can_id |= CAN_EFF_FLAG;
|
|
frame.can_dlc = 8;
|
|
frame.data[0] = 0xFF;
|
|
frame.data[1] = motctrl[iMotorIndex].iMotorPower; // motor power 0 = 0%, 250 = 100%
|
|
frame.data[2] = 0xFF;
|
|
frame.data[3] = 0xFF;
|
|
frame.data[4] = 0xFF;
|
|
frame.data[5] = 0xFF;
|
|
frame.data[6] = 0xFF;
|
|
frame.data[7] = 0xFF;
|
|
|
|
if (write(intf_data[iMotorIndex].socket, &frame, sizeof(frame)) != sizeof(frame))
|
|
{
|
|
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
/// @brief Read data from CAN interface
|
|
/// @param iMotorIndex
|
|
void Can_ReadData(int iMotorIndex)
|
|
{
|
|
ssize_t nbytes = 0;
|
|
struct can_frame frame;
|
|
|
|
IncBusTimeoutCounter(iMotorIndex);
|
|
|
|
// wir wollen immer alle verfügbaren Frames lesen
|
|
while ((nbytes = read(intf_data[iMotorIndex].socket, &frame, sizeof(frame))) > 0)
|
|
{
|
|
canid_t pgn = frame.can_id & 0x00FFFF00;
|
|
|
|
switch(pgn)
|
|
{
|
|
case 0x00F00300: // PGN 61443 "Electronic Engine Controller 2"
|
|
// haben wir selber gesendet -> ignorieren
|
|
break;
|
|
|
|
case 0x00F00500: // PGN 61445 "Electronic Transmission Controller 2"
|
|
// haben wir selber gesendet -> ignorieren
|
|
break;
|
|
|
|
case 0x00FF1300: // PGN 65299 "Manufacturer PGN"
|
|
// hier finden wir die Zustände der Schalter
|
|
Can_Read_Manu_PGN(iMotorIndex, &frame);
|
|
break;
|
|
|
|
case 0x00FF1400: // PGN 65300 "Manufacturer PGN 2"
|
|
// hier bekommen wir die Leistung des Motors angezeigt
|
|
Can_Read_Manu_PGN2(iMotorIndex, &frame);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// @brief Read PGN 65299
|
|
/// @param frame
|
|
void Can_Read_Manu_PGN(int iMotorIndex, struct can_frame *frame)
|
|
{
|
|
iBusTimeoutCounter = 0;
|
|
motctrl[iMotorIndex].nSwitchState = frame->data[4];
|
|
MqttClient_Publish_MotorSwitchState(iMotorIndex, motctrl[iMotorIndex].nSwitchState);
|
|
}
|
|
|
|
/// @brief Read PGN 65300
|
|
/// @param frame
|
|
void Can_Read_Manu_PGN2(int iMotorIndex, struct can_frame *frame)
|
|
{
|
|
motctrl[iMotorIndex].iActualMotorPowerW = (frame->data[6] << 16) | (frame->data[5] << 8) | frame->data[4];
|
|
MqttClient_Publish_MotorActualPowerW(iMotorIndex, motctrl[iMotorIndex].iActualMotorPowerW);
|
|
}
|