Line data Source code
1 : /*
2 : * AP_SBusOut.cpp
3 : *
4 : * Created on: Aug 19, 2017
5 : * Author: Mark Whitehorn
6 : *
7 : * method sbus1_out was ported from ardupilot/modules/PX4Firmware/src/lib/rc/sbus.c
8 : * which has the following license:
9 : *
10 : * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
11 : *
12 : * Redistribution and use in source and binary forms, with or without
13 : * modification, are permitted provided that the following conditions
14 : * are met:
15 : *
16 : * 1. Redistributions of source code must retain the above copyright
17 : * notice, this list of conditions and the following disclaimer.
18 : * 2. Redistributions in binary form must reproduce the above copyright
19 : * notice, this list of conditions and the following disclaimer in
20 : * the documentation and/or other materials provided with the
21 : * distribution.
22 : * 3. Neither the name PX4 nor the names of its contributors may be
23 : * used to endorse or promote products derived from this software
24 : * without specific prior written permission.
25 : *
26 : * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
27 : * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
28 : * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
29 : * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
30 : * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
31 : * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
32 : * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
33 : * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
34 : * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
35 : * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
36 : * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
37 : * POSSIBILITY OF SUCH DAMAGE.
38 : *
39 : */
40 :
41 : #include "AP_SBusOut_config.h"
42 :
43 : #if AP_SBUSOUTPUT_ENABLED
44 :
45 : #include "AP_SBusOut.h"
46 : #include <AP_Math/AP_Math.h>
47 : #include <AP_SerialManager/AP_SerialManager.h>
48 : #include <SRV_Channel/SRV_Channel.h>
49 :
50 : extern const AP_HAL::HAL& hal;
51 :
52 : #define SBUS_DEBUG 0
53 :
54 : #define SBUS_BSIZE 25
55 : #define SBUS_CHANNELS 16
56 : #define SBUS_MIN 875
57 :
58 : const AP_Param::GroupInfo AP_SBusOut::var_info[] = {
59 : // @Param: RATE
60 : // @DisplayName: SBUS default output rate
61 : // @Description: This sets the SBUS output frame rate in Hz.
62 : // @Range: 25 250
63 : // @User: Advanced
64 : // @Units: Hz
65 : AP_GROUPINFO("RATE", 1, AP_SBusOut, sbus_rate, 50),
66 :
67 : AP_GROUPEND
68 : };
69 :
70 :
71 : // constructor
72 2094 : AP_SBusOut::AP_SBusOut(void)
73 : {
74 : // set defaults from the parameter table
75 2094 : AP_Param::setup_object_defaults(this, var_info);
76 2094 : }
77 :
78 : /*
79 : format a SBUS output frame into a 25 byte buffer
80 : */
81 2560 : void AP_SBusOut::sbus_format_frame(uint16_t *channels, uint8_t num_channels, uint8_t buffer[SBUS_BSIZE])
82 : {
83 2560 : uint8_t index = 1;
84 2560 : uint8_t offset = 0;
85 :
86 2560 : memset(buffer, 0, SBUS_BSIZE);
87 2560 : buffer[0] = 0x0f;
88 :
89 : /* construct sbus frame representing channels 1 through 16 (max) */
90 2560 : const uint8_t nchan = MIN(num_channels, SBUS_CHANNELS);
91 23040 : for (unsigned i = 0; i < nchan; ++i) {
92 : /*protect from out of bounds values and limit to 11 bits*/
93 20480 : const uint16_t pwmval = MAX(channels[i], SBUS_MIN);
94 20480 : const uint32_t v1 = uint32_t(pwmval - SBUS_MIN) * 1600U;
95 20480 : uint16_t value = uint16_t(v1 / 1000U) + 1;
96 20480 : if (value > 0x07ff) {
97 0 : value = 0x07ff;
98 : }
99 :
100 : #if SBUS_DEBUG
101 : static uint16_t lastch9 = 0;
102 : if ((i==8) && (pwmval != lastch9)) {
103 : lastch9 = pwmval;
104 : hal.console->printf("channel 9 pwm: %04d\n", pwmval);
105 : }
106 : #endif
107 :
108 43520 : while (offset >= 8) {
109 23040 : ++index;
110 23040 : offset -= 8;
111 : }
112 :
113 20480 : buffer[index] |= (value << (offset)) & 0xff;
114 20480 : buffer[index + 1] |= (value >> (8 - offset)) & 0xff;
115 20480 : buffer[index + 2] |= (value >> (16 - offset)) & 0xff;
116 20480 : offset += 11;
117 : }
118 2560 : }
119 :
120 : /*
121 : * build and send sbus1 frame representing first 16 servo channels
122 : * input arg is pointer to uart
123 : */
124 : void
125 41692164 : AP_SBusOut::update()
126 : {
127 41692164 : if (!initialised) {
128 2056 : initialised = true;
129 2056 : init();
130 : }
131 :
132 41692164 : if (sbus1_uart == nullptr) {
133 41692164 : return;
134 : }
135 :
136 : // constrain output rate using sbus_frame_interval
137 : static uint32_t last_micros = 0;
138 0 : uint32_t now = AP_HAL::micros();
139 0 : if ((now - last_micros) <= sbus_frame_interval) {
140 0 : return;
141 : }
142 :
143 0 : last_micros = now;
144 :
145 : /* construct sbus frame representing channels 1 through 16 (max) */
146 0 : uint8_t nchan = MIN(NUM_SERVO_CHANNELS, SBUS_CHANNELS);
147 0 : uint16_t channels[SBUS_CHANNELS] {};
148 :
149 0 : for (unsigned i = 0; i < nchan; ++i) {
150 0 : SRV_Channel *c = SRV_Channels::srv_channel(i);
151 0 : if (c == nullptr) {
152 0 : continue;
153 : }
154 0 : channels[i] = c->get_output_pwm();
155 : }
156 : uint8_t buffer[SBUS_BSIZE];
157 0 : sbus_format_frame(channels, nchan, buffer);
158 :
159 : #if SBUS_DEBUG
160 : hal.gpio->pinMode(55, HAL_GPIO_OUTPUT);
161 : hal.gpio->write(55, 1);
162 : #endif
163 :
164 0 : sbus1_uart->write(buffer, sizeof(buffer));
165 :
166 : #if SBUS_DEBUG
167 : hal.gpio->pinMode(55, HAL_GPIO_OUTPUT);
168 : hal.gpio->write(55, 0);
169 : #endif
170 : }
171 :
172 2056 : void AP_SBusOut::init() {
173 2056 : uint16_t rate = sbus_rate.get();
174 :
175 : #if SBUS_DEBUG
176 : hal.console->printf("AP_SBusOut: init %d Hz\n", rate);
177 : #endif
178 :
179 : // subtract 500usec from requested frame interval to allow for latency
180 2056 : sbus_frame_interval = (1000UL * 1000UL) / rate - 500;
181 : // at 100,000 bps, a 300 bit sbus frame takes 3msec to transfer
182 : // require a minimum 700usec interframe gap
183 2056 : if (sbus_frame_interval < 3700) {
184 0 : sbus_frame_interval = 3700;
185 : }
186 :
187 2056 : AP_SerialManager *serial_manager = AP_SerialManager::get_singleton();
188 2056 : if (!serial_manager) {
189 0 : return;
190 : }
191 2056 : sbus1_uart = serial_manager->find_serial(AP_SerialManager::SerialProtocol_Sbus1,0);
192 :
193 2056 : if (sbus1_uart == nullptr) {
194 2056 : return;
195 : }
196 :
197 : // update baud param in case user looks at it
198 0 : serial_manager->set_and_default_baud(AP_SerialManager::SerialProtocol_Sbus1, 0, 100000);
199 :
200 0 : auto &uart = *sbus1_uart;
201 :
202 0 : uart.begin(100000, 16, 32);
203 0 : uart.configure_parity(2); // enable even parity
204 0 : uart.set_stop_bits(2);
205 0 : uart.set_unbuffered_writes(true);
206 0 : uart.set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
207 : }
208 :
209 : #endif // AP_SBUSOUTPUT_ENABLED
|
