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

OP-475: Starting to use the new FlightStatus object

Browse Source
This commit is contained in:
James Cotton 2011-05-03 11:04:44 -05:00
parent ae841d57cb
commit 80c839d5bb
8 changed files with 88 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
flight/CopterControl/Makefile
View File

@ -160,6 +160,7 @@ ifndef TESTAPP
SRC += $(OPUAVSYNTHDIR)/objectpersistence.c
SRC += $(OPUAVSYNTHDIR)/gcstelemetrystats.c
SRC += $(OPUAVSYNTHDIR)/flighttelemetrystats.c
SRC += $(OPUAVSYNTHDIR)/flightstatus.c
SRC += $(OPUAVSYNTHDIR)/systemstats.c
SRC += $(OPUAVSYNTHDIR)/systemalarms.c
SRC += $(OPUAVSYNTHDIR)/systemsettings.c

11
flight/Modules/Actuator/actuator.c
View File

@ -37,7 +37,7 @@
#include "systemsettings.h"
#include "actuatordesired.h"
#include "actuatorcommand.h"
#include "manualcontrolcommand.h"
#include "flightstatus.h"
#include "mixersettings.h"
#include "mixerstatus.h"
@ -135,7 +135,7 @@ static void actuatorTask(void* parameters)
MixerSettingsData mixerSettings;
ActuatorDesiredData desired;
MixerStatusData mixerStatus;
ManualControlCommandData manualControl;
FlightStatusData flightStatus;
ActuatorSettingsGet(&settings);
PIOS_Servo_SetHz(&settings.ChannelUpdateFreq[0], ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM);
@ -165,7 +165,7 @@ static void actuatorTask(void* parameters)
lastSysTime = thisSysTime;
ManualControlCommandGet(&manualControl);
FlightStatusGet(&flightStatus);
SystemSettingsGet(&sysSettings);
MixerStatusGet(&mixerStatus);
MixerSettingsGet (&mixerSettings);
@ -190,10 +190,9 @@ static void actuatorTask(void* parameters)
AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR);
bool armed = manualControl.Armed == MANUALCONTROLCOMMAND_ARMED_TRUE;
bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED;
bool positiveThrottle = desired.Throttle >= 0.00;
bool spinWhileArmed = settings.MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE &&
manualControl.Armed == MANUALCONTROLCOMMAND_ARMED_TRUE;
bool spinWhileArmed = settings.MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE;
float curve1 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve1);
float curve2 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve2);

12
flight/Modules/ManualControl/inc/manualcontrol.h
View File

@ -35,12 +35,12 @@
typedef enum {FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABILIZED = 2, FLIGHTMODE_GUIDANCE = 3} flightmode_path;
#define PARSE_FLIGHT_MODE(x) ( \
(x == MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) ? FLIGHTMODE_MANUAL : \
(x == MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED1) ? FLIGHTMODE_STABILIZED : \
(x == MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED2) ? FLIGHTMODE_STABILIZED : \
(x == MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED3) ? FLIGHTMODE_STABILIZED : \
(x == MANUALCONTROLCOMMAND_FLIGHTMODE_VELOCITYCONTROL) ? FLIGHTMODE_GUIDANCE : \
(x == MANUALCONTROLCOMMAND_FLIGHTMODE_POSITIONHOLD) ? FLIGHTMODE_GUIDANCE : FLIGHTMODE_UNDEFINED \
(x == FLIGHTSTATUS_FLIGHTMODE_MANUAL) ? FLIGHTMODE_MANUAL : \
(x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) ? FLIGHTMODE_STABILIZED : \
(x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) ? FLIGHTMODE_STABILIZED : \
(x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) ? FLIGHTMODE_STABILIZED : \
(x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) ? FLIGHTMODE_GUIDANCE : FLIGHTMODE_UNDEFINED \
)
int32_t ManualControlInitialize();

80
flight/Modules/ManualControl/manualcontrol.c
View File

