Commit 6a2fb26a authored by Sam Winkelstein's avatar Sam Winkelstein
Browse files

finished delta controller integrated into navigation_multicopter.c

parent 179c60a1
...@@ -294,6 +294,7 @@ void mixerInit(void) ...@@ -294,6 +294,7 @@ void mixerInit(void)
} else { } else {
motorYawMultiplier = 1; motorYawMultiplier = 1;
} }
init_delta_thrusters();
} }
void mixerResetDisarmedMotors(void) void mixerResetDisarmedMotors(void)
......
...@@ -49,7 +49,6 @@ FASTRAM int16_t lateral_thrust_command = 1000; ...@@ -49,7 +49,6 @@ FASTRAM int16_t lateral_thrust_command = 1000;
static deltaThruster_t thrusters[MAX_SUPPORTED_MOTORS]; static deltaThruster_t thrusters[MAX_SUPPORTED_MOTORS];
static int thruster_count; static int thruster_count;
static bool Thruster_control;
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, int sign);
...@@ -62,14 +61,13 @@ void init_delta_thrusters() { ...@@ -62,14 +61,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++){
thruster_control = false;
struct motorMixer_s m = getMotorMixerRow(i); struct motorMixer_s m = getMotorMixerRow(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 = ((absf(atan2f(x, z)) > .78) || (absf(atan2f(y, z)) > .78)); is_thruster = ((abs(atan2(x, z)) > .78) || (abs(atan2(y, z)) > .78));
} }
else if (x != 0 || y != 0){ else if (x != 0 || y != 0){
is_thruster = true; is_thruster = true;
...@@ -81,7 +79,6 @@ void init_delta_thrusters() { ...@@ -81,7 +79,6 @@ void init_delta_thrusters() {
thrusters[thruster_count].pitch = get_thruster_pitch(z, x); thrusters[thruster_count].pitch = get_thruster_pitch(z, x);
thrusters[thruster_count].roll = get_thruster_pitch(z, y); thrusters[thruster_count].roll = get_thruster_pitch(z, y);
m.throttle = 0.0; m.throttle = 0.0;
thruster_control = true;
thruster_count++; thruster_count++;
}//increase thruster count for calculation }//increase thruster count for calculation
} }
...@@ -90,7 +87,7 @@ void init_delta_thrusters() { ...@@ -90,7 +87,7 @@ void init_delta_thrusters() {
float get_thruster_pitch(float throttle, float forward){ float get_thruster_pitch(float throttle, float forward){
float pitch = 0; float pitch = 0;
if(throttle != 0) { if(throttle != 0) {
pitch = atan2f(forward, throttle); pitch = atan2(forward, throttle);
} }
return pitch; return pitch;
} }
...@@ -98,7 +95,7 @@ float get_thruster_pitch(float throttle, float forward){ ...@@ -98,7 +95,7 @@ float get_thruster_pitch(float throttle, float forward){
float get_thruster_roll(float throttle, float lateral) { // 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; float roll = 0;
if (throttle != 0){ if (throttle != 0){
roll = atan2f(lateral, throttle); //using atan roll roll = atan2(lateral, throttle); //using atan roll
} }
return roll; return roll;
} }
...@@ -110,10 +107,10 @@ float get_delta_pitch_angle( float desired_pitch_angle){ ...@@ -110,10 +107,10 @@ float get_delta_pitch_angle( float desired_pitch_angle){
float power_level = desired_pitch_angle*1.25; float power_level = desired_pitch_angle*1.25;
if (power_level > dead_band || power_level < -dead_band) { if (power_level > dead_band || power_level < -dead_band) {
pitch_angle = get_delta_pitch_angle(desired_pitch_angle); pitch_angle = get_delta_pitch_angle(desired_pitch_angle);
forward_thrust_command = constrain(1000* -desired_pitch_angle * 1.25, 1000, 2000); //range 1000 to 2000 forward_thrust_command = constrain(2000* -desired_pitch_angle * 1.25, -2000, 2000); //range 1000 to 2000
} }
else { else {
forward_thrust_command = 1000; forward_thrust_command = 0;
} }
return pitch_angle; return pitch_angle;
} }
...@@ -124,10 +121,10 @@ float get_delta_roll_angle( float desired_roll_angle){ ...@@ -124,10 +121,10 @@ float get_delta_roll_angle( float desired_roll_angle){
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 = get_delta_pitch_angle(desired_roll_angle);
lateral_thrust_command = constrain(1000* -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 = 1000; lateral_thrust_command = 0;
} }
} }
......
...@@ -19,6 +19,8 @@ typedef struct deltaThruster_S { ...@@ -19,6 +19,8 @@ typedef struct deltaThruster_S {
float roll; float roll;
} deltaThruster_t; } deltaThruster_t;
float get_delta_pitch_angle( float desired_pitch_angle);
float get_delta_roll_angle( float desired_roll_angle);
void init_delta_thrusters(); void init_delta_thrusters();
float calc_yaw_angle_offset(); float calc_yaw_angle_offset();
bool is_delta_thruster(int index); bool is_delta_thruster(int index);
......
...@@ -50,6 +50,7 @@ ...@@ -50,6 +50,7 @@
#include "navigation/navigation.h" #include "navigation/navigation.h"
#include "navigation/navigation_private.h" #include "navigation/navigation_private.h"
#include "navigation/navigation_delta_controller.h"
/*----------------------------------------------------------- /*-----------------------------------------------------------
* Altitude controller for multicopter aircraft * Altitude controller for multicopter aircraft
...@@ -622,16 +623,9 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA ...@@ -622,16 +623,9 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
const float desiredRoll = atan2_approx(accelRight * cos_approx(desiredPitch), GRAVITY_CMSS); 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 // Decide whether to get the requested acceleration from roll/pitch or directly from actuators
if (mixerHasForwardAuthority()) {
mixerForwardCommand = accelForward; posControl.rcAdjustment[PITCH] = constrain(RADIANS_TO_DECIDEGREES(get_delta_pitch_angle(desiredPitch)), -maxBankAngle, maxBankAngle);
} else { posControl.rcAdjustment[ROLL] = constrain(RADIANS_TO_DECIDEGREES(get_delta_roll_angle(desiredRoll)), -maxBankAngle, maxBankAngle);
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);
}
} }
...@@ -671,14 +665,17 @@ static void applyMulticopterPositionController(timeUs_t currentTimeUs) ...@@ -671,14 +665,17 @@ static void applyMulticopterPositionController(timeUs_t currentTimeUs)
} }
else { else {
/* No position data, disable automatic adjustment, rcCommand passthrough */ /* No position data, disable automatic adjustment, rcCommand passthrough */
/* this appears to be an all stop */
posControl.rcAdjustment[PITCH] = 0; posControl.rcAdjustment[PITCH] = 0;
posControl.rcAdjustment[ROLL] = 0; posControl.rcAdjustment[ROLL] = 0;
forward_thrust_command = 1000;
lateral_thrust_command = 1000;
bypassPositionController = true; bypassPositionController = true;
} }
if (!bypassPositionController) { if (!bypassPositionController) {
rcCommand[PITCH] = pidAngleToRcCommand(posControl.rcAdjustment[PITCH], pidProfile()->max_angle_inclination[FD_PITCH]); 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(posControl.rcAdjustment[ROLL], pidProfile()->max_angle_inclination[FD_ROLL]); 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