Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
supervision.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2012 The Paparazzi Team
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 
29 #include "paparazzi.h"
30 
31 //#include <stdint.h>
32 #ifndef INT32_MIN
33 #define INT32_MIN (-2147483647-1)
34 #endif
35 
36 #ifndef INT32_MAX
37 #define INT32_MAX (2147483647)
38 #endif
39 
40 #ifndef SUPERVISION_STOP_MOTOR
41 #define SUPERVISION_STOP_MOTOR 0
42 #endif
43 
44 #ifndef SUPERVISION_MIN_MOTOR_STARTUP
45 #define SUPERVISION_MIN_MOTOR_STARTUP SUPERVISION_MIN_MOTOR
46 #endif
47 
48 #if defined (SUPERVISION_MAX_NEGATIVE_MOTOR_STEP) || defined (SUPERVISION_MAX_POSITIVE_MOTOR_STEP)
49 #define SUPERVISION_USE_MAX_MOTOR_STEP_BINDING
50 
51 #ifndef SUPERVISION_MAX_NEGATIVE_MOTOR_STEP
52 #define SUPERVISION_MAX_NEGATIVE_MOTOR_STEP INT32_MIN
53 #endif
54 /*
55 #ifndef SUPERVISION_MAX_POSITIVE_MOTOR_STEP
56 #define SUPERVISION_MAX_POSITIVE_MOTOR_STEP INT32_MAX
57 #endif
58 */
59 #endif
60 
61 static const int32_t roll_coef[SUPERVISION_NB_MOTOR] = SUPERVISION_ROLL_COEF;
62 static const int32_t pitch_coef[SUPERVISION_NB_MOTOR] = SUPERVISION_PITCH_COEF;
63 static const int32_t yaw_coef[SUPERVISION_NB_MOTOR] = SUPERVISION_YAW_COEF;
64 static const int32_t thrust_coef[SUPERVISION_NB_MOTOR] = SUPERVISION_THRUST_COEF;
65 
67 
68 void supervision_init(void) {
69  uint8_t i;
70  for (i=0; i<SUPERVISION_NB_MOTOR; i++) {
71  supervision.commands[i] = 0;
72  supervision.trim[i] =
73  roll_coef[i] * SUPERVISION_TRIM_A +
74  pitch_coef[i] * SUPERVISION_TRIM_E +
75  yaw_coef[i] * SUPERVISION_TRIM_R;
78  }
80 }
81 
82 __attribute__ ((always_inline)) static inline void offset_commands(int32_t offset) {
83  uint8_t j;
84  for (j=0; j<SUPERVISION_NB_MOTOR; j++)
85  supervision.commands[j] += (offset);
86 }
87 
88 __attribute__ ((always_inline)) static inline void bound_commands(void) {
89  uint8_t j;
90  for (j=0; j<SUPERVISION_NB_MOTOR; j++)
91  Bound(supervision.commands[j],
93 }
94 
95 #ifdef SUPERVISION_USE_MAX_MOTOR_STEP_BINDING
96 __attribute__ ((always_inline)) static inline void bound_commands_step(void) {
97  uint8_t j;
98  static int32_t prev_commands[SUPERVISION_NB_MOTOR];
99  static uint8_t initialized = 0;
100 
101  if (initialized == 1) {
102  for (j=0; j<SUPERVISION_NB_MOTOR; j++) {
103  int32_t new_command_diff = supervision.commands[j] - prev_commands[j];
104  Bound(new_command_diff,
105  SUPERVISION_MAX_NEGATIVE_MOTOR_STEP, SUPERVISION_MAX_POSITIVE_MOTOR_STEP);
106  supervision.commands[j] = prev_commands[j] + new_command_diff;
107  }
108  }else{
109  initialized = 1;
110  }
111 
112  for (j=0; j<SUPERVISION_NB_MOTOR; j++)
113  prev_commands[j] = supervision.commands[j];
114 }
115 #else
116 __attribute__ ((always_inline)) static inline void bound_commands_step(void) {
117 }
118 #endif
119 
120 void supervision_run_spinup(uint32_t counter, uint32_t max_counter)
121 {
122  int i;
123  for (i = 0; i < SUPERVISION_NB_MOTOR; i++) {
124 #ifdef SUPERVISION_STARTUP_DELAY
125  if (counter > i * max_counter / (SUPERVISION_NB_MOTOR + SUPERVISION_STARTUP_DELAY)) {
126  if (counter > SUPERVISION_NB_MOTOR * max_counter / (SUPERVISION_NB_MOTOR + SUPERVISION_STARTUP_DELAY)) {
128  } else {
130  }
131  } else {
132  supervision.commands[i] = 0;
133  }
134 #else
135  if (counter < i * max_counter / SUPERVISION_NB_MOTOR) {
137  }
138 #endif
139  }
140 }
141 
142 void supervision_run(bool_t motors_on, bool_t override_on, int32_t in_cmd[] ) {
143  uint8_t i;
144  if (motors_on) {
145  int32_t min_cmd = INT32_MAX;
146  int32_t max_cmd = INT32_MIN;
147  /* do the mixing in float to avoid overflows, implicitly casted back to int32_t */
148  for (i=0; i<SUPERVISION_NB_MOTOR; i++) {
150  (thrust_coef[i] * in_cmd[COMMAND_THRUST] +
151  roll_coef[i] * in_cmd[COMMAND_ROLL] +
152  pitch_coef[i] * in_cmd[COMMAND_PITCH] +
153  yaw_coef[i] * in_cmd[COMMAND_YAW] +
154  supervision.trim[i]) / SUPERVISION_SCALE *
156  if (supervision.commands[i] < min_cmd)
157  min_cmd = supervision.commands[i];
158  if (supervision.commands[i] > max_cmd)
159  max_cmd = supervision.commands[i];
160  }
161  if (min_cmd < SUPERVISION_MIN_MOTOR && max_cmd > SUPERVISION_MAX_MOTOR)
163  if (min_cmd < SUPERVISION_MIN_MOTOR)
164  offset_commands(-(min_cmd - SUPERVISION_MIN_MOTOR));
165  if (max_cmd > SUPERVISION_MAX_MOTOR)
166  offset_commands(-(max_cmd - SUPERVISION_MAX_MOTOR));
167 
168  /* For testing motor failure */
169  if (motors_on && override_on) {
170  for (i = 0; i < SUPERVISION_NB_MOTOR; i++) {
173  }
174  }
175  bound_commands();
176  bound_commands_step();
177  }
178  else {
179  for (i=0; i<SUPERVISION_NB_MOTOR; i++) {
181  }
182  }
183 }
#define INT32_MIN
Definition: supervision.c:33
void supervision_run(bool_t motors_on, bool_t override_on, int32_t in_cmd[])
Definition: supervision.c:142
void supervision_run_spinup(uint32_t counter, uint32_t max_counter)
Definition: supervision.c:120
bool_t override_enabled[SUPERVISION_NB_MOTOR]
Definition: supervision.h:37
#define SUPERVISION_MIN_MOTOR_STARTUP
Definition: supervision.c:45
#define FALSE
Definition: imu_chimu.h:141
static const int32_t thrust_coef[SUPERVISION_NB_MOTOR]
Definition: supervision.c:64
#define SUPERVISION_MAX_MOTOR
Definition: guidance_v.h:43
#define SUPERVISION_STOP_MOTOR
Definition: supervision.c:41
uint32_t nb_failure
Definition: supervision.h:39
void supervision_init(void)
Definition: supervision.c:68
unsigned long uint32_t
Definition: types.h:18
Supervision.
signed long int32_t
Definition: types.h:19
int32_t trim[SUPERVISION_NB_MOTOR]
Definition: supervision.h:36
#define INT32_MAX
Definition: supervision.c:37
unsigned char uint8_t
Definition: types.h:14
int32_t commands[SUPERVISION_NB_MOTOR]
Definition: supervision.h:35
int32_t override_value[SUPERVISION_NB_MOTOR]
Definition: supervision.h:38
static const int32_t yaw_coef[SUPERVISION_NB_MOTOR]
Definition: supervision.c:63
#define MAX_PPRZ
Definition: paparazzi.h:8
struct Supervision supervision
Definition: supervision.c:66
__attribute__((always_inline))
Definition: supervision.c:82
#define SUPERVISION_MIN_MOTOR
Supervision default bounds In case Asctec controllers are used without supervision Used in control an...
Definition: guidance_v.h:40
static const int32_t roll_coef[SUPERVISION_NB_MOTOR]
Definition: supervision.c:61
static const int32_t pitch_coef[SUPERVISION_NB_MOTOR]
Definition: supervision.c:62