@ -41,6 +41,7 @@
#include "actuatordesired.h"
#include "stabilizationdesired.h"
#include "flighttelemetrystats.h"
#include "flightstatus.h"
// Private constants
#if defined(PIOS_MANUAL_STACK_SIZE)
@ -76,7 +77,7 @@ static portTickType lastSysTime;
// Private functions
static void updateActuatorDesired(ManualControlCommandData * cmd);
static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
static void processFlightMode(ManualControlCommandData * cmd, ManualControlSettingsData * settings, float flightMode);
static void processFlightMode(ManualControlSettingsData * settings, float flightMode);
static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
static void manualControlTask(void *parameters);
@ -129,12 +130,12 @@ static bool validInputRange(int16_t min, int16_t max, uint16_t value);
#define assumptions_flightmode ( \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL == (int) MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int) MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED1) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int) MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED2) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int) MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED3) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int) MANUALCONTROLCOMMAND_FLIGHTMODE_VELOCITYCONTROL) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int) MANUALCONTROLCOMMAND_FLIGHTMODE_POSITIONHOLD) \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL == (int) FLIGHTSTATUS_FLIGHTMODE_MANUAL) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int) FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int) FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) \
)
#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions7 && assumptions8 && assumptions_flightmode)
@ -163,7 +164,7 @@ static void manualControlTask(void *parameters)
{
ManualControlSettingsData settings;
ManualControlCommandData cmd;
FlightStatusData flightStatus;
float flightMode = 0;
uint8_t disconnected_count = 0;
@ -171,8 +172,8 @@ static void manualControlTask(void *parameters)
// Make sure unarmed on power up
ManualControlCommandGet(&cmd);
cmd.Armed = MANUALCONTROLCOMMAND_ARMED_FALSE;
ManualControlCommandSet(&cmd);
FlightStatusGet(&flightStatus);
flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
armState = ARM_STATE_DISARMED;
// Main task loop
@ -270,7 +271,7 @@ static void manualControlTask(void *parameters)
cmd.Accessory2 = (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE) ? scaledChannel[settings.Accessory2] : 0;
cmd.Accessory3 = (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY3_NONE) ? scaledChannel[settings.Accessory3] : 0;
processFlightMode(&cmd, &settings, flightMode);
processFlightMode(&settings, flightMode);
processArm(&cmd, &settings);
// Update cmd object
@ -283,8 +284,10 @@ static void manualControlTask(void *parameters)
}
FlightStatusGet(&flightStatus);
// Depending on the mode update the Stabilization or Actuator objects
switch(PARSE_FLIGHT_MODE(cmd.FlightMode)) {
switch(PARSE_FLIGHT_MODE(flightStatus.FlightMode)) {
case FLIGHTMODE_UNDEFINED:
// This reflects a bug in the code architecture!
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
@ -322,14 +325,16 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
StabilizationSettingsGet(&stabSettings);
uint8_t * stab_settings;
switch(cmd->FlightMode) {
case MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED1:
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
switch(flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
stab_settings = settings->Stabilization1Settings;
break;
case MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED2:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
stab_settings = settings->Stabilization2Settings;
break;
case MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED3:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
stab_settings = settings->Stabilization3Settings;
break;
default:
@ -425,6 +430,20 @@ static bool okToArm(void)
return true;
}
/**
* @brief Update the flightStatus object only if value changed. Reduces callbacks
* @param[in] val The new value
*/
static void setArmedIfChanged(uint8_t val) {
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if(flightStatus.Armed != val) {
flightStatus.Armed = val;
FlightStatusSet(&flightStatus);
}
}
/**
* @brief Process the inputs and determine whether to arm or not
* @param[out] cmd The structure to set the armed in
@ -432,11 +451,12 @@ static bool okToArm(void)
*/
static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings)
{
bool lowThrottle = cmd->Throttle <= 0;
if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) {
// In this configuration we always disarm
cmd->Armed = MANUALCONTROLCOMMAND_ARMED_FALSE;
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
} else {
// Not really needed since this function not called when disconnected
if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE)
@ -462,7 +482,7 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData
// The rest of these cases throttle is low
if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) {
// In this configuration, we go into armed state as soon as the throttle is low, never disarm
cmd->Armed = MANUALCONTROLCOMMAND_ARMED_TRUE;
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
return;
}
@ -490,7 +510,7 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData
switch(armState) {
case ARM_STATE_DISARMED:
cmd->Armed = MANUALCONTROLCOMMAND_ARMED_FALSE;
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
// only allow arming if it's OK too
if (manualArm && okToArm()) {
@ -500,6 +520,8 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData
break;
case ARM_STATE_ARMING_MANUAL:
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING);
if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
armState = ARM_STATE_ARMED;
else if (!manualArm)
@ -511,7 +533,7 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData
// we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled
armedDisarmStart = lastSysTime;
armState = ARM_STATE_DISARMING_TIMEOUT;
cmd->Armed = MANUALCONTROLCOMMAND_ARMED_TRUE;
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
break;
case ARM_STATE_DISARMING_TIMEOUT:
@ -542,15 +564,25 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData
* @param[in] settings The settings which indicate which position is which mode
* @param[in] flightMode the value of the switch position
*/
static void processFlightMode(ManualControlCommandData * cmd, ManualControlSettingsData * settings, float flightMode)
static void processFlightMode(ManualControlSettingsData * settings, float flightMode)
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
uint8_t newMode;
// Note here the code is ass
if (flightMode < -FLIGHT_MODE_LIMIT)
cmd->FlightMode = settings->FlightModePosition[0];
newMode = settings->FlightModePosition[0];
else if (flightMode > FLIGHT_MODE_LIMIT)
cmd->FlightMode = settings->FlightModePosition[2];
newMode = settings->FlightModePosition[2];
else
cmd->FlightMode = settings->FlightModePosition[1];
newMode = settings->FlightModePosition[1];
if(flightStatus.FlightMode != newMode) {
flightStatus.FlightMode = newMode;
FlightStatusSet(&flightStatus);
}
}
/**

22
flight/Modules/Stabilization/stabilization.c
View File

@ -39,9 +39,10 @@
#include "stabilizationdesired.h"
#include "attitudeactual.h"
#include "attituderaw.h"
#include "manualcontrolcommand.h"
#include "flightstatus.h"
#include "systemsettings.h"
#include "ahrssettings.h"
#include "manualcontrol.h" // Just to get a macro
#include "CoordinateConversions.h"
// Private constants
@ -125,7 +126,7 @@ static void stabilizationTask(void* parameters)
AttitudeActualData attitudeActual;
AttitudeRawData attitudeRaw;
SystemSettingsData systemSettings;
ManualControlCommandData manualControl;
FlightStatusData flightStatus;
SettingsUpdatedCb((UAVObjEvent *) NULL);
@ -142,19 +143,26 @@ static void stabilizationTask(void* parameters)
continue;
}
// Clear alarms
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
// Check how long since last update
thisSysTime = xTaskGetTickCount();
if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisSysTime;
ManualControlCommandGet(&manualControl);
FlightStatusGet(&flightStatus);
StabilizationDesiredGet(&stabDesired);
AttitudeActualGet(&attitudeActual);
AttitudeRawGet(&attitudeRaw);
RateDesiredGet(&rateDesired);
SystemSettingsGet(&systemSettings);
if(PARSE_FLIGHT_MODE(flightStatus.FlightMode) == FLIGHTMODE_MANUAL)
return;
#if defined(PIOS_QUATERNION_STABILIZATION)
// Quaternion calculation of error in each axis. Uses more memory.
float rpy_desired[3];
@ -261,10 +269,6 @@ static void stabilizationTask(void* parameters)
// Save dT
actuatorDesired.UpdateTime = dT * 1000;
if(manualControl.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL)
{
shouldUpdate = 0;
}
if(shouldUpdate)
@ -275,14 +279,12 @@ static void stabilizationTask(void* parameters)
ActuatorDesiredSet(&actuatorDesired);
}
if(manualControl.Armed == MANUALCONTROLCOMMAND_ARMED_FALSE ||
if(flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED ||
!shouldUpdate || (stabDesired.Throttle < 0))
{
ZeroPids();
}
// Clear alarms
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
}
}

8
flight/Modules/System/systemmod.c
View File

@ -41,7 +41,7 @@
#include "openpilot.h"
#include "systemmod.h"
#include "objectpersistence.h"
#include "manualcontrolcommand.h"
#include "flightstatus.h"
#include "systemstats.h"
#include "i2cstats.h"
#include "watchdogstatus.h"
@ -139,11 +139,11 @@ static void systemTask(void *parameters)
}
#endif
ManualControlCommandData manualControlCommandData;
ManualControlCommandGet(&manualControlCommandData);
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
// Wait until next period
if(manualControlCommandData.Armed == MANUALCONTROLCOMMAND_ARMED_TRUE) {
if(flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) {
vTaskDelayUntil(&lastSysTime, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2) );
} else {
vTaskDelayUntil(&lastSysTime, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS);

2
flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj
View File

@ -7384,6 +7384,7 @@
65C35E5E12EFB2F3004811C2 /* flightplansettings.xml */,
65C35E5F12EFB2F3004811C2 /* flightplanstatus.xml */,
65C35E6012EFB2F3004811C2 /* flighttelemetrystats.xml */,
65078B09136FCEE600536549 /* flightstatus.xml */,
65C35E6112EFB2F3004811C2 /* gcstelemetrystats.xml */,
65C35E6212EFB2F3004811C2 /* gpsposition.xml */,
65C35E6312EFB2F3004811C2 /* gpssatellites.xml */,
@ -7411,7 +7412,6 @@
65C35E7712EFB2F3004811C2 /* velocityactual.xml */,
65C35E7812EFB2F3004811C2 /* velocitydesired.xml */,
65C35E7912EFB2F3004811C2 /* watchdogstatus.xml */,
65078B09136FCEE600536549 /* flightstatus.xml */,
);
path = uavobjectdefinition;
sourceTree = "<group>";

1
shared/uavobjectdefinition/manualcontrolcommand.xml
View File

@ -2,7 +2,6 @@
<object name="ManualControlCommand" singleinstance="true" settings="false">
<description>The output from the @ref ManualControlModule which descodes the receiver inputs. Overriden by GCS for fly-by-wire control.</description>
<field name="Connected" units="" type="enum" elements="1" options="False,True"/>
<field name="Armed" units="" type="enum" elements="1" options="False,True"/>
<field name="Roll" units="%" type="float" elements="1"/>
<field name="Pitch" units="%" type="float" elements="1"/>
<field name="Yaw" units="%" type="float" elements="1"/>