Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
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_MAX_SATURATION_OFFSET
51 #define MOTOR_MIXING_MAX_SATURATION_OFFSET MAX_PPRZ/3
52 #endif
53 
54 #ifndef MOTOR_MIXING_MIN_MOTOR_STARTUP
55 #define MOTOR_MIXING_MIN_MOTOR_STARTUP MOTOR_MIXING_MIN_MOTOR
56 #endif
57 
58 #if defined (MOTOR_MIXING_MAX_NEGATIVE_MOTOR_STEP) || defined (MOTOR_MIXING_MAX_POSITIVE_MOTOR_STEP)
59 #define MOTOR_MIXING_USE_MAX_MOTOR_STEP_BINDING
60 
61 #ifndef MOTOR_MIXING_MAX_NEGATIVE_MOTOR_STEP
62 #define MOTOR_MIXING_MAX_NEGATIVE_MOTOR_STEP INT32_MIN
63 #endif
64 /*
65 #ifndef MOTOR_MIXING_MAX_POSITIVE_MOTOR_STEP
66 #define MOTOR_MIXING_MAX_POSITIVE_MOTOR_STEP INT32_MAX
67 #endif
68 */
69 #endif
70 
71 static const int32_t roll_coef[MOTOR_MIXING_NB_MOTOR] = MOTOR_MIXING_ROLL_COEF;
72 static const int32_t pitch_coef[MOTOR_MIXING_NB_MOTOR] = MOTOR_MIXING_PITCH_COEF;
73 static const int32_t yaw_coef[MOTOR_MIXING_NB_MOTOR] = MOTOR_MIXING_YAW_COEF;
74 static const int32_t thrust_coef[MOTOR_MIXING_NB_MOTOR] = MOTOR_MIXING_THRUST_COEF;
75 
77 
78 void motor_mixing_init(void) {
79  uint8_t i;
80  for (i=0; i<MOTOR_MIXING_NB_MOTOR; i++) {
81  motor_mixing.commands[i] = 0;
82  motor_mixing.trim[i] =
83  roll_coef[i] * MOTOR_MIXING_TRIM_ROLL +
84  pitch_coef[i] * MOTOR_MIXING_TRIM_PITCH +
85  yaw_coef[i] * MOTOR_MIXING_TRIM_YAW;
88  }
90 }
91 
92 __attribute__ ((always_inline)) static inline void offset_commands(int32_t offset) {
93  uint8_t j;
94  for (j=0; j<MOTOR_MIXING_NB_MOTOR; j++)
95  motor_mixing.commands[j] += (offset);
96 }
97 
98 __attribute__ ((always_inline)) static inline void bound_commands(void) {
99  uint8_t j;
100  for (j=0; j<MOTOR_MIXING_NB_MOTOR; j++)
101  Bound(motor_mixing.commands[j],
103 }
104 
105 #ifdef MOTOR_MIXING_USE_MAX_MOTOR_STEP_BINDING
106 __attribute__ ((always_inline)) static inline void bound_commands_step(void) {
107  uint8_t j;
108  static int32_t prev_commands[MOTOR_MIXING_NB_MOTOR];
109  static uint8_t initialized = 0;
110 
111  if (initialized == 1) {
112  for (j=0; j<MOTOR_MIXING_NB_MOTOR; j++) {
113  int32_t new_command_diff = motor_mixing.commands[j] - prev_commands[j];
114  Bound(new_command_diff,
115  MOTOR_MIXING_MAX_NEGATIVE_MOTOR_STEP, MOTOR_MIXING_MAX_POSITIVE_MOTOR_STEP);
116  motor_mixing.commands[j] = prev_commands[j] + new_command_diff;
117  }
118  }else{
119  initialized = 1;
120  }
121 
122  for (j=0; j<MOTOR_MIXING_NB_MOTOR; j++)
123  prev_commands[j] = motor_mixing.commands[j];
124 }
125 #else
126 __attribute__ ((always_inline)) static inline void bound_commands_step(void) {
127 }
128 #endif
129 
130 void motor_mixing_run_spinup(uint32_t counter, uint32_t max_counter)
131 {
132  int i;
133  for (i = 0; i < MOTOR_MIXING_NB_MOTOR; i++) {
134 #ifdef MOTOR_MIXING_STARTUP_DELAY
135  if (counter > i * max_counter / (MOTOR_MIXING_NB_MOTOR + MOTOR_MIXING_STARTUP_DELAY)) {
136  if (counter > MOTOR_MIXING_NB_MOTOR * max_counter / (MOTOR_MIXING_NB_MOTOR + MOTOR_MIXING_STARTUP_DELAY)) {
138  } else {
140  }
141  } else {
142  motor_mixing.commands[i] = 0;
143  }
144 #else
145  if (counter < i * max_counter / MOTOR_MIXING_NB_MOTOR) {
147  }
148 #endif
149  }
150 }
151 
152 void motor_mixing_run(bool_t motors_on, bool_t override_on, pprz_t in_cmd[] ) {
153  uint8_t i;
154  if (motors_on) {
155  int32_t min_cmd = INT32_MAX;
156  int32_t max_cmd = INT32_MIN;
157  /* do the mixing in float to avoid overflows, implicitly casted back to int32_t */
158  for (i=0; i<MOTOR_MIXING_NB_MOTOR; i++) {
160  (thrust_coef[i] * in_cmd[COMMAND_THRUST] +
161  roll_coef[i] * in_cmd[COMMAND_ROLL] +
162  pitch_coef[i] * in_cmd[COMMAND_PITCH] +
163  yaw_coef[i] * in_cmd[COMMAND_YAW] +
164  motor_mixing.trim[i]) / MOTOR_MIXING_SCALE *
166  if (motor_mixing.commands[i] < min_cmd)
167  min_cmd = motor_mixing.commands[i];
168  if (motor_mixing.commands[i] > max_cmd)
169  max_cmd = motor_mixing.commands[i];
170  }
171 
172  if (min_cmd < MOTOR_MIXING_MIN_MOTOR && max_cmd > MOTOR_MIXING_MAX_MOTOR)
174 
175  if (min_cmd < MOTOR_MIXING_MIN_MOTOR) {
176  int32_t saturation_offset = MOTOR_MIXING_MIN_MOTOR - min_cmd;
177  BoundAbs(saturation_offset, MOTOR_MIXING_MAX_SATURATION_OFFSET);
178  offset_commands(saturation_offset);
179  }
180  if (max_cmd > MOTOR_MIXING_MAX_MOTOR) {
181  int32_t saturation_offset = MOTOR_MIXING_MAX_MOTOR - max_cmd;
182  BoundAbs(saturation_offset, MOTOR_MIXING_MAX_SATURATION_OFFSET);
183  offset_commands(saturation_offset);
184  }
185 
186  /* For testing motor failure */
187  if (motors_on && override_on) {
188  for (i = 0; i < MOTOR_MIXING_NB_MOTOR; i++) {
191  }
192  }
193  bound_commands();
195  }
196  else {
197  for (i=0; i<MOTOR_MIXING_NB_MOTOR; i++) {
199  }
200  }
201 }
#define MOTOR_MIXING_MAX_MOTOR
Definition: motor_mixing.c:44
static const int32_t roll_coef[MOTOR_MIXING_NB_MOTOR]
Definition: motor_mixing.c:71
#define MOTOR_MIXING_MAX_SATURATION_OFFSET
Definition: motor_mixing.c:51
static const int32_t thrust_coef[MOTOR_MIXING_NB_MOTOR]
Definition: motor_mixing.c:74
static void bound_commands_step(void)
Definition: motor_mixing.c:126
void motor_mixing_run(bool_t motors_on, bool_t override_on, pprz_t in_cmd[])
Definition: motor_mixing.c:152
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:72
int32_t trim[MOTOR_MIXING_NB_MOTOR]
Definition: motor_mixing.h:37
static void bound_commands(void)
Definition: motor_mixing.c:98
static void offset_commands(int32_t offset)
Definition: motor_mixing.c:92
#define INT32_MAX
Definition: motor_mixing.c:37
#define FALSE
Definition: imu_chimu.h:141
#define MOTOR_MIXING_STOP_MOTOR
Definition: motor_mixing.c:47
void motor_mixing_init(void)
Definition: motor_mixing.c:78
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:73
uint32_t nb_failure
Definition: motor_mixing.h:40
unsigned char uint8_t
Definition: types.h:14
void motor_mixing_run_spinup(uint32_t counter, uint32_t max_counter)
Definition: motor_mixing.c:130
bool_t override_enabled[MOTOR_MIXING_NB_MOTOR]
Definition: motor_mixing.h:38
#define MOTOR_MIXING_MIN_MOTOR_STARTUP
Definition: motor_mixing.c:55
#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:76
#define MOTOR_MIXING_MIN_MOTOR
Definition: motor_mixing.c:43
#define MAX_PPRZ
Definition: paparazzi.h:8
Motor Mixing.