Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
iomcu.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2022 Freek van tieen <freek.v.tienen@gmail.com>
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, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
27 #include "iomcu.h"
28 #include "mcu_periph/uart.h"
29 #include <string.h>
30 
31 // 22 is enough for the rc_input page in one transfer
32 #define PKT_MAX_REGS 22
33 #define IOMCU_MAX_CHANNELS 16
34 
35 struct __attribute__((packed)) IOPacket {
42 };
43 
44 /*
45  values for pkt.code
46  */
47 enum iocode {
48  // read types
49  CODE_READ = 0,
51 
52  // reply codes
55  CODE_ERROR = 2
56 };
57 
58 // IO pages
59 enum iopage {
65  PAGE_RCIN = 5,
68  PAGE_SETUP = 50,
71  PAGE_MIXING = 200,
72  PAGE_GPIO = 201,
73 };
74 
75 // setup page registers
76 #define PAGE_REG_SETUP_FEATURES 0
77 #define P_SETUP_FEATURES_SBUS1_OUT 1
78 #define P_SETUP_FEATURES_SBUS2_OUT 2
79 #define P_SETUP_FEATURES_PWM_RSSI 4
80 #define P_SETUP_FEATURES_ADC_RSSI 8
81 #define P_SETUP_FEATURES_ONESHOT 16
82 #define P_SETUP_FEATURES_BRUSHED 32
83 
84 #define PAGE_REG_SETUP_ARMING 1
85 #define P_SETUP_ARMING_IO_ARM_OK (1<<0)
86 #define P_SETUP_ARMING_FMU_ARMED (1<<1)
87 #define P_SETUP_ARMING_RC_HANDLING_DISABLED (1<<6)
88 #define P_SETUP_ARMING_SAFETY_DISABLE_ON (1 << 11) // disable use of safety button for safety off->on
89 #define P_SETUP_ARMING_SAFETY_DISABLE_OFF (1 << 12) // disable use of safety button for safety on->off
90 
91 #define PAGE_REG_SETUP_PWM_RATE_MASK 2
92 #define PAGE_REG_SETUP_DEFAULTRATE 3
93 #define PAGE_REG_SETUP_ALTRATE 4
94 #define PAGE_REG_SETUP_REBOOT_BL 10
95 #define PAGE_REG_SETUP_CRC 11
96 #define PAGE_REG_SETUP_SBUS_RATE 19
97 #define PAGE_REG_SETUP_IGNORE_SAFETY 20 /* bitmask of surfaces to ignore the safety status */
98 #define PAGE_REG_SETUP_HEATER_DUTY_CYCLE 21
99 #define PAGE_REG_SETUP_DSM_BIND 22
100 #define PAGE_REG_SETUP_RC_PROTOCOLS 23 // uses 2 slots, 23 and 24
101 
102 // config page registers
103 #define PAGE_CONFIG_PROTOCOL_VERSION 0
104 #define PAGE_CONFIG_PROTOCOL_VERSION2 1
105 #define IOMCU_PROTOCOL_VERSION 4
106 #define IOMCU_PROTOCOL_VERSION2 10
107 
108 // magic value for rebooting to bootloader
109 #define REBOOT_BL_MAGIC 14662
110 
111 #define PAGE_REG_SETUP_FORCE_SAFETY_OFF 12
112 #define PAGE_REG_SETUP_FORCE_SAFETY_ON 14
113 #define FORCE_SAFETY_MAGIC 22027
114 
115 struct page_config {
118 };
119 
133 };
134 
142 };
143 
144 /*
145  data for mixing on FMU failsafe
146  */
147 struct page_mixing {
153 
154  // RC input arrays are in AETR order
160 
161  // gain for elevon and vtail mixing, x1000
163 
164  // channel which when high forces mixer
166 
167  // is the throttle an angle input?
169 
170  // mask of channels which are pure manual in override
172 
173  // enabled needs to be 1 to enable mixing
175 
177 };
178 
179 struct __attribute__((packed, aligned(2))) page_GPIO {
182 };
183 
184 static const uint8_t crc8_table[] = {
185  0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31,
186  0x24, 0x23, 0x2a, 0x2d, 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65,
187  0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, 0xe0, 0xe7, 0xee, 0xe9,
188  0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd,
189  0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1,
190  0xb4, 0xb3, 0xba, 0xbd, 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2,
191  0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, 0xb7, 0xb0, 0xb9, 0xbe,
192  0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a,
193  0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16,
194  0x03, 0x04, 0x0d, 0x0a, 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42,
195  0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, 0x89, 0x8e, 0x87, 0x80,
196  0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4,
197  0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8,
198  0xdd, 0xda, 0xd3, 0xd4, 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c,
199  0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, 0x19, 0x1e, 0x17, 0x10,
200  0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34,
201  0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f,
202  0x6a, 0x6d, 0x64, 0x63, 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b,
203  0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, 0xae, 0xa9, 0xa0, 0xa7,
204  0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83,
205  0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef,
206  0xfa, 0xfd, 0xf4, 0xf3
207 };
208 
209 /*
210  crc8 from trone driver by Luis Rodrigues
211  */
212 static uint8_t crc_crc8(const uint8_t *p, uint8_t len)
213 {
214  uint16_t crc = 0x0;
215 
216  while (len--) {
217  const uint16_t i = (crc ^ *p++) & 0xFF;
218  crc = (crc8_table[i] ^ (crc << 8)) & 0xFF;
219  }
220 
221  return crc & 0xFF;
222 }
223 
232 static void iomcu_write_registers(uint8_t page, uint8_t offset, uint8_t count, const uint16_t *regs)
233 {
234  // When we want to write more registers do a recursive call of depth 1
235  while (count > PKT_MAX_REGS) {
237  offset += PKT_MAX_REGS;
238  count -= PKT_MAX_REGS;
239  regs += PKT_MAX_REGS;
240  }
241 
242  // Create the packet
243  struct IOPacket pkt;
244  pkt.code = CODE_WRITE;
245  pkt.count = count;
246  pkt.page = page;
247  pkt.offset = offset;
248  pkt.crc = 0;
249  memcpy(pkt.regs, regs, 2 * count);
250  const uint8_t pkt_size = count * 2 + 4;
251  pkt.crc = crc_crc8((const uint8_t *)&pkt, pkt_size);
252 
253  // Send the packet
254  (INTERMCU_LINK).device.put_buffer(&(INTERMCU_LINK), 0, (uint8_t *)&pkt, pkt_size);
255 }
256 
265 {
267 }
268 
275 {
277 }
static const float offset[]
static float p[2][2]
#define PKT_MAX_REGS
Definition: iomcu.c:32
uint16_t protocol_version2
Definition: iomcu.c:117
uint16_t vservo
Definition: iomcu.c:123
uint16_t pwm[IOMCU_MAX_CHANNELS]
Definition: iomcu.c:140
uint32_t timestamp_ms
Definition: iomcu.c:122
uint16_t mixing_gain
Definition: iomcu.c:162
uint8_t err_write
Definition: iomcu.c:131
int8_t rc_chan_override
Definition: iomcu.c:165
uint8_t channel_mask
Definition: iomcu.c:180
uint8_t rc_channel[4]
Definition: iomcu.c:159
uint8_t err_uart
Definition: iomcu.c:132
static const uint8_t crc8_table[]
Definition: iomcu.c:184
void iomcu_set_heater_duty_cycle(uint8_t duty_cycle)
Set the IO MCU heater duty cycle.
Definition: iomcu.c:274
iocode
Definition: iomcu.c:47
@ CODE_SUCCESS
Definition: iomcu.c:53
@ CODE_CORRUPT
Definition: iomcu.c:54
@ CODE_READ
Definition: iomcu.c:49
@ CODE_WRITE
Definition: iomcu.c:50
@ CODE_ERROR
Definition: iomcu.c:55
uint16_t servo_trim[IOMCU_MAX_CHANNELS]
Definition: iomcu.c:150
uint32_t total_pkts
Definition: iomcu.c:126
uint16_t protocol_version
Definition: iomcu.c:116
uint8_t err_crc
Definition: iomcu.c:128
static void iomcu_write_registers(uint8_t page, uint8_t offset, uint8_t count, const uint16_t *regs)
Write registers to the IO MCU.
Definition: iomcu.c:232
uint8_t rc_reversed[IOMCU_MAX_CHANNELS]
Definition: iomcu.c:158
uint8_t flags_rc_ok
Definition: iomcu.c:138
uint8_t throttle_is_angle
Definition: iomcu.c:168
int16_t rssi
Definition: iomcu.c:141
uint16_t freemem
Definition: iomcu.c:121
uint8_t rc_protocol
Definition: iomcu.c:139
uint16_t rc_trim[4]
Definition: iomcu.c:157
#define PAGE_REG_SETUP_HEATER_DUTY_CYCLE
Definition: iomcu.c:98
uint8_t err_bad_opcode
Definition: iomcu.c:129
uint8_t servo_function[IOMCU_MAX_CHANNELS]
Definition: iomcu.c:151
#define IOMCU_MAX_CHANNELS
Definition: iomcu.c:33
iopage
Definition: iomcu.c:59
@ PAGE_ACTUATORS
Definition: iomcu.c:62
@ PAGE_CONFIG
Definition: iomcu.c:60
@ PAGE_SERVOS
Definition: iomcu.c:63
@ PAGE_GPIO
Definition: iomcu.c:72
@ PAGE_RCIN
Definition: iomcu.c:65
@ PAGE_PWM_INFO
Definition: iomcu.c:67
@ PAGE_RAW_RCIN
Definition: iomcu.c:64
@ PAGE_DIRECT_PWM
Definition: iomcu.c:69
@ PAGE_FAILSAFE_PWM
Definition: iomcu.c:70
@ PAGE_SETUP
Definition: iomcu.c:68
@ PAGE_MIXING
Definition: iomcu.c:71
@ PAGE_STATUS
Definition: iomcu.c:61
@ PAGE_RAW_ADC
Definition: iomcu.c:66
uint32_t num_errors
Definition: iomcu.c:125
static uint8_t crc_crc8(const uint8_t *p, uint8_t len)
Definition: iomcu.c:212
uint8_t flags_failsafe
Definition: iomcu.c:137
uint8_t servo_reversed[IOMCU_MAX_CHANNELS]
Definition: iomcu.c:152
uint16_t servo_max[IOMCU_MAX_CHANNELS]
Definition: iomcu.c:149
uint8_t enabled
Definition: iomcu.c:174
uint16_t vrssi
Definition: iomcu.c:124
uint8_t err_read
Definition: iomcu.c:130
uint16_t servo_min[IOMCU_MAX_CHANNELS]
Definition: iomcu.c:148
uint8_t output_mask
Definition: iomcu.c:181
static void iomcu_write_register(uint8_t page, uint8_t offset, uint16_t v)
Write a single register to the IO MCU.
Definition: iomcu.c:264
uint16_t rc_max[4]
Definition: iomcu.c:156
uint8_t pad
Definition: iomcu.c:176
uint16_t rc_min[4]
Definition: iomcu.c:155
uint16_t manual_rc_mask
Definition: iomcu.c:171
uint8_t flag_safety_off
Definition: iomcu.c:127
uint8_t count
Definition: iomcu.c:136
Driver to communicate with the ardupilot IO MCU.
uint8_t offset
Definition: iomcu.c:40
uint8_t code
Definition: iomcu.c:37
uint8_t page
Definition: iomcu.c:39
uint8_t count
Definition: iomcu.c:36
uint8_t crc
Definition: iomcu.c:38
uint16_t regs[PKT_MAX_REGS]
Definition: iomcu.c:41
Definition: iomcu.c:35
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
signed char int8_t
Typedef defining 8 bit char type.
Definition: vl53l1_types.h:103