Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
actuators_t4_uart.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2024 The Paparazzi Team
3 *
4 * This file is part of paparazzi
5 *
6 * paparazzi is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2, or (at your option)
9 * any later version.
10 *
11 * paparazzi is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with paparazzi; see the file COPYING. If not, see
18 * <http://www.gnu.org/licenses/>.
19 */
26
27#include "pprzlink/pprz_transport.h"
28#include "mcu_periph/uart.h"
29#include "mcu_periph/sys_time.h"
30#include <time.h>
31#include <sys/time.h>
32#include "modules/core/abi.h"
34
35/* Variables for outbound packet */
39float actuators_t4_extra_data_out[255]__attribute__((aligned));
40
41/* Variables for outbound packet */
43float actuators_t4_extra_data_in[255]__attribute__((aligned));
44
50
51static uint8_t actuators_t4_msg_buf_in[sizeof(struct ActuatorsT4In)*2]__attribute__((aligned));
52
53
54#if PERIODIC_TELEMETRY
55
57
58static void actuators_t4_downlink(struct transport_tx *trans, struct link_device *dev) {
59 int16_t esc_1_rpm_telemetry = actuators_t4_in.esc_1_rpm;
60 int16_t esc_2_rpm_telemetry = actuators_t4_in.esc_2_rpm;
61 int16_t esc_3_rpm_telemetry = actuators_t4_in.esc_3_rpm;
62 int16_t esc_4_rpm_telemetry = actuators_t4_in.esc_4_rpm;
63
64 int16_t esc_1_error_code_telemetry = actuators_t4_in.esc_1_error_code;
65 int16_t esc_2_error_code_telemetry = actuators_t4_in.esc_2_error_code;
66 int16_t esc_3_error_code_telemetry = actuators_t4_in.esc_3_error_code;
67 int16_t esc_4_error_code_telemetry = actuators_t4_in.esc_4_error_code;
68
69 int16_t esc_1_current_telemetry = actuators_t4_in.esc_1_current;
70 int16_t esc_2_current_telemetry = actuators_t4_in.esc_2_current;
71 int16_t esc_3_current_telemetry = actuators_t4_in.esc_3_current;
72 int16_t esc_4_current_telemetry = actuators_t4_in.esc_4_current;
73
74 int16_t esc_1_voltage_telemetry = actuators_t4_in.esc_1_voltage;
75 int16_t esc_2_voltage_telemetry = actuators_t4_in.esc_2_voltage;
76 int16_t esc_3_voltage_telemetry = actuators_t4_in.esc_3_voltage;
77 int16_t esc_4_voltage_telemetry = actuators_t4_in.esc_4_voltage;
78
79 // Not used, and if so maybe to uint16_t in Kelvin not Celcius
80 //int16_t esc_1_temperature_telemetry = actuators_t4_in.esc_1_temperature;
81 //int16_t esc_2_temperature_telemetry = actuators_t4_in.esc_2_temperature;
82 //int16_t esc_3_temperature_telemetry = actuators_t4_in.esc_3_temperature;
83 //int16_t esc_4_temperature_telemetry = actuators_t4_in.esc_4_temperature;
84
85 int16_t servo_1_angle_telemetry = actuators_t4_in.servo_1_angle;
86 int16_t servo_2_angle_telemetry = actuators_t4_in.servo_2_angle;
87 int16_t servo_3_angle_telemetry = actuators_t4_in.servo_3_angle;
88 int16_t servo_4_angle_telemetry = actuators_t4_in.servo_4_angle;
89 int16_t servo_5_angle_telemetry = actuators_t4_in.servo_5_angle;
90 int16_t servo_6_angle_telemetry = actuators_t4_in.servo_6_angle;
91 int16_t servo_7_angle_telemetry = actuators_t4_in.servo_7_angle;
92 int16_t servo_8_angle_telemetry = actuators_t4_in.servo_8_angle;
93 int16_t servo_9_angle_telemetry = actuators_t4_in.servo_9_angle;
94 int16_t servo_10_angle_telemetry = actuators_t4_in.servo_10_angle;
95 int16_t servo_11_angle_telemetry = actuators_t4_in.servo_11_angle;
96 int16_t servo_12_angle_telemetry = actuators_t4_in.servo_12_angle;
97
98 int16_t servo_1_load_telemetry = actuators_t4_in.servo_1_load;
99 int16_t servo_2_load_telemetry = actuators_t4_in.servo_2_load;
100 int16_t servo_3_load_telemetry = actuators_t4_in.servo_3_load;
101 int16_t servo_4_load_telemetry = actuators_t4_in.servo_4_load;
102 int16_t servo_5_load_telemetry = actuators_t4_in.servo_5_load;
103 int16_t servo_6_load_telemetry = actuators_t4_in.servo_6_load;
104 int16_t servo_7_load_telemetry = actuators_t4_in.servo_7_load;
105 int16_t servo_8_load_telemetry = actuators_t4_in.servo_8_load;
106 int16_t servo_9_load_telemetry = actuators_t4_in.servo_9_load;
107 int16_t servo_10_load_telemetry = actuators_t4_in.servo_10_load;
108 uint16_t bitmask_servo_health_telemetry = actuators_t4_in.bitmask_servo_health;
109
110 float rolling_msg_in_telemetry = actuators_t4_in.rolling_msg_in;
111 uint8_t rolling_msg_in_id_telemetry = actuators_t4_in.rolling_msg_in_id;
112
113 pprz_msg_send_ACTUATORS_T4_IN(trans, dev, AC_ID,
114 &esc_1_rpm_telemetry, &esc_2_rpm_telemetry, &esc_3_rpm_telemetry, &esc_4_rpm_telemetry,
115 &servo_1_angle_telemetry, &servo_2_angle_telemetry, &servo_3_angle_telemetry, &servo_4_angle_telemetry,
116 &servo_5_angle_telemetry, &servo_6_angle_telemetry, &servo_7_angle_telemetry, &servo_8_angle_telemetry,
117 &servo_9_angle_telemetry, &servo_10_angle_telemetry, &servo_11_angle_telemetry, &servo_12_angle_telemetry,
119 &rolling_msg_in_telemetry, &rolling_msg_in_id_telemetry,
120 &esc_1_error_code_telemetry, &esc_2_error_code_telemetry, &esc_3_error_code_telemetry, &esc_4_error_code_telemetry,
121 &servo_1_load_telemetry, &servo_2_load_telemetry, &servo_3_load_telemetry, &servo_4_load_telemetry,
122 &servo_5_load_telemetry, &servo_6_load_telemetry, &servo_7_load_telemetry, &servo_8_load_telemetry,
123 &servo_9_load_telemetry, &servo_10_load_telemetry,
124 &bitmask_servo_health_telemetry,
125 &esc_1_current_telemetry, &esc_2_current_telemetry, &esc_3_current_telemetry, &esc_4_current_telemetry,
126 &esc_1_voltage_telemetry, &esc_2_voltage_telemetry, &esc_3_voltage_telemetry, &esc_4_voltage_telemetry);
127}
128
129 static void actuators_t4_uplink(struct transport_tx *trans, struct link_device *dev) {
130
131 uint8_t esc_arm_telemetry = actuators_t4_out.esc_arm;
132 uint16_t servo_arm_telemetry = actuators_t4_out.servo_arm;
133 int16_t esc_1_dshot_cmd_telemetry = actuators_t4_out.esc_1_dshot_cmd;
134 int16_t esc_2_dshot_cmd_telemetry = actuators_t4_out.esc_2_dshot_cmd;
135 int16_t esc_3_dshot_cmd_telemetry = actuators_t4_out.esc_3_dshot_cmd;
136 int16_t esc_4_dshot_cmd_telemetry = actuators_t4_out.esc_4_dshot_cmd;
137
138 int16_t servo_1_angle_cmd_telemetry = actuators_t4_out.servo_1_cmd;
139 int16_t servo_2_angle_cmd_telemetry = actuators_t4_out.servo_2_cmd;
140 int16_t servo_3_angle_cmd_telemetry = actuators_t4_out.servo_3_cmd;
141 int16_t servo_4_angle_cmd_telemetry = actuators_t4_out.servo_4_cmd;
142 int16_t servo_5_angle_cmd_telemetry = actuators_t4_out.servo_5_cmd;
143 int16_t servo_6_angle_cmd_telemetry = actuators_t4_out.servo_6_cmd;
144 int16_t servo_7_angle_cmd_telemetry = actuators_t4_out.servo_7_cmd;
145 int16_t servo_8_angle_cmd_telemetry = actuators_t4_out.servo_8_cmd;
146 int16_t servo_9_angle_cmd_telemetry = actuators_t4_out.servo_9_cmd;
147 int16_t servo_10_angle_cmd_telemetry = actuators_t4_out.servo_10_cmd;
148 int16_t servo_11_angle_cmd_telemetry = actuators_t4_out.servo_11_cmd;
149 int16_t servo_12_angle_cmd_telemetry = actuators_t4_out.servo_12_cmd;
150
151 float rolling_msg_out_telemetry = actuators_t4_out.rolling_msg_out;
152 uint8_t rolling_msg_out_id_telemetry = actuators_t4_out.rolling_msg_out_id;
153
154 pprz_msg_send_ACTUATORS_T4_OUT(trans, dev, AC_ID,
155 &esc_arm_telemetry, &servo_arm_telemetry,
156 &esc_1_dshot_cmd_telemetry, &esc_2_dshot_cmd_telemetry, &esc_3_dshot_cmd_telemetry, &esc_4_dshot_cmd_telemetry,
157 &servo_1_angle_cmd_telemetry, &servo_2_angle_cmd_telemetry, &servo_3_angle_cmd_telemetry, &servo_4_angle_cmd_telemetry,
158 &servo_5_angle_cmd_telemetry, &servo_6_angle_cmd_telemetry, &servo_7_angle_cmd_telemetry, &servo_8_angle_cmd_telemetry,
159 &servo_9_angle_cmd_telemetry, &servo_10_angle_cmd_telemetry, &servo_11_angle_cmd_telemetry, &servo_12_angle_cmd_telemetry,
160 &rolling_msg_out_telemetry, &rolling_msg_out_id_telemetry);
161
162 }
163
164#endif // PERIODIC_TELEMETRY
165
166static void data_actuators_t4_out(uint8_t sender_id __attribute__((unused)), struct ActuatorsT4Out * actuators_t4_out_ptr, float * actuators_t4_extra_data_out_ptr){
167
168 /* Copying the struct to be transmitted and the extra data in a local variable: */
169 memcpy(&actuators_t4_out,actuators_t4_out_ptr,sizeof(struct ActuatorsT4Out));
170 memcpy(&actuators_t4_extra_data_out,actuators_t4_extra_data_out_ptr, sizeof(actuators_t4_extra_data_out) );
171
172 /* Increase the counter to track the sending messages: */
174 actuators_t4_out.rolling_msg_out_id = actuators_t4_out_msg_id;
176 if(actuators_t4_out_msg_id == 255){
178 }
179
180 /* Send the message over UART to the T4 Actuators Board: */
181 uint8_t *buf_send = (uint8_t *)&actuators_t4_out;
182 //Calculating the checksum
183 uint8_t checksum_out_local = 0;
184 for(uint16_t i = 0; i < sizeof(struct ActuatorsT4Out) - 1; i++){
185 checksum_out_local += buf_send[i];
186 }
187 actuators_t4_out.checksum_out = checksum_out_local;
188
189#ifdef ACTUATORS_T4_SIM
190 /* don't send the data if it is SIM */
191 //TODO: why not, could be useful for HITL
192#else
193 /* Do send the bytes */
194 uart_put_byte(&(ACTUATORS_T4_PORT), 0, START_BYTE_ACTUATORS_T4);
195 for(uint8_t i = 0; i < sizeof(struct ActuatorsT4Out) ; i++){
196 uart_put_byte(&(ACTUATORS_T4_PORT), 0, buf_send[i]);
197 }
198#endif
199}
200
202{
205
206 /* Init ABI bind msg: */
207 AbiBindMsgACTUATORS_T4_OUT(ABI_BROADCAST, &ACTUATORS_T4_OUT, data_actuators_t4_out);
208
209#if PERIODIC_TELEMETRY
212#endif
213}
214
215/* Send the received message over ABI so then another module can use this actuator state info in e.g. control modules */
217{
218 memcpy(&actuators_t4_in, &actuators_t4_msg_buf_in[1], sizeof(struct ActuatorsT4In)); //Starting from 1 to avoid reading the starting byte
219 /* Assign the rolling message: */
220 actuators_t4_extra_data_in[actuators_t4_in.rolling_msg_in_id] = actuators_t4_in.rolling_msg_in;
221 /* Send msg through ABI: */
222 AbiSendMsgACTUATORS_T4_IN(ABI_ACTUATORS_T4_IN_ID, &actuators_t4_in, &actuators_t4_extra_data_in[0]);
223}
224
225/* Event checking if serial packet are available on the bus */
227{
228 if(fabs(get_sys_time_float() - actuators_t4_last_ts) > 5){ //Reset received packets to zero every 5 second to update the statistics
231 }
232#ifdef ACTUATORS_T4_SIM //TODO: use the SIM, NPS and HITL flags ,but if HIL is used it should be able to send the data to the T4 board
233 /* Don't do anything if it is SIM */
234#else
235 while(uart_char_available(&(ACTUATORS_T4_PORT)) > 0) {
236 uint8_t actuators_t4_byte_in;
237 actuators_t4_byte_in = uart_getch(&(ACTUATORS_T4_PORT));
238 if ((actuators_t4_byte_in == START_BYTE_ACTUATORS_T4) || (actuators_t4_buf_in_cnt > 0)) {
239 actuators_t4_msg_buf_in[actuators_t4_buf_in_cnt] = actuators_t4_byte_in;
241 }
242 if (actuators_t4_buf_in_cnt > sizeof(struct ActuatorsT4In) ) {
244 uint8_t checksum_in_local = 0;
245 for(uint16_t i = 1; i < sizeof(struct ActuatorsT4In) ; i++){
246 checksum_in_local += actuators_t4_msg_buf_in[i];
247 }
248 if(checksum_in_local == actuators_t4_msg_buf_in[sizeof(struct ActuatorsT4In)]){
251 }
252 else {
254 }
255 }
256 }
257#endif
259}
260
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
Definition abi_common.h:101
#define ABI_BROADCAST
Broadcast address.
Definition abi_common.h:80
uint32_t actuators_t4_received_packets
void actuators_t4_uart_event()
static uint8_t actuators_t4_msg_buf_in[sizeof(struct ActuatorsT4In) *2]
The message buffer for the device chosen to be 2* message_size total.
uint8_t actuators_t4_out_msg_id
float actuators_t4_last_ts
float actuators_t4_extra_data_in[255]
float actuators_t4_extra_data_out[255]
static abi_event ACTUATORS_T4_OUT
struct ActuatorsT4In actuators_t4_in
struct ActuatorsT4Out actuators_t4_out
void actuators_t4_uart_parse_msg_in(void)
static void actuators_t4_uplink(struct transport_tx *trans, struct link_device *dev)
uint32_t actuators_t4_missed_packets_in
uint16_t actuators_t4_buf_in_cnt
void actuators_t4_uart_init()
static void actuators_t4_downlink(struct transport_tx *trans, struct link_device *dev)
static void data_actuators_t4_out(uint8_t sender_id, struct ActuatorsT4Out *actuators_t4_out_ptr, float *actuators_t4_extra_data_out_ptr)
uint16_t actuators_t4_message_frequency_in
Uses a T4 Actuators Board as fly by wire system. This Board can control serial bus servos,...
#define START_BYTE_ACTUATORS_T4
static struct uart_periph * dev
void uart_put_byte(struct uart_periph *periph, long fd, uint8_t data)
Definition uart_arch.c:306
int uart_char_available(struct uart_periph *p)
Check UART for available chars in receive buffer.
Definition uart_arch.c:357
uint8_t uart_getch(struct uart_periph *p)
Definition uart_arch.c:348
Architecture independent timing functions.
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition sys_time.h:241
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition telemetry.h:66
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
short int16_t
Typedef defining 16 bit short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.