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

Kommunikation hat funktioniert

Browse Source
This commit is contained in:
Bernhard
2025-12-06 19:20:49 +01:00
commit 9b3c4b3fd2
13 changed files with 1115 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
.gitignore vendored Executable file
View File

@@ -0,0 +1,5 @@
# ignore all files in bin folder
bin/
._*
.DS_Store

179
can/can_client.c Executable file
View File

@@ -0,0 +1,179 @@
#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];
/// @brief Open socket of CAN interface for the given motor
/// @param iMotorIndex
/// @param ifacename
/// @return
int Can_OpenInterface(int iMotorIndex, const char * ifacename)
{
strcpy(intf_data[iMotorIndex].iface_name, ifacename);
Can_SetMotorGear(iMotorIndex, 0);
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;
}
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)
{
// 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)
{
// 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))
{
}
}

52
can/can_client.h Executable file
View File

@@ -0,0 +1,52 @@
#if !defined(__CAN_CLIENT_H__)
#define __CAN_CLIENT_H__
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#define MOTOR_COUNT 2
// motor gear: 0x7C=reverse, 0x7D=neutral, 0x7E=forward
#define MOTOR_GEAR_REVERSE 0x7C
#define MOTOR_GEAR_NEUTRAL 0x7D
#define MOTOR_GEAR_FORWARD 0x7E
#define MOTOR_PWR_MIN_PCT 5
#define MOTOR_PWR_MAX_PCT 100
#define MOTOR_PWR_STEP 14
struct MOTOR_CONTROL_DATA
{
int iMotorGear;
int iMotorPower;
int iMotorPowerPct;
};
extern struct MOTOR_CONTROL_DATA motctrl[MOTOR_COUNT];
struct CAN_INTERFACE_DATA
{
int socket;
char iface_name[20];
struct ifreq ifr;
struct sockaddr_can addr;
};
int Can_OpenInterface(int iMotorIndex, const char * ifacename);
void Can_CloseInterface(int iMotorIndex);
void Can_SetMotorGear(int iMotorIndex, int iGear);
void Can_SetMotorPower(int iMotorIndex, int iPower);
void Can_TransmitMotorGear(int iMotorIndex);
void Can_TransmitMotorPower(int iMotorIndex);
#endif

211
io/io.c Executable file
View File

@@ -0,0 +1,211 @@
#include "io.h"
#include <can/can_client.h>
struct GPIO_KEY_DATA gpioKeyStop;
struct GPIO_KEY_DATA gpioKeyPwrUp;
struct GPIO_KEY_DATA gpioKeyPwrDown;
int IO_Init()
{
if (wiringPiSetupPinType(WPI_PIN_BCM))
{
printf("IO: Set up wiringPi failed!\n");
return 1;
}
// IO-Pins für Tasten konfigurieren
SetupKeyPin(&gpioKeyStop, GPIO_KEY_STOP);
SetupKeyPin(&gpioKeyPwrUp, GPIO_KEY_PWRUP);
SetupKeyPin(&gpioKeyPwrDown, GPIO_KEY_PWRDOWN);
// IO-Pins für Ausgänge konfigurieren
SetupOutputPin(GPIO_LED_MOTRUN);
// Einschaltsequenz
WriteOutputPin(GPIO_LED_MOTRUN, HIGH);
delay(500);
WriteOutputPin(GPIO_LED_MOTRUN, LOW);
delay(500);
WriteOutputPin(GPIO_LED_MOTRUN, HIGH);
delay(500);
WriteOutputPin(GPIO_LED_MOTRUN, LOW);
delay(500);
printf("IO initialized successfull!\n");
return 0;
}
void SetupKeyPin(struct GPIO_KEY_DATA *pdata, int iKeyPin)
{
pdata->iKeyPin = iKeyPin;
pdata->iKeyValue = 0;
pdata->iKeyRisingEdge = 0;
pdata->iKeyFallingEdge = 0;
pdata->nHighCycleCounter = 0;
pdata->nLowCycleCounter = 0;
pdata->iKeyPressedCycleCounter = 0;
pdata->iKeyRepeatCycleCounter = 0;
// Wenn Eingang verwendet wird
if (pdata->iKeyPin > 0)
{
pinMode(pdata->iKeyPin, INPUT);
pullUpDnControl(pdata->iKeyPin, PUD_UP);
}
}
void ReadKey(struct GPIO_KEY_DATA *pdata)
{
if (pdata->iKeyPin > 0)
{
int newval = pdata->iKeyValue;
if (digitalRead(pdata->iKeyPin) == LOW) // invertierte Logik weil wir PullUp-Widerstand bei Betätigung auf low ziehen
{
// Signal liegt an
if (pdata->nLowCycleCounter > 0)
{
pdata->nLowCycleCounter--;
}
if (pdata->nHighCycleCounter < KEY_RISING_FILTERCYCLES)
{
pdata->nHighCycleCounter++;
if (pdata->nHighCycleCounter >= KEY_RISING_FILTERCYCLES)
{
// gewünschte Anzahl Zyklen stabil
newval = 1;
}
}
}
else
{
// Signal liegt nicht an
if (pdata->nHighCycleCounter > 0)
{
pdata->nHighCycleCounter--;
}
if (pdata->nLowCycleCounter < KEY_FALLING_FILTERCYCLES)
{
pdata->nLowCycleCounter++;
if (pdata->nLowCycleCounter >= KEY_FALLING_FILTERCYCLES)
{
// gewünschte Anzahl Zyklen stabil
newval = 0;
}
}
}
if (newval && !pdata->iKeyValue)
{
// Taster wurde betätigt
pdata->iKeyRisingEdge = 1;
pdata->iKeyValue = newval;
pdata->iKeyPressedCycleCounter = 0;
pdata->iKeyRepeatCycleCounter = 0;
}
else if (pdata->iKeyValue && !newval)
{
// Taster wurde losgelassen
pdata->iKeyFallingEdge = 1;
pdata->iKeyValue = newval;
}
else
{
// Keine Änderung
pdata->iKeyRisingEdge = 0;
pdata->iKeyFallingEdge = 0;
if (pdata->iKeyValue)
{
// Wenn Taste gedrückt ist
if (pdata->iKeyPressedCycleCounter < KEY_START_REPEAT_CYCLECOUNT)
{
// Zyklen zählen
pdata->iKeyPressedCycleCounter++;
}
if (pdata->iKeyPressedCycleCounter >= KEY_START_REPEAT_CYCLECOUNT)
{
// Wenn Taste länger als KEY_START_REPEAT_CYCLECOUNT gedrückt ist
pdata->iKeyRepeatCycleCounter++;
if (pdata->iKeyRepeatCycleCounter >= KEY_REPEAT_CYCLECOUNT)
{
// alle KEY_REPEAT_CYCLECOUNT Zyklen Tastendruck signalisieren
pdata->iKeyRisingEdge = 1;
pdata->iKeyRepeatCycleCounter = 0;
}
}
}
}
}
}
void SetupOutputPin(int iOutPin)
{
if (iOutPin > 0)
{
pinMode(iOutPin, OUTPUT);
digitalWrite(iOutPin, LOW);
}
}
void WriteOutputPin(int iOutPin, int iValue)
{
if (iOutPin > 0)
{
digitalWrite(iOutPin, iValue);
}
}
void IO_DoCyclic()
{
ReadKey(&gpioKeyStop);
ReadKey(&gpioKeyPwrUp);
ReadKey(&gpioKeyPwrDown);
if (gpioKeyStop.iKeyValue)
{
// Stop-Taste betätigt -> hat Vorrang vor den anderen Tasten
if (gpioKeyStop.iKeyRisingEdge)
{
Can_SetMotorGear(0, 0);
}
}
else
{
if (gpioKeyPwrUp.iKeyRisingEdge)
{
// Leistung erhöhen
if (motctrl[0].iMotorGear == MOTOR_GEAR_NEUTRAL)
{
Can_SetMotorGear(0, 1);
Can_SetMotorPower(0, MOTOR_PWR_MIN_PCT);
}
else
{
Can_SetMotorPower(0, motctrl[0].iMotorPowerPct + MOTOR_PWR_STEP);
}
}
if (gpioKeyPwrDown.iKeyRisingEdge)
{
// Leistung verringern
if (motctrl[0].iMotorPowerPct > MOTOR_PWR_MIN_PCT)
{
Can_SetMotorPower(0, motctrl[0].iMotorPowerPct - MOTOR_PWR_STEP);
}
else
{
Can_SetMotorGear(0, 0);
}
}
}
}

37
io/io.h Executable file
View File

@@ -0,0 +1,37 @@
#if !defined(__IO_H__)
#define __IO_H__
#include <wiringPi.h>
#include <stdio.h>
#define GPIO_LED_MOTRUN 17 // GPIO Pin fuer LED Motor läuft
#define GPIO_KEY_STOP 26 // GPIO Pin fuer Taster Stop
#define GPIO_KEY_PWRUP 5 // GPIO Pin fuer Taster Leistung-Erhöhen
#define GPIO_KEY_PWRDOWN 6 // GPIO Pin fuer Taster Leistung-Verringern
#define KEY_RISING_FILTERCYCLES 5 // Filterwert für Eingänge steigende Flanke
#define KEY_FALLING_FILTERCYCLES 15 // Filterwert für Eingänge (Zyklen-Zähler)
#define KEY_START_REPEAT_CYCLECOUNT 50 // Anzahl Zyklen, nach denen Wiederholungen beginnen
#define KEY_REPEAT_CYCLECOUNT 50 // Anzahl Zyklen, nach den wiederholt wird
// Datenstruktur für einen Taster
struct GPIO_KEY_DATA
{
int iKeyPin;
int iKeyValue;
int iKeyRisingEdge;
int iKeyFallingEdge;
unsigned int nHighCycleCounter;
unsigned int nLowCycleCounter;
int iKeyPressedCycleCounter;
int iKeyRepeatCycleCounter;
};
int IO_Init();
void IO_DoCyclic();
void SetupKeyPin(struct GPIO_KEY_DATA *pdata, int iKeyPin);
void SetupOutputPin(int iOutPin);
void WriteOutputPin(int iOutPin, int iValue);
#endif

253
main.c Executable file
View File

@@ -0,0 +1,253 @@
#include <main.h>
#include <mqtt/mqtt_client.h>
#include <can/can_client.h>
#include <io/io.h>
// Period info of the realtime task
struct period_info pinfo;
int iThreadControl = 0; // 0: thread is running, <0: thread shall exit, >0 thread has exited
/// @brief Initialize period_info with period_ms for cyclic task
/// @param period_ms
/// @param pinfo
static void periodic_task_init(long period_ms, struct period_info *pinfo)
{
/* for simplicity, hardcoding a 1ms period */
pinfo->period_ns = period_ms * 1000000;
pinfo->cyclecounter = 0;
clock_gettime(CLOCK_MONOTONIC, &(pinfo->next_period));
pinfo->fStartTime = (float)pinfo->next_period.tv_sec + ((float)pinfo->next_period.tv_nsec / 1000000000.0);
}
/// @brief Wait in cyclic task the rest of time until the next cycle
/// @param pinfo
static void wait_rest_of_period(struct period_info *pinfo)
{
pinfo->next_period.tv_nsec += pinfo->period_ns;
while (pinfo->next_period.tv_nsec >= 1000000000) {
/* timespec nsec overflow */
pinfo->next_period.tv_sec++;
pinfo->next_period.tv_nsec -= 1000000000;
}
/* for simplicity, ignoring possibilities of signal wakes */
clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &pinfo->next_period, NULL);
}
/// @brief Get monotonic time in sec as float
/// @return
float clock_gettime_s()
{
struct timespec tm;
clock_gettime(CLOCK_MONOTONIC, &tm);
return (float)tm.tv_sec + ((float)tm.tv_nsec / 1000000000.0);
}
/// @brief Cyclic task, is called from thread_func every ms
/// @param pinfo
static void do_cyclic_1ms(struct period_info *pinfo)
{
uint16_t nCalled = 0;
if ((pinfo->cyclecounter % 10) == 0)
{
// called every 10ms
nCalled |= 0x1000;
IO_DoCyclic();
}
if ((pinfo->cyclecounter % 50) == 0)
{
// called every 50ms
nCalled |= 0x0001;
Can_TransmitMotorPower(0);
}
if (((pinfo->cyclecounter + 10) % 100) == 0)
{
// called every 100ms
nCalled |= 0x0010;
Can_TransmitMotorGear(0);
//printf("%.3f: 100ms-Cycle %ld...\n", clock_gettime_s() - pinfo->fStartTime, pinfo->cyclecounter);
}
if (((pinfo->cyclecounter + 20) % 100) == 0)
{
// called every 100ms
nCalled |= 0x0100;
MqttClient_Refresher();
}
if (((pinfo->cyclecounter + 30) % 500) == 0)
{
// called every 250ms
MqttClient_Publisher();
}
if (nCalled > 0)
{
//printf("%.3f: Called 0x%.4X at cycle %ld...\n", clock_gettime_s() - pinfo->fStartTime, nCalled, pinfo->cyclecounter);
}
}
/// @brief Our one and only realtime task
/// @param data
/// @return
void *thread_func(void *data)
{
// Open CAN interface first motor
if (Can_OpenInterface(0, "can0"))
{
printf("Can_OpenInterface() failed!\n");
return NULL;
}
// Connect to mqtt broker
while (MqttClient_Connect() && (iThreadControl == 0))
{
printf("MqttClient_Connect() failed!\n");
sleep(10);
}
// Initialize IO Ports
if (IO_Init())
{
printf("IO_Init() failed!\n");
return NULL;
}
// initialize cyclic task
periodic_task_init(1, &pinfo);
// cyclic call of do_cyclic_1ms()
while (iThreadControl == 0)
{
pinfo.cyclecounter++;
if (pinfo.cyclecounter >= 86400000)
{
// Reset cycle counter every 24h
pinfo.cyclecounter = 0;
}
do_cyclic_1ms(&pinfo);
wait_rest_of_period(&pinfo);
}
// Disconnect from mqtt broker
MqttClient_Close();
// Close CAN interface
Can_CloseInterface(0);
iThreadControl = 1;
return NULL;
}
/// @brief catch signals and set flag to terminate for the realtime thread
/// @param signo
void sig_handler(int signo)
{
if ((signo == SIGINT) || (signo == SIGTERM))
{
printf("Received signal %d\n", signo);
iThreadControl = -1; // signal realtime thread to exit
}
}
/// @brief Hauptfunktion Echtzeit-Task erstellen und starten
/// @param argc
/// @param argv
/// @return
int main(int argc, char* argv[])
{
struct sched_param param;
pthread_attr_t attr;
pthread_t thread;
int ret;
// catch signals
if (signal(SIGTERM, sig_handler) == SIG_ERR)
{
printf("Can't catch SIGTERM\n");
exit(-1);
}
if (signal(SIGINT, sig_handler) == SIG_ERR)
{
printf("Can't catch SIGINT\n");
exit(-2);
}
/* Lock memory */
if(mlockall(MCL_CURRENT | MCL_FUTURE) == -1)
{
printf("mlockall failed: %m\n");
exit(-3);
}
/* Initialize pthread attributes (default values) */
ret = pthread_attr_init(&attr);
if (ret)
{
printf("init pthread attributes failed\n");
goto out;
}
/* Set a specific stack size */
ret = pthread_attr_setstacksize(&attr, PTHREAD_STACK_MIN);
if (ret)
{
printf("pthread setstacksize failed\n");
goto out;
}
/* Set scheduler policy and priority of pthread */
ret = pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
if (ret)
{
printf("pthread setschedpolicy failed\n");
goto out;
}
param.sched_priority = 99; // Priority between 1 (low) and 99() high)
ret = pthread_attr_setschedparam(&attr, &param);
if (ret)
{
printf("pthread setschedparam failed\n");
goto out;
}
/* Use scheduling parameters of attr */
ret = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
if (ret)
{
printf("pthread setinheritsched failed\n");
goto out;
}
/* Create a pthread with specified attributes */
iThreadControl = 0;
ret = pthread_create(&thread, &attr, thread_func, NULL);
if (ret)
{
printf("create pthread failed\n");
goto out;
}
// join the thread and wait for it to exit
ret = pthread_join(thread, NULL);
if (ret)
{
printf("faild to join thread!\n");
}
out:
return ret;
}

