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

./git_commit_push.sh

Browse Source
This commit is contained in:
Bernhard
2025-12-17 19:19:44 +01:00
parent c393706088
commit dcb267df89
7 changed files with 278 additions and 152 deletions
Download Patch File Download Diff File Expand all files Collapse all files

155
can/can_client.c
View File

@@ -1,4 +1,6 @@
#include "main.h"
#include <can/can_client.h>
#include <io/io.h>
#include <mqtt/mqtt_client.h>
@@ -6,7 +8,10 @@
struct MOTOR_CONTROL_DATA motctrl[MOTOR_COUNT];
struct CAN_INTERFACE_DATA intf_data[MOTOR_COUNT];
int iBusTimeoutCounter = 0;
/// @brief Increase counter for cycles without received telegram
/// @param iMotorIndex
void IncBusTimeoutCounter(int iMotorIndex)
{
if (iBusTimeoutCounter < 2000)
@@ -14,11 +19,17 @@ void IncBusTimeoutCounter(int iMotorIndex)
iBusTimeoutCounter++;
if (iBusTimeoutCounter >= 2000)
{
// long time no telegram received -> motor is not connected anymore
motctrl[iMotorIndex].nSwitchState = 0;
motctrl[iMotorIndex].nDriveConnected = 0;
motctrl[iMotorIndex].nDriveReady = 0;
Can_SetMotorGear(iMotorIndex, 0);
MqttClient_Publish_MotorSwitchState(iMotorIndex, motctrl[iMotorIndex].nSwitchState);
}
}
}
/// @brief Open socket of CAN interface for the given motor
/// @param iMotorIndex
/// @param ifacename
@@ -26,12 +37,16 @@ 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].iMotorPower = 0;
motctrl[iMotorIndex].iMotorPowerPct = 0;
motctrl[iMotorIndex].nSwitchState = 0;
mylog(LOG_INFO, "CAN: PWR_MIN_PCT=%d PWR_MAX_PCT=%d PWR_STEP=%d", MOTOR_PWR_MIN_PCT, MOTOR_PWR_MAX_PCT, MOTOR_PWR_STEP);
strcpy(intf_data[iMotorIndex].iface_name, ifacename);
Can_SetMotorGear(iMotorIndex, MOTOR_GEAR_NEUTRAL);
Can_SetMotorPower(iMotorIndex, 0);
@@ -39,7 +54,7 @@ int Can_OpenInterface(int iMotorIndex, const char * ifacename)
// 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);
mylog(LOG_ERR, "CAN: Could not create socket for motor %d!", iMotorIndex);
return 1;
}
@@ -47,7 +62,7 @@ int Can_OpenInterface(int iMotorIndex, const char * ifacename)
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);
mylog(LOG_ERR, "CAN: Could not get interface index for motor %d!", iMotorIndex);
return 2;
}
@@ -57,25 +72,25 @@ int Can_OpenInterface(int iMotorIndex, const char * ifacename)
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);
mylog(LOG_ERR, "CAN: Could not bind socket to inteface for motor %d!", iMotorIndex);
return 3;
}
// socket auf nicht-blockierend umstellen
// make socket to nonblocking
int fcntl_flags = fcntl(intf_data[iMotorIndex].socket, F_GETFL, 0);
if (fcntl_flags < 0)
{
printf("Could not get file descriptor flags!\n");
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)
{
printf("Could not set file descriptor flags (set socket none-blocking)!\n");
mylog(LOG_ERR, "CAN: Could not set file descriptor flags (set socket none-blocking)!");
return 5;
}
printf("Interface %s (motor %d) opened!\n", ifacename, iMotorIndex);
mylog(LOG_INFO, "CAN: Interface %s (motor %d) opened!", ifacename, iMotorIndex);
return 0;
}
@@ -87,11 +102,11 @@ void Can_CloseInterface(int iMotorIndex)
{
if (close(intf_data[iMotorIndex].socket) < 0)
{
printf("Could not close socket of motor %d!\n", iMotorIndex);
mylog(LOG_ERR, "CAN: Could not close socket of motor %d!", iMotorIndex);
}
else
{
printf("Interface %s (motor %d) closed.\n", intf_data[iMotorIndex].iface_name, iMotorIndex);
mylog(LOG_INFO, "CAN: Interface %s (motor %d) closed.", intf_data[iMotorIndex].iface_name, iMotorIndex);
}
}
@@ -101,25 +116,29 @@ void Can_CloseInterface(int iMotorIndex)
/// @param iGear (-1=reverse, 0=neutral, 1=forward)
void Can_SetMotorGear(int iMotorIndex, int iGear)
{
if (iGear > 0)
if ((iGear > 0) && (motctrl[iMotorIndex].nDriveReady != 0))
{
if (motctrl[iMotorIndex].iMotorGear != MOTOR_GEAR_FORWARD)
{
MqttClient_Publish_MotorGear(iMotorIndex, iGear);
motctrl[iMotorIndex].iMotorGear = MOTOR_GEAR_FORWARD;
// motor is switched to forward -> set min. power
Can_SetMotorPower(iMotorIndex, MOTOR_PWR_MIN_PCT);
}
WriteOutputPin(GPIO_LED_MOTRUN, HIGH);
printf("Motor[%d]: Set gear forward.\n", iMotorIndex);
mylog(LOG_INFO, "CAN: Motor[%d]: Set gear forward.", iMotorIndex);
}
else if (iGear < 0)
else if ((iGear < 0) && (motctrl[iMotorIndex].nDriveReady != 0))
{
if (motctrl[iMotorIndex].iMotorGear != MOTOR_GEAR_REVERSE)
{
MqttClient_Publish_MotorGear(iMotorIndex, iGear);
motctrl[iMotorIndex].iMotorGear = MOTOR_GEAR_REVERSE;
// motor is switched to reverse -> set min. power
Can_SetMotorPower(iMotorIndex, MOTOR_PWR_MIN_PCT);
}
WriteOutputPin(GPIO_LED_MOTRUN, HIGH);
printf("Motor[%d]: Set gear reverse.\n", iMotorIndex);
mylog(LOG_INFO, "CAN: Motor[%d]: Set gear reverse.", iMotorIndex);
}
else
{
@@ -128,9 +147,10 @@ void Can_SetMotorGear(int iMotorIndex, int iGear)
MqttClient_Publish_MotorGear(iMotorIndex, iGear);
motctrl[iMotorIndex].iMotorGear = MOTOR_GEAR_NEUTRAL;
}
Can_SetMotorPower(iMotorIndex, MOTOR_PWR_MIN_PCT);
// motor is switch to neutral -> set power to 0
Can_SetMotorPower(iMotorIndex, 0);
WriteOutputPin(GPIO_LED_MOTRUN, LOW);
printf("Motor[%d]: Set gear neutral.\n", iMotorIndex);
mylog(LOG_INFO, "CAN: Motor[%d]: Set gear neutral.", iMotorIndex);
}
}
@@ -140,12 +160,19 @@ void Can_SetMotorGear(int iMotorIndex, int iGear)
/// @param iPower (Range: 0..100)
void Can_SetMotorPower(int iMotorIndex, int iPower)
{
if (iPower <= MOTOR_PWR_MIN_PCT)
if ((motctrl[iMotorIndex].iMotorGear == MOTOR_GEAR_NEUTRAL) || (motctrl[iMotorIndex].nDriveReady == 0))
{
// when motor is neutral or not ready set power to 0
motctrl[iMotorIndex].iMotorPowerPct = 0;
}
else if (iPower <= MOTOR_PWR_MIN_PCT)
{
// limit to min. power
motctrl[iMotorIndex].iMotorPowerPct = MOTOR_PWR_MIN_PCT;
}
else if (iPower >= MOTOR_PWR_MAX_PCT)
{
// limit to max. power
motctrl[iMotorIndex].iMotorPowerPct = MOTOR_PWR_MAX_PCT;
}
else
@@ -154,9 +181,10 @@ void Can_SetMotorPower(int iMotorIndex, int iPower)
}
MqttClient_Publish_MotorPower(iMotorIndex, motctrl[iMotorIndex].iMotorPowerPct);
// calc value for telegram
motctrl[iMotorIndex].iMotorPower = 250 * motctrl[iMotorIndex].iMotorPowerPct / 100;
printf("Motor[%d]: Set power to %d%% -> %d\n",
mylog(LOG_INFO, "CAN: Motor[%d]: Set power to %d%% -> %d",
iMotorIndex, motctrl[iMotorIndex].iMotorPowerPct, motctrl[iMotorIndex].iMotorPower);
}
@@ -165,27 +193,24 @@ void Can_SetMotorPower(int iMotorIndex, int iPower)
/// @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))
{
// 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))
{
}
}
}
@@ -194,27 +219,24 @@ void Can_TransmitMotorGear(int iMotorIndex)
/// @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))
{
// 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))
{
}
}
}
@@ -226,30 +248,34 @@ void Can_ReadData(int iMotorIndex)
ssize_t nbytes = 0;
struct can_frame frame;
// increment cycle counter
IncBusTimeoutCounter(iMotorIndex);
// wir wollen immer alle verfügbaren Frames lesen
while ((nbytes = read(intf_data[iMotorIndex].socket, &frame, sizeof(frame))) > 0)
// read one frame
if ((nbytes = read(intf_data[iMotorIndex].socket, &frame, sizeof(frame))) > 0)
{
canid_t pgn = frame.can_id & 0x00FFFF00;
switch(pgn)
{
case 0x00EF6400: // "repeat data"
break;
case 0x00F00300: // PGN 61443 "Electronic Engine Controller 2"
// haben wir selber gesendet -> ignorieren
// we have sent this -> ignore
break;
case 0x00F00500: // PGN 61445 "Electronic Transmission Controller 2"
// haben wir selber gesendet -> ignorieren
// we have sent this -> ignore
break;
case 0x00FF1300: // PGN 65299 "Manufacturer PGN"
// hier finden wir die Zustände der Schalter
// here we find the states of the switches
Can_Read_Manu_PGN(iMotorIndex, &frame);
break;
case 0x00FF1400: // PGN 65300 "Manufacturer PGN 2"
// hier bekommen wir die Leistung des Motors angezeigt
// here we find the actual power of the motor
Can_Read_Manu_PGN2(iMotorIndex, &frame);
break;
}
@@ -260,9 +286,16 @@ void Can_ReadData(int iMotorIndex)
/// @param frame
void Can_Read_Manu_PGN(int iMotorIndex, struct can_frame *frame)
{
// we received a valid telegram -> set timeout counter to zero
iBusTimeoutCounter = 0;
// get switch states
motctrl[iMotorIndex].nSwitchState = frame->data[4];
MqttClient_Publish_MotorSwitchState(iMotorIndex, motctrl[iMotorIndex].nSwitchState);
// set flags
motctrl[iMotorIndex].nDriveConnected = 1; // we received a PGN -> so we are connected
motctrl[iMotorIndex].nDriveReady = ((motctrl[iMotorIndex].nSwitchState & 0x80) != 0) ? 1 : 0; // this bit shows if the drive is ready to run
}
/// @brief Read PGN 65300

5
can/can_client.h
View File

@@ -5,6 +5,7 @@
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <syslog.h>
#include <fcntl.h>
#include <net/if.h>
@@ -23,10 +24,12 @@
#define MOTOR_PWR_MIN_PCT 15
#define MOTOR_PWR_MAX_PCT 100
#define MOTOR_PWR_STEP 12
#define MOTOR_PWR_STEP ((MOTOR_PWR_MAX_PCT - MOTOR_PWR_MIN_PCT) / 7)
struct MOTOR_CONTROL_DATA
{
char nDriveConnected;
char nDriveReady;
int iMotorGear;
int iMotorPower;
int iMotorPowerPct;