1 /*
2 * Copyright (c) 2015, Freescale Semiconductor, Inc.
3 * Copyright 2016-2017 NXP
4 * All rights reserved.
5 *
6 * SPDX-License-Identifier: BSD-3-Clause
7 */
8
9 /*! \file control.c
10 \brief Defines control sub-system
11
12 This file contains a UART implementation of the control subsystem. The
13 command interpreter and streaming functions are contained in two separate
14 files. So you can easily swap those out with only minor changes here.
15 */
16 #include "fsl_debug_console.h"
17 #include "board.h"
18 #include "pin_mux.h"
19 #include "fsl_uart.h"
20 #include "fsl_port.h"
21 #include "sensor_fusion.h"
22 #include "control.h"
23
24 #ifndef F_USE_WIRELESS_UART
25 #define F_USE_WIRELESS_UART 0x0001 ///< 0x0001 to include, 0x0000 otherwise
26 #endif
27 #ifndef F_USE_WIRED_UART
28 #define F_USE_WIRED_UART 0x0002 ///< 0x0002 to include, 0x0000 otherwise
29 #endif
30
31 #define CONTROL_BAUDRATE 115200 ///< Baudrate to be used for serial communications
32
33 uart_handle_t wired_uartHandle;
34 uart_handle_t wireless_uartHandle;
35
36 // global structures
37 uint8_t sUARTOutputBuffer[256]; // larger than the nominal 124 byte size for outgoing packets
38
39 // direct access to sfg here is the only place in the entire library where we cannot simply
40 // pass a pointer. This is because it is needed by the UART interrupt handlers. Since this
41 // only occurs here, in a subsystem which is defined to be application dependent, that is
42 // considered acceptable.
43 extern SensorFusionGlobals sfg;
44
45 // Blocking function to write a single byte to a specified UART
myUART_WriteByte(UART_Type * base,uint8_t data)46 void myUART_WriteByte(UART_Type *base, uint8_t data)
47 {
48 uint8_t flag = 0;
49 while (!flag)
50 {
51 flag = base->S1 & 0x80;
52 }
53 UART_WriteByte(base, data);
54 }
55
56 // Blocking function pipes specified buffer to both output UARTS
writeControlPort(ControlSubsystem * pComm,uint8_t buffer[],uint16_t nbytes)57 int8_t writeControlPort(ControlSubsystem *pComm, uint8_t buffer[], uint16_t nbytes)
58 {
59 uint16_t i;
60 for (i = 0; i < nbytes; i++)
61 {
62 #if F_USE_WIRED_UART
63 myUART_WriteByte(WIRED_UART, buffer[i]);
64 #endif
65 #if F_USE_WIRELESS_UART
66 myUART_WriteByte(WIRELESS_UART, buffer[i]);
67 #endif
68 }
69
70 return (0);
71 }
72
73 #if F_USE_WIRELESS_UART
74 // writeWirelessPort() is called from BlueRadios_Init(), which is used to
75 // initialize the Bluetooth module on NXP sensor shields. That function will
76 // obviously need to be replaced for alternate hardware.
writeWirelessPort(uint8_t buffer[],uint16_t nbytes)77 int8_t writeWirelessPort(uint8_t buffer[], uint16_t nbytes)
78 {
79 uint16_t i;
80 for (i = 0; i < nbytes; i++)
81 {
82 myUART_WriteByte(WIRELESS_UART, buffer[i]);
83 }
84
85 return (0);
86 }
87 // Wired and Wireless UART interrupt handlers are essentially identical.
WIRELESS_UART_IRQHandler(void)88 void WIRELESS_UART_IRQHandler(void)
89 {
90 uint8_t data;
91 status_t sts;
92 uint32_t nbytes; // number of bytes received
93 uint32_t flags;
94 static char iCommandBuffer_B[5] = "~~~~"; // 5 bytes long to include the unused terminating \0
95 sfg.setStatus(&sfg, RECEIVING_WIRELESS);
96 flags = UART_GetStatusFlags(WIRELESS_UART);
97 /* If new data arrived. */
98 if ((kUART_RxDataRegFullFlag | kUART_RxOverrunFlag) & flags)
99 {
100 sts = UART_TransferGetReceiveCount(WIRELESS_UART, &wireless_uartHandle, &nbytes);
101 if (sts == kStatus_Success)
102 {
103 data = UART_ReadByte(WIRELESS_UART);
104 DecodeCommandBytes(&sfg, iCommandBuffer_B, &data, 1);
105 }
106 }
107 }
108 // initialize BlueRadios BR-LE4.0-D2A Bluetooth module
109 // This is required for NXP FRDM-FXS-MULT2-B boards.
BlueRadios_Init(void)110 void BlueRadios_Init(void)
111 {
112 uint16_t ilen; // command string length
113
114 // transmit "ATSRM,2,0\r" to minimize traffic from the module
115 // command "ATSRM": sets the module response mode which configures how verbose the module will be
116 // 2: response mode at to minimal
117 // 0: disconnected mode is command mode
118 // \r: carriage return escape sequence
119 strcpy((char *)sUARTOutputBuffer, "ATSRM,2,0\r");
120 ilen = strlen((char *)sUARTOutputBuffer);
121 writeWirelessPort(sUARTOutputBuffer, ilen);
122 return;
123 }
124 #endif
125
126 #if F_USE_WIRED_UART
echo(uint8_t data)127 void echo(uint8_t data) // only used for comms debug
128 {
129 /* Send data only when UART TX register is empty and ring buffer has data to send out. */
130 if (kUART_TxDataRegEmptyFlag & UART_GetStatusFlags(WIRED_UART))
131 {
132 UART_WriteByte(WIRED_UART, data);
133 }
134 }
135 // Wired and Wireless UART interrupt handlers are essentially identical.
WIRED_UART_IRQHandler(void)136 void WIRED_UART_IRQHandler(void)
137 {
138 uint8_t data;
139 status_t sts;
140 uint32_t nbytes; // number of bytes received
141 uint32_t flags;
142 static char iCommandBuffer_A[5] = "~~~~"; // 5 bytes long to include the unused terminating \0
143
144 sfg.setStatus(&sfg, RECEIVING_WIRED);
145 flags = UART_GetStatusFlags(WIRED_UART);
146 /* If new data arrived. */
147 if ((kUART_RxDataRegFullFlag | kUART_RxOverrunFlag) & flags)
148 {
149 sts = UART_TransferGetReceiveCount(WIRED_UART, &wired_uartHandle, &nbytes);
150 if (sts == kStatus_Success)
151 {
152 data = UART_ReadByte(WIRED_UART);
153 DecodeCommandBytes(&sfg, iCommandBuffer_A, &data, 1);
154 }
155 }
156 }
157 #endif
158
159 /// Initialize the control subsystem and all related hardware
initializeControlPort(ControlSubsystem * pComm)160 int8_t initializeControlPort(
161 ControlSubsystem *pComm ///< pointer to the control subystem structure
162 )
163 {
164 uart_config_t config;
165 if (pComm)
166 {
167 pComm->DefaultQuaternionPacketType = Q3; // default to simplest algorithm
168 pComm->QuaternionPacketType = Q3; // default to simplest algorithm
169 pComm->AngularVelocityPacketOn = true; // transmit angular velocity packet
170 pComm->DebugPacketOn = true; // transmit debug packet
171 pComm->RPCPacketOn = true; // transmit roll, pitch, compass packet
172 pComm->AltPacketOn = true; // Altitude packet
173 pComm->AccelCalPacketOn = 0;
174 pComm->write = writeControlPort;
175 pComm->stream = CreateAndSendPackets;
176
177 #if F_USE_WIRED_UART
178 /* Initialize WIRED UART pins below - currently duplicates code in pin_mux.c */
179 CLOCK_EnableClock(WIRED_UART_PORT_CLKEN);
180 PORT_SetPinMux(WIRED_UART_PORT, WIRED_UART_RX_PIN, WIRED_UART_MUX);
181 PORT_SetPinMux(WIRED_UART_PORT, WIRED_UART_TX_PIN, WIRED_UART_MUX);
182 UART_GetDefaultConfig(&config);
183
184 config.baudRate_Bps = CONTROL_BAUDRATE;
185 config.enableTx = true;
186 config.enableRx = true;
187 config.rxFifoWatermark = 1;
188 UART_Init(WIRED_UART, &config, CLOCK_GetFreq(WIRED_UART_CLKSRC));
189
190 /* Enable RX interrupt. */
191 UART_EnableInterrupts(WIRED_UART, kUART_RxDataRegFullInterruptEnable |
192 kUART_RxOverrunInterruptEnable);
193 EnableIRQ(WIRED_UART_IRQn);
194 #endif
195 #if F_USE_WIRELESS_UART
196 /* Initialize WIRELESS UART pins below */
197 CLOCK_EnableClock(WIRELESS_UART_PORT_CLKEN);
198 PORT_SetPinMux(WIRELESS_UART_PORT, WIRELESS_UART_RX_PIN,
199 WIRELESS_UART_MUX);
200 PORT_SetPinMux(WIRELESS_UART_PORT, WIRELESS_UART_TX_PIN,
201 WIRELESS_UART_MUX);
202
203 UART_Init(WIRELESS_UART, &config, CLOCK_GetFreq(WIRELESS_UART_CLKSRC));
204 BlueRadios_Init();
205
206 /* Enable RX interrupt. */
207 UART_EnableInterrupts(WIRELESS_UART, kUART_RxDataRegFullInterruptEnable |
208 kUART_RxOverrunInterruptEnable);
209 EnableIRQ(WIRELESS_UART_IRQn);
210 #endif
211
212 return (0);
213 }
214 else
215 {
216 return (1);
217 }
218 }
219
