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)
} else {
motorYawMultiplier = 1;
}
init_delta_thrusters();
}
void mixerResetDisarmedMotors(void)
......
......@@ -49,7 +49,6 @@ FASTRAM int16_t lateral_thrust_command = 1000;
static deltaThruster_t thrusters[MAX_SUPPORTED_MOTORS];
static int thruster_count;
static bool Thruster_control;
static const float acceptable_angle_error = .05; //about 3 degrees
float lateral_thrust_roll(float desired_pitch_angle, int sign);
......@@ -62,14 +61,13 @@ void init_delta_thrusters() {
thruster_count = 0; //no thrusters by default
int i = 0;
for(i = 0; i < MAX_SUPPORTED_MOTORS; i++){
thruster_control = false;
struct motorMixer_s m = getMotorMixerRow(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 = ((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){
is_thruster = true;
......@@ -81,7 +79,6 @@ void init_delta_thrusters() {
thrusters[thruster_count].pitch = get_thruster_pitch(z, x);
thrusters[thruster_count].roll = get_thruster_pitch(z, y);
m.throttle = 0.0;
thruster_control = true;
thruster_count++;
}//increase thruster count for calculation
}
......@@ -90,7 +87,7 @@ void init_delta_thrusters() {
float get_thruster_pitch(float throttle, float forward){
float pitch = 0;
if(throttle != 0) {
pitch = atan2f(forward, throttle);
pitch = atan2(forward, throttle);
}
return pitch;
}
......@@ -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 roll = 0;
if (throttle != 0){
roll = atan2f(lateral, throttle); //using atan roll
roll = atan2(lateral, throttle); //using atan roll
}
return roll;
}
......@@ -110,10 +107,10 @@ float get_delta_pitch_angle( float desired_pitch_angle){
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(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 {
forward_thrust_command = 1000;
forward_thrust_command = 0;
}
return pitch_angle;
}
......@@ -124,10 +121,10 @@ float get_delta_roll_angle( float desired_roll_angle){
float power_level = desired_roll_angle*1.25;
if (power_level > dead_band || power_level < -dead_band) {
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 {
lateral_thrust_command = 1000;
lateral_thrust_command = 0;
}
}
......
......@@ -19,6 +19,8 @@ typedef struct deltaThruster_S {
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);
......
......@@ -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