26
main.h Executable file
View File

@@ -0,0 +1,26 @@
#if !defined(__MAIN_H__)
#define __MAIN_H__
#include <limits.h>
#include <pthread.h>
#include <sched.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <sys/mman.h>
#include <time.h>
#include <errno.h>
#include <signal.h>
struct period_info
{
struct timespec next_period;
long period_ns;
unsigned long cyclecounter;
float fStartTime;
};
extern struct period_info pinfo;
#endif

26
makefile Executable file
View File

@@ -0,0 +1,26 @@
# UNAME = $(shell uname -o)
CC = gcc
CFLAGS = -Wextra -Wall -std=gnu99 -I. -Wno-unused-parameter -Wno-unused-variable -Wno-duplicate-decl-specifier
MQTT_SOURCES = mqtt/mqtt_client.c
CAN_SOURCES = can/can_client.c
IO_SOURCES = io/io.c
PROG = bin/CanRtDriver
BINDIR = bin
all: $(BINDIR) $(PROG)
bin/CanRtDriver: main.c $(CAN_SOURCES) $(MQTT_SOURCES) $(IO_SOURCES)
$(CC) $(CFLAGS) $^ -lpthread -lmosquitto -lwiringPi -o $@
$(BINDIR):
mkdir -p $(BINDIR)
clean:
rm -rf $(BINDIR)
check: all
./$(MQTT_C_UNITTESTS)

