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.
102 lines
3.1 KiB
102 lines
3.1 KiB
/*
|
|
* msp_UltrasonicSensor.c
|
|
*
|
|
* Created on: Oct 8, 2024
|
|
* Author: akeguo
|
|
*/
|
|
#include "msp_Ultrasonic_Sensor.h"
|
|
#include "bsp_Error.pb.h"
|
|
#include "BHBF_ROBOT.h"
|
|
|
|
void decode_UltrasonicSensor(uint8_t *buffer, uint16_t length);
|
|
void GF_US_Inquiry(); //inqure data from the, in fact this is a modbus 03 function;
|
|
|
|
int32_t *USValue_1;
|
|
int32_t *USValue_2;
|
|
|
|
int32_t USValue_in1;
|
|
int32_t USValue_in2;
|
|
|
|
struct UARTHandler *Ultrasonic_sensor;
|
|
|
|
DispacherController *Ultrasonic_sensor_dispacherController;
|
|
void Ultrasonic_sensor_intialize(struct UARTHandler *Handler)
|
|
{
|
|
//uartHandler_intialize(&Force_sensor,Handler,10);
|
|
Ultrasonic_sensor = Handler;
|
|
Ultrasonic_sensor_dispacherController = Handler->dispacherController;
|
|
|
|
LOG("Ultrasonic_sensor_encoder_intialize");
|
|
|
|
//log_info("angle_encoder_intialize");
|
|
Ultrasonic_sensor_dispacherController->Add_Dispatcher_List(
|
|
Ultrasonic_sensor_dispacherController, GF_US_Inquiry);
|
|
|
|
HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,
|
|
"Ultrasonic_sensor", 0, ComError_Ultrasonic_sensor);
|
|
//uartHandler->Insert_HardWare_Entry_UART
|
|
|
|
USValue_1 = &USValue_in1;
|
|
USValue_2 = &USValue_in2;
|
|
|
|
LOG("Ultrasonic_sensor_intialize");
|
|
}
|
|
|
|
uint8_t Inquiry_Order_1[8] =
|
|
{ 0X01, 0X04, 0X00, 0X00, 0X00, 0X01, 0X31, 0XCA }; //01 04 00 00 00 01 31 CA
|
|
uint8_t Inquiry_Order_2[8] =
|
|
{ 0X02, 0X04, 0X00, 0X00, 0X00, 0X01, 0X31, 0XF9 }; //02 04 00 00 00 01 31 F9
|
|
|
|
void GF_US_Inquiry()
|
|
{
|
|
|
|
MB_ReadInputReg(&Ultrasonic_sensor->Tx_Buf, &Ultrasonic_sensor->TxCount, 1,
|
|
0x0000, 1); //03 command ; read 2 registers
|
|
|
|
Ultrasonic_sensor->UART_Tx(Ultrasonic_sensor); //send the data from the modbus command;
|
|
LOG("Ultrasonic_sensor_1 Version");
|
|
Ultrasonic_sensor->AddSendList(Ultrasonic_sensor, Ultrasonic_sensor->Tx_Buf,
|
|
Ultrasonic_sensor->TxCount, OneLineWaitTime,
|
|
decode_UltrasonicSensor);
|
|
|
|
MB_ReadInputReg(&Ultrasonic_sensor->Tx_Buf, &Ultrasonic_sensor->TxCount, 2,
|
|
0x0000, 1); //03 command ; read 2 registers
|
|
|
|
Ultrasonic_sensor->AddSendList(Ultrasonic_sensor, Ultrasonic_sensor->Tx_Buf,
|
|
Ultrasonic_sensor->TxCount, OneLineWaitTime,
|
|
decode_UltrasonicSensor);
|
|
LOG("Ultrasonic_sensor_2 Version");
|
|
|
|
}
|
|
|
|
|
|
|
|
void decode_UltrasonicSensor(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 (buffer[0] == 1 && crc_check == MB_CRC16(buffer, length - 2))
|
|
{
|
|
USValue_in1 = (int32_t) ((int32_t) buffer[3] << 8 | buffer[4]);
|
|
HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
|
|
"Ultrasonic_sensor", 1);
|
|
LOG("decode_force succeeded and the force is %d", *USValue_1);
|
|
} else if (buffer[0] == 2 && crc_check == MB_CRC16(buffer, length - 2))
|
|
{
|
|
USValue_in2 = (int32_t) ((int32_t) buffer[3] << 8 | buffer[4]);
|
|
HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
|
|
"Ultrasonic_sensor", 1);
|
|
LOG("decode_force succeeded and the force is %d", *USValue_2);
|
|
} else
|
|
{
|
|
//Decode Error；
|
|
//log_error("wire sensor decoding failed");
|
|
LOGFF(DL_ERROR, "Ultrasonic_sensor decoding failed");
|
|
|
|
}
|
|
|
|
}
|
|
|