mirrors/ugv_sdk
1
0
Fork 0
mirror of https://github.com/westonrobot/ugv_sdk synced 2023-04-08 06:32:14 +08:00
Code Issues Packages Projects Releases Wiki Activity

added time stamp to robot states

Browse Source
This commit is contained in:
Ruixiang Du
2021-07-15 15:25:49 +08:00
parent e96b078c05
commit f74ee40fc3
32 changed files with 248 additions and 328 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/protocol_v1/agilex_msg_parser_v1.c
View File

@@ -18,7 +18,7 @@ bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessage *msg) {
// if checksum not correct
if (!CalcCanFrameChecksumV1(rx_frame->can_id, (uint8_t *)rx_frame->data,
rx_frame->can_dlc)) {
printf("wrong checksum for id: %x-------------->\n", rx_frame->can_id);
// printf("wrong checksum for id: %x-------------->\n", rx_frame->can_id);
return false;
}

10
src/protocol_v2/agilex_msg_parser_v2.c
View File

@@ -182,11 +182,11 @@ bool DecodeCanFrameV2(const struct can_frame *rx_frame, AgxMessage *msg) {
((uint16_t)(frame->driver_voltage.low_byte) |
(uint16_t)(frame->driver_voltage.high_byte) << 8) *
0.1;
msg->body.actuator_ls_state_msg.driver_temperature =
(int16_t)((uint16_t)(frame->driver_temperature.low_byte) |
(uint16_t)(frame->driver_temperature.high_byte) << 8);
msg->body.actuator_ls_state_msg.motor_temperature =
frame->motor_temperature;
msg->body.actuator_ls_state_msg.driver_temp =
(int16_t)((uint16_t)(frame->driver_temp.low_byte) |
(uint16_t)(frame->driver_temp.high_byte) << 8);
msg->body.actuator_ls_state_msg.motor_temp =
frame->motor_temp;
msg->body.actuator_ls_state_msg.driver_state = frame->driver_state;
break;
}

4
src/protocol_v2/agilex_protocol_v2.h
View File

@@ -242,8 +242,8 @@ typedef struct {
typedef struct {
struct16_t driver_voltage;
struct16_t driver_temperature;
int8_t motor_temperature;
struct16_t driver_temp;
int8_t motor_temp;
uint8_t driver_state;
uint8_t reserved0;
uint8_t reserved1;

55
src/utilities/protocol_detector.cpp Normal file
View File

@@ -0,0 +1,55 @@
/*
* protocol_detector.cpp
*
* Created on: Jul 15, 2021 14:05
* Description:
*
* Copyright (c) 2021 Weston Robot Pte. Ltd.
*/
#include "ugv_sdk/utilities/protocol_detector.hpp"
#include "ugv_sdk/utilities/stopwatch.hpp"
namespace westonrobot {
void ProtocolDectctor::Connect(std::string can_name) {
can_ = std::make_shared<AsyncCAN>(can_name);
can_->SetReceiveCallback(
std::bind(&ProtocolDectctor::ParseCANFrame, this, std::placeholders::_1));
can_->StartListening();
}
void ProtocolDectctor::Connect(std::string uart_name, uint32_t baudrate) {}
ProtocolVersion ProtocolDectctor::DetectProtocolVersion(uint32_t timeout_sec) {
msg_v1_detected_ = false;
msg_v2_detected_ = false;
StopWatch sw;
Timer timer;
while (sw.stoc() < timeout_sec) {
timer.reset();
if (msg_v1_detected_ || msg_v2_detected_) break;
timer.sleep_until_ms(50);
}
// make sure only one version is detected
if (msg_v1_detected_ && msg_v2_detected_) return ProtocolVersion::UNKONWN;
if (msg_v1_detected_)
return ProtocolVersion::AGX_V1;
else if (msg_v2_detected_)
return ProtocolVersion::AGX_V2;
return ProtocolVersion::UNKONWN;
};
void ProtocolDectctor::ParseCANFrame(can_frame *rx_frame) {
// state feedback frame with id 0x151 is unique to V1 protocol
if (rx_frame->can_id == 0x151) {
msg_v1_detected_ = true;
}
// rc state frame with id 0x241 is unique to V2 protocol
else if (rx_frame->can_id == 0x241) {
msg_v2_detected_ = true;
}
}
} // namespace westonrobot