Commit 90993fcc authored by Sam Winkelstein's avatar Sam Winkelstein
Browse files

delta controller complete

parent 6a2fb26a
...@@ -44,7 +44,7 @@ FILE_COMPILE_FOR_SPEED ...@@ -44,7 +44,7 @@ FILE_COMPILE_FOR_SPEED
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "fc/controlrate_profile.h" #include "fc/controlrate_profile.h"
#include "fc/settings.h" #include "fc/settings.h"
#include "navigation/navigation_delta_controller.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/mixer.h" #include "flight/mixer.h"
...@@ -52,7 +52,7 @@ FILE_COMPILE_FOR_SPEED ...@@ -52,7 +52,7 @@ FILE_COMPILE_FOR_SPEED
#include "flight/servos.h" #include "flight/servos.h"
#include "navigation/navigation.h" #include "navigation/navigation.h"
#include "navigation/navigation_delta_controller.h"
#include "rx/rx.h" #include "rx/rx.h"
#include "sensors/battery.h" #include "sensors/battery.h"
...@@ -60,6 +60,7 @@ FILE_COMPILE_FOR_SPEED ...@@ -60,6 +60,7 @@ FILE_COMPILE_FOR_SPEED
FASTRAM int16_t mixerForwardCommand = 0; FASTRAM int16_t mixerForwardCommand = 0;
FASTRAM int16_t mixerLateralCommand = 0; FASTRAM int16_t mixerLateralCommand = 0;
FASTRAM int16_t motor[MAX_SUPPORTED_MOTORS]; FASTRAM int16_t motor[MAX_SUPPORTED_MOTORS];
FASTRAM motorMixer_t delta_motor_mixer[MAX_SUPPORTED_MOTORS];
FASTRAM int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; FASTRAM int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
static float motorMixRange; static float motorMixRange;
static float mixerScale = 1.0f; static float mixerScale = 1.0f;
...@@ -75,6 +76,7 @@ static EXTENDED_FASTRAM int throttleRangeMin = 0; ...@@ -75,6 +76,7 @@ static EXTENDED_FASTRAM int throttleRangeMin = 0;
static EXTENDED_FASTRAM int throttleRangeMax = 0; static EXTENDED_FASTRAM int throttleRangeMax = 0;
static EXTENDED_FASTRAM int8_t motorYawMultiplier = 1; static EXTENDED_FASTRAM int8_t motorYawMultiplier = 1;
int motorZeroCommand = 0; int motorZeroCommand = 0;
PG_REGISTER_WITH_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, PG_REVERSIBLE_MOTORS_CONFIG, 0); PG_REGISTER_WITH_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, PG_REVERSIBLE_MOTORS_CONFIG, 0);
...@@ -294,7 +296,7 @@ void mixerInit(void) ...@@ -294,7 +296,7 @@ void mixerInit(void)
} else { } else {
motorYawMultiplier = 1; motorYawMultiplier = 1;
} }
init_delta_thrusters();
} }
void mixerResetDisarmedMotors(void) void mixerResetDisarmedMotors(void)
...@@ -352,11 +354,13 @@ static uint16_t handleOutputScaling( ...@@ -352,11 +354,13 @@ static uint16_t handleOutputScaling(
} }
return value; return value;
} }
static void applyFlipOverAfterCrashModeToMotors(void) { static void applyFlipOverAfterCrashModeToMotors(void) {
if (ARMING_FLAG(ARMED)) { if (ARMING_FLAG(ARMED)) {
const float flipPowerFactor = ((float)motorConfig()->flipOverAfterPowerFactor)/100.0f; const float flipPowerFactor = ((float)motorConfig()->flipOverAfterPowerFactor)/100.0f;
const float stickDeflectionAbs = ABS(((float) rcCommand[PITCH]) / 500.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 stickDeflectionRollAbs = ABS(((float) rcCommand[ROLL]) / 500.0f);
const float stickDeflectionYawAbs = ABS(((float) rcCommand[YAW]) / 500.0f); const float stickDeflectionYawAbs = ABS(((float) rcCommand[YAW]) / 500.0f);
//deflection stick position //deflection stick position
...@@ -676,7 +680,13 @@ void FAST_CODE mixTable(const float dT) ...@@ -676,7 +680,13 @@ void FAST_CODE mixTable(const float dT)
if (ARMING_FLAG(ARMED)) { if (ARMING_FLAG(ARMED)) {
const motorStatus_e currentMotorStatus = getMotorStatus(); const motorStatus_e currentMotorStatus = getMotorStatus();
for (int i = 0; i < motorCount; i++) { 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()) { if (failsafeIsActive()) {
motor[i] = constrain(motor[i], motorConfig()->mincommand, motorConfig()->maxthrottle); motor[i] = constrain(motor[i], motorConfig()->mincommand, motorConfig()->maxthrottle);
...@@ -751,7 +761,9 @@ motorStatus_e getMotorStatus(void) ...@@ -751,7 +761,9 @@ motorStatus_e getMotorStatus(void)
void loadPrimaryMotorMixer(void) { void loadPrimaryMotorMixer(void) {
for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
currentMixer[i] = *primaryMotorMixer(i); currentMixer[i] = *primaryMotorMixer(i);
delta_motor_mixer[i] = *primaryMotorMixer(i);
} }
init_delta_thrusters();
} }
bool areMotorsRunning(void) bool areMotorsRunning(void)
...@@ -770,6 +782,8 @@ bool areMotorsRunning(void) ...@@ -770,6 +782,8 @@ bool areMotorsRunning(void)
} }
struct motorMixer_s getMotorMixerRow(int row){ struct motorMixer_s getMotorMixerRow(int row){
if(row < MAX_SUPPORTED_MOTORS) if(row < MAX_SUPPORTED_MOTORS) {
return *primaryMotorMixer(row); return currentMixer[row];
}
return currentMixer[0];
} }
...@@ -116,7 +116,7 @@ extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; ...@@ -116,7 +116,7 @@ extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
extern int mixerThrottleCommand; extern int mixerThrottleCommand;
extern int16_t mixerForwardCommand; extern int16_t mixerForwardCommand;
extern int16_t mixerLateralCommand; extern int16_t mixerLateralCommand;
extern struct motorMixer_s delta_motor_mixer[MAX_SUPPORTED_MOTORS];
int getThrottleIdleValue(void); int getThrottleIdleValue(void);
int16_t getThrottlePercent(void); int16_t getThrottlePercent(void);
uint8_t getMotorCount(void); uint8_t getMotorCount(void);
......
...@@ -31,6 +31,7 @@ FILE_COMPILE_FOR_SPEED ...@@ -31,6 +31,7 @@ FILE_COMPILE_FOR_SPEED
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "fc/controlrate_profile.h" #include "fc/controlrate_profile.h"
#include "fc/settings.h" #include "fc/settings.h"
#include "fc/cli.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/imu.h" #include "flight/imu.h"
...@@ -47,12 +48,12 @@ FILE_COMPILE_FOR_SPEED ...@@ -47,12 +48,12 @@ FILE_COMPILE_FOR_SPEED
FASTRAM int16_t forward_thrust_command = 1000; FASTRAM int16_t forward_thrust_command = 1000;
FASTRAM int16_t lateral_thrust_command = 1000; FASTRAM int16_t lateral_thrust_command = 1000;
static deltaThruster_t thrusters[MAX_SUPPORTED_MOTORS]; FASTRAM deltaThruster_t thrusters[MAX_SUPPORTED_MOTORS];
static int thruster_count; FASTRAM int thruster_count;
static const float acceptable_angle_error = .05; //about 3 degrees static const float acceptable_angle_error = .05; //about 3 degrees
float lateral_thrust_roll(float desired_pitch_angle, int sign); float lateral_thrust_roll(float desired_pitch_angle);
float forward_thrust_pitch(float desired_pitch_angle, int sign); float forward_thrust_pitch(float desired_pitch_angle);
float get_thruster_pitch(float throttle, float forward); float get_thruster_pitch(float throttle, float forward);
float get_thruster_roll(float throttle, float lateral); float get_thruster_roll(float throttle, float lateral);
const float dead_band = .15f; const float dead_band = .15f;
...@@ -61,13 +62,13 @@ void init_delta_thrusters() { ...@@ -61,13 +62,13 @@ void init_delta_thrusters() {
thruster_count = 0; //no thrusters by default thruster_count = 0; //no thrusters by default
int i = 0; int i = 0;
for(i = 0; i < MAX_SUPPORTED_MOTORS; i++){ for(i = 0; i < MAX_SUPPORTED_MOTORS; i++){
struct motorMixer_s m = getMotorMixerRow(i); motorMixer_t m = delta_motor_mixer[i];
float z = m.throttle; // local z component float z = m.throttle; // local z component
float x = m.forward; // local x component float x = m.forward; // local x component
float y = m.lateral; // local y component float y = m.lateral; // local y component
bool is_thruster = false; // nothing is a thruster by default 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 if (z != 0) { // if z thrust is zero then it has to be a thruster
is_thruster = ((abs(atan2(x, z)) > .78) || (abs(atan2(y, z)) > .78)); is_thruster = ((abs(atan2(z, x)) > .78) || (abs(atan2(z, x)) > .78));
} }
else if (x != 0 || y != 0){ else if (x != 0 || y != 0){
is_thruster = true; is_thruster = true;
...@@ -119,28 +120,29 @@ float get_delta_roll_angle( float desired_roll_angle){ ...@@ -119,28 +120,29 @@ float get_delta_roll_angle( float desired_roll_angle){
float roll_angle = desired_roll_angle; float roll_angle = desired_roll_angle;
//note this means output thruster //note this means output thruster
float power_level = desired_roll_angle*1.25; float power_level = desired_roll_angle*1.25;
if (power_level > dead_band || power_level < -dead_band) { if (power_level > dead_band || power_level < -dead_band){
roll_angle = get_delta_pitch_angle(desired_roll_angle); roll_angle = forward_thrust_pitch(desired_roll_angle);
lateral_thrust_command = constrain(2000* -desired_roll_angle * 1.25, -2000, 2000); //range 1000 to 2000 lateral_thrust_command = constrain(2000* -desired_roll_angle * 1.25, -2000, 2000); //range 1000 to 2000
} }
else { else {
lateral_thrust_command = 0; lateral_thrust_command = 0;
} }
return roll_angle;
} }
//hopefully this is still fast enough to still be used //hopefully this is still fast enough to still be used
// I might make these private // I might make these private
float forward_thrust_pitch(float desired_pitch_angle, int sign){ float forward_thrust_pitch(float desired_pitch_angle){
float pitch_angle = 0; float pitch_angle = 0;
float component_count = 0; float component_count = 0;
for(int i = 0; i < thruster_count; i++) { for(int i = 0; i < thruster_count; i++) {
deltaThruster_t thruster = thrusters[i]; deltaThruster_t thruster = thrusters[i];
if (sign == 1) { if (desired_pitch_angle < 0) {
if (thruster.forward > 0) { if (thruster.forward > 0) {
pitch_angle += thruster.pitch * thruster.forward; pitch_angle += thruster.pitch * thruster.forward;
component_count += thruster.forward; component_count += thruster.forward;
} }
} else if (sign == -1) { } else if (desired_pitch_angle > 0) {
if (thruster.forward < 0) { if (thruster.forward < 0) {
pitch_angle += thruster.pitch * thruster.forward; pitch_angle += thruster.pitch * thruster.forward;
component_count += thruster.forward; component_count += thruster.forward;
...@@ -156,25 +158,25 @@ float forward_thrust_pitch(float desired_pitch_angle, int sign){ ...@@ -156,25 +158,25 @@ float forward_thrust_pitch(float desired_pitch_angle, int sign){
return pitch_angle; return pitch_angle;
} }
float lateral_thrust_roll(float desired_pitch_angle, int sign){ float lateral_thrust_roll(float desired_roll_angle){
float roll_angle = 0; float roll_angle = 0;
float component_count = 0; float component_count = 0;
for(int i = 0; i < thruster_count; i++) { for(int i = 0; i < thruster_count; i++) {
deltaThruster_t thruster = thrusters[i]; deltaThruster_t thruster = thrusters[i];
if (sign == 1) { if (desired_roll_angle > 0) {
if (thruster.lateral > 0) { if (thruster.lateral < 0) {
roll_angle += thruster.roll * thruster.lateral; roll_angle += thruster.roll * thruster.lateral;
component_count += thruster.lateral; component_count += thruster.lateral;
} }
} else if (sign == -1) { } else if (desired_roll_angle < 0) {
if (thruster.roll < 0) { if (thruster.lateral > 0) {
roll_angle += thruster.roll * thruster.lateral; roll_angle += thruster.roll * thruster.lateral;
component_count += thruster.lateral; component_count += thruster.lateral;
} }
} }
} }
if(component_count == 0){ if(component_count == 0){
roll_angle = desired_pitch_angle; roll_angle = desired_roll_angle;
} }
else{ else{
roll_angle = roll_angle/component_count; roll_angle = roll_angle/component_count;
...@@ -193,4 +195,4 @@ float convert_rc_to_angle(int rc_input){ ...@@ -193,4 +195,4 @@ float convert_rc_to_angle(int rc_input){
float scaled_rc = rc_input/500.0f; float scaled_rc = rc_input/500.0f;
scaled_rc *= .78; //angle in radians for calculations. scaled_rc *= .78; //angle in radians for calculations.
return scaled_rc; return scaled_rc;
} }
\ No newline at end of file
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