You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
76 lines
2.4 KiB
76 lines
2.4 KiB
2 days ago
|
/*
|
||
|
|
* msp_Force_Sensor.c
|
||
|
|
*
|
||
|
|
* Created on: Oct 8, 2024
|
||
|
|
* Author: akeguo
|
||
|
|
*/
|
||
|
|
#include "msp_Dynamometer_Sensor.h"
|
||
|
|
#include "bsp_Error.pb.h"
|
||
|
|
#include "BHBF_ROBOT.h"
|
||
|
|
|
||
|
|
|
||
|
|
void decode_Dynamometer(uint8_t *buffer, uint16_t length);
|
||
|
|
void GF_DS_Inquiry();//inqure data from the, in fact this is a modbus 03 function;
|
||
|
|
|
||
|
|
int32_t* DynamometerValue_in;
|
||
|
|
struct UARTHandler *Dynamometer_sensor;
|
||
|
|
|
||
|
|
DispacherController *Dynamometer_sensor_dispacherController;
|
||
|
|
void Dynamometer_sensor_intialize(struct UARTHandler *Handler)
|
||
|
|
{
|
||
|
|
//uartHandler_intialize(&Force_sensor,Handler,10);
|
||
|
|
Dynamometer_sensor = Handler;
|
||
|
|
Dynamometer_sensor->UART_Decode=decode_Dynamometer;
|
||
|
|
Dynamometer_sensor->Wait_time = 10; //等待10ms 最低不要低于4；
|
||
|
|
Dynamometer_sensor_dispacherController=Handler->dispacherController;
|
||
|
|
Dynamometer_sensor_dispacherController->DispacherCallTime = 50;//10 是100ms 难道这个2ms的定时器是10ms的？
|
||
|
|
LOG("Dynamometer_encoder_intialize");
|
||
|
|
|
||
|
|
//log_info("angle_encoder_intialize");
|
||
|
|
Dynamometer_sensor_dispacherController->Add_Dispatcher_List(Dynamometer_sensor_dispacherController,GF_DS_Inquiry);
|
||
|
|
Dynamometer_sensor_dispacherController->Dispacher_Enable = 1;
|
||
|
|
|
||
|
|
HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"Dynamometer_sensor",0,Dynamometer_sensor);
|
||
|
|
//uartHandler->Insert_HardWare_Entry_UART
|
||
|
|
LOG("Dynamometer_intialize");
|
||
|
|
}
|
||
|
|
|
||
|
|
uint8_t Inquiry_Order_Dynamometer[8]={0X01, 0X03, 0X00, 0X04, 0X00, 0X01, 0XC5, 0XCB}; //01 03 00 04 00 01 C5 CB
|
||
|
|
void GF_DS_Inquiry()
|
||
|
|
{
|
||
|
|
|
||
|
|
MB_ReadHoldingReg(&Dynamometer_sensor->Tx_Buf,
|
||
|
|
&Dynamometer_sensor->TxCount, 1, 0x0004, 1); //03 command ; read 2 registers
|
||
|
|
|
||
|
|
Dynamometer_sensor->UART_Tx(Dynamometer_sensor);//send the data from the modbus command;
|
||
|
|
LOG("GF_DS_Inquiry Second Version");
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
void decode_Dynamometer(uint8_t *buffer, uint16_t length)
|
||
|
|
{
|
||
|
|
uint8_t data[20];
|
||
|
|
memcpy(data,buffer,length);
|
||
|
|
LOG("start decoding and the length is %d",length);
|
||
|
|
/* CRC 校验 */
|
||
|
|
uint16_t crc_check = ((buffer[length - 1] << 8) | buffer[length - 2]);
|
||
|
|
/* CRC 校验正确 */
|
||
|
|
if (crc_check == MB_CRC16(buffer, length - 2))
|
||
|
|
{
|
||
|
|
*DynamometerValue_in = (int32_t)((int32_t)buffer[3]<<8|buffer[4]);
|
||
|
|
|
||
|
|
HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"Dynamometer_sensor",1);
|
||
|
|
LOG("decode_force succeeded and the force is %d",*DynamometerValue_in);
|
||
|
|
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
//Decode Error；
|
||
|
|
//log_error("wire sensor decoding failed");
|
||
|
|
LOGFF(DL_ERROR,"Dynamometer_sensor decoding failed");
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|