From f91aee06c7474f987a5c66a2f41622bcfe5bc49f Mon Sep 17 00:00:00 2001 From: Bernhard Date: Mon, 19 Jan 2026 19:35:41 +0100 Subject: [PATCH] Korrektur bei Leistungsvorgabe in Schritten --- can/can_client.c | 221 +++++++++++++++++++++++++++-------------------- 1 file changed, 125 insertions(+), 96 deletions(-) diff --git a/can/can_client.c b/can/can_client.c index f9697f7..8f3e962 100755 --- a/can/can_client.c +++ b/can/can_client.c @@ -5,6 +5,8 @@ #include #include +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; + } } } }