bernhard/CanRtDriver
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Korrektur bei Leistungsvorgabe in Schritten

Browse Source
This commit is contained in:
Bernhard
2026-01-19 19:35:41 +01:00
parent 1b265c71f0
commit f91aee06c7
1 changed files with 125 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files

221
can/can_client.c
View File

@@ -5,6 +5,8 @@
#include <io/io.h>
#include <mqtt/mqtt_client.h>
int iCanSimu = 0;
struct MOTOR_CONTROL_DATA motctrl[MOTOR_COUNT];
struct CAN_INTERFACE_DATA intf_data[MOTOR_COUNT];
@@ -37,13 +39,13 @@ void IncBusTimeoutCounter(int iMotorIndex)
int Can_OpenInterface(int iMotorIndex, const char * ifacename)
{
// Init control data
motctrl[iMotorIndex].nDriveConnected = 0;
motctrl[iMotorIndex].nDriveReady = 0;
motctrl[iMotorIndex].iActualMotorPowerW = 0;
motctrl[iMotorIndex].iMotorGear = MOTOR_GEAR_NEUTRAL;
motctrl[iMotorIndex].iMotorPowerRaw = 0;
motctrl[iMotorIndex].iMotorPowerSteps = 0;
motctrl[iMotorIndex].nSwitchState = 0;
motctrl[iMotorIndex].nDriveConnected = 0;
motctrl[iMotorIndex].nDriveReady = 0;
mylog(LOG_INFO, "CAN: PWR_MAX_RAW=%d PWR_STEP_COUNT=%d", MOTOR_PWR_MAX_RAW, MOTOR_PWR_STEP_COUNT);
@@ -51,46 +53,57 @@ int Can_OpenInterface(int iMotorIndex, const char * 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)
if (iCanSimu)
{
mylog(LOG_ERR, "CAN: Could not create socket for motor %d!", iMotorIndex);
return 1;
mylog(LOG_INFO, "CAN: Using simulation mode (motor %d).", iMotorIndex);
intf_data[iMotorIndex].socket = -1;
motctrl[iMotorIndex].nSwitchState = 0xF5;
motctrl[iMotorIndex].nDriveConnected = 1;
motctrl[iMotorIndex].nDriveReady = 1;
}
else
{
// first we have to create a socket
if ((intf_data[iMotorIndex].socket = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0)
{
mylog(LOG_ERR, "CAN: Could not create socket for motor %d!", 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)
{
mylog(LOG_ERR, "CAN: Could not get interface index for motor %d!", iMotorIndex);
return 2;
}
// 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)
{
mylog(LOG_ERR, "CAN: Could not get interface index for motor %d!", 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)
{
mylog(LOG_ERR, "CAN: Could not bind socket to inteface for motor %d!", iMotorIndex);
return 3;
}
// 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)
{
mylog(LOG_ERR, "CAN: Could not bind socket to inteface for motor %d!", iMotorIndex);
return 3;
}
// make socket to nonblocking
int fcntl_flags = fcntl(intf_data[iMotorIndex].socket, F_GETFL, 0);
if (fcntl_flags < 0)
{
mylog(LOG_ERR, "CAN: Could not get file descriptor flags!");
return 4;
}
fcntl_flags |= O_NONBLOCK;
if (fcntl(intf_data[iMotorIndex].socket, F_SETFL, fcntl_flags) < 0)
{
mylog(LOG_ERR, "CAN: Could not set file descriptor flags (set socket none-blocking)!");
return 5;
}
// make socket to nonblocking
int fcntl_flags = fcntl(intf_data[iMotorIndex].socket, F_GETFL, 0);
if (fcntl_flags < 0)
{
mylog(LOG_ERR, "CAN: Could not get file descriptor flags!");
return 4;
}
fcntl_flags |= O_NONBLOCK;
if (fcntl(intf_data[iMotorIndex].socket, F_SETFL, fcntl_flags) < 0)
{
mylog(LOG_ERR, "CAN: Could not set file descriptor flags (set socket none-blocking)!");
return 5;
}
mylog(LOG_INFO, "CAN: Interface %s (motor %d) opened!", ifacename, iMotorIndex);
mylog(LOG_INFO, "CAN: Interface %s (motor %d) opened!", ifacename, iMotorIndex);
}
return 0;
}
@@ -100,13 +113,20 @@ int Can_OpenInterface(int iMotorIndex, const char * ifacename)
/// @param iMotorIndex
void Can_CloseInterface(int iMotorIndex)
{
if (close(intf_data[iMotorIndex].socket) < 0)
if (intf_data[iMotorIndex].socket >= 0)
{
mylog(LOG_ERR, "CAN: Could not close socket of motor %d!", iMotorIndex);
if (close(intf_data[iMotorIndex].socket) < 0)
{
mylog(LOG_ERR, "CAN: Could not close socket of motor %d!", iMotorIndex);
}
else
{
mylog(LOG_INFO, "CAN: Interface %s (motor %d) closed.", intf_data[iMotorIndex].iface_name, iMotorIndex);
}
}
else
{
mylog(LOG_INFO, "CAN: Interface %s (motor %d) closed.", intf_data[iMotorIndex].iface_name, iMotorIndex);
mylog(LOG_INFO, "CAN: Close simulation mode.");
}
}
@@ -188,7 +208,7 @@ void Can_SetMotorPower(int iMotorIndex, int iPower)
}
else
{
motctrl[iMotorIndex].iMotorPowerRaw = (((MOTOR_PWR_MAX_RAW - MOTOR_PWR_MIN_RAW) * motctrl[iMotorIndex].iMotorPowerSteps) / MOTOR_PWR_STEP_COUNT) + MOTOR_PWR_MIN_RAW;
motctrl[iMotorIndex].iMotorPowerRaw = (((MOTOR_PWR_MAX_RAW - MOTOR_PWR_MIN_RAW) * (motctrl[iMotorIndex].iMotorPowerSteps - 1)) / (MOTOR_PWR_STEP_COUNT - 1)) + MOTOR_PWR_MIN_RAW;
}
mylog(LOG_INFO, "CAN: Motor[%d]: Set power to %d -> %d",
@@ -201,23 +221,26 @@ void Can_SetMotorPower(int iMotorIndex, int iPower)
void Can_TransmitMotorGear(int iMotorIndex)
{
// 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))
if (intf_data[iMotorIndex].socket >= 0)
{
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))
{
}
}
}
@@ -227,23 +250,26 @@ void Can_TransmitMotorGear(int iMotorIndex)
void Can_TransmitMotorPower(int iMotorIndex)
{
// 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].iMotorPowerRaw; // 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))
if (intf_data[iMotorIndex].socket >= 0)
{
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].iMotorPowerRaw; // 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))
{
}
}
}
@@ -252,39 +278,42 @@ void Can_TransmitMotorPower(int iMotorIndex)
/// @param iMotorIndex
void Can_ReadData(int iMotorIndex)
{
ssize_t nbytes = 0;
struct can_frame frame;
// increment cycle counter
IncBusTimeoutCounter(iMotorIndex);
// read one frame
if ((nbytes = read(intf_data[iMotorIndex].socket, &frame, sizeof(frame))) > 0)
if (intf_data[iMotorIndex].socket >= 0)
{
canid_t pgn = frame.can_id & 0x00FFFF00;
ssize_t nbytes = 0;
struct can_frame frame;
switch(pgn)
// increment cycle counter
IncBusTimeoutCounter(iMotorIndex);
// read one frame
if ((nbytes = read(intf_data[iMotorIndex].socket, &frame, sizeof(frame))) > 0)
{
case 0x00EF6400: // "repeat data"
break;
canid_t pgn = frame.can_id & 0x00FFFF00;
case 0x00F00300: // PGN 61443 "Electronic Engine Controller 2"
// we have sent this -> ignore
break;
switch(pgn)
{
case 0x00EF6400: // "repeat data"
break;
case 0x00F00500: // PGN 61445 "Electronic Transmission Controller 2"
// we have sent this -> ignore
break;
case 0x00F00300: // PGN 61443 "Electronic Engine Controller 2"
// we have sent this -> ignore
break;
case 0x00FF1300: // PGN 65299 "Manufacturer PGN"
// here we find the states of the switches
Can_Read_Manu_PGN(iMotorIndex, &frame);
break;
case 0x00F00500: // PGN 61445 "Electronic Transmission Controller 2"
// we have sent this -> ignore
break;
case 0x00FF1400: // PGN 65300 "Manufacturer PGN 2"
// here we find the actual power of the motor
Can_Read_Manu_PGN2(iMotorIndex, &frame);
break;
case 0x00FF1300: // PGN 65299 "Manufacturer PGN"
// here we find the states of the switches
Can_Read_Manu_PGN(iMotorIndex, &frame);
break;
case 0x00FF1400: // PGN 65300 "Manufacturer PGN 2"
// here we find the actual power of the motor
Can_Read_Manu_PGN2(iMotorIndex, &frame);
break;
}
}
}
}