mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-17 02:52:12 +01:00
OP-312: Create a StabilizationDesired object that is the generic input to
Stabilization, carries the desired rate or attitude as well as a flag on how to intepret it. git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2930 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124
peabody124
parent
672b9ce961
commit
3e17c2ff55
@ -159,13 +159,13 @@ SRC += $(OPUAVSYNTHDIR)/flighttelemetrystats.c
|
||||
SRC += $(OPUAVSYNTHDIR)/systemstats.c
|
||||
SRC += $(OPUAVSYNTHDIR)/systemalarms.c
|
||||
SRC += $(OPUAVSYNTHDIR)/systemsettings.c
|
||||
SRC += $(OPUAVSYNTHDIR)/stabilizationdesired.c
|
||||
SRC += $(OPUAVSYNTHDIR)/stabilizationsettings.c
|
||||
SRC += $(OPUAVSYNTHDIR)/actuatorcommand.c
|
||||
SRC += $(OPUAVSYNTHDIR)/actuatordesired.c
|
||||
SRC += $(OPUAVSYNTHDIR)/actuatorsettings.c
|
||||
SRC += $(OPUAVSYNTHDIR)/attituderaw.c
|
||||
SRC += $(OPUAVSYNTHDIR)/attitudeactual.c
|
||||
SRC += $(OPUAVSYNTHDIR)/attitudedesired.c
|
||||
SRC += $(OPUAVSYNTHDIR)/manualcontrolcommand.c
|
||||
SRC += $(OPUAVSYNTHDIR)/taskinfo.c
|
||||
SRC += $(OPUAVSYNTHDIR)/i2cstats.c
|
||||
|
||||
@ -48,11 +48,11 @@
|
||||
#include "guidancesettings.h"
|
||||
#include "attituderaw.h"
|
||||
#include "attitudeactual.h"
|
||||
#include "attitudedesired.h"
|
||||
#include "positiondesired.h" // object that will be updated by the module
|
||||
#include "positionactual.h"
|
||||
#include "manualcontrolcommand.h"
|
||||
#include "nedaccel.h"
|
||||
#include "stabilizationdesired.h"
|
||||
#include "stabilizationsettings.h"
|
||||
#include "systemsettings.h"
|
||||
#include "velocitydesired.h"
|
||||
@ -269,7 +269,7 @@ static void updateVtolDesiredAttitude()
|
||||
|
||||
VelocityDesiredData velocityDesired;
|
||||
VelocityActualData velocityActual;
|
||||
AttitudeDesiredData attitudeDesired;
|
||||
StabilizationDesiredData stabDesired;
|
||||
AttitudeActualData attitudeActual;
|
||||
NedAccelData nedAccel;
|
||||
GuidanceSettingsData guidanceSettings;
|
||||
@ -295,13 +295,13 @@ static void updateVtolDesiredAttitude()
|
||||
|
||||
VelocityActualGet(&velocityActual);
|
||||
VelocityDesiredGet(&velocityDesired);
|
||||
AttitudeDesiredGet(&attitudeDesired);
|
||||
StabilizationDesiredGet(&stabDesired);
|
||||
VelocityDesiredGet(&velocityDesired);
|
||||
AttitudeActualGet(&attitudeActual);
|
||||
StabilizationSettingsGet(&stabSettings);
|
||||
NedAccelGet(&nedAccel);
|
||||
|
||||
attitudeDesired.Yaw = 0; // try and face north
|
||||
stabDesired.Yaw = 0; // try and face north
|
||||
|
||||
// Compute desired north command
|
||||
northError = velocityDesired.North - velocityActual.North;
|
||||
@ -330,14 +330,14 @@ static void updateVtolDesiredAttitude()
|
||||
downIntegral * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KI] -
|
||||
nedAccel.Down * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KD]);
|
||||
|
||||
attitudeDesired.Throttle = bound(downCommand, 0, 1);
|
||||
stabDesired.Throttle = bound(downCommand, 0, 1);
|
||||
|
||||
// Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the
|
||||
// craft should move similarly for 5 deg roll versus 5 deg pitch
|
||||
attitudeDesired.Pitch = bound(-northCommand * cosf(attitudeActual.Yaw * M_PI / 180) +
|
||||
stabDesired.Pitch = bound(-northCommand * cosf(attitudeActual.Yaw * M_PI / 180) +
|
||||
-eastCommand * sinf(attitudeActual.Yaw * M_PI / 180),
|
||||
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch);
|
||||
attitudeDesired.Roll = bound(-northCommand * sinf(attitudeActual.Yaw * M_PI / 180) +
|
||||
stabDesired.Roll = bound(-northCommand * sinf(attitudeActual.Yaw * M_PI / 180) +
|
||||
eastCommand * cosf(attitudeActual.Yaw * M_PI / 180),
|
||||
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch);
|
||||
|
||||
@ -345,10 +345,14 @@ static void updateVtolDesiredAttitude()
|
||||
// For now override throttle with manual control. Disable at your risk, quad goes to China.
|
||||
ManualControlCommandData manualControl;
|
||||
ManualControlCommandGet(&manualControl);
|
||||
attitudeDesired.Throttle = manualControl.Throttle;
|
||||
stabDesired.Throttle = manualControl.Throttle;
|
||||
}
|
||||
|
||||
AttitudeDesiredSet(&attitudeDesired);
|
||||
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
||||
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
||||
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
||||
|
||||
StabilizationDesiredSet(&stabDesired);
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@ -39,7 +39,7 @@
|
||||
#include "stabilizationsettings.h"
|
||||
#include "manualcontrolcommand.h"
|
||||
#include "actuatordesired.h"
|
||||
#include "attitudedesired.h"
|
||||
#include "stabilizationdesired.h"
|
||||
#include "flighttelemetrystats.h"
|
||||
|
||||
// Private constants
|
||||
@ -72,68 +72,71 @@ static xTaskHandle taskHandle;
|
||||
static ArmState_t armState;
|
||||
|
||||
// Private functions
|
||||
static void updateActuatorDesired(ManualControlCommandData * cmd);
|
||||
static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings, float flightMode);
|
||||
|
||||
static void manualControlTask(void *parameters);
|
||||
static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral, int16_t deadband_percent);
|
||||
static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time);
|
||||
static bool okToArm(void);
|
||||
|
||||
#define assumptions1 ( \
|
||||
((int)MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_NONE == (int)MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE) && \
|
||||
((int)MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_RATE == (int)MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE) && \
|
||||
((int)MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_ATTITUDE == (int)MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE) \
|
||||
((int)MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
|
||||
((int)MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
|
||||
((int)MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
|
||||
)
|
||||
|
||||
#define assumptions2 ( \
|
||||
((int)MANUALCONTROLSETTINGS_POS1FLIGHTMODE_MANUAL == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) && \
|
||||
((int)MANUALCONTROLSETTINGS_POS1FLIGHTMODE_STABILIZED == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED) && \
|
||||
((int)MANUALCONTROLSETTINGS_POS1FLIGHTMODE_AUTO == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO) \
|
||||
((int)MANUALCONTROLSETTINGS_POS1FLIGHTMODE_MANUAL == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) && \
|
||||
((int)MANUALCONTROLSETTINGS_POS1FLIGHTMODE_STABILIZED == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED) && \
|
||||
((int)MANUALCONTROLSETTINGS_POS1FLIGHTMODE_AUTO == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO) \
|
||||
)
|
||||
|
||||
#define assumptions3 ( \
|
||||
((int)MANUALCONTROLSETTINGS_POS2STABILIZATIONSETTINGS_NONE == (int)MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE) && \
|
||||
((int)MANUALCONTROLSETTINGS_POS2STABILIZATIONSETTINGS_RATE == (int)MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE) && \
|
||||
((int)MANUALCONTROLSETTINGS_POS2STABILIZATIONSETTINGS_ATTITUDE == (int)MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE) \
|
||||
((int)MANUALCONTROLSETTINGS_POS2STABILIZATIONSETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
|
||||
((int)MANUALCONTROLSETTINGS_POS2STABILIZATIONSETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
|
||||
((int)MANUALCONTROLSETTINGS_POS2STABILIZATIONSETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
|
||||
)
|
||||
|
||||
#define assumptions4 ( \
|
||||
((int)MANUALCONTROLSETTINGS_POS2FLIGHTMODE_MANUAL == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) && \
|
||||
((int)MANUALCONTROLSETTINGS_POS2FLIGHTMODE_STABILIZED == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED) && \
|
||||
((int)MANUALCONTROLSETTINGS_POS2FLIGHTMODE_AUTO == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO) \
|
||||
((int)MANUALCONTROLSETTINGS_POS2FLIGHTMODE_MANUAL == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) && \
|
||||
((int)MANUALCONTROLSETTINGS_POS2FLIGHTMODE_STABILIZED == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED) && \
|
||||
((int)MANUALCONTROLSETTINGS_POS2FLIGHTMODE_AUTO == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO) \
|
||||
)
|
||||
|
||||
#define assumptions5 ( \
|
||||
((int)MANUALCONTROLSETTINGS_POS3STABILIZATIONSETTINGS_NONE == (int)MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE) && \
|
||||
((int)MANUALCONTROLSETTINGS_POS3STABILIZATIONSETTINGS_RATE == (int)MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE) && \
|
||||
((int)MANUALCONTROLSETTINGS_POS3STABILIZATIONSETTINGS_ATTITUDE == (int)MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE) \
|
||||
((int)MANUALCONTROLSETTINGS_POS3STABILIZATIONSETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
|
||||
((int)MANUALCONTROLSETTINGS_POS3STABILIZATIONSETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
|
||||
((int)MANUALCONTROLSETTINGS_POS3STABILIZATIONSETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
|
||||
)
|
||||
|
||||
#define assumptions6 ( \
|
||||
((int)MANUALCONTROLSETTINGS_POS3FLIGHTMODE_MANUAL == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) && \
|
||||
((int)MANUALCONTROLSETTINGS_POS3FLIGHTMODE_STABILIZED == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED) && \
|
||||
((int)MANUALCONTROLSETTINGS_POS3FLIGHTMODE_AUTO == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO) \
|
||||
((int)MANUALCONTROLSETTINGS_POS3FLIGHTMODE_MANUAL == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) && \
|
||||
((int)MANUALCONTROLSETTINGS_POS3FLIGHTMODE_STABILIZED == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED) && \
|
||||
((int)MANUALCONTROLSETTINGS_POS3FLIGHTMODE_AUTO == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO) \
|
||||
)
|
||||
|
||||
|
||||
#define ARMING_CHANNEL_ROLL 0
|
||||
#define ARMING_CHANNEL_PITCH 1
|
||||
#define ARMING_CHANNEL_YAW 2
|
||||
#define ARMING_CHANNEL_ROLL 0
|
||||
#define ARMING_CHANNEL_PITCH 1
|
||||
#define ARMING_CHANNEL_YAW 2
|
||||
|
||||
#define assumptions7 ( \
|
||||
( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_ROLL) && \
|
||||
( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_ROLL) && \
|
||||
( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_PITCH) && \
|
||||
( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_PITCH) && \
|
||||
( ((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_YAW) && \
|
||||
( ((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_YAW) \
|
||||
( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_ROLL) && \
|
||||
( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_ROLL) && \
|
||||
( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_PITCH) && \
|
||||
( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_PITCH) && \
|
||||
( ((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_YAW) && \
|
||||
( ((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_YAW) \
|
||||
)
|
||||
|
||||
#define assumptions8 ( \
|
||||
( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \
|
||||
( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) && \
|
||||
( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \
|
||||
( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) && \
|
||||
( ((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \
|
||||
( ((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) \
|
||||
( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \
|
||||
( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) && \
|
||||
( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \
|
||||
( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) && \
|
||||
( ((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \
|
||||
( ((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) \
|
||||
)
|
||||
|
||||
|
||||
@ -167,14 +170,10 @@ int32_t ManualControlInitialize()
|
||||
static void manualControlTask(void *parameters)
|
||||
{
|
||||
ManualControlSettingsData settings;
|
||||
StabilizationSettingsData stabSettings;
|
||||
ManualControlCommandData cmd;
|
||||
ActuatorDesiredData actuator;
|
||||
AttitudeDesiredData attitude;
|
||||
portTickType lastSysTime;
|
||||
|
||||
|
||||
float flightMode;
|
||||
|
||||
float flightMode = 0;
|
||||
|
||||
uint8_t disconnected_count = 0;
|
||||
uint8_t connected_count = 0;
|
||||
@ -197,7 +196,6 @@ static void manualControlTask(void *parameters)
|
||||
|
||||
// Read settings
|
||||
ManualControlSettingsGet(&settings);
|
||||
StabilizationSettingsGet(&stabSettings);
|
||||
|
||||
if (ManualControlCommandReadOnly(&cmd)) {
|
||||
FlightTelemetryStatsData flightTelemStats;
|
||||
@ -262,25 +260,13 @@ static void manualControlTask(void *parameters)
|
||||
else
|
||||
cmd.Accessory3 = 0;
|
||||
|
||||
if (flightMode < -FLIGHT_MODE_LIMIT) {
|
||||
// Position 1
|
||||
for(int i = 0; i < 3; i++) {
|
||||
cmd.StabilizationSettings[i] = settings.Pos1StabilizationSettings[i]; // See assumptions1
|
||||
}
|
||||
cmd.FlightMode = settings.Pos1FlightMode; // See assumptions2
|
||||
} else if (flightMode > FLIGHT_MODE_LIMIT) {
|
||||
// Position 3
|
||||
for(int i = 0; i < 3; i++) {
|
||||
cmd.StabilizationSettings[i] = settings.Pos3StabilizationSettings[i]; // See assumptions5
|
||||
}
|
||||
cmd.FlightMode = settings.Pos3FlightMode; // See assumptions6
|
||||
} else {
|
||||
// Position 2
|
||||
for(int i = 0; i < 3; i++) {
|
||||
cmd.StabilizationSettings[i] = settings.Pos2StabilizationSettings[i]; // See assumptions3
|
||||
}
|
||||
cmd.FlightMode = settings.Pos2FlightMode; // See assumptions4
|
||||
}
|
||||
if (flightMode < -FLIGHT_MODE_LIMIT)
|
||||
cmd.FlightMode = settings.Pos1FlightMode;
|
||||
else if (flightMode > FLIGHT_MODE_LIMIT)
|
||||
cmd.FlightMode = settings.Pos3FlightMode;
|
||||
else
|
||||
cmd.FlightMode = settings.Pos2FlightMode;
|
||||
|
||||
// Update the ManualControlCommand object
|
||||
ManualControlCommandSet(&cmd);
|
||||
// This seems silly to set then get, but the reason is if the GCS is
|
||||
@ -457,55 +443,63 @@ static void manualControlTask(void *parameters)
|
||||
|
||||
|
||||
// Depending on the mode update the Stabilization or Actuator objects
|
||||
if (cmd.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) {
|
||||
actuator.Roll = cmd.Roll;
|
||||
actuator.Pitch = cmd.Pitch;
|
||||
actuator.Yaw = cmd.Yaw;
|
||||
actuator.Throttle = cmd.Throttle;
|
||||
ActuatorDesiredSet(&actuator);
|
||||
} else if (cmd.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED) {
|
||||
switch(cmd.StabilizationSettings[MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ROLL]) {
|
||||
case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE:
|
||||
attitude.Roll = cmd.Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL];
|
||||
break;
|
||||
case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE:
|
||||
attitude.Roll = cmd.Roll * stabSettings.RollMax;
|
||||
break;
|
||||
case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE:
|
||||
attitude.Roll = cmd.Roll;
|
||||
break;
|
||||
}
|
||||
switch(cmd.StabilizationSettings[MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_PITCH]) {
|
||||
case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE:
|
||||
attitude.Pitch = cmd.Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH];
|
||||
break;
|
||||
case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE:
|
||||
attitude.Pitch = cmd.Pitch * stabSettings.PitchMax;
|
||||
break;
|
||||
case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE:
|
||||
attitude.Pitch = cmd.Pitch;
|
||||
break;
|
||||
}
|
||||
switch(cmd.StabilizationSettings[MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_YAW]) {
|
||||
case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE:
|
||||
attitude.Yaw = cmd.Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW];
|
||||
break;
|
||||
case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE:
|
||||
attitude.Yaw = fmod(cmd.Yaw * 180.0, 360);
|
||||
break;
|
||||
case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE:
|
||||
attitude.Yaw = cmd.Yaw;
|
||||
break;
|
||||
}
|
||||
attitude.Throttle = (cmd.Throttle < 0) ? -1 : cmd.Throttle;
|
||||
for(int i = 0; i < 3; i++) {
|
||||
attitude.StabilizationSettings[i] = cmd.StabilizationSettings[i];
|
||||
}
|
||||
AttitudeDesiredSet(&attitude);
|
||||
}
|
||||
if (cmd.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL)
|
||||
updateActuatorDesired(&cmd);
|
||||
else if (cmd.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED)
|
||||
updateStabilizationDesired(&cmd, &settings, flightMode);
|
||||
}
|
||||
}
|
||||
|
||||
static void updateActuatorDesired(ManualControlCommandData * cmd)
|
||||
{
|
||||
ActuatorDesiredData actuator;
|
||||
ActuatorDesiredGet(&actuator);
|
||||
actuator.Roll = cmd->Roll;
|
||||
actuator.Pitch = cmd->Pitch;
|
||||
actuator.Yaw = cmd->Yaw;
|
||||
actuator.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
|
||||
ActuatorDesiredSet(&actuator);
|
||||
}
|
||||
|
||||
static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings, float flightMode)
|
||||
{
|
||||
StabilizationDesiredData stabilization;
|
||||
StabilizationDesiredGet(&stabilization);
|
||||
|
||||
StabilizationSettingsData stabSettings;
|
||||
StabilizationSettingsGet(&stabSettings);
|
||||
|
||||
uint8_t * stab_settings;
|
||||
if (flightMode < -FLIGHT_MODE_LIMIT)
|
||||
stab_settings = settings->Pos1StabilizationSettings;
|
||||
else if (flightMode > FLIGHT_MODE_LIMIT)
|
||||
stab_settings = settings->Pos3StabilizationSettings;
|
||||
else
|
||||
stab_settings = settings->Pos2StabilizationSettings;
|
||||
|
||||
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = stab_settings[0];
|
||||
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1];
|
||||
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2];
|
||||
|
||||
stabilization.Roll = (stab_settings[0] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_NONE) ? cmd->Roll :
|
||||
(stab_settings[0] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_RATE) ? cmd->Roll * stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_ROLL] :
|
||||
(stab_settings[0] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_ATTITUDE) ? cmd->Roll * stabSettings.RollMax :
|
||||
0; // this is an invalid mode
|
||||
;
|
||||
stabilization.Roll = (stab_settings[1] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_NONE) ? cmd->Pitch :
|
||||
(stab_settings[1] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_RATE) ? cmd->Pitch * stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_PITCH] :
|
||||
(stab_settings[1] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
|
||||
0; // this is an invalid mode
|
||||
|
||||
stabilization.Roll = (stab_settings[2] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_NONE) ? cmd->Yaw :
|
||||
(stab_settings[2] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_RATE) ? cmd->Yaw * stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_YAW] :
|
||||
(stab_settings[2] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_ATTITUDE) ? fmod(cmd->Yaw * 180.0, 360) :
|
||||
0; // this is an invalid mode
|
||||
|
||||
stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
|
||||
StabilizationDesiredSet(&stabilization);
|
||||
}
|
||||
|
||||
/**
|
||||
* Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range.
|
||||
*/
|
||||
|
||||
@ -36,7 +36,7 @@
|
||||
#include "stabilizationsettings.h"
|
||||
#include "actuatordesired.h"
|
||||
#include "ratedesired.h"
|
||||
#include "attitudedesired.h"
|
||||
#include "stabilizationdesired.h"
|
||||
#include "attitudeactual.h"
|
||||
#include "attituderaw.h"
|
||||
#include "manualcontrolcommand.h"
|
||||
@ -119,7 +119,7 @@ static void stabilizationTask(void* parameters)
|
||||
|
||||
|
||||
ActuatorDesiredData actuatorDesired;
|
||||
AttitudeDesiredData attitudeDesired;
|
||||
StabilizationDesiredData stabDesired;
|
||||
RateDesiredData rateDesired;
|
||||
AttitudeActualData attitudeActual;
|
||||
AttitudeRawData attitudeRaw;
|
||||
@ -148,14 +148,14 @@ static void stabilizationTask(void* parameters)
|
||||
lastSysTime = thisSysTime;
|
||||
|
||||
ManualControlCommandGet(&manualControl);
|
||||
AttitudeDesiredGet(&attitudeDesired);
|
||||
StabilizationDesiredGet(&stabDesired);
|
||||
AttitudeActualGet(&attitudeActual);
|
||||
AttitudeRawGet(&attitudeRaw);
|
||||
RateDesiredGet(&rateDesired);
|
||||
SystemSettingsGet(&systemSettings);
|
||||
|
||||
|
||||
float *attitudeDesiredAxis = &attitudeDesired.Roll;
|
||||
float *attitudeDesiredAxis = &stabDesired.Roll;
|
||||
float *attitudeActualAxis = &attitudeActual.Roll;
|
||||
float *actuatorDesiredAxis = &actuatorDesired.Roll;
|
||||
float *rateDesiredAxis = &rateDesired.Roll;
|
||||
@ -163,13 +163,13 @@ static void stabilizationTask(void* parameters)
|
||||
//Calculate desired rate
|
||||
for(int8_t ct=0; ct< MAX_AXES; ct++)
|
||||
{
|
||||
switch(attitudeDesired.StabilizationSettings[ct])
|
||||
switch(stabDesired.StabilizationMode[ct])
|
||||
{
|
||||
case ATTITUDEDESIRED_STABILIZATIONSETTINGS_RATE:
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
|
||||
rateDesiredAxis[ct] = attitudeDesiredAxis[ct];
|
||||
break;
|
||||
|
||||
case ATTITUDEDESIRED_STABILIZATIONSETTINGS_ATTITUDE:
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
|
||||
rateDesiredAxis[ct] = ApplyPid(&pids[PID_ROLL + ct], attitudeDesiredAxis[ct], attitudeActualAxis[ct], 1);
|
||||
break;
|
||||
}
|
||||
@ -192,18 +192,35 @@ static void stabilizationTask(void* parameters)
|
||||
}
|
||||
|
||||
}
|
||||
switch(attitudeDesired.StabilizationSettings[ct])
|
||||
switch(stabDesired.StabilizationMode[ct])
|
||||
{
|
||||
case ATTITUDEDESIRED_STABILIZATIONSETTINGS_RATE:
|
||||
case ATTITUDEDESIRED_STABILIZATIONSETTINGS_ATTITUDE:
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
|
||||
{
|
||||
float command = ApplyPid(&pids[PID_RATE_ROLL + ct], rateDesiredAxis[ct], attitudeRaw.gyros[ct], 0);
|
||||
actuatorDesiredAxis[ct] = bound(command);
|
||||
break;
|
||||
}
|
||||
case ATTITUDEDESIRED_STABILIZATIONSETTINGS_NONE:
|
||||
actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]);
|
||||
break;
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE:
|
||||
//actuatorDesiredAxis[ct] = bound(manualAxis[ct]);
|
||||
//shouldUpdate = 1;
|
||||
switch (ct)
|
||||
{
|
||||
case ROLL:
|
||||
actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]);
|
||||
shouldUpdate = 1;
|
||||
break;
|
||||
case PITCH:
|
||||
actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]);
|
||||
shouldUpdate = 1;
|
||||
break;
|
||||
case YAW:
|
||||
actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]);
|
||||
shouldUpdate = 1;
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@ -218,14 +235,14 @@ static void stabilizationTask(void* parameters)
|
||||
|
||||
if(shouldUpdate)
|
||||
{
|
||||
actuatorDesired.Throttle = attitudeDesired.Throttle;
|
||||
actuatorDesired.Throttle = stabDesired.Throttle;
|
||||
if(dT > 15)
|
||||
actuatorDesired.NumLongUpdates++;
|
||||
ActuatorDesiredSet(&actuatorDesired);
|
||||
}
|
||||
|
||||
if(manualControl.Armed == MANUALCONTROLCOMMAND_ARMED_FALSE ||
|
||||
!shouldUpdate || (attitudeDesired.Throttle < 0))
|
||||
!shouldUpdate || (stabDesired.Throttle < 0))
|
||||
{
|
||||
ZeroPids();
|
||||
}
|
||||
|
||||
@ -31,9 +31,7 @@ UAVOBJSRCFILENAMES += ahrscalibration
|
||||
UAVOBJSRCFILENAMES += ahrssettings
|
||||
UAVOBJSRCFILENAMES += ahrsstatus
|
||||
UAVOBJSRCFILENAMES += attitudeactual
|
||||
UAVOBJSRCFILENAMES += attitudedesired
|
||||
UAVOBJSRCFILENAMES += attituderaw
|
||||
# UAVOBJSRCFILENAMES += attitudesettings CC only
|
||||
UAVOBJSRCFILENAMES += baroaltitude
|
||||
UAVOBJSRCFILENAMES += batterysettings
|
||||
UAVOBJSRCFILENAMES += firmwareiapobj
|
||||
@ -59,6 +57,7 @@ UAVOBJSRCFILENAMES += positionactual
|
||||
UAVOBJSRCFILENAMES += positiondesired
|
||||
UAVOBJSRCFILENAMES += ratedesired
|
||||
UAVOBJSRCFILENAMES += sonaraltitude
|
||||
UAVOBJSRCFILENAMES += stabilizationdesired
|
||||
UAVOBJSRCFILENAMES += stabilizationsettings
|
||||
UAVOBJSRCFILENAMES += systemalarms
|
||||
UAVOBJSRCFILENAMES += systemsettings
|
||||
|
||||
@ -74,6 +74,8 @@
|
||||
6528CCE212E40F6700CF5144 /* pios_adxl345.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_adxl345.h; sourceTree = "<group>"; };
|
||||
65322D3B122841F60046CD7C /* gpstime.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = gpstime.xml; sourceTree = "<group>"; };
|
||||
65345C871288668B00A5E4E8 /* guidancesettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = guidancesettings.xml; sourceTree = "<group>"; };
|
||||
6536D47B1307962C0042A298 /* stabilizationdesired.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = stabilizationdesired.xml; sourceTree = "<group>"; };
|
||||
6536D4881307AB950042A298 /* UAVObjects.inc */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.pascal; path = UAVObjects.inc; sourceTree = "<group>"; };
|
||||
65408AA812BB1648004DACC5 /* i2cstats.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = i2cstats.xml; sourceTree = "<group>"; };
|
||||
6543304F121980300063F913 /* insgps.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = insgps.h; sourceTree = "<group>"; };
|
||||
654612D812B5E9A900B719D0 /* pios_iap.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_iap.c; sourceTree = "<group>"; };
|
||||
@ -2633,7 +2635,6 @@
|
||||
65C35E5412EFB2F3004811C2 /* ahrssettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = ahrssettings.xml; sourceTree = "<group>"; };
|
||||
65C35E5512EFB2F3004811C2 /* ahrsstatus.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = ahrsstatus.xml; sourceTree = "<group>"; };
|
||||
65C35E5612EFB2F3004811C2 /* attitudeactual.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = attitudeactual.xml; sourceTree = "<group>"; };
|
||||
65C35E5712EFB2F3004811C2 /* attitudedesired.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = attitudedesired.xml; sourceTree = "<group>"; };
|
||||
65C35E5812EFB2F3004811C2 /* attituderaw.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = attituderaw.xml; sourceTree = "<group>"; };
|
||||
65C35E5912EFB2F3004811C2 /* baroaltitude.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = baroaltitude.xml; sourceTree = "<group>"; };
|
||||
65C35E5A12EFB2F3004811C2 /* batterysettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = batterysettings.xml; sourceTree = "<group>"; };
|
||||
@ -7365,7 +7366,6 @@
|
||||
65C35E5412EFB2F3004811C2 /* ahrssettings.xml */,
|
||||
65C35E5512EFB2F3004811C2 /* ahrsstatus.xml */,
|
||||
65C35E5612EFB2F3004811C2 /* attitudeactual.xml */,
|
||||
65C35E5712EFB2F3004811C2 /* attitudedesired.xml */,
|
||||
65C35E5812EFB2F3004811C2 /* attituderaw.xml */,
|
||||
65E410AE12F65AEA00725888 /* attitudesettings.xml */,
|
||||
65C35E5912EFB2F3004811C2 /* baroaltitude.xml */,
|
||||
@ -7392,6 +7392,7 @@
|
||||
65C35E6E12EFB2F3004811C2 /* positionactual.xml */,
|
||||
65C35E6F12EFB2F3004811C2 /* positiondesired.xml */,
|
||||
65C35E7012EFB2F3004811C2 /* ratedesired.xml */,
|
||||
6536D47B1307962C0042A298 /* stabilizationdesired.xml */,
|
||||
65C35E7112EFB2F3004811C2 /* stabilizationsettings.xml */,
|
||||
65C35E7212EFB2F3004811C2 /* systemalarms.xml */,
|
||||
65C35E7312EFB2F3004811C2 /* systemsettings.xml */,
|
||||
@ -7520,6 +7521,7 @@
|
||||
65E8EF1E11EEA61E00BBF654 /* OpenPilot */ = {
|
||||
isa = PBXGroup;
|
||||
children = (
|
||||
6536D4881307AB950042A298 /* UAVObjects.inc */,
|
||||
65E8EF1F11EEA61E00BBF654 /* Makefile */,
|
||||
65E8EF2011EEA61E00BBF654 /* Makefile.posix */,
|
||||
65E8EF5B11EEA61E00BBF654 /* System */,
|
||||
|
||||
@ -8,7 +8,6 @@
|
||||
<field name="Yaw" units="%" type="float" elements="1"/>
|
||||
<field name="Throttle" units="%" type="float" elements="1"/>
|
||||
<field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized,Auto"/>
|
||||
<field name="StabilizationSettings" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude"/>
|
||||
<field name="Accessory1" units="%" type="float" elements="1"/>
|
||||
<field name="Accessory2" units="%" type="float" elements="1"/>
|
||||
<field name="Accessory3" units="%" type="float" elements="1"/>
|
||||
|
||||
@ -1,14 +1,14 @@
|
||||
<xml>
|
||||
<object name="AttitudeDesired" singleinstance="true" settings="false">
|
||||
<object name="StabilizationDesired" singleinstance="true" settings="false">
|
||||
<description>The desired attitude that @ref StabilizationModule will try and achieve if FlightMode is Stabilized. Comes from @ref ManaulControlModule.</description>
|
||||
<field name="Roll" units="degrees" type="float" elements="1"/>
|
||||
<field name="Pitch" units="degrees" type="float" elements="1"/>
|
||||
<field name="Yaw" units="degrees" type="float" elements="1"/>
|
||||
<field name="Throttle" units="%" type="float" elements="1"/>
|
||||
<field name="StabilizationSettings" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude"/>
|
||||
<field name="StabilizationMode" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="false" updatemode="manual" period="0"/>
|
||||
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
|
||||
<logging updatemode="never" period="0"/>
|
||||
</object>
|
||||
</xml>
|
||||
</xml>
|
||||
Loading…
x
Reference in New Issue
Block a user