/
main_ap.c
813 lines (665 loc) · 21.4 KB
/
main_ap.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
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
/*
* Copyright (C) 2003-2010 The Paparazzi Team
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file firmwares/fixedwing/main_ap.c
*
* AP ( AutoPilot ) tasks
*
* This process is reponsible for the collecting the different sensors data,
* calling the appropriate estimation algorithms and running the different control loops.
*/
#define MODULES_C
#define ABI_C
#include <math.h>
#include "firmwares/fixedwing/main_ap.h"
#include "mcu.h"
#include "mcu_periph/sys_time.h"
#include "inter_mcu.h"
#include "link_mcu.h"
// Sensors
#if USE_GPS
#include "subsystems/gps.h"
#endif
#if USE_IMU
#include "subsystems/imu.h"
#endif
#if USE_AHRS
#include "subsystems/ahrs.h"
#endif
#if USE_AHRS_ALIGNER
#include "subsystems/ahrs/ahrs_aligner.h"
#endif
#if USE_BARO_BOARD
#include "subsystems/sensors/baro.h"
PRINT_CONFIG_MSG_VALUE("USE_BARO_BOARD is TRUE, reading onboard baro: ", BARO_BOARD)
#endif
#include "subsystems/ins.h"
// autopilot & control
#include "state.h"
#include "firmwares/fixedwing/autopilot.h"
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
#include CTRL_TYPE_H
#include "firmwares/fixedwing/nav.h"
#include "generated/flight_plan.h"
#ifdef TRAFFIC_INFO
#include "subsystems/navigation/traffic_info.h"
#endif
// datalink & telemetry
#include "subsystems/datalink/datalink.h"
#include "subsystems/datalink/telemetry.h"
#include "subsystems/settings.h"
#include "subsystems/datalink/xbee.h"
#include "subsystems/datalink/w5100.h"
// modules & settings
#include "generated/modules.h"
#include "generated/settings.h"
#if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
#include "rc_settings.h"
#endif
#include "subsystems/abi.h"
#include "led.h"
#ifdef USE_NPS
#include "nps_autopilot.h"
#endif
/* Default trim commands for roll, pitch and yaw */
#ifndef COMMAND_ROLL_TRIM
#define COMMAND_ROLL_TRIM 0
#endif
#ifndef COMMAND_PITCH_TRIM
#define COMMAND_PITCH_TRIM 0
#endif
#ifndef COMMAND_YAW_TRIM
#define COMMAND_YAW_TRIM 0
#endif
/* if PRINT_CONFIG is defined, print some config options */
PRINT_CONFIG_VAR(PERIODIC_FREQUENCY)
PRINT_CONFIG_VAR(NAVIGATION_FREQUENCY)
PRINT_CONFIG_VAR(CONTROL_FREQUENCY)
/* TELEMETRY_FREQUENCY is defined in generated/periodic_telemetry.h
* defaults to 60Hz or set by TELEMETRY_FREQUENCY configure option in airframe file
*/
PRINT_CONFIG_VAR(TELEMETRY_FREQUENCY)
/* MODULES_FREQUENCY is defined in generated/modules.h
* according to main_freq parameter set for modules in airframe file
*/
PRINT_CONFIG_VAR(MODULES_FREQUENCY)
#if USE_BARO_BOARD
#ifndef BARO_PERIODIC_FREQUENCY
#define BARO_PERIODIC_FREQUENCY 50
#endif
PRINT_CONFIG_VAR(BARO_PERIODIC_FREQUENCY)
#endif
#if USE_AHRS && USE_IMU
#ifdef AHRS_PROPAGATE_FREQUENCY
#if (AHRS_PROPAGATE_FREQUENCY > PERIODIC_FREQUENCY)
#warning "PERIODIC_FREQUENCY should be least equal or greater than AHRS_PROPAGATE_FREQUENCY"
INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY to at least ",AHRS_PROPAGATE_FREQUENCY)
#endif
#endif
static inline void on_gyro_event( void );
static inline void on_accel_event( void );
static inline void on_mag_event( void );
volatile uint8_t ahrs_timeout_counter = 0;
//FIXME not the correct place
static void send_filter_status(void) {
uint8_t mde = 3;
if (ahrs.status == AHRS_UNINIT) mde = 2;
if (ahrs_timeout_counter > 10) mde = 5;
uint16_t val = 0;
DOWNLINK_SEND_STATE_FILTER_STATUS(DefaultChannel, DefaultDevice, &mde, &val);
}
#endif // USE_AHRS && USE_IMU
#if USE_GPS
static inline void on_gps_solution( void );
#endif
// what version is this ????
static const uint16_t version = 1;
#if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
static uint8_t mcu1_ppm_cpt;
#endif
tid_t modules_tid; ///< id for modules_periodic_task() timer
tid_t telemetry_tid; ///< id for telemetry_periodic() timer
tid_t sensors_tid; ///< id for sensors_task() timer
tid_t attitude_tid; ///< id for attitude_loop() timer
tid_t navigation_tid; ///< id for navigation_task() timer
tid_t monitor_tid; ///< id for monitor_task() timer
#if USE_BARO_BOARD
tid_t baro_tid; ///< id for baro_periodic() timer
#endif
void init_ap( void ) {
#ifndef SINGLE_MCU /** init done in main_fbw in single MCU */
mcu_init();
#endif /* SINGLE_MCU */
/****** initialize and reset state interface ********/
stateInit();
/************* Sensors initialization ***************/
#if USE_GPS
gps_init();
#endif
#if USE_IMU
imu_init();
#endif
#if USE_AHRS_ALIGNER
ahrs_aligner_init();
#endif
#if USE_AHRS
ahrs_init();
#endif
#if USE_AHRS && USE_IMU
register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_filter_status);
#endif
#if USE_BARO_BOARD
baro_init();
#endif
ins_init();
/************* Links initialization ***************/
#if defined MCU_SPI_LINK || defined MCU_UART_LINK || defined MCU_CAN_LINK
link_mcu_init();
#endif
#if USE_AUDIO_TELEMETRY
audio_telemetry_init();
#endif
/************ Internal status ***************/
autopilot_init();
h_ctl_init();
v_ctl_init();
nav_init();
modules_init();
settings_init();
/**** start timers for periodic functions *****/
sensors_tid = sys_time_register_timer(1./PERIODIC_FREQUENCY, NULL);
navigation_tid = sys_time_register_timer(1./NAVIGATION_FREQUENCY, NULL);
attitude_tid = sys_time_register_timer(1./CONTROL_FREQUENCY, NULL);
modules_tid = sys_time_register_timer(1./MODULES_FREQUENCY, NULL);
telemetry_tid = sys_time_register_timer(1./TELEMETRY_FREQUENCY, NULL);
monitor_tid = sys_time_register_timer(1.0, NULL);
#if USE_BARO_BOARD
baro_tid = sys_time_register_timer(1./BARO_PERIODIC_FREQUENCY, NULL);
#endif
/** - start interrupt task */
mcu_int_enable();
#if defined DATALINK
#if DATALINK == XBEE
xbee_init();
#endif
#if DATALINK == W5100
w5100_init();
#endif
#endif /* DATALINK */
#if defined AEROCOMM_DATA_PIN
IO0DIR |= _BV(AEROCOMM_DATA_PIN);
IO0SET = _BV(AEROCOMM_DATA_PIN);
#endif
/************ Multi-uavs status ***************/
#ifdef TRAFFIC_INFO
traffic_info_init();
#endif
/* set initial trim values.
* these are passed to fbw via inter_mcu.
*/
ap_state->command_roll_trim = COMMAND_ROLL_TRIM;
ap_state->command_pitch_trim = COMMAND_PITCH_TRIM;
ap_state->command_yaw_trim = COMMAND_YAW_TRIM;
}
void handle_periodic_tasks_ap(void) {
if (sys_time_check_and_ack_timer(sensors_tid))
sensors_task();
#if USE_BARO_BOARD
if (sys_time_check_and_ack_timer(baro_tid))
baro_periodic();
#endif
if (sys_time_check_and_ack_timer(navigation_tid))
navigation_task();
#ifndef AHRS_TRIGGERED_ATTITUDE_LOOP
if (sys_time_check_and_ack_timer(attitude_tid))
attitude_loop();
#endif
if (sys_time_check_and_ack_timer(modules_tid))
modules_periodic_task();
if (sys_time_check_and_ack_timer(monitor_tid))
monitor_task();
if (sys_time_check_and_ack_timer(telemetry_tid)) {
reporting_task();
LED_PERIODIC();
}
}
/******************** Interaction with FBW *****************************/
/** Update paparazzi mode.
*/
#if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
static inline uint8_t pprz_mode_update( void ) {
if ((pprz_mode != PPRZ_MODE_HOME &&
pprz_mode != PPRZ_MODE_GPS_OUT_OF_ORDER)
#ifdef UNLOCKED_HOME_MODE
|| TRUE
#endif
) {
#ifndef RADIO_AUTO_MODE
return ModeUpdate(pprz_mode, PPRZ_MODE_OF_PULSE(fbw_state->channels[RADIO_MODE]));
#else
INFO("Using RADIO_AUTO_MODE to switch between AUTO1 and AUTO2.")
/* If RADIO_AUTO_MODE is enabled mode swithing will be seperated between two switches/channels
* RADIO_MODE will switch between PPRZ_MODE_MANUAL and any PPRZ_MODE_AUTO mode selected by RADIO_AUTO_MODE.
*
* This is mainly a cludge for entry level radios with no three-way switch but two available two-way switches which can be used.
*/
if(PPRZ_MODE_OF_PULSE(fbw_state->channels[RADIO_MODE]) == PPRZ_MODE_MANUAL) {
/* RADIO_MODE in MANUAL position */
return ModeUpdate(pprz_mode, PPRZ_MODE_MANUAL);
} else {
/* RADIO_MODE not in MANUAL position.
* Select AUTO mode bassed on RADIO_AUTO_MODE channel
*/
return ModeUpdate(pprz_mode, (fbw_state->channels[RADIO_AUTO_MODE] > THRESHOLD2) ? PPRZ_MODE_AUTO2 : PPRZ_MODE_AUTO1);
}
#endif // RADIO_AUTO_MODE
} else
return FALSE;
}
#else // not RADIO_CONTROL
static inline uint8_t pprz_mode_update( void ) {
return FALSE;
}
#endif
static inline uint8_t mcu1_status_update( void ) {
uint8_t new_status = fbw_state->status;
if (mcu1_status != new_status) {
bool_t changed = ((mcu1_status&MASK_FBW_CHANGED) != (new_status&MASK_FBW_CHANGED));
mcu1_status = new_status;
return changed;
}
return FALSE;
}
/** Send back uncontrolled channels.
*/
static inline void copy_from_to_fbw ( void ) {
#ifdef SetAutoCommandsFromRC
SetAutoCommandsFromRC(ap_state->commands, fbw_state->channels);
#elif defined RADIO_YAW && defined COMMAND_YAW
ap_state->commands[COMMAND_YAW] = fbw_state->channels[RADIO_YAW];
#endif
}
/** mode to enter when RC is lost in PPRZ_MODE_MANUAL or PPRZ_MODE_AUTO1 */
#ifndef RC_LOST_MODE
#define RC_LOST_MODE PPRZ_MODE_HOME
#endif
/**
* Function to be called when a message from FBW is available
*/
static inline void telecommand_task( void ) {
uint8_t mode_changed = FALSE;
copy_from_to_fbw();
uint8_t really_lost = bit_is_set(fbw_state->status, STATUS_RADIO_REALLY_LOST) && (pprz_mode == PPRZ_MODE_AUTO1 || pprz_mode == PPRZ_MODE_MANUAL);
if (pprz_mode != PPRZ_MODE_HOME && pprz_mode != PPRZ_MODE_GPS_OUT_OF_ORDER && launch) {
if (too_far_from_home) {
pprz_mode = PPRZ_MODE_HOME;
mode_changed = TRUE;
}
if (really_lost) {
pprz_mode = RC_LOST_MODE;
mode_changed = TRUE;
}
}
if (bit_is_set(fbw_state->status, AVERAGED_CHANNELS_SENT)) {
bool_t pprz_mode_changed = pprz_mode_update();
mode_changed |= pprz_mode_changed;
#if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS
bool_t calib_mode_changed = RcSettingsModeUpdate(fbw_state->channels);
rc_settings(calib_mode_changed || pprz_mode_changed);
mode_changed |= calib_mode_changed;
#endif
}
mode_changed |= mcu1_status_update();
if ( mode_changed ) autopilot_send_mode();
#if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
/** In AUTO1 mode, compute roll setpoint and pitch setpoint from
* \a RADIO_ROLL and \a RADIO_PITCH \n
*/
if (pprz_mode == PPRZ_MODE_AUTO1) {
/** Roll is bounded between [-AUTO1_MAX_ROLL;AUTO1_MAX_ROLL] */
h_ctl_roll_setpoint = FLOAT_OF_PPRZ(fbw_state->channels[RADIO_ROLL], 0., AUTO1_MAX_ROLL);
/** Pitch is bounded between [-AUTO1_MAX_PITCH;AUTO1_MAX_PITCH] */
h_ctl_pitch_setpoint = FLOAT_OF_PPRZ(fbw_state->channels[RADIO_PITCH], 0., AUTO1_MAX_PITCH);
} /** Else asynchronously set by \a h_ctl_course_loop() */
/** In AUTO1, throttle comes from RADIO_THROTTLE
In MANUAL, the value is copied to get it in the telemetry */
if (pprz_mode == PPRZ_MODE_MANUAL || pprz_mode == PPRZ_MODE_AUTO1) {
v_ctl_throttle_setpoint = fbw_state->channels[RADIO_THROTTLE];
}
/** else asynchronously set by v_ctl_climb_loop(); */
mcu1_ppm_cpt = fbw_state->ppm_cpt;
#endif // RADIO_CONTROL
vsupply = fbw_state->vsupply;
current = fbw_state->current;
energy = fbw_state->energy;
#ifdef RADIO_CONTROL
/* the SITL check is a hack to prevent "automatic" launch in NPS */
#ifndef SITL
if (!autopilot_flight_time) {
if (pprz_mode == PPRZ_MODE_AUTO2 && fbw_state->channels[RADIO_THROTTLE] > THROTTLE_THRESHOLD_TAKEOFF) {
launch = TRUE;
}
}
#endif
#endif
}
/**************************** Periodic tasks ***********************************/
/**
* Send a series of initialisation messages followed by a stream of periodic ones.
* Called at 60Hz.
*/
void reporting_task( void ) {
static uint8_t boot = TRUE;
/** initialisation phase during boot */
if (boot) {
DOWNLINK_SEND_BOOT(DefaultChannel, DefaultDevice, &version);
boot = FALSE;
}
/** then report periodicly */
else {
//PeriodicSendAp(DefaultChannel, DefaultDevice);
periodic_telemetry_send_Ap();
}
}
#ifdef FAILSAFE_DELAY_WITHOUT_GPS
#define GpsTimeoutError (sys_time.nb_sec - gps.last_3dfix_time > FAILSAFE_DELAY_WITHOUT_GPS)
#endif
/**
* Compute desired_course
*/
void navigation_task( void ) {
#if defined FAILSAFE_DELAY_WITHOUT_GPS
/** This section is used for the failsafe of GPS */
static uint8_t last_pprz_mode;
/** If aircraft is launched and is in autonomus mode, go into
PPRZ_MODE_GPS_OUT_OF_ORDER mode (Failsafe) if we lost the GPS */
if (launch) {
if (GpsTimeoutError) {
if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME) {
last_pprz_mode = pprz_mode;
pprz_mode = PPRZ_MODE_GPS_OUT_OF_ORDER;
autopilot_send_mode();
gps_lost = TRUE;
}
} else if (gps_lost) { /* GPS is ok */
/** If aircraft was in failsafe mode, come back in previous mode */
pprz_mode = last_pprz_mode;
gps_lost = FALSE;
autopilot_send_mode();
}
}
#endif /* GPS && FAILSAFE_DELAY_WITHOUT_GPS */
common_nav_periodic_task_4Hz();
if (pprz_mode == PPRZ_MODE_HOME)
nav_home();
else if (pprz_mode == PPRZ_MODE_GPS_OUT_OF_ORDER)
nav_without_gps();
else
nav_periodic_task();
#ifdef TCAS
CallTCAS();
#endif
#ifndef PERIOD_NAVIGATION_Ap_0 // If not sent periodically (in default 0 mode)
SEND_NAVIGATION(DefaultChannel, DefaultDevice);
#endif
/* The nav task computes only nav_altitude. However, we are interested
by desired_altitude (= nav_alt+alt_shift) in any case.
So we always run the altitude control loop */
if (v_ctl_mode == V_CTL_MODE_AUTO_ALT)
v_ctl_altitude_loop();
if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME
|| pprz_mode == PPRZ_MODE_GPS_OUT_OF_ORDER) {
#ifdef H_CTL_RATE_LOOP
/* Be sure to be in attitude mode, not roll */
h_ctl_auto1_rate = FALSE;
#endif
if (lateral_mode >=LATERAL_MODE_COURSE)
h_ctl_course_loop(); /* aka compute nav_desired_roll */
// climb_loop(); //4Hz
}
}
#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
volatile uint8_t new_ins_attitude = 0;
#endif
void attitude_loop( void ) {
#if USE_INFRARED
ahrs_update_infrared();
#endif /* USE_INFRARED */
if (pprz_mode >= PPRZ_MODE_AUTO2)
{
if (v_ctl_mode == V_CTL_MODE_AUTO_THROTTLE) {
v_ctl_throttle_setpoint = nav_throttle_setpoint;
v_ctl_pitch_setpoint = nav_pitch;
}
else if (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB)
{
v_ctl_climb_loop();
}
#if defined V_CTL_THROTTLE_IDLE
Bound(v_ctl_throttle_setpoint, TRIM_PPRZ(V_CTL_THROTTLE_IDLE*MAX_PPRZ), MAX_PPRZ);
#endif
#ifdef V_CTL_POWER_CTL_BAT_NOMINAL
if (vsupply > 0.) {
v_ctl_throttle_setpoint *= 10. * V_CTL_POWER_CTL_BAT_NOMINAL / (float)vsupply;
v_ctl_throttle_setpoint = TRIM_UPPRZ(v_ctl_throttle_setpoint);
}
#endif
h_ctl_pitch_setpoint = v_ctl_pitch_setpoint; // Copy the pitch setpoint from the guidance to the stabilization control
Bound(h_ctl_pitch_setpoint, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT);
if (kill_throttle || (!autopilot_flight_time && !launch))
v_ctl_throttle_setpoint = 0;
}
h_ctl_attitude_loop(); /* Set h_ctl_aileron_setpoint & h_ctl_elevator_setpoint */
v_ctl_throttle_slew();
ap_state->commands[COMMAND_THROTTLE] = v_ctl_throttle_slewed;
ap_state->commands[COMMAND_ROLL] = -h_ctl_aileron_setpoint;
ap_state->commands[COMMAND_PITCH] = h_ctl_elevator_setpoint;
#if defined MCU_SPI_LINK || defined MCU_UART_LINK || defined MCU_CAN_LINK
link_mcu_send();
#elif defined INTER_MCU && defined SINGLE_MCU
/**Directly set the flag indicating to FBW that shared buffer is available*/
inter_mcu_received_ap = TRUE;
#endif
}
/** Run at PERIODIC_FREQUENCY (60Hz if not defined) */
void sensors_task( void ) {
#if USE_IMU
imu_periodic();
#if USE_AHRS
if (ahrs_timeout_counter < 255)
ahrs_timeout_counter ++;
#endif // USE_AHRS
#endif // USE_IMU
//FIXME: this is just a kludge
#if USE_AHRS && defined SITL && !USE_NPS
// dt is not really used in ahrs_sim
ahrs_propagate(1./PERIODIC_FREQUENCY);
#endif
#if USE_GPS
gps_periodic_check();
#endif
ins_periodic();
}
#ifdef LOW_BATTERY_KILL_DELAY
#warning LOW_BATTERY_KILL_DELAY has been renamed to CATASTROPHIC_BAT_KILL_DELAY, please update your airframe file!
#endif
/** Maximum time allowed for catastrophic battery level before going into kill mode */
#ifndef CATASTROPHIC_BAT_KILL_DELAY
#define CATASTROPHIC_BAT_KILL_DELAY 5
#endif
/** Maximum distance from HOME waypoint before going into kill mode */
#ifndef KILL_MODE_DISTANCE
#define KILL_MODE_DISTANCE (1.5*MAX_DIST_FROM_HOME)
#endif
/** Define minimal speed for takeoff in m/s */
#define MIN_SPEED_FOR_TAKEOFF 5.
/** monitor stuff run at 1Hz */
void monitor_task( void ) {
if (autopilot_flight_time)
autopilot_flight_time++;
#if defined DATALINK || defined SITL
datalink_time++;
#endif
static uint8_t t = 0;
if (vsupply < CATASTROPHIC_BAT_LEVEL*10)
t++;
else
t = 0;
kill_throttle |= (t >= CATASTROPHIC_BAT_KILL_DELAY);
kill_throttle |= launch && (dist2_to_home > Square(KILL_MODE_DISTANCE));
if (!autopilot_flight_time &&
*stateGetHorizontalSpeedNorm_f() > MIN_SPEED_FOR_TAKEOFF) {
autopilot_flight_time = 1;
launch = TRUE; /* Not set in non auto launch */
uint16_t time_sec = sys_time.nb_sec;
DOWNLINK_SEND_TAKEOFF(DefaultChannel, DefaultDevice, &time_sec);
}
}
/*********** EVENT ***********************************************************/
void event_task_ap( void ) {
#ifndef SINGLE_MCU
#if USE_I2C0 || USE_I2C1 || USE_I2C2 || USE_I2C3
i2c_event();
#endif
#endif
#if USE_AHRS && USE_IMU
ImuEvent(on_gyro_event, on_accel_event, on_mag_event);
#endif
#ifdef InsEvent
TODO("calling InsEvent, remove me..")
InsEvent(NULL);
#endif
#if USE_GPS
GpsEvent(on_gps_solution);
#endif /* USE_GPS */
#if USE_BARO_BOARD
BaroEvent();
#endif
DatalinkEvent();
#if defined MCU_SPI_LINK || defined MCU_UART_LINK
link_mcu_event_task();
#endif
if (inter_mcu_received_fbw) {
/* receive radio control task from fbw */
inter_mcu_received_fbw = FALSE;
telecommand_task();
}
modules_event_task();
#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
if (new_ins_attitude > 0)
{
attitude_loop();
new_ins_attitude = 0;
}
#endif
} /* event_task_ap() */
#if USE_GPS
static inline void on_gps_solution( void ) {
ins_update_gps();
#if USE_AHRS
ahrs_update_gps();
#endif
#ifdef GPS_TRIGGERED_FUNCTION
GPS_TRIGGERED_FUNCTION();
#endif
}
#endif
#if USE_AHRS
#if USE_IMU
static inline void on_accel_event( void ) {
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS accel update.")
// timestamp in usec when last callback was received
static uint32_t last_ts = 0;
// current timestamp
uint32_t now_ts = get_sys_time_usec();
// dt between this and last callback in seconds
float dt = (float)(now_ts - last_ts) / 1e6;
last_ts = now_ts;
#else
PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS accel update.")
PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY)
const float dt = 1./AHRS_CORRECT_FREQUENCY;
#endif
imu_scale_accel(&imu);
if (ahrs.status != AHRS_UNINIT) {
ahrs_update_accel(dt);
}
}
static inline void on_gyro_event( void ) {
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS/INS propagation.")
// timestamp in usec when last callback was received
static uint32_t last_ts = 0;
// current timestamp
uint32_t now_ts = get_sys_time_usec();
// dt between this and last callback in seconds
float dt = (float)(now_ts - last_ts) / 1e6;
last_ts = now_ts;
#else
PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS/INS propagation.")
PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
#endif
ahrs_timeout_counter = 0;
imu_scale_gyro(&imu);
#if USE_AHRS_ALIGNER
// Run aligner on raw data as it also makes averages.
if (ahrs.status == AHRS_UNINIT) {
ahrs_aligner_run();
if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED)
ahrs_align();
return;
}
#endif
ahrs_propagate(dt);
#if defined SITL && USE_NPS
if (nps_bypass_ahrs) sim_overwrite_ahrs();
#endif
#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
new_ins_attitude = 1;
#endif
}
static inline void on_mag_event(void)
{
#if USE_MAGNETOMETER
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS mag update.")
// timestamp in usec when last callback was received
static uint32_t last_ts = 0;
// current timestamp
uint32_t now_ts = get_sys_time_usec();
// dt between this and last callback in seconds
float dt = (float)(now_ts - last_ts) / 1e6;
last_ts = now_ts;
#else
PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS mag update.")
PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY)
const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY);
#endif
imu_scale_mag(&imu);
if (ahrs.status == AHRS_RUNNING) {
ahrs_update_mag(dt);
}
#endif
}
#endif // USE_IMU
#endif // USE_AHRS