281
mqtt/mqtt_client.c Executable file
View File

@@ -0,0 +1,281 @@
/*
alternativer Client: libmosquitto-dev:
https://mosquitto.org/api/files/mosquitto-h.html
*/
#include <main.h>
#include <mqtt/mqtt_client.h>
#include <can/can_client.h>
#include <string.h>
#include <mosquitto.h>
const char* mqtt_topic_status_cyclecounter = "Pool/Status/CycleCounter";
const char* mqtt_topic_motor1_gear = "Pool/Motor1/Gear";
int iMqttMotor1Gear = 0;
const char* mqtt_topic_motor1_power = "Pool/Motor1/Power";
int iMqttMotor1Power = 0;
const char* mqtt_topic_motor2_gear = "Pool/Motor2/Gear";
int iMqttMotor2Gear = 0;
const char* mqtt_topic_motor2_power = "Pool/Motor2/Power";
int iMqttMotor2Power = 0;
const char* mqtt_broker_addr = "127.0.0.1";
const int mqtt_broker_port = 1883;
struct mosquitto *mosq; /**< Libmosquito MQTT client instance. */
int iHadConnectError = 0;
void my_message_callback(struct mosquitto *mosq, void *userdata, const struct mosquitto_message *message)
{
char* topic_value = (char *)malloc(message->payloadlen + 1);
memcpy(topic_value, message->payload, message->payloadlen);
topic_value[message->payloadlen] = '\0';
if (strcmp(message->topic, mqtt_topic_motor1_gear) == 0)
{
int val = 9999;
if (sscanf(topic_value, "%d", &val))
{
printf("%ld: Received value for mqtt_topic_motor1_gear: %d\n", pinfo.cyclecounter, val);
iMqttMotor1Gear = val;
Can_SetMotorGear(0, val);
}
else
{
printf("%ld: Received mqtt_topic_motor1_gear: %s\n", pinfo.cyclecounter, topic_value);
}
}
else if (strcmp(message->topic, mqtt_topic_motor1_power) == 0)
{
int val = 9999;
if (sscanf(topic_value, "%d", &val))
{
printf("%ld: Received value for mqtt_topic_motor1_power: %d\n", pinfo.cyclecounter, val);
iMqttMotor1Power = val;
Can_SetMotorPower(0, val);
}
else
{
printf("%ld: Received mqtt_topic_motor1_power: %s\n", pinfo.cyclecounter, topic_value);
}
}
else if (strcmp(message->topic, mqtt_topic_motor2_gear) == 0)
{
int val = 9999;
if (sscanf(topic_value, "%d", &val))
{
printf("%ld: Received value for mqtt_topic_motor2_gear: %d\n", pinfo.cyclecounter, val);
iMqttMotor2Gear = val;
Can_SetMotorGear(1, val);
}
else
{
printf("%ld: Received mqtt_topic_motor2_gear: %s\n", pinfo.cyclecounter, topic_value);
}
}
else if (strcmp(message->topic, mqtt_topic_motor2_power) == 0)
{
int val = 9999;
if (sscanf(topic_value, "%d", &val))
{
printf("%ld: Received value for mqtt_topic_motor2_power: %d\n", pinfo.cyclecounter, val);
iMqttMotor2Power = val;
Can_SetMotorPower(1, val);
}
else
{
printf("%ld: Received mqtt_topic_motor2_power: %s\n", pinfo.cyclecounter, topic_value);
}
}
else
{
printf("%ld: Received publish('%s'): %s\n", pinfo.cyclecounter, message->topic, topic_value);
}
free(topic_value);
}
int MqttClient_Connect()
{
int major, minor, revision;
mosquitto_lib_version(&major, &minor, &revision);
printf("Libmosquitto version: %d.%d.%d\n", major, minor, revision);
// libmosquitto initialization
mosquitto_lib_init();
// create an instance of a mosquitto lib object
mosq = mosquitto_new(NULL, true, NULL);
if (mosq == NULL)
{
printf("Failed to create mosquitto client!/n");
iHadConnectError++;
return 1;
}
// Define a function which will be called by libmosquitto client every time when there is a new MQTT message
mosquitto_message_callback_set(mosq, my_message_callback);
// Connect to MQTT broker
if (mosquitto_connect(mosq, mqtt_broker_addr, mqtt_broker_port, 60) != MOSQ_ERR_SUCCESS)
{
printf("Error: connecting to MQTT broker failed\n");
MqttClient_Close();
iHadConnectError++;
return 2;
}
if (iHadConnectError)
{
/* Wenn wir Verbindungsfehler hatten, dann befinden wir uns wohl in Boot-Prozess und der Mosquitto ist
gerade erst gestartet. Wir müssen hier etwas warten, sonst funktioniert das Subscriben nicht */
sleep(10);
}
// publish all topics we want to subscribe
char message[10];
snprintf(message, sizeof(message), "0");
if (mosquitto_publish(mosq, NULL, mqtt_topic_motor1_gear, strlen(message), &message, 0, false) != MOSQ_ERR_SUCCESS)
{
printf("mosquitto_publish(mqtt_topic_motor1_gear) fehlgeschlagen!\n");
MqttClient_Close();
iHadConnectError++;
return 10;
}
if (mosquitto_publish(mosq, NULL, mqtt_topic_motor1_power, strlen(message), &message, 0, false) != MOSQ_ERR_SUCCESS)
{
printf("mosquitto_publish(mqtt_topic_motor1_power) fehlgeschlagen!\n");
MqttClient_Close();
iHadConnectError++;
return 11;
}
if (mosquitto_publish(mosq, NULL, mqtt_topic_motor2_gear, strlen(message), &message, 0, false) != MOSQ_ERR_SUCCESS)
{
printf("mosquitto_publish(mqtt_topic_motor2_gear) fehlgeschlagen!\n");
MqttClient_Close();
iHadConnectError++;
return 12;
}
if (mosquitto_publish(mosq, NULL, mqtt_topic_motor2_power, strlen(message), &message, 0, false) != MOSQ_ERR_SUCCESS)
{
printf("mosquitto_publish(mqtt_topic_motor2_power) fehlgeschlagen!\n");
MqttClient_Close();
iHadConnectError++;
return 13;
}
// subscribe all needed topics
if (mosquitto_subscribe(mosq, NULL, mqtt_topic_motor1_gear, 0) != MOSQ_ERR_SUCCESS)
{
printf("mosquitto_subscribe(mqtt_topic_motor1_gear) fehlgeschlagen!\n");
MqttClient_Close();
iHadConnectError++;
return 20;
}
if (mosquitto_subscribe(mosq, NULL, mqtt_topic_motor1_power, 0) != MOSQ_ERR_SUCCESS)
{
printf("mosquitto_subscribe(mqtt_topic_motor1_power) fehlgeschlagen!\n");
MqttClient_Close();
iHadConnectError++;
return 21;
}
if (mosquitto_subscribe(mosq, NULL, mqtt_topic_motor2_gear, 0) != MOSQ_ERR_SUCCESS)
{
printf("mosquitto_subscribe(mqtt_topic_motor2_gear) fehlgeschlagen!\n");
MqttClient_Close();
iHadConnectError++;
return 22;
}
if (mosquitto_subscribe(mosq, NULL, mqtt_topic_motor2_power, 0) != MOSQ_ERR_SUCCESS)
{
printf("mosquitto_subscribe(mqtt_topic_motor2_power) fehlgeschlagen!\n");
MqttClient_Close();
iHadConnectError++;
return 23;
}
printf("MQTT: Connected successfull!\n");
return 0;
}
void MqttClient_Close()
{
//Clean up/destroy objects created by libmosquitto
mosquitto_destroy(mosq);
mosquitto_lib_cleanup();
printf("MQTT: Disconnected!\n");
}
void MqttClient_Refresher()
{
mosquitto_loop(mosq, 0, 1);
}
void MqttClient_Publisher()
{
char message[100];
snprintf(message, sizeof(message), "%ld", pinfo.cyclecounter);
mosquitto_publish(mosq, NULL, mqtt_topic_status_cyclecounter, strlen(message), message, 0, false);
}
void MqttClient_Publish_MotorGear(int iMotorIndex, int iGear)
{
if (iMotorIndex == 0)
{
if (iGear != iMqttMotor1Gear)
{
iMqttMotor1Gear = iGear;
char message[100];
snprintf(message, sizeof(message), "%d", iGear);
mosquitto_publish(mosq, NULL, mqtt_topic_motor1_gear, strlen(message), message, 0, false);
}
}
else if (iMotorIndex == 1)
{
if (iGear != iMqttMotor2Gear)
{
iMqttMotor2Gear = iGear;
char message[100];
snprintf(message, sizeof(message), "%d", iGear);
mosquitto_publish(mosq, NULL, mqtt_topic_motor2_gear, strlen(message), message, 0, false);
}
}
}
void MqttClient_Publish_MotorPower(int iMotorIndex, int iPower)
{
if (iMotorIndex == 0)
{
if (iPower != iMqttMotor1Power)
{
iMqttMotor1Power = iPower;
char message[100];
snprintf(message, sizeof(message), "%d", iPower);
mosquitto_publish(mosq, NULL, mqtt_topic_motor1_power, strlen(message), message, 0, false);
}
}
else if (iMotorIndex == 1)
{
if (iPower != iMqttMotor2Power)
{
iMqttMotor2Power = iPower;
char message[100];
snprintf(message, sizeof(message), "%d", iPower);
mosquitto_publish(mosq, NULL, mqtt_topic_motor2_power, strlen(message), message, 0, false);
}
}
}

