Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
protocol.h
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 #pragma once
35 
36 #include <inttypes.h>
37 
72 /* Per C, this is safe for all 2's complement systems */
73 #define REG_TO_SIGNED(_reg) ((int16_t)(_reg))
74 #define SIGNED_TO_REG(_signed) ((uint16_t)(_signed))
75 
76 #define REG_TO_BOOL(_reg) ((bool)(_reg))
77 
78 #define PX4IO_PROTOCOL_VERSION 5
79 
80 /* maximum allowable sizes on this protocol version */
81 #define PX4IO_PROTOCOL_MAX_CONTROL_COUNT 8
83 /* static configuration page */
84 #define PX4IO_PAGE_CONFIG 0
85 #define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* PX4IO_PROTOCOL_VERSION */
86 #define PX4IO_P_CONFIG_HARDWARE_VERSION 1 /* magic numbers TBD */
87 #define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */
88 #define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */
89 #define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */
90 #define PX4IO_P_CONFIG_ACTUATOR_COUNT 5 /* hardcoded max actuator output count */
91 #define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */
92 #define PX4IO_P_CONFIG_ADC_INPUT_COUNT 7 /* hardcoded max ADC inputs */
93 #define PX4IO_MAX_TRANSFER_LEN 64
94 
95 /* dynamic status page */
96 #define PX4IO_PAGE_STATUS 1
97 #define PX4IO_P_STATUS_FREEMEM 0
98 #define PX4IO_P_STATUS_CPULOAD 1
99 
100 #define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */
101 #define PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED (1 << 0) /* arm-ok and locally armed */
102 #define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 1) /* RC input is valid */
103 #define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 2) /* PPM input is valid */
104 #define PX4IO_P_STATUS_FLAGS_RC_DSM (1 << 3) /* DSM input is valid */
105 #define PX4IO_P_STATUS_FLAGS_RC_SBUS (1 << 4) /* SBUS input is valid */
106 #define PX4IO_P_STATUS_FLAGS_FMU_OK (1 << 5) /* controls from FMU are valid */
107 #define PX4IO_P_STATUS_FLAGS_RAW_PWM (1 << 6) /* raw PWM from FMU */
108 #define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 7) /* the arming state between IO and FMU is in sync */
109 #define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 8) /* initialisation of the IO completed without error */
110 #define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 9) /* failsafe is active */
111 #define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 10) /* safety is off */
112 #define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED (1 << 11) /* FMU was initialized and OK once */
113 #define PX4IO_P_STATUS_FLAGS_RC_ST24 (1 << 12) /* ST24 input is valid */
114 #define PX4IO_P_STATUS_FLAGS_RC_SUMD (1 << 13) /* SUMD input is valid */
115 #define PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT (1 << 14) /* px4io safety button was pressed for longer than 1 second */
116 
117 #define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */
118 #define PX4IO_P_STATUS_ALARMS_RC_LOST (1 << 0) /* timed out waiting for RC input */
119 #define PX4IO_P_STATUS_ALARMS_PWM_ERROR (1 << 1) /* PWM configuration or output was bad */
120 
121 #define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */
122 #define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */
123 
124 /* array of PWM servo output values, microseconds */
125 #define PX4IO_PAGE_SERVOS 3 /* 0..CONFIG_ACTUATOR_COUNT-1 */
126 
127 /* array of raw RC input values, microseconds */
128 #define PX4IO_PAGE_RAW_RC_INPUT 4
129 #define PX4IO_P_RAW_RC_COUNT 0 /* number of valid channels */
130 #define PX4IO_P_RAW_RC_FLAGS 1 /* RC detail status flags */
131 #define PX4IO_P_RAW_RC_FLAGS_FRAME_DROP (1 << 0) /* single frame drop */
132 #define PX4IO_P_RAW_RC_FLAGS_FAILSAFE (1 << 1) /* receiver is in failsafe mode */
133 #define PX4IO_P_RAW_RC_FLAGS_RC_DSM11 (1 << 2) /* DSM decoding is 11 bit mode */
134 #define PX4IO_P_RAW_RC_FLAGS_MAPPING_OK (1 << 3) /* Channel mapping is ok */
135 #define PX4IO_P_RAW_RC_FLAGS_RC_OK (1 << 4) /* RC reception ok */
136 
137 #define PX4IO_P_RAW_RC_NRSSI 2 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */
138 #define PX4IO_P_RAW_RC_DATA 3 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */
139 #define PX4IO_P_RAW_FRAME_COUNT 4 /* Number of total received frames (wrapping counter) */
140 #define PX4IO_P_RAW_LOST_FRAME_COUNT 5 /* Number of total dropped frames (wrapping counter) */
141 #define PX4IO_P_RAW_RC_BASE 6 /* CONFIG_RC_INPUT_COUNT channels from here */
142 
143 /* array of raw ADC values */
144 #define PX4IO_PAGE_RAW_ADC_INPUT 6 /* 0..CONFIG_ADC_INPUT_COUNT-1 */
145 
146 /* PWM servo information */
147 #define PX4IO_PAGE_PWM_INFO 7
148 #define PX4IO_RATE_MAP_BASE 0 /* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */
149 
150 /* setup page */
151 #define PX4IO_PAGE_SETUP 50
152 #define PX4IO_P_SETUP_FEATURES 0
153 #define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0)
154 #define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1)
155 #define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 2)
157 #define PX4IO_P_SETUP_ARMING 1 /* arming controls */
158 #define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */
159 #define PX4IO_P_SETUP_ARMING_FMU_ARMED (1 << 1) /* FMU is already armed */
160 #define PX4IO_P_SETUP_ARMING_FMU_PREARMED (1 << 2) /* FMU is already prearmed */
161 #define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 3) /* use custom failsafe values */
162 #define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 4) /* If set, the system operates normally, but won't actuate any servos */
163 #define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 5) /* If set, the system will always output the failsafe values */
164 #define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 6) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */
165 
166 #define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */
167 #define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */
168 #define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */
169 #define PX4IO_P_SETUP_VSERVO_SCALE 5 /* hardware rev [2] servo voltage correction factor (float) */
170 #define PX4IO_P_SETUP_DSM 6 /* DSM bind state */
171 enum { /* DSM bind states */
177 };
178 /* 8 */
179 #define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */
180 
181 #define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */
182 #define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */
183 
184 #define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */
185 /* storage space of 12 occupied by CRC */
186 #define PX4IO_P_SETUP_SAFETY_BUTTON_ACK 14
187 #define PX4IO_P_SETUP_SAFETY_OFF 15
188 #define PX4IO_P_SETUP_SBUS_RATE 16
189 #define PX4IO_P_SETUP_THERMAL 17
190 #define PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION 18
191 #define PX4IO_P_SETUP_PWM_RATE_GROUP0 19 /* Configure timer group 0 update rate in Hz */
192 #define PX4IO_P_SETUP_PWM_RATE_GROUP1 20 /* Configure timer group 1 update rate in Hz */
193 #define PX4IO_P_SETUP_PWM_RATE_GROUP2 21 /* Configure timer group 2 update rate in Hz */
194 #define PX4IO_P_SETUP_PWM_RATE_GROUP3 22 /* Configure timer group 3 update rate in Hz */
195 
196 #define PX4IO_THERMAL_IGNORE UINT16_MAX
197 #define PX4IO_THERMAL_OFF 0
198 #define PX4IO_THERMAL_FULL 10000
199 
200 /* PWM output */
201 #define PX4IO_PAGE_DIRECT_PWM 54
203 /* PWM failsafe values - zero disables the output */
204 #define PX4IO_PAGE_FAILSAFE_PWM 55
206 /* Debug and test page - not used in normal operation */
207 #define PX4IO_PAGE_TEST 127
208 #define PX4IO_P_TEST_LED 0
210 /* PWM disarmed values that are active */
211 #define PX4IO_PAGE_DISARMED_PWM 109 /* 0..CONFIG_ACTUATOR_COUNT-1 */
212 
216 #define PKT_MAX_REGS 32 // by agreement w/FMU
217 
218 #pragma pack(push, 1)
219 struct IOPacket {
221  uint8_t crc;
222  uint8_t page;
223  uint8_t offset;
225 };
226 #pragma pack(pop)
227 
228 #if (PX4IO_MAX_TRANSFER_LEN > PKT_MAX_REGS * 2)
229 #error The max transfer length of the IO protocol must not be larger than the IO packet size
230 #endif
231 
232 #define PKT_CODE_READ 0x00 /* FMU->IO read transaction */
233 #define PKT_CODE_WRITE 0x40 /* FMU->IO write transaction */
234 #define PKT_CODE_SUCCESS 0x00 /* IO->FMU success reply */
235 #define PKT_CODE_CORRUPT 0x40 /* IO->FMU bad packet reply */
236 #define PKT_CODE_ERROR 0x80 /* IO->FMU register op error reply */
237 
238 #define PKT_CODE_MASK 0xc0
239 #define PKT_COUNT_MASK 0x3f
240 
241 #define PKT_COUNT(_p) ((_p).count_code & PKT_COUNT_MASK)
242 #define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK)
243 #define PKT_SIZE(_p) ((size_t)((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p))))
244 
245 static const uint8_t crc8_tab[256] __attribute__((unused)) = {
246  0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15,
247  0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D,
248  0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65,
249  0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D,
250  0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5,
251  0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD,
252  0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85,
253  0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD,
254  0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2,
255  0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA,
256  0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2,
257  0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A,
258  0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32,
259  0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A,
260  0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42,
261  0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A,
262  0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C,
263  0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4,
264  0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC,
265  0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4,
266  0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C,
267  0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44,
268  0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C,
269  0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34,
270  0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B,
271  0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63,
272  0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B,
273  0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13,
274  0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB,
275  0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83,
276  0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB,
277  0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3
278 };
279 
280 static uint8_t crc_packet(struct IOPacket *pkt) __attribute__((unused));
281 static uint8_t
282 crc_packet(struct IOPacket *pkt)
283 {
284  uint8_t *end = (uint8_t *)(&pkt->regs[PKT_COUNT(*pkt)]);
285  uint8_t *p = (uint8_t *)pkt;
286  uint8_t c = 0;
287 
288  while (p < end) {
289  c = crc8_tab[c ^ * (p++)];
290  }
291 
292  return c;
293 }
static float p[2][2]
#define PKT_MAX_REGS
Serial protocol encapsulation.
Definition: protocol.h:216
#define PKT_COUNT(_p)
Definition: protocol.h:241
uint8_t offset
Definition: iomcu.c:40
@ dsm_bind_send_pulses
Definition: protocol.h:175
@ dsm_bind_power_up
Definition: protocol.h:173
@ dsm_bind_set_rx_out
Definition: protocol.h:174
@ dsm_bind_power_down
Definition: protocol.h:172
@ dsm_bind_reinit_uart
Definition: protocol.h:176
uint8_t page
Definition: iomcu.c:39
uint8_t count_code
Definition: protocol.h:220
static const uint8_t crc8_tab[256]
Definition: protocol.h:245
static uint8_t crc_packet(struct IOPacket *pkt)
Definition: protocol.h:282
uint8_t crc
Definition: iomcu.c:38
uint16_t regs[PKT_MAX_REGS]
Definition: iomcu.c:41
Definition: iomcu.c:35
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98