drone_project/thread_3.c at main · vbartu/drone_project · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
#include <stdio.h>
#include <stdbool.h>

#include "main.h"
#include "lib/angles.h"
#include "lib/uart.h"
#include "lib/timer.h"
#include "lib/communication.h"


#define LOOP_PERIOD 100000 // us

/** Function prototypes ----------------------------------------------------- */
static void rx_callback(uint8_t msg_len, uint8_t* msg);

/** Function definitions ---------------------------------------------------- */
static void rx_callback(uint8_t msg_len, uint8_t* msg) {
	switch (msg[0]) { // opcode
	case COMM_OP_GET_PID_VALUES:
		{
			pid_instance_t* pid_ins;
			if (msg[1] == COMM_PID_PITCH) pid_ins = &pitch_pid;
			else if (msg[1] == COMM_PID_ROLL) pid_ins = &roll_pid;
			else if (msg[1] == COMM_PID_YAW) pid_ins = &yaw_pid;
			else if (msg[1] == COMM_PID_PITCH_VEL) pid_ins = &pitch_vel_pid;
			else if (msg[1] == COMM_PID_ROLL_VEL) pid_ins = &roll_vel_pid;
			else if (msg[1] == COMM_PID_YAW_VEL) pid_ins = &yaw_vel_pid;

			comm_pid_values_t pid_values;
			pid_values.opcode = COMM_OP_RSP_GET_PID_VALUES;
			pid_values.pid_id = msg[1];
			pthread_mutex_lock(&pid_mtx);
			pid_values.kp = pid_ins->kp*100;
			pid_values.ki = pid_ins->ki*100;
			pid_values.kd = pid_ins->kd*100;
			pthread_mutex_unlock(&pid_mtx);
			comm_send_msg(sizeof(pid_values), (uint8_t*)&pid_values);
			break;
		}
	case COMM_OP_SET_PID_VALUES:
		{
			pid_instance_t* pid_ins;
			if (msg[1] == COMM_PID_PITCH) pid_ins = &pitch_pid;
			else if (msg[1] == COMM_PID_ROLL) pid_ins = &roll_pid;
			else if (msg[1] == COMM_PID_YAW) pid_ins = &yaw_pid;
			else if (msg[1] == COMM_PID_PITCH_VEL) pid_ins = &pitch_vel_pid;
			else if (msg[1] == COMM_PID_ROLL_VEL) pid_ins = &roll_vel_pid;
			else if (msg[1] == COMM_PID_YAW_VEL) pid_ins = &yaw_vel_pid;

			comm_pid_values_t* pid_values = (comm_pid_values_t*)msg;
			pthread_mutex_lock(&pid_mtx);
			pid_ins->kp = pid_values->kp / 100;
			pid_ins->ki = pid_values->ki / 100;
			pid_ins->kd = pid_values->kd / 100;
			pthread_mutex_unlock(&pid_mtx);
			uint8_t rsp[] = {COMM_OP_RSP_SET_PID_VALUES, msg[1]};
			comm_send_msg(sizeof(rsp), rsp);
			break;
		}
	default:
		break;
	}
}

/** Public functions -------------------------------------------------------- */
void* thread_3_main(void* args)
{
	pthread_mutex_lock(&print_mtx);
	printf("Thread 3\n");
	pthread_mutex_unlock(&print_mtx);

	comm_init(rx_callback);

	uint64_t next_time = micros();
	while (true) {
		comm_process();

		if (next_time < micros()) {
			angles_t a = get_angles();
			comm_data_t data;
			data.opcode = COMM_OP_RSP_ANGLE_VALUES;
			data.x = a.pitch*100;
			data.y = a.roll*100;
			data.z = a.yaw*100;
			comm_send_msg(sizeof(data), (uint8_t*)&data);
			next_time += LOOP_PERIOD;
		}
	}
}