Commit 7da728a8 authored by Sam Winkelstein's avatar Sam Winkelstein
Browse files

Merge branch '6dof-mixer-upgrade' into 'master'

6dof mixer upgrade

See merge request !4
parents a7b8a27e 90993fcc
......@@ -546,6 +546,8 @@ main_sources(COMMON_SRC
navigation/navigation_fixedwing.c
navigation/navigation_fw_launch.c
navigation/navigation_geo.c
navigation/navigation_delta_controller.c
navigation/navigation_delta_controller.h
navigation/navigation_multicopter.c
navigation/navigation_pos_estimator.c
navigation/navigation_pos_estimator_private.h
......
......@@ -52,7 +52,7 @@ FILE_COMPILE_FOR_SPEED
#include "flight/servos.h"
#include "navigation/navigation.h"
#include "navigation/navigation_delta_controller.h"
#include "rx/rx.h"
#include "sensors/battery.h"
......@@ -60,6 +60,7 @@ FILE_COMPILE_FOR_SPEED
FASTRAM int16_t mixerForwardCommand = 0;
FASTRAM int16_t mixerLateralCommand = 0;
FASTRAM int16_t motor[MAX_SUPPORTED_MOTORS];
FASTRAM motorMixer_t delta_motor_mixer[MAX_SUPPORTED_MOTORS];
FASTRAM int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
static float motorMixRange;
static float mixerScale = 1.0f;
......@@ -75,6 +76,7 @@ static EXTENDED_FASTRAM int throttleRangeMin = 0;
static EXTENDED_FASTRAM int throttleRangeMax = 0;
static EXTENDED_FASTRAM int8_t motorYawMultiplier = 1;
int motorZeroCommand = 0;
PG_REGISTER_WITH_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, PG_REVERSIBLE_MOTORS_CONFIG, 0);
......@@ -294,6 +296,7 @@ void mixerInit(void)
} else {
motorYawMultiplier = 1;
}
}
void mixerResetDisarmedMotors(void)
......@@ -351,10 +354,12 @@ static uint16_t handleOutputScaling(
}
return value;
}
static void applyFlipOverAfterCrashModeToMotors(void) {
if (ARMING_FLAG(ARMED)) {
const float flipPowerFactor = ((float)motorConfig()->flipOverAfterPowerFactor)/100.0f;
const float stickDeflectionAbs = ABS(((float) rcCommand[PITCH]) / 500.0f);
const float stickDeflectionPitchAbs = ABS(((float) rcCommand[PITCH]) / 500.0f);
const float stickDeflectionRollAbs = ABS(((float) rcCommand[ROLL]) / 500.0f);
const float stickDeflectionYawAbs = ABS(((float) rcCommand[YAW]) / 500.0f);
......@@ -577,8 +582,8 @@ void FAST_CODE mixTable(const float dT)
input[ROLL] = axisPID[ROLL];
input[PITCH] = axisPID[PITCH];
input[YAW] = axisPID[YAW];
input[3] = mixerForwardCommand;
input[4] = mixerLateralCommand;
input[3] = forward_thrust_command;
input[4] = lateral_thrust_command;
}
// Initial mixer concept by bdoiron74 reused and optimized for Air Mode
......@@ -675,7 +680,13 @@ void FAST_CODE mixTable(const float dT)
if (ARMING_FLAG(ARMED)) {
const motorStatus_e currentMotorStatus = getMotorStatus();
for (int i = 0; i < motorCount; i++) {
motor[i] = rpyMix[i] + constrain(mixerThrottleCommand * currentMixer[i].throttle, throttleMin, throttleMax);
if(!is_delta_thruster(i)) {
motor[i] = rpyMix[i] +
constrain(mixerThrottleCommand * currentMixer[i].throttle, throttleMin, throttleMax);
}
else{
constrain(rpyMix[i], throttleMin, throttleMax);
}
if (failsafeIsActive()) {
motor[i] = constrain(motor[i], motorConfig()->mincommand, motorConfig()->maxthrottle);
......@@ -750,7 +761,9 @@ motorStatus_e getMotorStatus(void)
void loadPrimaryMotorMixer(void) {
for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
currentMixer[i] = *primaryMotorMixer(i);
delta_motor_mixer[i] = *primaryMotorMixer(i);
}
init_delta_thrusters();
}
bool areMotorsRunning(void)
......@@ -767,3 +780,10 @@ bool areMotorsRunning(void)
return false;
}
struct motorMixer_s getMotorMixerRow(int row){
if(row < MAX_SUPPORTED_MOTORS) {
return currentMixer[row];
}
return currentMixer[0];
}
......@@ -116,7 +116,7 @@ extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
extern int mixerThrottleCommand;
extern int16_t mixerForwardCommand;
extern int16_t mixerLateralCommand;
extern struct motorMixer_s delta_motor_mixer[MAX_SUPPORTED_MOTORS];
int getThrottleIdleValue(void);
int16_t getThrottlePercent(void);
uint8_t getMotorCount(void);
......@@ -137,4 +137,6 @@ void stopMotors(void);
void stopPwmAllMotors(void);
void loadPrimaryMotorMixer(void);
bool areMotorsRunning(void);
\ No newline at end of file
bool areMotorsRunning(void);
struct motorMixer_s getMotorMixerRow(int row_number);
\ No newline at end of file
//
// Created by Sam Winkelstein 4/26/21
//
#include "navigation/navigation_delta_controller.h"
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <math.h>
#include "platform.h"
FILE_COMPILE_FOR_SPEED
#include "build/debug.h"
#include "common/axis.h"
#include "common/filter.h"
#include "common/maths.h"
#include "common/utils.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/pwm_output.h"
#include "drivers/pwm_mapping.h"
#include "drivers/time.h"
#include "fc/config.h"
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "fc/controlrate_profile.h"
#include "fc/settings.h"
#include "fc/cli.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/servos.h"
#include "navigation/navigation.h"
#include "rx/rx.h"
#include "sensors/battery.h"
FASTRAM int16_t forward_thrust_command = 1000;
FASTRAM int16_t lateral_thrust_command = 1000;
FASTRAM deltaThruster_t thrusters[MAX_SUPPORTED_MOTORS];
FASTRAM int thruster_count;
static const float acceptable_angle_error = .05; //about 3 degrees
float lateral_thrust_roll(float desired_pitch_angle);
float forward_thrust_pitch(float desired_pitch_angle);
float get_thruster_pitch(float throttle, float forward);
float get_thruster_roll(float throttle, float lateral);
const float dead_band = .15f;
void init_delta_thrusters() {
thruster_count = 0; //no thrusters by default
int i = 0;
for(i = 0; i < MAX_SUPPORTED_MOTORS; i++){
motorMixer_t m = delta_motor_mixer[i];
float z = m.throttle; // local z component
float x = m.forward; // local x component
float y = m.lateral; // local y component
bool is_thruster = false; // nothing is a thruster by default
if (z != 0) { // if z thrust is zero then it has to be a thruster
is_thruster = ((abs(atan2(z, x)) > .78) || (abs(atan2(z, x)) > .78));
}
else if (x != 0 || y != 0){
is_thruster = true;
}
if (is_thruster) { // this is my current cutoff I might change it later
thrusters[thruster_count].vertical = z; //components based on local cooridinates
thrusters[thruster_count].lateral = y;
thrusters[thruster_count].forward = x;
thrusters[thruster_count].pitch = get_thruster_pitch(z, x);
thrusters[thruster_count].roll = get_thruster_pitch(z, y);
m.throttle = 0.0;
thruster_count++;
}//increase thruster count for calculation
}
}
float get_thruster_pitch(float throttle, float forward){
float pitch = 0;
if(throttle != 0) {
pitch = atan2(forward, throttle);
}
return pitch;
}
// calculate desired pitch if thrusters are active
float get_thruster_roll(float throttle, float lateral) { // calculate desired pitch if thrusters are active
float roll = 0;
if (throttle != 0){
roll = atan2(lateral, throttle); //using atan roll
}
return roll;
}
float get_delta_pitch_angle( float desired_pitch_angle){
float pitch_angle = desired_pitch_angle;
//note this means output thruster
float power_level = desired_pitch_angle*1.25;
if (power_level > dead_band || power_level < -dead_band) {
pitch_angle = get_delta_pitch_angle(desired_pitch_angle);
forward_thrust_command = constrain(2000* -desired_pitch_angle * 1.25, -2000, 2000); //range 1000 to 2000
}
else {
forward_thrust_command = 0;
}
return pitch_angle;
}
float get_delta_roll_angle( float desired_roll_angle){
float roll_angle = desired_roll_angle;
//note this means output thruster
float power_level = desired_roll_angle*1.25;
if (power_level > dead_band || power_level < -dead_band){
roll_angle = forward_thrust_pitch(desired_roll_angle);
lateral_thrust_command = constrain(2000* -desired_roll_angle * 1.25, -2000, 2000); //range 1000 to 2000
}
else {
lateral_thrust_command = 0;
}
return roll_angle;
}
//hopefully this is still fast enough to still be used
// I might make these private
float forward_thrust_pitch(float desired_pitch_angle){
float pitch_angle = 0;
float component_count = 0;
for(int i = 0; i < thruster_count; i++) {
deltaThruster_t thruster = thrusters[i];
if (desired_pitch_angle < 0) {
if (thruster.forward > 0) {
pitch_angle += thruster.pitch * thruster.forward;
component_count += thruster.forward;
}
} else if (desired_pitch_angle > 0) {
if (thruster.forward < 0) {
pitch_angle += thruster.pitch * thruster.forward;
component_count += thruster.forward;
}
}
}
if(component_count == 0){
pitch_angle = desired_pitch_angle;
}
else{
pitch_angle = pitch_angle/component_count;
}
return pitch_angle;
}
float lateral_thrust_roll(float desired_roll_angle){
float roll_angle = 0;
float component_count = 0;
for(int i = 0; i < thruster_count; i++) {
deltaThruster_t thruster = thrusters[i];
if (desired_roll_angle > 0) {
if (thruster.lateral < 0) {
roll_angle += thruster.roll * thruster.lateral;
component_count += thruster.lateral;
}
} else if (desired_roll_angle < 0) {
if (thruster.lateral > 0) {
roll_angle += thruster.roll * thruster.lateral;
component_count += thruster.lateral;
}
}
}
if(component_count == 0){
roll_angle = desired_roll_angle;
}
else{
roll_angle = roll_angle/component_count;
}
return roll_angle;
}
bool is_delta_thruster(int index){
for(int i = 0; i < thruster_count; i++){
if(thrusters[i].motor_port == index)
return true;
}
return false;
}
float convert_rc_to_angle(int rc_input){
float scaled_rc = rc_input/500.0f;
scaled_rc *= .78; //angle in radians for calculations.
return scaled_rc;
}
//
// Created by Sam Winkelstein on 4/26/21.
//
#pragma once
#include "config/parameter_group.h"
/*** Delta Thruster struct for motor mixer ***/
extern int16_t forward_thrust_command;
extern int16_t lateral_thrust_command;
extern bool thruster_control;
typedef struct deltaThruster_S {
bool bidirectional; //this might be_important_later_on
float forward; // forward component given by the mixer, this value is local cooridinates to the drone
float lateral; // lateral component given by the mixer, this value is local cooridinates to the drone
float vertical; // throttle component given by the mixer, this value is local cooridinates to the drone
uint8_t motor_port; //no need to use an entire integer here
float pitch;
float roll;
} deltaThruster_t;
float get_delta_pitch_angle( float desired_pitch_angle);
float get_delta_roll_angle( float desired_roll_angle);
void init_delta_thrusters();
float calc_yaw_angle_offset();
bool is_delta_thruster(int index);
float convert_rc_to_angle(int rc_input);
void output_thrusters( float desired_angle);
......@@ -50,6 +50,7 @@
#include "navigation/navigation.h"
#include "navigation/navigation_private.h"
#include "navigation/navigation_delta_controller.h"
/*-----------------------------------------------------------
* Altitude controller for multicopter aircraft
......@@ -622,16 +623,9 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
const float desiredRoll = atan2_approx(accelRight * cos_approx(desiredPitch), GRAVITY_CMSS);
// Decide whether to get the requested acceleration from roll/pitch or directly from actuators
if (mixerHasForwardAuthority()) {
mixerForwardCommand = accelForward;
} else {
posControl.rcAdjustment[PITCH] = constrain(RADIANS_TO_DECIDEGREES(desiredPitch), -maxBankAngle, maxBankAngle);
}
if (mixerHasLateralAuthority()) {
mixerLateralCommand = accelRight;
} else {
posControl.rcAdjustment[ROLL] = constrain(RADIANS_TO_DECIDEGREES(desiredRoll), -maxBankAngle, maxBankAngle);
}
posControl.rcAdjustment[PITCH] = constrain(RADIANS_TO_DECIDEGREES(get_delta_pitch_angle(desiredPitch)), -maxBankAngle, maxBankAngle);
posControl.rcAdjustment[ROLL] = constrain(RADIANS_TO_DECIDEGREES(get_delta_roll_angle(desiredRoll)), -maxBankAngle, maxBankAngle);
}
......@@ -671,14 +665,17 @@ static void applyMulticopterPositionController(timeUs_t currentTimeUs)
}
else {
/* No position data, disable automatic adjustment, rcCommand passthrough */
/* this appears to be an all stop */
posControl.rcAdjustment[PITCH] = 0;
posControl.rcAdjustment[ROLL] = 0;
forward_thrust_command = 1000;
lateral_thrust_command = 1000;
bypassPositionController = true;
}
if (!bypassPositionController) {
rcCommand[PITCH] = pidAngleToRcCommand(posControl.rcAdjustment[PITCH], pidProfile()->max_angle_inclination[FD_PITCH]);
rcCommand[ROLL] = pidAngleToRcCommand(posControl.rcAdjustment[ROLL], pidProfile()->max_angle_inclination[FD_ROLL]);
rcCommand[PITCH] = pidAngleToRcCommand(RADIANS_TO_CENTIDEGREES(get_delta_pitch_angle(CENTIDEGREES_TO_RADIANS(posControl.rcAdjustment[PITCH]))), pidProfile()->max_angle_inclination[FD_PITCH]);
rcCommand[ROLL] = pidAngleToRcCommand(RADIANS_TO_CENTIDEGREES(get_delta_pitch_angle(CENTIDEGREES_TO_RADIANS(posControl.rcAdjustment[ROLL]))), pidProfile()->max_angle_inclination[FD_ROLL]);
}
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment