-
Notifications
You must be signed in to change notification settings - Fork 1.1k
/
main_rctx.c
265 lines (221 loc) · 5.77 KB
/
main_rctx.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
/*
- 0x101..0x10F reserved for rc transmitters
- rc transmitters broadcast their messages (dest = 0xFFFF)
- aircrafts are configured (conf.xml) to one rc transmitter (e.g. 0x102)
- md5 sum be broadcasted?
- rc tx receives md5 sum messages and displays that on LEDs
- rc tx is switchable in addresses by button
- XBee-message
ID is AC_ID for aircraft, 0x100 for ground station
1 A XBEE_START (0x7E)
2 B LENGTH_MSB (A->E)
3 C LENGTH_LSB
XBEE_PAYLOAD
4 0 XBEE_TX16 (0x01) / XBEE_RX16 (0x81)
5 1 FRAME_ID (0) / SRC_ID_MSB
6 2 DEST_ID_MSB / SRC_ID_LSB
7 3 DEST_ID_LSB / XBEE_RSSI
8 4 TX16_OPTIONS (0) / RX16_OPTIONS
PPRZ_DATA
9 0 SENDER_ID
10 1 MSG_ID
MSG_PAYLOAD
11 0 RCTX_MODE
12 1 THOTTLE_LSB
13 2 THOTTLE_MSB
14 3 ROLL_LSB
15 4 ROLL_MSB
16 5 PITCH_LSB
17 6 PITCH_MSB
18 E XBEE_CHECKSUM (sum[A->D])
- messages.xml
<message name="RC_3CH" ID="27">
<field name="mode" type="uint8" unit="byte_mask"></field>
<field name="throttle" type="int16" unit="pprz" format="%d"/>
<field name="roll" type="int16" unit="pprz" format="%d"/>
<field name="pitch" type="int16" unit="pprz" format="%d"/>
</message>
- xbee.h
#ifdef USE_DOWNLINK_BROADCAST
#define GROUND_STATION_ADDR 0xFFFF
#else
#define GROUND_STATION_ADDR 0x0100
#endif
- datalink.c
#ifdef USE_RC_TELEMETRY
if (msg_id == DL_RC_3CH && DL_RC_3CH_ac_id(dl_buffer) == TX_ID) {
LED_TOGGLE(3);
bla_throttle = DL_RC_3CH_throttle(dl_buffer);
bla_roll = DL_RC_3CH_roll(dl_buffer);
bla_pitch = DL_RC_3CH_pitch(dl_buffer);
} else
#endif // USE_RC_TELEMETRY
*/
#include <stdio.h>
#include "std.h"
#include "init_hw.h"
#include "sys_time.h"
#include "adc.h"
#include "led.h"
#include "interrupt_hw.h"
#include "uart_hw.h"
#include "uart.h"
#include "autopilot.h"
#include "subsystems/datalink/datalink.h"
#include "messages.h"
#include "subsystems/datalink/downlink.h"
#if ((AC_ID > 0x108) || (AC_ID < (0x101)))
//#error aircraft ID should be 0x101..0x108 for RC transmitter
#endif
/** Maximum time allowed for low battery level */
#define LOW_BATTERY_DELAY 5
#ifdef ADC
struct adc_buf vsupply_adc_buf;
#ifndef VoltageOfAdc
#define VoltageOfAdc(adc) DefaultVoltageOfAdc(adc)
#endif
#endif
#define LOW_BATTERY_DECIVOLT (CATASTROPHIC_BAT_LEVEL*10)
uint16_t rctx_vsupply_decivolt;
uint8_t rctx_under_voltage;
uint8_t rctx_mode;
void init_rctx( void );
void event_task_rctx( void);
void periodic_task_rctx( void );
// datalink.c
#define SenderIdOfMsg(x) (x[0])
#define IdOfMsg(x) (x[1])
uint8_t dl_buffer[MSG_SIZE] __attribute__ ((aligned));
bool dl_msg_available;
uint16_t datalink_time;
void dl_parse_msg(void) {
datalink_time = 0;
uint8_t msg_id = IdOfMsg(dl_buffer);
if (msg_id == DL_PING) {
DOWNLINK_SEND_PONG();
} else
{ /* Last else */ }
}
/********** MAIN *************************************************************/
int main( void ) {
init_rctx();
while(1) {
if (sys_time_periodic())
periodic_task_rctx();
event_task_rctx();
}
return 0;
}
/********** INIT *************************************************************/
void init_rctx( void ) {
hw_init();
sys_time_init();
#ifdef LED
led_init();
#endif
#ifdef USE_UART1
Uart1Init();
#endif
#ifdef ADC
adc_init();
adc_buf_channel(ADC_CHANNEL_VSUPPLY, &vsupply_adc_buf, DEFAULT_AV_NB_SAMPLE);
#endif
#ifdef RADIO_CONTROL
ppm_init();
#endif
int_enable();
/** - wait 0.5s (for modem init ?) */
uint8_t init_cpt = 30;
while (init_cpt) {
if (sys_time_periodic())
init_cpt--;
}
#if defined DATALINK
#if DATALINK == XBEE
xbee_init();
#endif
#endif /* DATALINK */
}
/********** EVENT ************************************************************/
void event_task_rctx( void) {
#ifdef RADIO_CONTROL
if (ppm_valid) {
ppm_valid = FALSE;
radio_control_event_task();
#ifdef USE_RCTX_MODE_SWITCH
// TODO: set rxtx_mode from GPIO connected switch (e.g. I2C pins)
#else
rctx_mode = AP_MODE_OF_PULSE(rc_values[RADIO_MODE], 0) & 3;
#endif
rctx_mode |= rctx_under_voltage << 2;
LED_TOGGLE(3);
if (1)
// TODO: check XBee busy pin
// TODO: send only if aircraft is listening
// TODO: send (here) only in auto1 and manual
{
DOWNLINK_SEND_RC_3CH(
&rctx_mode,
&rc_values[RADIO_THROTTLE],
&rc_values[RADIO_ROLL],
&rc_values[RADIO_PITCH]);
}
}
#endif
#if defined DATALINK
#if DATALINK == XBEE
if (XBeeBuffer()) {
ReadXBeeBuffer();
if (xbee_msg_received) {
xbee_parse_payload();
xbee_msg_received = FALSE;
}
}
#endif
if (dl_msg_available) {
dl_parse_msg();
dl_msg_available = FALSE;
}
#endif /* DATALINK */
}
/************* PERIODIC ******************************************************/
void periodic_task_rctx( void ) {
static uint8_t _10Hz = 0;
static uint8_t _1Hz = 0;
_10Hz++;
if (_10Hz >= 6) _10Hz = 0;
_1Hz++;
if (_1Hz>=60) _1Hz=0;
#ifdef RADIO_CONTROL
radio_control_periodic_task();
#endif
#ifdef DOWNLINK
// TODO: needed? fbw_downlink_periodic_task();
#endif
#ifdef ADC
if (!_10Hz)
rctx_vsupply_decivolt = VoltageOfAdc((10*(vsupply_adc_buf.sum/vsupply_adc_buf.av_nb_sample)));
if (!_1Hz) {
static uint8_t t = 0;
if (rctx_vsupply_decivolt < LOW_BATTERY_DECIVOLT) {
t++;
} else {
t = 0;
rctx_under_voltage = 0;
}
if (t >= LOW_BATTERY_DELAY) {
LED_TOGGLE(1);
rctx_under_voltage = 1;
}
if (0)
// TODO: send (here) only in auto2
{
DOWNLINK_SEND_RC_3CH(
&rctx_mode,
&rc_values[RADIO_THROTTLE],
&rc_values[RADIO_ROLL],
&rc_values[RADIO_PITCH]);
}
}
#endif
}