Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
motor_mixing.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 #if defined MOTOR_MIXING_MIN_MOTOR || defined MOTOR_MIXING_MAX_MOTOR
41 #error MIN_MOTOR and MAX_MOTOR have to be set via neutral/max of the respective servo
42 #endif
43 #define MOTOR_MIXING_MIN_MOTOR 0
44 #define MOTOR_MIXING_MAX_MOTOR MAX_PPRZ
45 
46 #ifndef MOTOR_MIXING_STOP_MOTOR
47 #define MOTOR_MIXING_STOP_MOTOR -MAX_PPRZ
48 #endif
49 
50 #ifndef MOTOR_MIXING_TRIM_ROLL
51 #define MOTOR_MIXING_TRIM_ROLL 0
52 #endif
53 
54 #ifndef MOTOR_MIXING_TRIM_PITCH
55 #define MOTOR_MIXING_TRIM_PITCH 0
56 #endif
57 
58 #ifndef MOTOR_MIXING_TRIM_YAW
59 #define MOTOR_MIXING_TRIM_YAW 0
60 #endif
61 
68 #ifndef MOTOR_MIXING_MAX_SATURATION_OFFSET
69 #define MOTOR_MIXING_MAX_SATURATION_OFFSET MAX_PPRZ/10
70 #endif
71 
72 #ifndef MOTOR_MIXING_MIN_MOTOR_STARTUP
73 #define MOTOR_MIXING_MIN_MOTOR_STARTUP MOTOR_MIXING_MIN_MOTOR
74 #endif
75 
76 #if defined (MOTOR_MIXING_MAX_NEGATIVE_MOTOR_STEP) || defined (MOTOR_MIXING_MAX_POSITIVE_MOTOR_STEP)
77 #define MOTOR_MIXING_USE_MAX_MOTOR_STEP_BINDING
78 
79 #ifndef MOTOR_MIXING_MAX_NEGATIVE_MOTOR_STEP
80 #define MOTOR_MIXING_MAX_NEGATIVE_MOTOR_STEP INT32_MIN
81 #endif
82 /*
83 #ifndef MOTOR_MIXING_MAX_POSITIVE_MOTOR_STEP
84 #define MOTOR_MIXING_MAX_POSITIVE_MOTOR_STEP INT32_MAX
85 #endif
86 */
87 #endif
88 
89 static const int32_t roll_coef[MOTOR_MIXING_NB_MOTOR] = MOTOR_MIXING_ROLL_COEF;
90 static const int32_t pitch_coef[MOTOR_MIXING_NB_MOTOR] = MOTOR_MIXING_PITCH_COEF;
91 static const int32_t yaw_coef[MOTOR_MIXING_NB_MOTOR] = MOTOR_MIXING_YAW_COEF;
92 static const int32_t thrust_coef[MOTOR_MIXING_NB_MOTOR] = MOTOR_MIXING_THRUST_COEF;
93 
95 
96 void motor_mixing_init(void) {
97  uint8_t i;
98  for (i=0; i<MOTOR_MIXING_NB_MOTOR; i++) {
99  motor_mixing.commands[i] = 0;
100  motor_mixing.trim[i] =
106  }
109 }
110 
111 __attribute__ ((always_inline)) static inline void offset_commands(int32_t offset) {
112  uint8_t j;
113  for (j=0; j<MOTOR_MIXING_NB_MOTOR; j++)
114  motor_mixing.commands[j] += (offset);
115 }
116 
117 __attribute__ ((always_inline)) static inline void bound_commands(void) {
118  uint8_t j;
119  for (j=0; j<MOTOR_MIXING_NB_MOTOR; j++)
120  Bound(motor_mixing.commands[j],
122 }
123 
124 #ifdef MOTOR_MIXING_USE_MAX_MOTOR_STEP_BINDING
125 __attribute__ ((always_inline)) static inline void bound_commands_step(void) {
126  uint8_t j;
127  static int32_t prev_commands[MOTOR_MIXING_NB_MOTOR];
128  static uint8_t initialized = 0;
129 
130  if (initialized == 1) {
131  for (j=0; j<MOTOR_MIXING_NB_MOTOR; j++) {
132  int32_t new_command_diff = motor_mixing.commands[j] - prev_commands[j];
133  Bound(new_command_diff,
134  MOTOR_MIXING_MAX_NEGATIVE_MOTOR_STEP, MOTOR_MIXING_MAX_POSITIVE_MOTOR_STEP);
135  motor_mixing.commands[j] = prev_commands[j] + new_command_diff;
136  }
137  }else{
138  initialized = 1;
139  }
140 
141  for (j=0; j<MOTOR_MIXING_NB_MOTOR; j++)
142  prev_commands[j] = motor_mixing.commands[j];
143 }
144 #else
145 __attribute__ ((always_inline)) static inline void bound_commands_step(void) {
146 }
147 #endif
148 
149 void motor_mixing_run_spinup(uint32_t counter, uint32_t max_counter)
150 {
151  int i;
152  for (i = 0; i < MOTOR_MIXING_NB_MOTOR; i++) {
153 #ifdef MOTOR_MIXING_STARTUP_DELAY
154  if (counter > i * max_counter / (MOTOR_MIXING_NB_MOTOR + MOTOR_MIXING_STARTUP_DELAY)) {
155  if (counter > MOTOR_MIXING_NB_MOTOR * max_counter / (MOTOR_MIXING_NB_MOTOR + MOTOR_MIXING_STARTUP_DELAY)) {
157  } else {
159  }
160  } else {
161  motor_mixing.commands[i] = 0;
162  }
163 #else
164  if (counter < i * max_counter / MOTOR_MIXING_NB_MOTOR) {
166  }
167 #endif
168  }
169 }
170 
171 void motor_mixing_run(bool_t motors_on, bool_t override_on, pprz_t in_cmd[] ) {
172  uint8_t i;
173 #if !HITL
174  if (motors_on) {
175 #else
176  if (FALSE) {
177 #endif
178  int32_t min_cmd = INT32_MAX;
179  int32_t max_cmd = INT32_MIN;
180  /* do the mixing in float to avoid overflows, implicitly casted back to int32_t */
181  for (i=0; i<MOTOR_MIXING_NB_MOTOR; i++) {
183  (thrust_coef[i] * in_cmd[COMMAND_THRUST] +
184  roll_coef[i] * in_cmd[COMMAND_ROLL] +
185  pitch_coef[i] * in_cmd[COMMAND_PITCH] +
186  yaw_coef[i] * in_cmd[COMMAND_YAW] +
187  motor_mixing.trim[i]) / MOTOR_MIXING_SCALE *
189  if (motor_mixing.commands[i] < min_cmd)
190  min_cmd = motor_mixing.commands[i];
191  if (motor_mixing.commands[i] > max_cmd)
192  max_cmd = motor_mixing.commands[i];
193  }
194 
195  if (min_cmd < MOTOR_MIXING_MIN_MOTOR && max_cmd > MOTOR_MIXING_MAX_MOTOR)
197 
198  /* In case of both min and max saturation, only lower the throttle
199  * instead of applying both. This should prevent your quad shooting up,
200  * but it might loose altitude in case of such a saturation failure.
201  */
202  if (max_cmd > MOTOR_MIXING_MAX_MOTOR) {
203  int32_t saturation_offset = MOTOR_MIXING_MAX_MOTOR - max_cmd;
204  BoundAbs(saturation_offset, MOTOR_MIXING_MAX_SATURATION_OFFSET);
205  offset_commands(saturation_offset);
207  }
208  else if (min_cmd < MOTOR_MIXING_MIN_MOTOR) {
209  int32_t saturation_offset = MOTOR_MIXING_MIN_MOTOR - min_cmd;
210  BoundAbs(saturation_offset, MOTOR_MIXING_MAX_SATURATION_OFFSET);
211  offset_commands(saturation_offset);
213  }
214 
215  /* For testing motor failure */
216  if (motors_on && override_on) {
217  for (i = 0; i < MOTOR_MIXING_NB_MOTOR; i++) {
220  }
221  }
222  bound_commands();
224  }
225  else {
226  for (i=0; i<MOTOR_MIXING_NB_MOTOR; i++) {
228  }
229  }
230 }
#define MOTOR_MIXING_MAX_MOTOR
Definition: motor_mixing.c:44
static const int32_t roll_coef[MOTOR_MIXING_NB_MOTOR]
Definition: motor_mixing.c:89
#define MOTOR_MIXING_MAX_SATURATION_OFFSET
Maximum offset in case of saturation.
Definition: motor_mixing.c:69
static const int32_t thrust_coef[MOTOR_MIXING_NB_MOTOR]
Definition: motor_mixing.c:92
static void bound_commands_step(void)
Definition: motor_mixing.c:145
void motor_mixing_run(bool_t motors_on, bool_t override_on, pprz_t in_cmd[])
Definition: motor_mixing.c:171
int16_t pprz_t
Definition: paparazzi.h:6
int32_t commands[MOTOR_MIXING_NB_MOTOR]
Definition: motor_mixing.h:36
static const int32_t pitch_coef[MOTOR_MIXING_NB_MOTOR]
Definition: motor_mixing.c:90
int32_t trim[MOTOR_MIXING_NB_MOTOR]
Definition: motor_mixing.h:37
static void bound_commands(void)
Definition: motor_mixing.c:117
static void offset_commands(int32_t offset)
Definition: motor_mixing.c:111
#define INT32_MAX
Definition: motor_mixing.c:37
#define FALSE
Definition: imu_chimu.h:141
uint32_t nb_saturation
Definition: motor_mixing.h:40
#define MOTOR_MIXING_STOP_MOTOR
Definition: motor_mixing.c:47
void motor_mixing_init(void)
Definition: motor_mixing.c:96
unsigned long uint32_t
Definition: types.h:18
signed long int32_t
Definition: types.h:19
static const int32_t yaw_coef[MOTOR_MIXING_NB_MOTOR]
Definition: motor_mixing.c:91
#define MOTOR_MIXING_TRIM_YAW
Definition: motor_mixing.c:59
uint32_t nb_failure
Definition: motor_mixing.h:41
unsigned char uint8_t
Definition: types.h:14
void motor_mixing_run_spinup(uint32_t counter, uint32_t max_counter)
Definition: motor_mixing.c:149
bool_t override_enabled[MOTOR_MIXING_NB_MOTOR]
Definition: motor_mixing.h:38
#define MOTOR_MIXING_MIN_MOTOR_STARTUP
Definition: motor_mixing.c:73
#define INT32_MIN
Definition: motor_mixing.c:33
int32_t override_value[MOTOR_MIXING_NB_MOTOR]
Definition: motor_mixing.h:39
struct MotorMixing motor_mixing
Definition: motor_mixing.c:94
#define MOTOR_MIXING_MIN_MOTOR
Definition: motor_mixing.c:43
#define MOTOR_MIXING_TRIM_ROLL
Definition: motor_mixing.c:51
#define MAX_PPRZ
Definition: paparazzi.h:8
#define MOTOR_MIXING_TRIM_PITCH
Definition: motor_mixing.c:55
Motor Mixing.