13
mqtt/mqtt_client.h Executable file
View File

@@ -0,0 +1,13 @@
#if !defined(__MQTT_CLIENT_H__)
#define __MQTT_CLIENT_H__
#define USE_MOSQUITTO_LIB
int MqttClient_Connect();
void MqttClient_Close();
void MqttClient_Refresher();
void MqttClient_Publisher();
void MqttClient_Publish_MotorGear(int iMotorIndex, int iGear);
void MqttClient_Publish_MotorPower(int iMotorIndex, int iPower);
#endif

23
scripts/CanRtDriver.service Executable file
View File

@@ -0,0 +1,23 @@
## Service Definition
## Datei gehört in /etc/systemd/system
## aktivieren dann mit:
## - sudo systemctl daemon-reload
## - sudo systemctl enable CanRtDriver.service
## - sudo systemctl start CanRtDriver.service
[Unit]
Description=CAN-Bus Treiber für Gegenstromanlage
After=network.target
[Service]
ExecStart=/home/veltmann/Documents/CanRtDriver/bin/CanRtDriver
# Oder: ExecStart=/usr/bin/python3 /home/pi/meinprogramm.py
WorkingDirectory=/home/veltmann/Documents/CanRtDriver/bin
StandardOutput=inherit
StandardError=inherit
Restart=always
#User=veltmann
# Optional: Group=meinbenutzergruppe
[Install]
WantedBy=multi-user.target

7
scripts/can_link_up.sh Executable file
View File

@@ -0,0 +1,7 @@
#!/bin/bash
ip link set can0 up type can bitrate 250000
ip link set can1 up type can bitrate 250000
ifconfig can0 txqueuelen 65536
ifconfig can1 txqueuelen 65536

2
scripts/candump.sh Executable file
View File

@@ -0,0 +1,2 @@
candump can1