rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-02 10:24:11 +01:00
Code Issues Releases Activity

Flight: Tweaked the throttle handling and VTOL use of it to make sure engines can be shut off easily in stabilization mode.

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1593 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-09-12 02:54:49 +00:00 committed by peabody124
parent 56131476ef
commit e90e714c2b
3 changed files with 42 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
flight/OpenPilot/Modules/Actuator/actuator.c
View File

@ -292,6 +292,9 @@ static int32_t mixerVTOL(const ActuatorSettingsData* settings, const ActuatorDes
VTOLStatusGet(&vtolStatus);
const int vtolMin = -1;
int vtolMax = 1;
if(desired->Throttle < 0)
vtolMax = -1;
if(((settings->VTOLMotorN != ACTUATORSETTINGS_VTOLMOTORN_NONE) +
(settings->VTOLMotorNE != ACTUATORSETTINGS_VTOLMOTORS_NONE) +
@ -308,7 +311,7 @@ static int32_t mixerVTOL(const ActuatorSettingsData* settings, const ActuatorDes
vtolStatus.MotorN = bound(desired->Throttle * vtolSettings.MotorN[VTOLSETTINGS_MOTORN_THROTTLE] +
desired->Pitch * vtolSettings.MotorN[VTOLSETTINGS_MOTORN_PITCH] +
desired->Roll * vtolSettings.MotorN[VTOLSETTINGS_MOTORN_ROLL] +
desired->Yaw * vtolSettings.MotorN[VTOLSETTINGS_MOTORN_YAW],vtolMin,1);
desired->Yaw * vtolSettings.MotorN[VTOLSETTINGS_MOTORN_YAW],vtolMin,vtolMax);
cmd->Channel[settings->VTOLMotorN] = scaleChannel(vtolStatus.MotorN,
settings->ChannelMax[settings->VTOLMotorN],
settings->ChannelMin[settings->VTOLMotorN],
@ -318,7 +321,7 @@ static int32_t mixerVTOL(const ActuatorSettingsData* settings, const ActuatorDes
vtolStatus.MotorNE = bound(desired->Throttle * vtolSettings.MotorNE[VTOLSETTINGS_MOTORNE_THROTTLE] +
desired->Pitch * vtolSettings.MotorNE[VTOLSETTINGS_MOTORNE_PITCH] +
desired->Roll * vtolSettings.MotorNE[VTOLSETTINGS_MOTORNE_ROLL] +
desired->Yaw * vtolSettings.MotorNE[VTOLSETTINGS_MOTORNE_YAW],vtolMin,1);
desired->Yaw * vtolSettings.MotorNE[VTOLSETTINGS_MOTORNE_YAW],vtolMin,vtolMax);
cmd->Channel[settings->VTOLMotorNE] = scaleChannel(vtolStatus.MotorNE,
settings->ChannelMax[settings->VTOLMotorNE],
settings->ChannelMin[settings->VTOLMotorNE],
@ -328,7 +331,7 @@ static int32_t mixerVTOL(const ActuatorSettingsData* settings, const ActuatorDes
vtolStatus.MotorE = bound(desired->Throttle * vtolSettings.MotorE[VTOLSETTINGS_MOTORE_THROTTLE] +
desired->Pitch * vtolSettings.MotorE[VTOLSETTINGS_MOTORE_PITCH] +
desired->Roll * vtolSettings.MotorE[VTOLSETTINGS_MOTORE_ROLL] +
desired->Yaw * vtolSettings.MotorE[VTOLSETTINGS_MOTORE_YAW],vtolMin,1);
desired->Yaw * vtolSettings.MotorE[VTOLSETTINGS_MOTORE_YAW],vtolMin,vtolMax);
cmd->Channel[settings->VTOLMotorE] = scaleChannel(vtolStatus.MotorE,
settings->ChannelMax[settings->VTOLMotorE],
settings->ChannelMin[settings->VTOLMotorE],
@ -338,7 +341,7 @@ static int32_t mixerVTOL(const ActuatorSettingsData* settings, const ActuatorDes
vtolStatus.MotorSE = bound(desired->Throttle * vtolSettings.MotorSE[VTOLSETTINGS_MOTORSE_THROTTLE] +
desired->Pitch * vtolSettings.MotorSE[VTOLSETTINGS_MOTORSE_PITCH] +
desired->Roll * vtolSettings.MotorSE[VTOLSETTINGS_MOTORSE_ROLL] +
desired->Yaw * vtolSettings.MotorSE[VTOLSETTINGS_MOTORSE_YAW],vtolMin,1);
desired->Yaw * vtolSettings.MotorSE[VTOLSETTINGS_MOTORSE_YAW],vtolMin,vtolMax);
cmd->Channel[settings->VTOLMotorSE] = scaleChannel(vtolStatus.MotorSE,
settings->ChannelMax[settings->VTOLMotorSE],
settings->ChannelMin[settings->VTOLMotorSE],
@ -348,7 +351,7 @@ static int32_t mixerVTOL(const ActuatorSettingsData* settings, const ActuatorDes
vtolStatus.MotorS = bound(desired->Throttle * vtolSettings.MotorS[VTOLSETTINGS_MOTORS_THROTTLE] +
desired->Pitch * vtolSettings.MotorS[VTOLSETTINGS_MOTORS_PITCH] +
desired->Roll * vtolSettings.MotorS[VTOLSETTINGS_MOTORS_ROLL] +
desired->Yaw * vtolSettings.MotorS[VTOLSETTINGS_MOTORS_YAW],vtolMin,1);
desired->Yaw * vtolSettings.MotorS[VTOLSETTINGS_MOTORS_YAW],vtolMin,vtolMax);
cmd->Channel[settings->VTOLMotorS] = scaleChannel(vtolStatus.MotorS,
settings->ChannelMax[settings->VTOLMotorS],
settings->ChannelMin[settings->VTOLMotorS],
@ -358,7 +361,7 @@ static int32_t mixerVTOL(const ActuatorSettingsData* settings, const ActuatorDes
vtolStatus.MotorSW = bound(desired->Throttle * vtolSettings.MotorSW[VTOLSETTINGS_MOTORSW_THROTTLE] +
desired->Pitch * vtolSettings.MotorSW[VTOLSETTINGS_MOTORSW_PITCH] +
desired->Roll * vtolSettings.MotorSW[VTOLSETTINGS_MOTORSW_ROLL] +
desired->Yaw * vtolSettings.MotorSW[VTOLSETTINGS_MOTORSW_YAW],vtolMin,1);
desired->Yaw * vtolSettings.MotorSW[VTOLSETTINGS_MOTORSW_YAW],vtolMin,vtolMax);
cmd->Channel[settings->VTOLMotorSW] = scaleChannel(vtolStatus.MotorSW,
settings->ChannelMax[settings->VTOLMotorSW],
settings->ChannelMin[settings->VTOLMotorSW],
@ -368,7 +371,7 @@ static int32_t mixerVTOL(const ActuatorSettingsData* settings, const ActuatorDes
vtolStatus.MotorW = bound(desired->Throttle * vtolSettings.MotorW[VTOLSETTINGS_MOTORW_THROTTLE] +
desired->Pitch * vtolSettings.MotorW[VTOLSETTINGS_MOTORW_PITCH] +
desired->Roll * vtolSettings.MotorW[VTOLSETTINGS_MOTORW_ROLL] +
desired->Yaw * vtolSettings.MotorW[VTOLSETTINGS_MOTORW_YAW],vtolMin,1);
desired->Yaw * vtolSettings.MotorW[VTOLSETTINGS_MOTORW_YAW],vtolMin,vtolMax);
cmd->Channel[settings->VTOLMotorW] = scaleChannel(vtolStatus.MotorW,
settings->ChannelMax[settings->VTOLMotorW],
settings->ChannelMin[settings->VTOLMotorW],
@ -378,7 +381,7 @@ static int32_t mixerVTOL(const ActuatorSettingsData* settings, const ActuatorDes
vtolStatus.MotorNW = bound(desired->Throttle * vtolSettings.MotorNW[VTOLSETTINGS_MOTORNW_THROTTLE] +
desired->Pitch * vtolSettings.MotorNW[VTOLSETTINGS_MOTORNW_PITCH] +
desired->Roll * vtolSettings.MotorNW[VTOLSETTINGS_MOTORNW_ROLL] +
desired->Yaw * vtolSettings.MotorNW[VTOLSETTINGS_MOTORNW_YAW],vtolMin,1);
desired->Yaw * vtolSettings.MotorNW[VTOLSETTINGS_MOTORNW_YAW],vtolMin,vtolMax);
cmd->Channel[settings->VTOLMotorNW] = scaleChannel(vtolStatus.MotorNW,
settings->ChannelMax[settings->VTOLMotorNW],
settings->ChannelMin[settings->VTOLMotorNW],

5
flight/OpenPilot/Modules/ManualControl/manualcontrol.c
View File

@ -250,7 +250,10 @@ static void manualControlTask(void* parameters)
{
attitude.Yaw = (cmd.Yaw*180.0);
}
attitude.Throttle = cmd.Throttle*stabSettings.ThrottleMax;
if(cmd.Throttle < 0)
attitude.Throttle = -1;
else
attitude.Throttle = cmd.Throttle*stabSettings.ThrottleMax;
AttitudeDesiredSet(&attitude);
}

32
ground/src/experimental/SerialLogger/analyzeINSGPS.m
View File

@ -1,4 +1,4 @@
function [q gyro accel time] = analyzeINSGPS(fn)
function [q gyro accel rpy time] = analyzeINSGPS(fn)
% Analyzes data collected from SerialLogger while DUMP_FRIENDLY
% enabled in AHRS
%
@ -50,15 +50,37 @@ accel = cat(1,data.accel);
gyro = cat(1,data.gyro);
time = (1:size(q,1)) / 50;
h(1) = subplot(311);
rpy = Quaternion2RPY(q);
h(1) = subplot(411);
plot(time,q)
ylabel('Quaternion');
h(2) = subplot(312);
h(2) = subplot(412);
plot(time,rpy);
ylabel('RPY'); legend('Roll','Pitch','Yaw');
h(3) = subplot(413);
plot(time,accel);
ylabel('m/s')
h(3) = subplot(313);
h(4) = subplot(414);
plot(time,gyro);
ylabel('rad/sec');
xlabel('Time (s)')
linkaxes(h,'x');
linkaxes(h,'x');
function rpy = Quaternion2RPY(q)
RAD2DEG = 180/pi;
qs = q .* q;
R13 = 2*(q(:,1).*q(:,4) -q(:,1).*q(:,3)); %2*(q[1]*q[3]-q[0]*q[2]);
R11 = qs(:,1) + qs(:,2) - qs(:,3) - qs(:,4); %q0s+q1s-q2s-q3s;
R12 = 2*(q(:,2).*q(:,3) + q(:,1).*q(:,4)); %2*(q[1]*q[2]+q[0]*q[3]);
R23 = 2*(q(:,3).*q(:,4) + q(:,1) .* q(:,2)); %2*(q[2]*q[3]+q[0]*q[1]);
R33 = qs(:,1)-qs(:,2)-qs(:,3)+qs(:,4); %q0s-q1s-q2s+q3s;
rpy(:,2)=RAD2DEG*asin(-R13); % pitch always between -pi/2 to pi/2
rpy(:,3)=RAD2DEG*atan2(R12,R11);
rpy(:,1)=RAD2DEG*atan2(R23,R33);