Skip to content

Commit 24636ea

Browse files
zhora15zhora15
committed
add fdcan
1 parent 09e54fd commit 24636ea

14 files changed

Lines changed: 306 additions & 959 deletions

File tree

‎firmware/src/application/Protocol/Protocol.cpp‎

Lines changed: 33 additions & 43 deletions
Original file line numberDiff line numberDiff line change
@@ -3,22 +3,15 @@
33
#include "ServoController.h"
44
#include "SensorPolling.h"
55

6-
TaskHandle_t Protocol::task_handle = NULL;
7-
TimerHandle_t Protocol::inform_timer_handle = NULL;
6+
Protocol& Protocol::getInstance() {
7+
static Protocol _instance;
8+
return _instance;
9+
}
810

9-
void motor_control_request_handle(
10-
const uint8_t* request_data, uint32_t request_data_size, uint8_t* response_data,
11-
uint32_t& response_data_size) {
12-
if (request_data_size != sizeof(protocol_request_motor)) {
13-
int32_t status = 0;
14-
protocol_request_motor request = *(protocol_request_motor*)request_data;
15-
// MotorControl& MT = MotorControl::getInstance();
16-
// status |= MT.enable(request.is_enable);
17-
// status |= MT.set_speed(request.target_speed);
18-
protocol_response_motor response = {.status = status};
19-
memcpy(response_data, &response, sizeof(protocol_response_motor));
20-
response_data_size = sizeof(protocol_response_motor);
21-
}
11+
void Protocol::on_new_request() {
12+
uint32_t task_notify = TASK_NOTIFY_RX_DATA_SIGNAL;
13+
BaseType_t task_woken = pdFALSE;
14+
xTaskNotifyFromISR(task_handle, task_notify, eSetBits, &task_woken);
2215
}
2316

2417
void servo_control_request_handle(
@@ -28,7 +21,6 @@ void servo_control_request_handle(
2821
int32_t status = 0;
2922
protocol_request_servo request = *(protocol_request_servo*)request_data;
3023
ServoController& SC = ServoController::getInstance();
31-
status |= SC.enable(request.is_enable);
3224
status |= SC.set_angle(request.left_servo_angle, request.right_servo_angle);
3325
protocol_response_servo response = {.status = status};
3426
memcpy(response_data, &response, sizeof(protocol_response_servo));
@@ -39,14 +31,12 @@ void servo_control_request_handle(
3931
void Protocol::init() {
4032
BaseType_t status = xTaskCreate(startTaskImpl, "protocol_task", 512, this, 3, &task_handle);
4133
if (status == pdTRUE) {
42-
bind(MOTOR_TAG, motor_control_request_handle);
43-
bind(SERVO_TAG, servo_control_request_handle);
44-
inform_timer_handle = xTimerCreate(
45-
"inform_tim", pdMS_TO_TICKS(20), pdTRUE, NULL, Protocol::send_inform_callback);
34+
bind(SERVO_TARGET_ANGLES_MSG_TAG, servo_control_request_handle);
35+
4636
communication_unit.set_rx_complete_callback(Protocol::on_new_request);
4737
communication_unit.init();
38+
4839
vTaskResume(task_handle);
49-
xTimerStart(inform_timer_handle, 10);
5040
} else {
5141
printf("Error сreating protocol task\n");
5242
}
@@ -68,11 +58,7 @@ void Protocol::task() {
6858
if ((tag >= 0) && (tag < CALLBACKS_MAX_NUM) && (request_callback[tag]) != nullptr) {
6959
request_callback[tag](
7060
request_data, request_data_size, response_data, response_data_size);
71-
communication_unit.transmit(
72-
tag + RESPONSE_BASE_ID_OFFSET, response_data, response_data_size);
7361
}
74-
} else if (task_notify == TASK_NOTIFY_INFORM_SIGNAL) {
75-
transmit_inform_messages();
7662
}
7763
task_notify = 0;
7864
}
@@ -85,23 +71,27 @@ void Protocol::bind(uint32_t tag, request_callback_t callback) {
8571
}
8672
}
8773

88-
void Protocol::transmit_inform_messages() {
89-
protocol_inform_imu_data imu_data;
90-
protocol_inform_motion_data move_data;
91-
// static MotorControl &MT = MotorControl::getInstance();
92-
static SensorPolling& SP = SensorPolling::getInstance();
93-
94-
int32_t status = 0;
95-
96-
status |= SP.get_magn_data(imu_data.magn_x_axis, imu_data.magn_y_axis, imu_data.magn_z_axis);
97-
imu_data.data_updated = (status == 0);
98-
communication_unit.transmit(
99-
IMU_TAG + INFORM_BASE_ID_OFFSET, (uint8_t*)&imu_data, sizeof(imu_data));
74+
void Protocol::transmit_encoders_speed(float left_fr_speed, float right_fr_speed,
75+
float left_rear_speed, float right_rear_speed) {
76+
uint8_t msg_buff[16] = {0};
77+
memcpy(&(msg_buff[0]), &left_fr_speed, sizeof(left_fr_speed));
78+
memcpy(&(msg_buff[4]), &right_fr_speed, sizeof(right_fr_speed));
79+
memcpy(&(msg_buff[8]), &left_rear_speed, sizeof(left_rear_speed));
80+
memcpy(&(msg_buff[12]), &right_rear_speed, sizeof(right_rear_speed));
81+
communication_unit.transmit(ENCODERS_SPEED_MSG_TAG, msg_buff, 16);
82+
}
10083

101-
// move_data.motor_velocity = MT.get_speed();
102-
move_data.front_left_wheel_velocity = 0.0f;
103-
move_data.front_right_wheel_velocity = 0.0f;
104-
move_data.data_updated = true;
105-
communication_unit.transmit(
106-
MOTION_TAG + INFORM_BASE_ID_OFFSET, (uint8_t*)&move_data, sizeof(move_data));
84+
void Protocol::transmit_servo_angles(float left_angle, float right_angle) {
85+
uint8_t msg_buff[8] = {0};
86+
memcpy(&(msg_buff[0]), &left_angle, sizeof(left_angle));
87+
memcpy(&(msg_buff[4]), &right_angle, sizeof(right_angle));
88+
communication_unit.transmit(SERVO_MEASURED_ANGLES_MSG_TAG, msg_buff, 8);
10789
}
90+
91+
void Protocol::transmit_magn_data(uint16_t x_axis, uint16_t y_axis, uint16_t z_axis) {
92+
uint8_t msg_buff[6] = {0};
93+
memcpy(&(msg_buff[0]), &x_axis, sizeof(x_axis));
94+
memcpy(&(msg_buff[2]), &y_axis, sizeof(y_axis));
95+
memcpy(&(msg_buff[4]), &z_axis, sizeof(z_axis));
96+
communication_unit.transmit(MAGN_Z_MSG_TAG, msg_buff, 6);
97+
}

‎firmware/src/application/Protocol/include/Protocol.h‎

Lines changed: 52 additions & 112 deletions
Original file line numberDiff line numberDiff line change
@@ -1,14 +1,15 @@
11
#ifndef TRUCK_HW_APPLICATION_PROTOCOL_INCLUDE_PROTOCOL_H_
22
#define TRUCK_HW_APPLICATION_PROTOCOL_INCLUDE_PROTOCOL_H_
3+
34
#include <cstdint>
45
#include <cstring>
6+
#include <array>
57
#include "FreeRTOS.h"
68
#include "task.h"
79
#include "timers.h"
810
#include "fdcan.h"
911

1012
#define TASK_NOTIFY_RX_DATA_SIGNAL (uint32_t)(0x01)
11-
#define TASK_NOTIFY_INFORM_SIGNAL (uint32_t)(0x02)
1213

1314
#pragma pack(push, 1)
1415
typedef struct {
@@ -64,135 +65,74 @@ typedef struct {
6465
#define CALLBACKS_MAX_NUM 20
6566
#define REQUEST_BASE_ID_OFFSET 0
6667
#define INFORM_BASE_ID_OFFSET CALLBACKS_MAX_NUM
67-
#define RESPONSE_BASE_ID_OFFSET (CALLBACKS_MAX_NUM*2)
68-
69-
#define CONFIG_TAG 1
70-
#define MOTOR_TAG 2
71-
#define SERVO_TAG 3
72-
73-
#define IMU_TAG 1
74-
#define MOTION_TAG 2
7568

76-
#define CONFIG_RESPONSE_ID (RESPONSE_BASE_ID_OFFSET + CONFIG_TAG)
77-
#define CONFIG_REQUEST_ID (REQUEST_BASE_ID_OFFSET + CONFIG_TAG)
69+
#define SERVO_TARGET_ANGLES_MSG_TAG (REQUEST_BASE_ID_OFFSET + 1)
7870

79-
#define MOTOR_RESPONSE_ID (RESPONSE_BASE_ID_OFFSET + MOTOR_TAG)
80-
#define MOTOR_REQUEST_ID (REQUEST_BASE_ID_OFFSET + MOTOR_TAG)
81-
82-
#define SERVO_RESPONSE_ID (RESPONSE_BASE_ID_OFFSET + SERVO_TAG)
83-
#define SERVO_REQUEST_ID (REQUEST_BASE_ID_OFFSET + SERVO_TAG)
71+
#define ENCODERS_SPEED_MSG_TAG (INFORM_BASE_ID_OFFSET + 1)
72+
#define SERVO_MEASURED_ANGLES_MSG_TAG (INFORM_BASE_ID_OFFSET + 2)
73+
#define MAGN_Z_MSG_TAG (INFORM_BASE_ID_OFFSET + 3)
8474

8575
typedef void(*request_callback_t)(const uint8_t *request_data, uint32_t request_data_size,
8676
uint8_t *response_data, uint32_t &response_data_size);
8777

88-
//class Message {
89-
// protected:
90-
// uint32_t id;
91-
// uint32_t len = 0;
92-
// bool is_enable;
93-
// uint32_t transmit_period_ms;
94-
// uint32_t last_transmit_ms;
95-
//
96-
// public:
97-
// bool need_to_transmit() {
98-
// bool is_needed = false;
99-
// if (is_enable && (xTaskGetTickCount() - last_transmit_ms) > transmit_period_ms) {
100-
// is_needed = true;
101-
// }
102-
// return is_needed;
103-
// }
104-
// virtual int32_t get_message_content(uint8_t *data, float motor_speed) { return -1; };
105-
// virtual int32_t get_message_content(uint8_t *data, float x, float y, float z) { return -1; };
106-
// virtual int32_t get_message_content(uint8_t *data) { return -1; };
107-
// virtual int32_t handle_request(uint8_t* data, uint32_t data_size) { return -1; };
108-
//};
109-
//
110-
//class IMU_Sensor_Message : Message {
111-
// private:
112-
//#pragma pack(push, 1)
113-
// typedef struct {
114-
// float x_axis;
115-
// float y_axis;
116-
// float z_axis;
117-
// } protocol_inform_imu;
118-
//#pragma pack(pop)
119-
//
120-
// public:
121-
// int32_t get_message_content(uint8_t *data, float x, float y, float z) override {
122-
//
123-
// auto elem = protocol_inform_imu{.x_axis = x, .y_axis = y, .z_axis = z};
124-
// if (data == nullptr) {
125-
// return -3;
126-
// }
127-
// memcpy(data, &elem, len);
128-
// return len;
129-
// }
130-
//};
131-
//
132-
//class Motor_Message : Message {
133-
// private:
134-
//#pragma pack(push, 1)
135-
// typedef struct {
136-
// float motor_speed;
137-
// } protocol_inform_motor;
138-
//#pragma pack(pop)
139-
// public:
140-
// int32_t get_message_content(uint8_t *data, float motor_speed) override {
141-
// auto elem = protocol_inform_motor{.motor_speed = motor_speed};
142-
// if (data == nullptr) {
143-
// return -2;
144-
// }
145-
// memcpy(data, &elem, len);
146-
// return len;
147-
// }
148-
//};
149-
150-
//class Config_Request : Message {
151-
// private:
152-
//#pragma pack(push, 1)
153-
// typedef struct {
154-
// float motor_speed;
155-
// } protocol_inform_motor;
156-
//#pragma pack(pop)
157-
// public:
158-
// int32_t handle_request(uint8_t *data, float motor_speed) override {
159-
// auto elem = protocol_inform_motor{.motor_speed = motor_speed};
160-
// if (data == nullptr) {
161-
// return -2;
162-
// }
163-
// memcpy(data, &elem, len);
164-
// return len;
165-
// }
166-
//};
167-
16878
class Protocol {
16979
private:
170-
FDCAN communication_unit;
80+
#pragma pack(push, 1)
81+
typedef struct {
82+
uint8_t left_angle;
83+
uint8_t right_angle;
84+
} request_target_servo_angles;
85+
#pragma pack(pop)
86+
87+
#pragma pack(push, 1)
88+
typedef struct {
89+
float left_angle;
90+
float right_angle;
91+
} telemetry_servo_measured_angles;
92+
#pragma pack(pop)
93+
94+
#pragma pack(push, 1)
95+
typedef struct {
96+
float left_front_speed;
97+
float right_front_speed;
98+
} telemetry_encoders_speed;
99+
#pragma pack(pop)
100+
101+
#pragma pack(push, 1)
102+
typedef struct {
103+
uint16_t magn_x;
104+
uint16_t magn_y;
105+
uint16_t magn_z;
106+
} telemetry_magnitometer_data;
107+
#pragma pack(pop)
108+
109+
FDCAN& communication_unit;
171110
static TaskHandle_t task_handle;
172-
static TimerHandle_t inform_timer_handle;
111+
173112
void task();
174-
request_callback_t request_callback[CALLBACKS_MAX_NUM] = {nullptr};
175113
static void startTaskImpl(void *_this) {
176114
((Protocol *) _this)->task();
177115
}
116+
117+
std::array<request_callback_t, CALLBACKS_MAX_NUM> request_callback;
178118
void bind(uint32_t tag, request_callback_t callback);
179119

180-
void transmit_inform_messages();
181-
static void send_inform_callback(TimerHandle_t timer_handle) {
182-
if (timer_handle != NULL) {
183-
uint32_t task_notify = TASK_NOTIFY_INFORM_SIGNAL;
184-
BaseType_t task_woken = pdFALSE;
185-
xTaskNotifyFromISR(task_handle, task_notify, eSetBits, &task_woken);
186-
}
187-
}
120+
static void on_new_request();
121+
122+
Protocol() : communication_unit(FDCAN::getInstance()) {
123+
request_callback.fill(nullptr);
124+
};
125+
~Protocol() {};
126+
Protocol(const Protocol &obj) = delete;
127+
Protocol &operator=(const Protocol &obj) = delete;
188128

189-
static void on_new_request() {
190-
uint32_t task_notify = TASK_NOTIFY_RX_DATA_SIGNAL;
191-
BaseType_t task_woken = pdFALSE;
192-
xTaskNotifyFromISR(task_handle, task_notify, eSetBits, &task_woken);
193-
}
194129
public:
130+
static Protocol &getInstance();
195131
void init();
196132
void start();
133+
void transmit_encoders_speed(float left_fr_speed, float right_fr_speed,
134+
float left_rear_speed, float right_rear_speed);
135+
void transmit_servo_angles(float left_angle, float right_angle);
136+
void transmit_magn_data(uint16_t x_axis, uint16_t y_axis, uint16_t z_axis);
197137
};
198138
#endif //TRUCK_HW_APPLICATION_PROTOCOL_INCLUDE_PROTOCOL_H_

0 commit comments

Comments
 (0)