Paparazzi UAS
v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
actuators_sbus.c
Go to the documentation of this file.
1
/*
2
* Copyright (C) 2015 Freek van Tienen <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
26
27
#include "
modules/actuators/actuators.h
"
28
#include "
modules/actuators/actuators_sbus.h
"
29
#include "generated/airframe.h"
30
#include "
mcu_periph/uart.h
"
31
32
/* Currently we only support 7 channels */
33
#if SERVOS_SBUS_NB > ACTUATORS_SBUS_MAX_NB
34
#error SBUS actuators only support less then 7 servos
35
#endif
36
37
/* Calculate the frequency divider to aim at 7 ms */
38
#if PERIODIC_FREQUENCY < 150
39
#error Sbus actuators need at leest a frequency of 150 Hz
40
#else
41
static
uint8_t
freq_trig = PERIODIC_FREQUENCY / 142.0 + 0.5;
// Round it to nearest value
42
#endif
43
44
/* Main actuator structure */
45
struct
ActuatorsSbus
actuators_sbus
;
46
static
inline
void
actuators_sbus_send
(
struct
link_device *
dev
);
47
48
/*
49
* Initialize the sbus devices (UART output devices)
50
*/
51
void
actuators_sbus_init
(
void
)
52
{
53
actuators_sbus
.device = &((ACTUATORS_SBUS_DEV).device);
54
55
uart_periph_set_bits_stop_parity
(&ACTUATORS_SBUS_DEV,
UBITS_8
,
USTOP_2
,
UPARITY_EVEN
);
56
uart_periph_set_baudrate
(&ACTUATORS_SBUS_DEV,
B100000
);
57
// Try to invert RX data logic when available in hardware periph
58
uart_periph_invert_data_logic
(&ACTUATORS_SBUS_DEV,
true
,
true
);
59
}
60
61
/*
62
* Transmit the sbus output at 1 / 7ms
63
*/
64
void
actuators_sbus_commit
(
void
)
65
{
66
static
uint8_t
cnt = 0;
67
68
// Only send every 7 ms
69
cnt++;
70
if
(cnt == freq_trig) {
71
actuators_sbus_send
(
actuators_sbus
.device);
72
cnt = 0;
73
}
74
}
75
76
void
actuators_sbus_set
(
uint8_t
idx
,
int16_t
value)
77
{
78
actuators_sbus
.cmds[
idx
] = value;
79
}
80
81
/*
82
* Actually transmit the sbus output on a link device
83
*
84
* The protocol is 25 Byte long and is send every 14ms (analog mode) or 7ms (highspeed mode).
85
* One Byte = 1 startbit + 8 databit + 1 paritybit + 2 stopbit (8E2), baudrate = 100'000 bit/s
86
* The highest bit is send first. The logic is inverted (Level High = 1)
87
*
88
* [startbyte] [data1] [data2] .... [data22] [flags][endbyte]
89
*
90
* SBUS protocol
91
*/
92
93
#define SBUS_START_BYTE 0x0f
94
#define SBUS_END_BYTE 0x00
95
96
#define SBUS_BIT_PER_CHANNEL 11
97
98
static
inline
void
actuators_sbus_send
(
struct
link_device *
dev
)
99
{
100
uint8_t
frame
[25] = {0};
101
102
frame
[0] =
SBUS_START_BYTE
;
103
frame
[1] = (
actuators_sbus
.cmds[0] & 0x07FF);
104
frame
[2] = (
actuators_sbus
.cmds[0] & 0x07FF) >> 8 | (
actuators_sbus
.cmds[1] & 0x07FF) << 3;
105
frame
[3] = (
actuators_sbus
.cmds[1] & 0x07FF) >> 5 | (
actuators_sbus
.cmds[2] & 0x07FF) << 6;
106
frame
[4] = (
actuators_sbus
.cmds[2] & 0x07FF) >> 2;
107
frame
[5] = (
actuators_sbus
.cmds[2] & 0x07FF) >> 10 | (
actuators_sbus
.cmds[3] & 0x07FF) << 1;
108
frame
[6] = (
actuators_sbus
.cmds[3] & 0x07FF) >> 7 | (
actuators_sbus
.cmds[4] & 0x07FF) << 4;
109
frame
[7] = (
actuators_sbus
.cmds[4] & 0x07FF) >> 4 | (
actuators_sbus
.cmds[5] & 0x07FF) << 7;
110
frame
[8] = (
actuators_sbus
.cmds[5] & 0x07FF) >> 1;
111
frame
[9] = (
actuators_sbus
.cmds[5] & 0x07FF) >> 9 | (
actuators_sbus
.cmds[6] & 0x07FF) << 2;
112
frame
[10] = (
actuators_sbus
.cmds[6] & 0x07FF) >> 6 | (
actuators_sbus
.cmds[7] & 0x07FF) << 5;
113
frame
[11] = (
actuators_sbus
.cmds[7] & 0x07FF) >> 3;
114
frame
[12] = (
actuators_sbus
.cmds[8] & 0x07FF);
115
frame
[13] = (
actuators_sbus
.cmds[8] & 0x07FF) >> 8 | (
actuators_sbus
.cmds[9] & 0x07FF) << 3;
116
frame
[14] = (
actuators_sbus
.cmds[9] & 0x07FF) >> 5 | (
actuators_sbus
.cmds[10] & 0x07FF) << 6;
117
frame
[15] = (
actuators_sbus
.cmds[10] & 0x07FF) >> 2;
118
frame
[16] = (
actuators_sbus
.cmds[10] & 0x07FF) >> 10 | (
actuators_sbus
.cmds[11] & 0x07FF) << 1;
119
frame
[17] = (
actuators_sbus
.cmds[11] & 0x07FF) >> 7 | (
actuators_sbus
.cmds[12] & 0x07FF) << 4;
120
frame
[18] = (
actuators_sbus
.cmds[12] & 0x07FF) >> 4 | (
actuators_sbus
.cmds[13] & 0x07FF) << 7;
121
frame
[19] = (
actuators_sbus
.cmds[13] & 0x07FF) >> 1;
122
frame
[20] = (
actuators_sbus
.cmds[13] & 0x07FF) >> 9 | (
actuators_sbus
.cmds[14] & 0x07FF) << 2;
123
frame
[21] = (
actuators_sbus
.cmds[14] & 0x07FF) >> 6 | (
actuators_sbus
.cmds[15] & 0x07FF) << 5;
124
frame
[22] = (
actuators_sbus
.cmds[15] & 0x07FF) >> 3;
125
frame
[23] = 0x00;
// No frame lost, switches off, no failsafe
126
frame
[24] =
SBUS_END_BYTE
;
// stop byte
127
128
dev
->put_buffer(
dev
->periph, 0,
frame
, 25);
129
130
}
SBUS_START_BYTE
#define SBUS_START_BYTE
Definition
actuators_sbus.c:93
SBUS_END_BYTE
#define SBUS_END_BYTE
Definition
actuators_sbus.c:94
actuators_sbus_send
static void actuators_sbus_send(struct link_device *dev)
Definition
actuators_sbus.c:98
actuators_sbus
struct ActuatorsSbus actuators_sbus
Definition
actuators_sbus.c:45
actuators_sbus_init
void actuators_sbus_init(void)
Definition
actuators_sbus.c:51
actuators_sbus_set
void actuators_sbus_set(uint8_t idx, int16_t value)
Definition
actuators_sbus.c:76
actuators_sbus_commit
void actuators_sbus_commit(void)
Definition
actuators_sbus.c:64
actuators_sbus.h
Sbus actuator driver, which can output as 7 sbus channels at ~11ms.
ActuatorsSbus
Definition
actuators_sbus.h:36
frame
static uint8_t frame[20]
Definition
cc2500_frsky_x.c:163
B100000
#define B100000
Definition
uart_arch.h:47
dev
static struct uart_periph * dev
Definition
e_identification_fr.c:40
uart_periph_set_bits_stop_parity
void uart_periph_set_bits_stop_parity(struct uart_periph *periph, uint8_t bits, uint8_t stop, uint8_t parity)
Definition
uart_arch.c:296
uart_periph_set_baudrate
void uart_periph_set_baudrate(struct uart_periph *periph, uint32_t baud)
Definition
uart_arch.c:280
actuators.h
Hardware independent API for actuators (servos, motor controllers).
idx
static uint32_t idx
Definition
nps_radio_control_spektrum.c:105
UBITS_8
#define UBITS_8
Definition
serial_port.c:50
USTOP_2
#define USTOP_2
Definition
serial_port.c:53
UPARITY_EVEN
#define UPARITY_EVEN
Definition
serial_port.c:57
uart_periph_invert_data_logic
void WEAK uart_periph_invert_data_logic(struct uart_periph *p, bool invert_rx, bool invert_tx)
Definition
uart.c:194
uart.h
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
int16_t
short int16_t
Typedef defining 16 bit short type.
Definition
vl53l1_types.h:93
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition
vl53l1_types.h:98
sw
airborne
modules
actuators
actuators_sbus.c
Generated on
for Paparazzi UAS by
1.15.0