mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-06 16:58:48 +08:00
[motor_mixing] old supervision
- rename supervision to motor_mixing - split into a separated subsystem - pwm_supervision is now pwm direct + motor mixing
This commit is contained in:
Gautier Hattenberger
@@ -32,17 +32,6 @@
|
||||
#include "generated/airframe.h"
|
||||
#include "firmwares/rotorcraft/guidance/guidance_v_ref.h"
|
||||
|
||||
/** Supervision default bounds
|
||||
* In case Asctec controllers are used without supervision
|
||||
* Used in control and adaptation filter
|
||||
* */
|
||||
#ifndef SUPERVISION_MIN_MOTOR
|
||||
#define SUPERVISION_MIN_MOTOR 1
|
||||
#endif
|
||||
#ifndef SUPERVISION_MAX_MOTOR
|
||||
#define SUPERVISION_MAX_MOTOR 200
|
||||
#endif
|
||||
|
||||
#include "firmwares/rotorcraft/guidance/guidance_v_adpt.h"
|
||||
|
||||
#define GUIDANCE_V_MODE_KILL 0
|
||||
|
||||
@@ -37,6 +37,9 @@
|
||||
|
||||
#include "firmwares/rotorcraft/commands.h"
|
||||
#include "subsystems/actuators.h"
|
||||
#if USE_MOTOR_MIXING
|
||||
#include "subsystems/actuators/motor_mixing.h"
|
||||
#endif
|
||||
|
||||
#include "subsystems/imu.h"
|
||||
#include "subsystems/gps.h"
|
||||
@@ -100,6 +103,10 @@ STATIC_INLINE void main_init( void ) {
|
||||
stateInit();
|
||||
|
||||
actuators_init();
|
||||
#if USE_MOTOR_MIXING
|
||||
motor_mixing_init();
|
||||
#endif
|
||||
|
||||
radio_control_init();
|
||||
|
||||
#if DATALINK == XBEE
|
||||
|
||||
@@ -62,10 +62,6 @@ void actuators_asctec_init(void) {
|
||||
actuators_delay_time = 0;
|
||||
#endif
|
||||
|
||||
#ifdef ACTUATORS_ASCTEC_V2_PROTOCOL
|
||||
supervision_init();
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
#ifndef ACTUATORS_ASCTEC_V2_PROTOCOL
|
||||
|
||||
@@ -29,9 +29,6 @@
|
||||
#include "mcu_periph/i2c.h"
|
||||
|
||||
#include "generated/airframe.h"
|
||||
#ifdef ACTUATORS_ASCTEC_V2_PROTOCOL
|
||||
#include "subsystems/actuators/supervision.h"
|
||||
#endif
|
||||
|
||||
enum actuators_asctec_cmd { NONE,
|
||||
TEST,
|
||||
|
||||
@@ -38,7 +38,6 @@ bool_t actuators_delay_done;
|
||||
|
||||
void actuators_mkk_init(void) {
|
||||
|
||||
supervision_init();
|
||||
const uint8_t actuators_addr[ACTUATORS_MKK_NB] = ACTUATORS_MKK_ADDR;
|
||||
for (uint8_t i=0; i<ACTUATORS_MKK_NB; i++) {
|
||||
actuators_mkk.trans[i].type = I2CTransTx;
|
||||
|
||||
@@ -30,7 +30,6 @@
|
||||
#include "mcu_periph/i2c.h"
|
||||
|
||||
#include "generated/airframe.h"
|
||||
#include "subsystems/actuators/supervision.h"
|
||||
|
||||
|
||||
struct ActuatorsMkk {
|
||||
|
||||
@@ -23,9 +23,6 @@
|
||||
|
||||
void actuators_pwm_init(void)
|
||||
{
|
||||
#if USE_SUPERVISION
|
||||
supervision_init();
|
||||
#endif
|
||||
// implemented in arch files
|
||||
actuators_pwm_arch_init();
|
||||
}
|
||||
|
||||
@@ -28,10 +28,6 @@
|
||||
#include BOARD_CONFIG
|
||||
#include "subsystems/actuators/actuators_pwm_arch.h"
|
||||
|
||||
#if USE_SUPERVISION
|
||||
#include "subsystems/actuators/supervision.h"
|
||||
#endif
|
||||
|
||||
extern void actuators_pwm_init(void);
|
||||
|
||||
#define ActuatorsPwmInit() actuators_pwm_init()
|
||||
|
||||
@@ -37,7 +37,6 @@ bool_t actuators_delay_done;
|
||||
|
||||
void actuators_skiron_init(void) {
|
||||
|
||||
supervision_init();
|
||||
actuators_skiron.trans.type = I2CTransTx;
|
||||
actuators_skiron.trans.len_w = SERVOS_SKIRON_NB;
|
||||
actuators_skiron.trans.slave_addr = ACTUATORS_SKIRON_I2C_ADDR;
|
||||
|
||||
@@ -30,8 +30,6 @@
|
||||
#include "mcu_periph/i2c.h"
|
||||
|
||||
#include "generated/airframe.h"
|
||||
#include "subsystems/actuators/supervision.h"
|
||||
|
||||
|
||||
// Use I2C broadcast adderss
|
||||
#define ACTUATORS_SKIRON_I2C_ADDR 0x00
|
||||
|
||||
@@ -0,0 +1,186 @@
|
||||
/*
|
||||
* Copyright (C) 2008-2012 The Paparazzi Team
|
||||
*
|
||||
* This file is part of Paparazzi.
|
||||
*
|
||||
* Paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* Paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
/** @file motor_mixing.c
|
||||
* Motor Mixing.
|
||||
* Handles the mapping of roll/pitch/yaw commands
|
||||
* to actual motor commands.
|
||||
*/
|
||||
|
||||
#include "subsystems/actuators/motor_mixing.h"
|
||||
#include "paparazzi.h"
|
||||
|
||||
//#include <stdint.h>
|
||||
#ifndef INT32_MIN
|
||||
#define INT32_MIN (-2147483647-1)
|
||||
#endif
|
||||
|
||||
#ifndef INT32_MAX
|
||||
#define INT32_MAX (2147483647)
|
||||
#endif
|
||||
|
||||
#define MOTOR_MIXING_MIN_MOTOR 0
|
||||
#define MOTOR_MIXING_MAX_MOTOR MAX_PPRZ
|
||||
|
||||
#ifndef MOTOR_MIXING_STOP_MOTOR
|
||||
#define MOTOR_MIXING_STOP_MOTOR -MAX_PPRZ
|
||||
#endif
|
||||
|
||||
#ifndef MOTOR_MIXING_MIN_MOTOR_STARTUP
|
||||
#define MOTOR_MIXING_MIN_MOTOR_STARTUP MOTOR_MIXING_MIN_MOTOR
|
||||
#endif
|
||||
|
||||
#if defined (MOTOR_MIXING_MAX_NEGATIVE_MOTOR_STEP) || defined (MOTOR_MIXING_MAX_POSITIVE_MOTOR_STEP)
|
||||
#define MOTOR_MIXING_USE_MAX_MOTOR_STEP_BINDING
|
||||
|
||||
#ifndef MOTOR_MIXING_MAX_NEGATIVE_MOTOR_STEP
|
||||
#define MOTOR_MIXING_MAX_NEGATIVE_MOTOR_STEP INT32_MIN
|
||||
#endif
|
||||
/*
|
||||
#ifndef MOTOR_MIXING_MAX_POSITIVE_MOTOR_STEP
|
||||
#define MOTOR_MIXING_MAX_POSITIVE_MOTOR_STEP INT32_MAX
|
||||
#endif
|
||||
*/
|
||||
#endif
|
||||
|
||||
static const int32_t roll_coef[MOTOR_MIXING_NB_MOTOR] = MOTOR_MIXING_ROLL_COEF;
|
||||
static const int32_t pitch_coef[MOTOR_MIXING_NB_MOTOR] = MOTOR_MIXING_PITCH_COEF;
|
||||
static const int32_t yaw_coef[MOTOR_MIXING_NB_MOTOR] = MOTOR_MIXING_YAW_COEF;
|
||||
static const int32_t thrust_coef[MOTOR_MIXING_NB_MOTOR] = MOTOR_MIXING_THRUST_COEF;
|
||||
|
||||
struct MotorMixing motor_mixing;
|
||||
|
||||
void motor_mixing_init(void) {
|
||||
uint8_t i;
|
||||
for (i=0; i<MOTOR_MIXING_NB_MOTOR; i++) {
|
||||
motor_mixing.commands[i] = 0;
|
||||
motor_mixing.trim[i] =
|
||||
roll_coef[i] * MOTOR_MIXING_TRIM_ROLL +
|
||||
pitch_coef[i] * MOTOR_MIXING_TRIM_PITCH +
|
||||
yaw_coef[i] * MOTOR_MIXING_TRIM_YAW;
|
||||
motor_mixing.override_enabled[i] = FALSE;
|
||||
motor_mixing.override_value[i] = MOTOR_MIXING_STOP_MOTOR;
|
||||
}
|
||||
motor_mixing.nb_failure = 0;
|
||||
}
|
||||
|
||||
__attribute__ ((always_inline)) static inline void offset_commands(int32_t offset) {
|
||||
uint8_t j;
|
||||
for (j=0; j<MOTOR_MIXING_NB_MOTOR; j++)
|
||||
motor_mixing.commands[j] += (offset);
|
||||
}
|
||||
|
||||
__attribute__ ((always_inline)) static inline void bound_commands(void) {
|
||||
uint8_t j;
|
||||
for (j=0; j<MOTOR_MIXING_NB_MOTOR; j++)
|
||||
Bound(motor_mixing.commands[j],
|
||||
MOTOR_MIXING_MIN_MOTOR, MOTOR_MIXING_MAX_MOTOR);
|
||||
}
|
||||
|
||||
#ifdef MOTOR_MIXING_USE_MAX_MOTOR_STEP_BINDING
|
||||
__attribute__ ((always_inline)) static inline void bound_commands_step(void) {
|
||||
uint8_t j;
|
||||
static int32_t prev_commands[MOTOR_MIXING_NB_MOTOR];
|
||||
static uint8_t initialized = 0;
|
||||
|
||||
if (initialized == 1) {
|
||||
for (j=0; j<MOTOR_MIXING_NB_MOTOR; j++) {
|
||||
int32_t new_command_diff = motor_mixing.commands[j] - prev_commands[j];
|
||||
Bound(new_command_diff,
|
||||
MOTOR_MIXING_MAX_NEGATIVE_MOTOR_STEP, MOTOR_MIXING_MAX_POSITIVE_MOTOR_STEP);
|
||||
motor_mixing.commands[j] = prev_commands[j] + new_command_diff;
|
||||
}
|
||||
}else{
|
||||
initialized = 1;
|
||||
}
|
||||
|
||||
for (j=0; j<MOTOR_MIXING_NB_MOTOR; j++)
|
||||
prev_commands[j] = motor_mixing.commands[j];
|
||||
}
|
||||
#else
|
||||
__attribute__ ((always_inline)) static inline void bound_commands_step(void) {
|
||||
}
|
||||
#endif
|
||||
|
||||
void motor_mixing_run_spinup(uint32_t counter, uint32_t max_counter)
|
||||
{
|
||||
int i;
|
||||
for (i = 0; i < MOTOR_MIXING_NB_MOTOR; i++) {
|
||||
#ifdef MOTOR_MIXING_STARTUP_DELAY
|
||||
if (counter > i * max_counter / (MOTOR_MIXING_NB_MOTOR + MOTOR_MIXING_STARTUP_DELAY)) {
|
||||
if (counter > MOTOR_MIXING_NB_MOTOR * max_counter / (MOTOR_MIXING_NB_MOTOR + MOTOR_MIXING_STARTUP_DELAY)) {
|
||||
motor_mixing.commands[i] = MOTOR_MIXING_MIN_MOTOR_STARTUP + (MOTOR_MIXING_MIN_MOTOR - MOTOR_MIXING_MIN_MOTOR_STARTUP) * counter / max_counter;
|
||||
} else {
|
||||
motor_mixing.commands[i] = MOTOR_MIXING_MIN_MOTOR_STARTUP;
|
||||
}
|
||||
} else {
|
||||
motor_mixing.commands[i] = 0;
|
||||
}
|
||||
#else
|
||||
if (counter < i * max_counter / MOTOR_MIXING_NB_MOTOR) {
|
||||
motor_mixing.commands[i] = MOTOR_MIXING_MIN_MOTOR_STARTUP;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
void motor_mixing_run(bool_t motors_on, bool_t override_on, int32_t in_cmd[] ) {
|
||||
uint8_t i;
|
||||
if (motors_on) {
|
||||
int32_t min_cmd = INT32_MAX;
|
||||
int32_t max_cmd = INT32_MIN;
|
||||
/* do the mixing in float to avoid overflows, implicitly casted back to int32_t */
|
||||
for (i=0; i<MOTOR_MIXING_NB_MOTOR; i++) {
|
||||
motor_mixing.commands[i] = MOTOR_MIXING_MIN_MOTOR +
|
||||
(thrust_coef[i] * in_cmd[COMMAND_THRUST] +
|
||||
roll_coef[i] * in_cmd[COMMAND_ROLL] +
|
||||
pitch_coef[i] * in_cmd[COMMAND_PITCH] +
|
||||
yaw_coef[i] * in_cmd[COMMAND_YAW] +
|
||||
motor_mixing.trim[i]) / MOTOR_MIXING_SCALE *
|
||||
(MOTOR_MIXING_MAX_MOTOR - MOTOR_MIXING_MIN_MOTOR) / MAX_PPRZ;
|
||||
if (motor_mixing.commands[i] < min_cmd)
|
||||
min_cmd = motor_mixing.commands[i];
|
||||
if (motor_mixing.commands[i] > max_cmd)
|
||||
max_cmd = motor_mixing.commands[i];
|
||||
}
|
||||
if (min_cmd < MOTOR_MIXING_MIN_MOTOR && max_cmd > MOTOR_MIXING_MAX_MOTOR)
|
||||
motor_mixing.nb_failure++;
|
||||
if (min_cmd < MOTOR_MIXING_MIN_MOTOR)
|
||||
offset_commands(-(min_cmd - MOTOR_MIXING_MIN_MOTOR));
|
||||
if (max_cmd > MOTOR_MIXING_MAX_MOTOR)
|
||||
offset_commands(-(max_cmd - MOTOR_MIXING_MAX_MOTOR));
|
||||
|
||||
/* For testing motor failure */
|
||||
if (motors_on && override_on) {
|
||||
for (i = 0; i < MOTOR_MIXING_NB_MOTOR; i++) {
|
||||
if (motor_mixing.override_enabled[i])
|
||||
motor_mixing.commands[i] = motor_mixing.override_value[i];
|
||||
}
|
||||
}
|
||||
bound_commands();
|
||||
bound_commands_step();
|
||||
}
|
||||
else {
|
||||
for (i=0; i<MOTOR_MIXING_NB_MOTOR; i++) {
|
||||
motor_mixing.commands[i] = MOTOR_MIXING_STOP_MOTOR;
|
||||
}
|
||||
}
|
||||
}
|
||||
+14
-14
@@ -19,30 +19,30 @@
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
/** @file supervision.h
|
||||
* Supervision.
|
||||
/** @file motor_mixing.h
|
||||
* Motor Mixing.
|
||||
* Handles the mapping of roll/pitch/yaw commands
|
||||
* to actual motor commands.
|
||||
*/
|
||||
|
||||
#ifndef SUPERVISION_H
|
||||
#define SUPERVISION_H
|
||||
#ifndef MOTOR_MIXING_H
|
||||
#define MOTOR_MIXING_H
|
||||
|
||||
#include "std.h"
|
||||
#include "generated/airframe.h"
|
||||
|
||||
struct Supervision {
|
||||
int32_t commands[SUPERVISION_NB_MOTOR];
|
||||
int32_t trim[SUPERVISION_NB_MOTOR];
|
||||
bool_t override_enabled[SUPERVISION_NB_MOTOR];
|
||||
int32_t override_value[SUPERVISION_NB_MOTOR];
|
||||
struct MotorMixing {
|
||||
int32_t commands[MOTOR_MIXING_NB_MOTOR];
|
||||
int32_t trim[MOTOR_MIXING_NB_MOTOR];
|
||||
bool_t override_enabled[MOTOR_MIXING_NB_MOTOR];
|
||||
int32_t override_value[MOTOR_MIXING_NB_MOTOR];
|
||||
uint32_t nb_failure;
|
||||
};
|
||||
|
||||
extern struct Supervision supervision;
|
||||
extern struct MotorMixing motor_mixing;
|
||||
|
||||
extern void supervision_init(void);
|
||||
extern void supervision_run(bool_t motors_on, bool_t override_on, int32_t in_cmd[]);
|
||||
extern void supervision_run_spinup(uint32_t counter, uint32_t max_counter);
|
||||
extern void motor_mixing_init(void);
|
||||
extern void motor_mixing_run(bool_t motors_on, bool_t override_on, int32_t in_cmd[]);
|
||||
extern void motor_mixing_run_spinup(uint32_t counter, uint32_t max_counter);
|
||||
|
||||
#endif /* SUPERVISION_H */
|
||||
#endif /* MOTOR_MIXING_H */
|
||||
@@ -1,186 +0,0 @@
|
||||
/*
|
||||
* Copyright (C) 2008-2012 The Paparazzi Team
|
||||
*
|
||||
* This file is part of Paparazzi.
|
||||
*
|
||||
* Paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* Paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
/** @file supervision.c
|
||||
* Supervision.
|
||||
* Handles the mapping of roll/pitch/yaw commands
|
||||
* to actual motor commands.
|
||||
*/
|
||||
|
||||
#include "subsystems/actuators/supervision.h"
|
||||
#include "paparazzi.h"
|
||||
|
||||
//#include <stdint.h>
|
||||
#ifndef INT32_MIN
|
||||
#define INT32_MIN (-2147483647-1)
|
||||
#endif
|
||||
|
||||
#ifndef INT32_MAX
|
||||
#define INT32_MAX (2147483647)
|
||||
#endif
|
||||
|
||||
#define SUPERVISION_MIN_MOTOR 0
|
||||
#define SUPERVISION_MAX_MOTOR MAX_PPRZ
|
||||
|
||||
#ifndef SUPERVISION_STOP_MOTOR
|
||||
#define SUPERVISION_STOP_MOTOR -MAX_PPRZ
|
||||
#endif
|
||||
|
||||
#ifndef SUPERVISION_MIN_MOTOR_STARTUP
|
||||
#define SUPERVISION_MIN_MOTOR_STARTUP SUPERVISION_MIN_MOTOR
|
||||
#endif
|
||||
|
||||
#if defined (SUPERVISION_MAX_NEGATIVE_MOTOR_STEP) || defined (SUPERVISION_MAX_POSITIVE_MOTOR_STEP)
|
||||
#define SUPERVISION_USE_MAX_MOTOR_STEP_BINDING
|
||||
|
||||
#ifndef SUPERVISION_MAX_NEGATIVE_MOTOR_STEP
|
||||
#define SUPERVISION_MAX_NEGATIVE_MOTOR_STEP INT32_MIN
|
||||
#endif
|
||||
/*
|
||||
#ifndef SUPERVISION_MAX_POSITIVE_MOTOR_STEP
|
||||
#define SUPERVISION_MAX_POSITIVE_MOTOR_STEP INT32_MAX
|
||||
#endif
|
||||
*/
|
||||
#endif
|
||||
|
||||
static const int32_t roll_coef[SUPERVISION_NB_MOTOR] = SUPERVISION_ROLL_COEF;
|
||||
static const int32_t pitch_coef[SUPERVISION_NB_MOTOR] = SUPERVISION_PITCH_COEF;
|
||||
static const int32_t yaw_coef[SUPERVISION_NB_MOTOR] = SUPERVISION_YAW_COEF;
|
||||
static const int32_t thrust_coef[SUPERVISION_NB_MOTOR] = SUPERVISION_THRUST_COEF;
|
||||
|
||||
struct Supervision supervision;
|
||||
|
||||
void supervision_init(void) {
|
||||
uint8_t i;
|
||||
for (i=0; i<SUPERVISION_NB_MOTOR; i++) {
|
||||
supervision.commands[i] = 0;
|
||||
supervision.trim[i] =
|
||||
roll_coef[i] * SUPERVISION_TRIM_A +
|
||||
pitch_coef[i] * SUPERVISION_TRIM_E +
|
||||
yaw_coef[i] * SUPERVISION_TRIM_R;
|
||||
supervision.override_enabled[i] = FALSE;
|
||||
supervision.override_value[i] = SUPERVISION_STOP_MOTOR;
|
||||
}
|
||||
supervision.nb_failure = 0;
|
||||
}
|
||||
|
||||
__attribute__ ((always_inline)) static inline void offset_commands(int32_t offset) {
|
||||
uint8_t j;
|
||||
for (j=0; j<SUPERVISION_NB_MOTOR; j++)
|
||||
supervision.commands[j] += (offset);
|
||||
}
|
||||
|
||||
__attribute__ ((always_inline)) static inline void bound_commands(void) {
|
||||
uint8_t j;
|
||||
for (j=0; j<SUPERVISION_NB_MOTOR; j++)
|
||||
Bound(supervision.commands[j],
|
||||
SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR);
|
||||
}
|
||||
|
||||
#ifdef SUPERVISION_USE_MAX_MOTOR_STEP_BINDING
|
||||
__attribute__ ((always_inline)) static inline void bound_commands_step(void) {
|
||||
uint8_t j;
|
||||
static int32_t prev_commands[SUPERVISION_NB_MOTOR];
|
||||
static uint8_t initialized = 0;
|
||||
|
||||
if (initialized == 1) {
|
||||
for (j=0; j<SUPERVISION_NB_MOTOR; j++) {
|
||||
int32_t new_command_diff = supervision.commands[j] - prev_commands[j];
|
||||
Bound(new_command_diff,
|
||||
SUPERVISION_MAX_NEGATIVE_MOTOR_STEP, SUPERVISION_MAX_POSITIVE_MOTOR_STEP);
|
||||
supervision.commands[j] = prev_commands[j] + new_command_diff;
|
||||
}
|
||||
}else{
|
||||
initialized = 1;
|
||||
}
|
||||
|
||||
for (j=0; j<SUPERVISION_NB_MOTOR; j++)
|
||||
prev_commands[j] = supervision.commands[j];
|
||||
}
|
||||
#else
|
||||
__attribute__ ((always_inline)) static inline void bound_commands_step(void) {
|
||||
}
|
||||
#endif
|
||||
|
||||
void supervision_run_spinup(uint32_t counter, uint32_t max_counter)
|
||||
{
|
||||
int i;
|
||||
for (i = 0; i < SUPERVISION_NB_MOTOR; i++) {
|
||||
#ifdef SUPERVISION_STARTUP_DELAY
|
||||
if (counter > i * max_counter / (SUPERVISION_NB_MOTOR + SUPERVISION_STARTUP_DELAY)) {
|
||||
if (counter > SUPERVISION_NB_MOTOR * max_counter / (SUPERVISION_NB_MOTOR + SUPERVISION_STARTUP_DELAY)) {
|
||||
supervision.commands[i] = SUPERVISION_MIN_MOTOR_STARTUP + (SUPERVISION_MIN_MOTOR - SUPERVISION_MIN_MOTOR_STARTUP) * counter / max_counter;
|
||||
} else {
|
||||
supervision.commands[i] = SUPERVISION_MIN_MOTOR_STARTUP;
|
||||
}
|
||||
} else {
|
||||
supervision.commands[i] = 0;
|
||||
}
|
||||
#else
|
||||
if (counter < i * max_counter / SUPERVISION_NB_MOTOR) {
|
||||
supervision.commands[i] = SUPERVISION_MIN_MOTOR_STARTUP;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
void supervision_run(bool_t motors_on, bool_t override_on, int32_t in_cmd[] ) {
|
||||
uint8_t i;
|
||||
if (motors_on) {
|
||||
int32_t min_cmd = INT32_MAX;
|
||||
int32_t max_cmd = INT32_MIN;
|
||||
/* do the mixing in float to avoid overflows, implicitly casted back to int32_t */
|
||||
for (i=0; i<SUPERVISION_NB_MOTOR; i++) {
|
||||
supervision.commands[i] = SUPERVISION_MIN_MOTOR +
|
||||
(thrust_coef[i] * in_cmd[COMMAND_THRUST] +
|
||||
roll_coef[i] * in_cmd[COMMAND_ROLL] +
|
||||
pitch_coef[i] * in_cmd[COMMAND_PITCH] +
|
||||
yaw_coef[i] * in_cmd[COMMAND_YAW] +
|
||||
supervision.trim[i]) / SUPERVISION_SCALE *
|
||||
(SUPERVISION_MAX_MOTOR - SUPERVISION_MIN_MOTOR) / MAX_PPRZ;
|
||||
if (supervision.commands[i] < min_cmd)
|
||||
min_cmd = supervision.commands[i];
|
||||
if (supervision.commands[i] > max_cmd)
|
||||
max_cmd = supervision.commands[i];
|
||||
}
|
||||
if (min_cmd < SUPERVISION_MIN_MOTOR && max_cmd > SUPERVISION_MAX_MOTOR)
|
||||
supervision.nb_failure++;
|
||||
if (min_cmd < SUPERVISION_MIN_MOTOR)
|
||||
offset_commands(-(min_cmd - SUPERVISION_MIN_MOTOR));
|
||||
if (max_cmd > SUPERVISION_MAX_MOTOR)
|
||||
offset_commands(-(max_cmd - SUPERVISION_MAX_MOTOR));
|
||||
|
||||
/* For testing motor failure */
|
||||
if (motors_on && override_on) {
|
||||
for (i = 0; i < SUPERVISION_NB_MOTOR; i++) {
|
||||
if (supervision.override_enabled[i])
|
||||
supervision.commands[i] = supervision.override_value[i];
|
||||
}
|
||||
}
|
||||
bound_commands();
|
||||
bound_commands_step();
|
||||
}
|
||||
else {
|
||||
for (i=0; i<SUPERVISION_NB_MOTOR; i++) {
|
||||
supervision.commands[i] = SUPERVISION_STOP_MOTOR;
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user