Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
glide_wing_lock.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) Kevin van Hecke
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, see
18  * <http://www.gnu.org/licenses/>.
19  */
27 
28 #include "pprzlink/messages.h"
29 #include "mcu_periph/uart.h"
31 
32 #include "mcu_periph/adc.h"
33 #include "subsystems/commands.h"
35 #include "autopilot.h"
36 
37 struct adc_buf adcbuf;
38 
40 
41 #ifndef WING_POS_DOWN_THRESH
42 #define WING_POS_DOWN_THRESH 100
43 #endif
44 #ifndef WING_POS_LOCK_MIN_THRESH
45 #define WING_POS_LOCK_MIN_THRESH 2000
46 #endif
47 #ifndef WING_POS_LOCK_MAX_THRESH
48 #define WING_POS_LOCK_MAX_THRESH 2100
49 #endif
50 #ifndef WING_POS_NOMINAL_THRUST
51 #define WING_POS_NOMINAL_THRUST 5000
52 #endif
53 #ifndef WING_POS_LOCK_SWITCH
54 #define WING_POS_LOCK_SWITCH RADIO_AUX2
55 #endif
56 
57 
59 {
60  adc_buf_channel(ADC_CHANNEL_MOTORSENSOR, &adcbuf, 1);
61 }
62 
64 {
65  static int lockstate = 0;
66  if (radio_control.values[WING_POS_LOCK_SWITCH] > (MIN_PPRZ / 2)) { // check glide switch
67  float wpos = adcbuf.sum / adcbuf.av_nb_sample;
68  switch (lockstate) {
69  case 0:
70  if (wpos < WING_POS_DOWN_THRESH) { //set wings to fixed speed for one rotation starting from when wings are at lowest position
71  lock_wings = 1;
72  lockstate++;
73  }
74  break;
75  case 1:
76  if (wpos > WING_POS_LOCK_MIN_THRESH) { //start wait for a rotation
77  lockstate++;
78  }
79  break;
80  case 2:
81  if (wpos < WING_POS_DOWN_THRESH) { //rotation finished
82  lockstate++;
83  }
84  break;
85  case 3:
86  if (wpos > WING_POS_LOCK_MIN_THRESH && wpos < WING_POS_LOCK_MAX_THRESH) { // wait for exact wing position
87  //esc brakes when throttle = 0, which should lock the wing in this position;
88  lock_wings = 2;
89  lockstate++;
90  }
91  break;
92  default:
93  break;
94  }
95  } else {
96  lock_wings = 0;
97  lockstate = 0;
98  }
99 }
100 
102 {
104  DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, DefaultDevice, &wpos, &wpos);
105 }
106 
107 void set_rotorcraft_commands(pprz_t *cmd_out, int32_t *cmd_in, bool in_flight, bool motors_on)
108 {
109  if (!(in_flight)) { cmd_in[COMMAND_YAW] = 0; }
110  if (!(motors_on)) { cmd_in[COMMAND_THRUST] = 0; }
111  cmd_out[COMMAND_ROLL] = cmd_in[COMMAND_ROLL];
112  cmd_out[COMMAND_PITCH] = cmd_in[COMMAND_PITCH];
113  cmd_out[COMMAND_YAW] = cmd_in[COMMAND_YAW];
114  cmd_out[COMMAND_THRUST] = cmd_in[COMMAND_THRUST];
115 
116  if (lock_wings == 1 && motors_on && cmd_out[COMMAND_THRUST] > 0) {
117  cmd_out[COMMAND_THRUST] = WING_POS_NOMINAL_THRUST;
118  } else if (lock_wings == 2) {
119  cmd_out[COMMAND_THRUST] = 0;
120  }
121 }
122 
123 
124 
uint8_t av_nb_sample
Definition: adc.h:57
unsigned short uint16_t
Definition: types.h:16
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
void glide_wing_lock_event()
#define MIN_PPRZ
Definition: paparazzi.h:9
#define WING_POS_LOCK_SWITCH
int16_t pprz_t
Definition: paparazzi.h:6
void adc_buf_channel(uint8_t adc_channel, struct adc_buf *s, uint8_t av_nb_sample)
Link between ChibiOS ADC drivers and Paparazzi adc_buffers.
Definition: adc_arch.c:281
uint32_t sum
Definition: adc.h:54
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:69
arch independent ADC (Analog to Digital Converter) API
Generic interface for all ADC hardware drivers, independent from microcontroller architecture.
Definition: adc.h:53
Hardware independent code for commands handling.
struct RadioControl radio_control
Definition: radio_control.c:30
void set_rotorcraft_commands(pprz_t *cmd_out, int32_t *cmd_in, bool in_flight, bool motors_on)
Set Rotorcraft commands.
signed long int32_t
Definition: types.h:19
#define WING_POS_NOMINAL_THRUST
Core autopilot interface common to all firmwares.
#define WING_POS_LOCK_MIN_THRESH
#define WING_POS_LOCK_MAX_THRESH
void glide_wing_lock_init(void)
int lock_wings
#define WING_POS_DOWN_THRESH
void glide_wing_lock_periodic()
struct adc_buf adcbuf