Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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 
radio_control.h
uint16_t
unsigned short uint16_t
Definition: types.h:16
WING_POS_LOCK_MAX_THRESH
#define WING_POS_LOCK_MAX_THRESH
Definition: glide_wing_lock.c:48
glide_wing_lock_periodic
void glide_wing_lock_periodic()
Definition: glide_wing_lock.c:101
adc_buf_channel
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:289
set_rotorcraft_commands
void set_rotorcraft_commands(pprz_t *cmd_out, int32_t *cmd_in, bool in_flight, bool motors_on)
Set Rotorcraft commands.
Definition: glide_wing_lock.c:107
lock_wings
int lock_wings
Definition: glide_wing_lock.c:39
adc.h
arch independent ADC (Analog to Digital Converter) API
adc_buf::sum
uint32_t sum
Definition: adc.h:54
WING_POS_NOMINAL_THRUST
#define WING_POS_NOMINAL_THRUST
Definition: glide_wing_lock.c:51
uart.h
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
pprz_t
int16_t pprz_t
Definition: paparazzi.h:6
WING_POS_LOCK_MIN_THRESH
#define WING_POS_LOCK_MIN_THRESH
Definition: glide_wing_lock.c:45
glide_wing_lock_init
void glide_wing_lock_init(void)
Definition: glide_wing_lock.c:58
MIN_PPRZ
#define MIN_PPRZ
Definition: paparazzi.h:9
adc_buf::av_nb_sample
uint8_t av_nb_sample
Definition: adc.h:57
autopilot.h
WING_POS_DOWN_THRESH
#define WING_POS_DOWN_THRESH
Definition: glide_wing_lock.c:42
glide_wing_lock_event
void glide_wing_lock_event()
Definition: glide_wing_lock.c:63
int32_t
signed long int32_t
Definition: types.h:19
glide_wing_lock.h
commands.h
Hardware independent code for commands handling.
adcbuf
struct adc_buf adcbuf
Definition: glide_wing_lock.c:37
WING_POS_LOCK_SWITCH
#define WING_POS_LOCK_SWITCH
Definition: glide_wing_lock.c:54
radio_control
struct RadioControl radio_control
Definition: radio_control.c:30
RadioControl::values
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:69
adc_buf
Generic interface for all ADC hardware drivers, independent from microcontroller architecture.
Definition: adc.h:53