Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 #if PERIODIC_TELEMETRY
98 static void send_motor_mixing(struct transport_tx *trans, struct link_device *dev)
99 {
100  int16_t motors[MOTOR_MIXING_NB_MOTOR];
101  for (uint8_t i = 0; i < MOTOR_MIXING_NB_MOTOR; i++)
102  {
103  motors[i] = (int16_t)motor_mixing.commands[i];
104  }
105  pprz_msg_send_MOTOR_MIXING(trans, dev, AC_ID , MOTOR_MIXING_NB_MOTOR, motors);
106 }
107 #endif
108 
110 {
111  uint8_t i;
112  for (i = 0; i < MOTOR_MIXING_NB_MOTOR; i++) {
113  motor_mixing.commands[i] = 0;
114  motor_mixing.trim[i] =
118  motor_mixing.override_enabled[i] = false;
120  }
123 #if PERIODIC_TELEMETRY
125 #endif
126 }
127 
129 {
130  uint8_t j;
131  for (j = 0; j < MOTOR_MIXING_NB_MOTOR; j++) {
132  motor_mixing.commands[j] += (offset);
133  }
134 }
135 
136 static void bound_commands(void)
137 {
138  uint8_t j;
139  for (j = 0; j < MOTOR_MIXING_NB_MOTOR; j++) {
141  }
142 }
143 
144 static void bound_commands_step(void)
145 {
146 #ifdef MOTOR_MIXING_USE_MAX_MOTOR_STEP_BINDING
147  uint8_t j;
148  static int32_t prev_commands[MOTOR_MIXING_NB_MOTOR];
149  static uint8_t initialized = 0;
150 
151  if (initialized == 1) {
152  for (j = 0; j < MOTOR_MIXING_NB_MOTOR; j++) {
153  int32_t new_command_diff = motor_mixing.commands[j] - prev_commands[j];
154  Bound(new_command_diff,
155  MOTOR_MIXING_MAX_NEGATIVE_MOTOR_STEP, MOTOR_MIXING_MAX_POSITIVE_MOTOR_STEP);
156  motor_mixing.commands[j] = prev_commands[j] + new_command_diff;
157  }
158  } else {
159  initialized = 1;
160  }
161 
162  for (j = 0; j < MOTOR_MIXING_NB_MOTOR; j++) {
163  prev_commands[j] = motor_mixing.commands[j];
164  }
165 #endif
166 }
167 
169 {
170  int i;
171  for (i = 0; i < MOTOR_MIXING_NB_MOTOR; i++) {
172 #ifdef MOTOR_MIXING_STARTUP_DELAY
173  if (counter > i * max_counter / (MOTOR_MIXING_NB_MOTOR + MOTOR_MIXING_STARTUP_DELAY)) {
174  if (counter > MOTOR_MIXING_NB_MOTOR * max_counter /
175  (MOTOR_MIXING_NB_MOTOR + MOTOR_MIXING_STARTUP_DELAY))
176  {
179  } else {
181  }
182  } else {
183  motor_mixing.commands[i] = 0;
184  }
185 #else
186  if (counter < i * max_counter / MOTOR_MIXING_NB_MOTOR) {
188  }
189 #endif
190  }
191 }
192 
193 void motor_mixing_run(bool motors_on, bool override_on, pprz_t in_cmd[])
194 {
195  uint8_t i;
196 #if !HITL
197  if (motors_on) {
198 #else
199  if (FALSE) {
200 #endif
201 
202  int32_t tmp_cmd;
203  int32_t max_overflow = 0;
204 
205  /* first calculate the highest priority part of the command:
206  * - add trim + roll + pitch + thrust for each motor
207  * - calc max saturation/overflow when yaw command is also added
208  */
209  for (i = 0; i < MOTOR_MIXING_NB_MOTOR; i++) {
211  roll_coef[i] * in_cmd[COMMAND_ROLL] +
212  pitch_coef[i] * in_cmd[COMMAND_PITCH] +
213  thrust_coef[i] * in_cmd[COMMAND_THRUST];
214 
215  /* compute the command with yaw for each motor to check how much it would saturate */
216  tmp_cmd = motor_mixing.commands[i] + yaw_coef[i] * in_cmd[COMMAND_YAW];
217  tmp_cmd /= MOTOR_MIXING_SCALE;
218 
219  /* remember max overflow (how much in saturation) */
220  if (-tmp_cmd > max_overflow) {
221  max_overflow = -tmp_cmd;
222  }
223  else if (tmp_cmd - MAX_PPRZ > max_overflow) {
224  max_overflow = tmp_cmd - MAX_PPRZ;
225  }
226  }
227 
228  /* calculate how much authority is left for yaw command */
229  int32_t yaw_authority = ABS(in_cmd[COMMAND_YAW]) - max_overflow;
230  Bound(yaw_authority, 0, MAX_PPRZ);
231  int32_t bounded_yaw_cmd = in_cmd[COMMAND_YAW];
232  BoundAbs(bounded_yaw_cmd, yaw_authority);
233 
234  /* min/max of commands */
235  int32_t min_cmd = INT32_MAX;
236  int32_t max_cmd = INT32_MIN;
237 
238  /* add the bounded yaw command and scale */
239  for (i = 0; i < MOTOR_MIXING_NB_MOTOR; i++) {
240  motor_mixing.commands[i] += yaw_coef[i] * bounded_yaw_cmd;
241  motor_mixing.commands[i] /= MOTOR_MIXING_SCALE;
242 
243  /* remember min/max */
244  if (motor_mixing.commands[i] < min_cmd) {
245  min_cmd = motor_mixing.commands[i];
246  }
247  if (motor_mixing.commands[i] > max_cmd) {
248  max_cmd = motor_mixing.commands[i];
249  }
250  }
251 
252  if (min_cmd < MOTOR_MIXING_MIN_MOTOR && max_cmd > MOTOR_MIXING_MAX_MOTOR) {
254  }
255 
256  /* In case of both min and max saturation, only lower the throttle
257  * instead of applying both. This should prevent your quad shooting up,
258  * but it might loose altitude in case of such a saturation failure.
259  */
260  if (max_cmd > MOTOR_MIXING_MAX_MOTOR) {
261  int32_t saturation_offset = MOTOR_MIXING_MAX_MOTOR - max_cmd;
262  BoundAbs(saturation_offset, MOTOR_MIXING_MAX_SATURATION_OFFSET);
263  offset_commands(saturation_offset);
265  } else if (min_cmd < MOTOR_MIXING_MIN_MOTOR) {
266  int32_t saturation_offset = MOTOR_MIXING_MIN_MOTOR - min_cmd;
267  BoundAbs(saturation_offset, MOTOR_MIXING_MAX_SATURATION_OFFSET);
268  offset_commands(saturation_offset);
270  }
271 
272  /* For testing motor failure */
273  if (motors_on && override_on) {
274  for (i = 0; i < MOTOR_MIXING_NB_MOTOR; i++) {
277  }
278  }
279  }
280  bound_commands();
282  } else {
283  for (i = 0; i < MOTOR_MIXING_NB_MOTOR; i++) {
285  }
286  }
287 }
static const float offset[]
uint32_t counter
Definition: ins_flow.c:192
static void offset_commands(int32_t offset)
Definition: motor_mixing.c:128
#define INT32_MAX
Definition: motor_mixing.c:37
static const int32_t pitch_coef[MOTOR_MIXING_NB_MOTOR]
Definition: motor_mixing.c:90
static const int32_t yaw_coef[MOTOR_MIXING_NB_MOTOR]
Definition: motor_mixing.c:91
static void bound_commands_step(void)
Definition: motor_mixing.c:144
#define MOTOR_MIXING_TRIM_YAW
Definition: motor_mixing.c:59
#define MOTOR_MIXING_TRIM_ROLL
Definition: motor_mixing.c:51
void motor_mixing_run_spinup(uint32_t counter, uint32_t max_counter)
Definition: motor_mixing.c:168
struct MotorMixing motor_mixing
Definition: motor_mixing.c:94
#define MOTOR_MIXING_MIN_MOTOR
Definition: motor_mixing.c:43
#define INT32_MIN
Definition: motor_mixing.c:33
static void send_motor_mixing(struct transport_tx *trans, struct link_device *dev)
Definition: motor_mixing.c:98
#define MOTOR_MIXING_MIN_MOTOR_STARTUP
Definition: motor_mixing.c:73
#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
void motor_mixing_run(bool motors_on, bool override_on, pprz_t in_cmd[])
Definition: motor_mixing.c:193
void motor_mixing_init(void)
Definition: motor_mixing.c:109
#define MOTOR_MIXING_MAX_MOTOR
Definition: motor_mixing.c:44
#define MOTOR_MIXING_STOP_MOTOR
Definition: motor_mixing.c:47
static const int32_t roll_coef[MOTOR_MIXING_NB_MOTOR]
Definition: motor_mixing.c:89
#define MOTOR_MIXING_TRIM_PITCH
Definition: motor_mixing.c:55
static void bound_commands(void)
Definition: motor_mixing.c:136
Motor Mixing.
int32_t override_value[MOTOR_MIXING_NB_MOTOR]
Definition: motor_mixing.h:40
uint32_t nb_failure
Definition: motor_mixing.h:42
int32_t trim[MOTOR_MIXING_NB_MOTOR]
Definition: motor_mixing.h:38
uint32_t nb_saturation
Definition: motor_mixing.h:41
int32_t commands[MOTOR_MIXING_NB_MOTOR]
Definition: motor_mixing.h:37
bool override_enabled[MOTOR_MIXING_NB_MOTOR]
Definition: motor_mixing.h:39
int16_t pprz_t
Definition: paparazzi.h:6
#define MAX_PPRZ
Definition: paparazzi.h:8
#define FALSE
Definition: std.h:5
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98