rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-29 14:52:12 +01:00
Code Issues Releases Activity

Merge remote-tracking branch 'origin/next' into shared/OP-1252_update_to_qt521

Browse Source
This commit is contained in:
Alessio Morale 2014-04-08 11:31:23 +02:00
commit c83284b21e
1617 changed files with 216999 additions and 25717 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
Makefile
View File

@ -450,11 +450,25 @@ else
GCS_SILENT := silent
endif
.NOTPARALLEL:
.PHONY: openpilotgcs
openpilotgcs: uavobjects_gcs
$(V1) $(MKDIR) -p $(BUILD_DIR)/$@_$(GCS_BUILD_CONF)
$(V1) ( cd $(BUILD_DIR)/$@_$(GCS_BUILD_CONF) && \
$(QMAKE) $(ROOT_DIR)/ground/openpilotgcs/openpilotgcs.pro -spec $(QT_SPEC) -r CONFIG+="$(GCS_BUILD_CONF) $(GCS_SILENT)" $(GCS_QMAKE_OPTS) && \
openpilotgcs: uavobjects_gcs openpilotgcs_qmake openpilotgcs_make
.PHONY: openpilotgcs_qmake
openpilotgcs_qmake:
ifeq ($(QMAKE_SKIP),)
$(V1) $(MKDIR) -p $(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF)
$(V1) ( cd $(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF) && \
$(QMAKE) $(ROOT_DIR)/ground/openpilotgcs/openpilotgcs.pro -spec $(QT_SPEC) -r CONFIG+="$(GCS_BUILD_CONF) $(GCS_SILENT)" $(GCS_QMAKE_OPTS) \
)
else
@$(ECHO) "skipping qmake"
endif
.PHONY: openpilotgcs_make
openpilotgcs_make:
$(V1) $(MKDIR) -p $(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF)
$(V1) ( cd $(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF)/$(MAKE_DIR) && \
$(MAKE) -w ; \
)
@ -709,7 +723,7 @@ $(OPFW_RESOURCE): $(FW_TARGETS)
# If opfw_resource or all firmware are requested, GCS should depend on the resource
ifneq ($(strip $(filter opfw_resource all all_fw all_flight,$(MAKECMDGOALS))),)
$(eval openpilotgcs: $(OPFW_RESOURCE))
$(eval openpilotgcs_qmake: $(OPFW_RESOURCE))
endif
# Packaging targets: package, clean_package
@ -946,6 +960,9 @@ help:
@$(ECHO)
@$(ECHO) " [GCS]"
@$(ECHO) " gcs - Build the Ground Control System (GCS) application (debug|release)"
@$(ECHO) " Skip qmake: QMAKE_SKIP=1"
@$(ECHO) " Compile specific directory: MAKE_DIR=<dir>"
@$(ECHO) " Example: make gcs QMAKE_SKIP=1 MAKE_DIR=src/plugins/coreplugin"
@$(ECHO) " gcs_clean - Remove the Ground Control System (GCS) application (debug|release)"
@$(ECHO) " Supported build configurations: GCS_BUILD_CONF=debug|release (default is $(GCS_BUILD_CONF))"
@$(ECHO)

38
flight/libraries/alarms.c
View File

@ -32,11 +32,17 @@
#include "inc/alarms.h"
// Private constants
#ifndef PIOS_ALARM_GRACETIME
// alarm cannot be turned off for at least 1000 milliseconds
// to prevent event system overload through flapping alarms
#define PIOS_ALARM_GRACETIME 1000
#endif // PIOS_ALARM_GRACETIME
// Private types
// Private variables
static xSemaphoreHandle lock;
static volatile uint16_t lastAlarmChange[SYSTEMALARMS_ALARM_NUMELEM] = { 0 }; // this deliberately overflows every 2^16 milliseconds to save memory
// Private functions
static int32_t hasSeverity(SystemAlarmsAlarmOptions severity);
@ -63,7 +69,7 @@ int32_t AlarmsInitialize(void)
*/
int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity)
{
SystemAlarmsData alarms;
SystemAlarmsAlarmData alarms;
// Check that this is a valid alarm
if (alarm >= SYSTEMALARMS_ALARM_NUMELEM) {
@ -74,10 +80,14 @@ int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity
xSemaphoreTakeRecursive(lock, portMAX_DELAY);
// Read alarm and update its severity only if it was changed
SystemAlarmsGet(&alarms);
if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] != severity) {
cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] = severity;
SystemAlarmsSet(&alarms);
SystemAlarmsAlarmGet(&alarms);
uint16_t flightTime = (uint16_t)xTaskGetTickCount() * (uint16_t)portTICK_RATE_MS; // this deliberately overflows every 2^16 milliseconds to save memory
if ((flightTime - lastAlarmChange[alarm] > PIOS_ALARM_GRACETIME &&
cast_struct_to_array(alarms, alarms.Actuator)[alarm] != severity)
|| cast_struct_to_array(alarms, alarms.Actuator)[alarm] < severity) {
cast_struct_to_array(alarms, alarms.Actuator)[alarm] = severity;
lastAlarmChange[alarm] = flightTime;
SystemAlarmsAlarmSet(&alarms);
}
// Release lock
@ -110,10 +120,14 @@ int32_t ExtendedAlarmsSet(SystemAlarmsAlarmElem alarm,
// Read alarm and update its severity only if it was changed
SystemAlarmsGet(&alarms);
if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] != severity) {
uint16_t flightTime = (uint16_t)xTaskGetTickCount() * (uint16_t)portTICK_RATE_MS; // this deliberately overflows every 2^16 milliseconds to save memory
if ((flightTime - lastAlarmChange[alarm] > PIOS_ALARM_GRACETIME &&
cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] != severity)
|| cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] < severity) {
cast_struct_to_array(alarms.ExtendedAlarmStatus, alarms.ExtendedAlarmStatus.BootFault)[alarm] = status;
cast_struct_to_array(alarms.ExtendedAlarmSubStatus, alarms.ExtendedAlarmStatus.BootFault)[alarm] = subStatus;
cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] = severity;
lastAlarmChange[alarm] = flightTime;
SystemAlarmsSet(&alarms);
}
@ -129,7 +143,7 @@ int32_t ExtendedAlarmsSet(SystemAlarmsAlarmElem alarm,
*/
SystemAlarmsAlarmOptions AlarmsGet(SystemAlarmsAlarmElem alarm)
{
SystemAlarmsData alarms;
SystemAlarmsAlarmData alarms;
// Check that this is a valid alarm
if (alarm >= SYSTEMALARMS_ALARM_NUMELEM) {
@ -137,8 +151,8 @@ SystemAlarmsAlarmOptions AlarmsGet(SystemAlarmsAlarmElem alarm)
}
// Read alarm
SystemAlarmsGet(&alarms);
return cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm];
SystemAlarmsAlarmGet(&alarms);
return cast_struct_to_array(alarms, alarms.Actuator)[alarm];
}
/**
@ -220,17 +234,17 @@ int32_t AlarmsHasCritical()
*/
static int32_t hasSeverity(SystemAlarmsAlarmOptions severity)
{
SystemAlarmsData alarms;
SystemAlarmsAlarmData alarms;
// Lock
xSemaphoreTakeRecursive(lock, portMAX_DELAY);
// Read alarms
SystemAlarmsGet(&alarms);
SystemAlarmsAlarmGet(&alarms);
// Go through alarms and check if any are of the given severity or higher
for (uint32_t n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n) {
if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[n] >= severity) {
if (cast_struct_to_array(alarms, alarms.Actuator)[n] >= severity) {
xSemaphoreGiveRecursive(lock);
return 1;
}

47
flight/libraries/sanitycheck.c
View File

@ -34,6 +34,7 @@
// UAVOs
#include <manualcontrolsettings.h>
#include <flightmodesettings.h>
#include <systemsettings.h>
#include <systemalarms.h>
#include <taskinfo.h>
@ -85,47 +86,47 @@ int32_t configuration_check()
// For each available flight mode position sanity check the available
// modes
uint8_t num_modes;
uint8_t modes[MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM];
uint8_t modes[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
ManualControlSettingsFlightModeNumberGet(&num_modes);
ManualControlSettingsFlightModePositionGet(modes);
FlightModeSettingsFlightModePositionGet(modes);
for (uint32_t i = 0; i < num_modes; i++) {
switch (modes[i]) {
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_MANUAL:
if (multirotor) {
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED1:
severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(1, multirotor) : severity;
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED2:
severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(2, multirotor) : severity;
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED3:
severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(3, multirotor) : severity;
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_AUTOTUNE:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE:
if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_AUTOTUNE)) {
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD:
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
}
// TODO: put check equivalent to TASK_MONITOR_IsRunning
// here as soon as available for delayed callbacks
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { // Revo supports altitude hold
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
@ -133,7 +134,7 @@ int32_t configuration_check()
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_LAND:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
@ -141,7 +142,7 @@ int32_t configuration_check()
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POI:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
@ -149,7 +150,7 @@ int32_t configuration_check()
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else {
@ -163,7 +164,7 @@ int32_t configuration_check()
}
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
@ -201,23 +202,23 @@ int32_t configuration_check()
static int32_t check_stabilization_settings(int index, bool multirotor)
{
// Make sure the modes have identical sizes
if (MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM != MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NUMELEM ||
MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM != MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NUMELEM) {
if (FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM != FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_NUMELEM ||
FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM != FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_NUMELEM) {
return SYSTEMALARMS_ALARM_ERROR;
}
uint8_t modes[MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM];
uint8_t modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM];
// Get the different axis modes for this switch position
switch (index) {
case 1:
ManualControlSettingsStabilization1SettingsArrayGet(modes);
FlightModeSettingsStabilization1SettingsArrayGet(modes);
break;
case 2:
ManualControlSettingsStabilization2SettingsArrayGet(modes);
FlightModeSettingsStabilization2SettingsArrayGet(modes);
break;
case 3:
ManualControlSettingsStabilization3SettingsArrayGet(modes);
FlightModeSettingsStabilization3SettingsArrayGet(modes);
break;
default:
return SYSTEMALARMS_ALARM_ERROR;
@ -226,14 +227,14 @@ static int32_t check_stabilization_settings(int index, bool multirotor)
// For multirotors verify that nothing is set to "none"
if (multirotor) {
for (uint32_t i = 0; i < NELEMENTS(modes); i++) {
if (modes[i] == MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE) {
if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NONE) {
return SYSTEMALARMS_ALARM_ERROR;
}
}
}
// Warning: This assumes that certain conditions in the XML file are met. That
// MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE has the same numeric value for each channel
// FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NONE has the same numeric value for each channel
// and is the same for STABILIZATIONDESIRED_STABILIZATIONMODE_NONE
return SYSTEMALARMS_ALARM_OK;

67
flight/modules/Actuator/actuator.c
View File

@ -129,6 +129,9 @@ int32_t ActuatorInitialize()
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
ActuatorDesiredConnectQueue(queue);
// Register AccessoryDesired (Secondary input to this module)
AccessoryDesiredInitialize();
// Primary output of this module
ActuatorCommandInitialize();
@ -166,6 +169,9 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
ActuatorDesiredData desired;
MixerStatusData mixerStatus;
FlightStatusData flightStatus;
SystemSettingsThrustControlOptions thrustType;
float throttleDesired;
float collectiveDesired;
/* Read initial values of ActuatorSettings */
ActuatorSettingsData actuatorSettings;
@ -220,6 +226,41 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
FlightStatusGet(&flightStatus);
ActuatorDesiredGet(&desired);
ActuatorCommandGet(&command);
SystemSettingsThrustControlGet(&thrustType);
// read in throttle and collective -demultiplex thrust
switch (thrustType) {
case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE:
throttleDesired = desired.Thrust;
ManualControlCommandCollectiveGet(&collectiveDesired);
break;
case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE:
ManualControlCommandThrottleGet(&throttleDesired);
collectiveDesired = desired.Thrust;
break;
default:
ManualControlCommandThrottleGet(&throttleDesired);
ManualControlCommandCollectiveGet(&collectiveDesired);
}
bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED;
// safety settings
if (!armed) {
throttleDesired = 0;
}
if (throttleDesired <= 0.00f || !armed) {
// force set all other controls to zero if throttle is cut (previously set in Stabilization)
if (actuatorSettings.LowThrottleZeroAxis.Roll == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
desired.Roll = 0;
}
if (actuatorSettings.LowThrottleZeroAxis.Pitch == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
desired.Pitch = 0;
}
if (actuatorSettings.LowThrottleZeroAxis.Yaw == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
desired.Yaw = 0;
}
}
#ifdef DIAG_MIXERSTATUS
MixerStatusGet(&mixerStatus);
@ -238,18 +279,18 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR);
bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED;
bool positiveThrottle = desired.Throttle >= 0.00f;
bool activeThrottle = (throttleDesired < 0.00f || throttleDesired > 0.00f);
bool positiveThrottle = (throttleDesired > 0.00f);
bool spinWhileArmed = actuatorSettings.MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE;
float curve1 = MixerCurve(desired.Throttle, mixerSettings.ThrottleCurve1, MIXERSETTINGS_THROTTLECURVE1_NUMELEM);
float curve1 = MixerCurve(throttleDesired, mixerSettings.ThrottleCurve1, MIXERSETTINGS_THROTTLECURVE1_NUMELEM);
// The source for the secondary curve is selectable
float curve2 = 0;
AccessoryDesiredData accessory;
switch (mixerSettings.Curve2Source) {
case MIXERSETTINGS_CURVE2SOURCE_THROTTLE:
curve2 = MixerCurve(desired.Throttle, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
curve2 = MixerCurve(throttleDesired, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
break;
case MIXERSETTINGS_CURVE2SOURCE_ROLL:
curve2 = MixerCurve(desired.Roll, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
@ -262,8 +303,7 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
curve2 = MixerCurve(desired.Yaw, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
break;
case MIXERSETTINGS_CURVE2SOURCE_COLLECTIVE:
ManualControlCommandCollectiveGet(&curve2);
curve2 = MixerCurve(curve2, mixerSettings.ThrottleCurve2,
curve2 = MixerCurve(collectiveDesired, mixerSettings.ThrottleCurve2,
MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
break;
case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0:
@ -295,7 +335,7 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
continue;
}
if ((mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_SERVO)) {
if ((mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_SERVO)) {
status[ct] = ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, dTSeconds);
} else {
status[ct] = -1;
@ -317,6 +357,16 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
}
}
// Reversable Motors are like Motors but go to neutral instead of minimum
if (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) {
// If not armed or motor is inactive - no "spinwhilearmed" for this engine type
if (!armed || !activeThrottle) {
filterAccumulator[ct] = 0;
lastResult[ct] = 0;
status[ct] = 0; // force neutral throttle
}
}
// If an accessory channel is selected for direct bypass mode
// In this configuration the accessory channel is scaled and mapped
// directly to output. Note: THERE IS NO SAFETY CHECK HERE FOR ARMING
@ -419,6 +469,7 @@ float ProcessMixer(const int index, const float curve1, const float curve2,
(((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_PITCH] / 128.0f) * desired->Pitch) +
(((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_YAW] / 128.0f) * desired->Yaw);
// note: no feedforward for reversable motors yet for safety reasons
if (mixer->type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
if (result < 0.0f) { // idle throttle
result = 0.0f;
@ -537,7 +588,7 @@ static void setFailsafe(const ActuatorSettingsData *actuatorSettings, const Mixe
for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) {
if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
Channel[n] = actuatorSettings->ChannelMin[n];
} else if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_SERVO) {
} else if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_SERVO || mixers[n].type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) {
Channel[n] = actuatorSettings->ChannelNeutral[n];
} else {
Channel[n] = 0;

27
flight/modules/Airspeed/airspeed.c
View File

@ -41,6 +41,7 @@
#include "hwsettings.h"
#include "airspeedsettings.h"
#include "airspeedsensor.h" // object that will be updated by the module
#include "baro_airspeed_ms4525do.h"
#include "baro_airspeed_etasv3.h"
#include "baro_airspeed_mpxv.h"
#include "gps_airspeed.h"
@ -59,7 +60,7 @@
static xTaskHandle taskHandle;
static bool airspeedEnabled = false;
static AirspeedSettingsData airspeedSettings;
static AirspeedSettingsAirspeedSensorTypeOptions lastAirspeedSensorType = 0;
static int8_t airspeedADCPin = -1;
@ -118,6 +119,8 @@ int32_t AirspeedInitialize()
}
}
lastAirspeedSensorType = airspeedSettings.AirspeedSensorType;
AirspeedSensorInitialize();
AirspeedSettingsInitialize();
@ -152,6 +155,21 @@ static void airspeedTask(__attribute__((unused)) void *parameters)
// Update the airspeed object
AirspeedSensorGet(&airspeedData);
// if sensor type changed and the last sensor was
// either Eagletree or PixHawk, reset Airspeed alarm
if (airspeedSettings.AirspeedSensorType != lastAirspeedSensorType) {
switch (lastAirspeedSensorType) {
// Eagletree or PixHawk => Reset Airspeed alams
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_EAGLETREEAIRSPEEDV3:
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_PIXHAWKAIRSPEEDMS4525DO:
AlarmsDefault(SYSTEMALARMS_ALARM_AIRSPEED);
break;
// else do not reset Airspeed alarm
default: break;
}
lastAirspeedSensorType = airspeedSettings.AirspeedSensorType;
}
switch (airspeedSettings.AirspeedSensorType) {
#if defined(PIOS_INCLUDE_MPXV)
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002:
@ -165,10 +183,17 @@ static void airspeedTask(__attribute__((unused)) void *parameters)
// Eagletree Airspeed v3
baro_airspeedGetETASV3(&airspeedData, &airspeedSettings);
break;
#endif
#if defined(PIOS_INCLUDE_MS4525DO)
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_PIXHAWKAIRSPEEDMS4525DO:
// PixHawk Airpeed based on MS4525DO
baro_airspeedGetMS4525DO(&airspeedData, &airspeedSettings);
break;
#endif
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GROUNDSPEEDBASEDWINDESTIMATION:
gps_airspeedGet(&airspeedData, &airspeedSettings);
break;
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_NONE:
default:
airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
}

174
flight/modules/Airspeed/baro_airspeed_ms4525do.c Normal file
View File

@ -0,0 +1,174 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AirspeedModule Airspeed Module
* @brief Communicate with airspeed sensors and return values
* @{
*
* @file baro_airspeed_ms4525do.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Airspeed module, handles temperature and pressure readings from MS4525DO
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/**
* Output object: BaroAirspeed
*
* This module will periodically update the value of the BaroAirspeed object.
*
*/
#include "openpilot.h"
#include "hwsettings.h"
#include "airspeedsettings.h"
#include "airspeedsensor.h" // object that will be updated by the module
#include "taskinfo.h"
#if defined(PIOS_INCLUDE_MS4525DO)
#define CALIBRATION_IDLE_MS 0 // Time to wait before calibrating, in [ms]
#define CALIBRATION_COUNT_MS 4000 // Time to spend calibrating, in [ms]
#define FILTER_SHIFT 5 // Barry Dorr filter parameter k
#define P0 101325.0f // standard pressure
#define CCEXPONENT 0.2857142857f // exponent of compressibility correction 2/7
#define CASFACTOR 760.8802669f // sqrt(5) * speed of sound at standard
#define TASFACTOR 0.05891022589f // 1/sqrt(T0)
#define max(x, y) ((x) >= (y) ? (x) : (y))
// Private types
// Private functions definitions
static int8_t baro_airspeedReadMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings);
// Private variables
static uint16_t calibrationCount = 0;
static uint32_t filter_reg = 0; // Barry Dorr filter register
void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings)
{
// request measurement first
int8_t retVal = PIOS_MS4525DO_Request();
if (retVal != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR);
return;
}
// Datasheet of MS4525DO: conversion needs 0.5 ms + 20% more when status bit used
// delay by one Tick or at least 2 ms
const portTickType xDelay = max(2 / portTICK_RATE_MS, 1);
vTaskDelay(xDelay);
// read the sensor
retVal = baro_airspeedReadMS4525DO(airspeedSensor, airspeedSettings);
switch (retVal) {
case 0: AlarmsClear(SYSTEMALARMS_ALARM_AIRSPEED);
break;
case -4:
case -5:
case -7: AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING);
break;
case -1:
case -2:
case -3:
case -6:
default: AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR);
}
}
// Private functions
static int8_t baro_airspeedReadMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings)
{
// Check to see if airspeed sensor is returning airspeedSensor
uint16_t values[2];
int8_t retVal = PIOS_MS4525DO_Read(values);
if (retVal == 0) {
airspeedSensor->SensorValue = values[0];
airspeedSensor->SensorValueTemperature = values[1];
} else {
airspeedSensor->SensorValue = -1;
airspeedSensor->SensorValueTemperature = -1;
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
airspeedSensor->CalibratedAirspeed = 0;
return retVal;
}
// only calibrate if no stored calibration is available
if (!airspeedSettings->ZeroPoint) {
// Calibrate sensor by averaging zero point value
if (calibrationCount <= CALIBRATION_IDLE_MS / airspeedSettings->SamplePeriod) {
calibrationCount++;
filter_reg = (airspeedSensor->SensorValue << FILTER_SHIFT);
return -7;
} else if (calibrationCount <= (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS) / airspeedSettings->SamplePeriod) {
calibrationCount++;
// update filter register
filter_reg = filter_reg - (filter_reg >> FILTER_SHIFT) + airspeedSensor->SensorValue;
if (calibrationCount > (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS) / airspeedSettings->SamplePeriod) {
// Scale output for unity gain.
airspeedSettings->ZeroPoint = (uint16_t)(filter_reg >> FILTER_SHIFT);
AirspeedSettingsZeroPointSet(&airspeedSettings->ZeroPoint);
calibrationCount = 0;
}
return -7;
}
}
/* Compute airspeed
assume sensor is A Type and has a range of 1 psi, i.e. Pmin=-1.0 psi and Pmax=1.0 psi
Datasheet pressure: output = 0.8 * 16383 / (Pmax-Pmin) * (P - Pmin) + 0.1 * 16383
Inversion: P = (10*output - 81915)/65532 in psi
1 psi = 6894,757293168 Pa
P = (10*output - 81915)*0.1052120688 in Pa
Datasheet temperature: output = (T+50)*2047 / 200
Inversion: T = (200*out - 102350)/2047 in C
T = (200*out - 102350)/2047 + 273.15 in K
*/
const float dP = (10 * (int32_t)(airspeedSensor->SensorValue - airspeedSettings->ZeroPoint)) * 0.1052120688f;
const float T = (float)(200 * (int32_t)airspeedSensor->SensorValueTemperature - 102350) / 2047 + 273.15f;
airspeedSensor->DifferentialPressure = dP;
airspeedSensor->Temperature = T;
// CAS = Csound * sqrt( 5 *( (dP/P0 +1)^(2/7) - 1) )
// TAS = Csound * sqrt( 5 T/T0 *( (dP/P0 +1)^(2/7) - 1) )
// where Csound = 340.276 m/s at standard condition T0=288.15 K and P0 = 101315 Pa
airspeedSensor->CalibratedAirspeed = airspeedSettings->Scale * CASFACTOR * sqrtf(powf(fabsf(dP) / P0 + 1.0f, CCEXPONENT) - 1.0f);
airspeedSensor->TrueAirspeed = airspeedSensor->CalibratedAirspeed * TASFACTOR * sqrtf(T);
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
return retVal;
}
#endif /* if defined(PIOS_INCLUDE_MS4525DO) */
/**
* @}
* @}
*/

43
flight/modules/Airspeed/inc/baro_airspeed_ms4525do.h Normal file
View File

@ -0,0 +1,43 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AirspeedModule Airspeed Module
* @brief Calculate airspeed from measurements of differential pressure from a MS4525DO (PixHawk airspeed sensor)
* @{
*
* @file baro_airspeed_mas4525do.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Airspeed module, reads temperature and pressure from MS4525DO
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef BARO_AIRSPEED_MS4525DO_H
#define BARO_AIRSPEED_MS4525DO_H
#if defined(PIOS_INCLUDE_MS4525DO)
void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings);
#endif /* PIOS_INCLUDE_MS4525DO */
#endif // BARO_AIRSPEED_MS4525DO_H
/**
* @}
* @}
*/

16
flight/modules/Altitude/revolution/altitude.c
View File

@ -55,6 +55,8 @@
// Private variables
static xTaskHandle taskHandle;
static RevoSettingsBaroTempCorrectionPolynomialData baroCorrection;
static RevoSettingsBaroTempCorrectionExtentData baroCorrectionExtent;
static volatile bool tempCorrectionEnabled;
// Private functions
static void altitudeTask(void *parameters);
static void SettingsUpdatedCb(UAVObjEvent *ev);
@ -81,7 +83,7 @@ int32_t AltitudeInitialize()
BaroSensorInitialize();
RevoSettingsInitialize();
RevoSettingsConnectCallback(&SettingsUpdatedCb);
SettingsUpdatedCb(NULL);
#if defined(PIOS_INCLUDE_HCSR04)
SonarAltitudeInitialize();
#endif
@ -163,9 +165,14 @@ static void altitudeTask(__attribute__((unused)) void *parameters)
temp = PIOS_MS5611_GetTemperature();
press = PIOS_MS5611_GetPressure();
float temp2 = temp * temp;
press = press - baroCorrection.a + temp * baroCorrection.b + temp2 * baroCorrection.c + temp * temp2 * baroCorrection.d;
if (tempCorrectionEnabled) {
// pressure bias = A + B*t + C*t^2 + D * t^3
// in case the temperature is outside of the calibrated range, uses the nearest extremes
float ctemp = temp > baroCorrectionExtent.max ? baroCorrectionExtent.max :
(temp < baroCorrectionExtent.min ? baroCorrectionExtent.min : temp);
press -= baroCorrection.a + ((baroCorrection.d * ctemp + baroCorrection.c) * ctemp + baroCorrection.b) * ctemp;
}
float altitude = 44330.0f * (1.0f - powf((press) / MS5611_P0, (1.0f / 5.255f)));
if (!isnan(altitude)) {
@ -181,6 +188,9 @@ static void altitudeTask(__attribute__((unused)) void *parameters)
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection);
RevoSettingsBaroTempCorrectionExtentGet(&baroCorrectionExtent);
tempCorrectionEnabled = !(baroCorrectionExtent.max - baroCorrectionExtent.min < 0.1f ||
(baroCorrection.a < 1e-9f && baroCorrection.b < 1e-9f && baroCorrection.c < 1e-9f && baroCorrection.d < 1e-9f));
}
/**
* @}

28
flight/modules/AltitudeHold/altitudehold.c
View File

@ -116,7 +116,7 @@ MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart);
*/
static void altitudeHoldTask(void)
{
static float startThrottle = 0.5f;
static float startThrust = 0.5f;
// make sure we run only when we are supposed to run
FlightStatusData flightStatus;
@ -129,7 +129,7 @@ static void altitudeHoldTask(void)
default:
pid_zero(&pid0);
pid_zero(&pid1);
StabilizationDesiredThrottleGet(&startThrottle);
StabilizationDesiredThrustGet(&startThrust);
PIOS_CALLBACKSCHEDULER_Schedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
return;
@ -162,30 +162,30 @@ static void altitudeHoldTask(void)
AltitudeHoldStatusSet(&altitudeHoldStatus);
float throttle;
float thrust;
switch (altitudeHoldDesired.ControlMode) {
case ALTITUDEHOLDDESIRED_CONTROLMODE_THROTTLE:
throttle = altitudeHoldDesired.SetPoint;
case ALTITUDEHOLDDESIRED_CONTROLMODE_THRUST:
thrust = altitudeHoldDesired.SetPoint;
break;
default:
// velocity control loop
throttle = startThrottle - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS);
thrust = startThrust - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS);
if (throttle >= 1.0f) {
throttle = 1.0f;
if (thrust >= 1.0f) {
thrust = 1.0f;
}
if (throttle <= 0.0f) {
throttle = 0.0f;
if (thrust <= 0.0f) {
thrust = 0.0f;
}
break;
}
StabilizationDesiredData stab;
StabilizationDesiredGet(&stab);
stab.Roll = altitudeHoldDesired.Roll;
stab.Pitch = altitudeHoldDesired.Pitch;
stab.Yaw = altitudeHoldDesired.Yaw;
stab.Throttle = throttle;
stab.Roll = altitudeHoldDesired.Roll;
stab.Pitch = altitudeHoldDesired.Pitch;
stab.Yaw = altitudeHoldDesired.Yaw;
stab.Thrust = thrust;
stab.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stab.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stab.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;

154
flight/modules/Attitude/attitude.c
View File

@ -57,6 +57,7 @@
#include "accelstate.h"
#include "attitudestate.h"
#include "attitudesettings.h"
#include "accelgyrosettings.h"
#include "flightstatus.h"
#include "manualcontrolcommand.h"
#include "taskinfo.h"
@ -70,7 +71,6 @@
#define SENSOR_PERIOD 4
#define UPDATE_RATE 25.0f
#define GYRO_NEUTRAL 1665
#define UPDATE_EXPECTED (1.0f / 666.0f)
#define UPDATE_MIN 1.0e-6f
@ -102,24 +102,42 @@ static float accels_filtered[3];
static float grot_filtered[3];
static float yawBiasRate = 0;
static float rollPitchBiasRate = 0.0f;
static float gyroGain = 0.42f;
static int16_t accelbias[3];
static AccelGyroSettingsaccel_biasData accel_bias;
static float q[4] = { 1, 0, 0, 0 };
static float R[3][3];
static int8_t rotate = 0;
static bool zero_during_arming = false;
static bool bias_correct_gyro = true;
// static float gyros_passed[3];
// temp coefficient to calculate gyro bias
static bool apply_gyro_temp = false;
static bool apply_accel_temp = false;
static AccelGyroSettingsgyro_temp_coeffData gyro_temp_coeff;;
static AccelGyroSettingsaccel_temp_coeffData accel_temp_coeff;
static AccelGyroSettingstemp_calibrated_extentData temp_calibrated_extent;
// Accel and Gyro scaling (this is the product of sensor scale and adjustement in AccelGyroSettings
static AccelGyroSettingsgyro_scaleData gyro_scale;
static AccelGyroSettingsaccel_scaleData accel_scale;
// For running trim flights
static volatile bool trim_requested = false;
static volatile int32_t trim_accels[3];
static volatile int32_t trim_samples;
int32_t const MAX_TRIM_FLIGHT_SAMPLES = 65535;
#define GRAV 9.81f
#define ACCEL_SCALE (GRAV * 0.004f)
#define GRAV 9.81f
#define STD_CC_ACCEL_SCALE (GRAV * 0.004f)
/* 0.004f is gravity / LSB */
#define STD_CC_ANALOG_GYRO_NEUTRAL 1665
#define STD_CC_ANALOG_GYRO_GAIN 0.42f
// Used to detect CC vs CC3D
static const struct pios_board_info *bdinfo = &pios_board_info_blob;
#define BOARDISCC3D (bdinfo->board_rev == 0x02)
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
@ -142,6 +160,7 @@ int32_t AttitudeInitialize(void)
{
AttitudeStateInitialize();
AttitudeSettingsInitialize();
AccelGyroSettingsInitialize();
AccelStateInitialize();
GyroStateInitialize();
@ -172,7 +191,7 @@ int32_t AttitudeInitialize(void)
trim_requested = false;
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
AccelGyroSettingsConnectCallback(&settingsUpdatedCb);
return 0;
}
@ -196,9 +215,7 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
}
const struct pios_board_info *bdinfo = &pios_board_info_blob;
bool cc3d = bdinfo->board_rev == 0x02;
bool cc3d = BOARDISCC3D;
if (cc3d) {
#if defined(PIOS_INCLUDE_MPU6000)
@ -280,8 +297,6 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
}
}
float gyros_passed[3];
/**
* Get an update from the sensors
* @param[in] attitudeRaw Populate the UAVO instead of saving right here
@ -309,9 +324,9 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
}
// First sample is temperature
gyros->x = -(gyro[1] - GYRO_NEUTRAL) * gyroGain;
gyros->y = (gyro[2] - GYRO_NEUTRAL) * gyroGain;
gyros->z = -(gyro[3] - GYRO_NEUTRAL) * gyroGain;
gyros->x = -(gyro[1] - STD_CC_ANALOG_GYRO_NEUTRAL) * gyro_scale.X;
gyros->y = (gyro[2] - STD_CC_ANALOG_GYRO_NEUTRAL) * gyro_scale.Y;
gyros->z = -(gyro[3] - STD_CC_ANALOG_GYRO_NEUTRAL) * gyro_scale.Z;
int32_t x = 0;
int32_t y = 0;
@ -325,9 +340,10 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
y += -accel_data.y;
z += -accel_data.z;
} while ((i < 32) && (samples_remaining > 0));
// gyros->temperature = samples_remaining;
float accel[3] = { (float)x / i, (float)y / i, (float)z / i };
float accel[3] = { accel_scale.X * (float)x / i,
accel_scale.Y * (float)y / i,
accel_scale.Z * (float)z / i };
if (rotate) {
// TODO: rotate sensors too so stabilization is well behaved
@ -365,9 +381,9 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
}
// Scale accels and correct bias
accelState->x = (accelState->x - accelbias[0]) * ACCEL_SCALE;
accelState->y = (accelState->y - accelbias[1]) * ACCEL_SCALE;
accelState->z = (accelState->z - accelbias[2]) * ACCEL_SCALE;
accelState->x -= accel_bias.X;
accelState->y -= accel_bias.Y;
accelState->z -= accel_bias.Z;
if (bias_correct_gyro) {
// Applying integral component here so it can be seen on the gyros and correct bias
@ -395,7 +411,7 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
* @param[in] attitudeRaw Populate the UAVO instead of saving right here
* @return 0 if successfull, -1 if not
*/
struct pios_mpu6000_data mpu6000_data;
static struct pios_mpu6000_data mpu6000_data;
static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *gyrosData)
{
float accels[3], gyros[3];
@ -411,15 +427,30 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
if (GyroStateReadOnly() || AccelStateReadOnly()) {
return 0;
}
gyros[0] = mpu6000_data.gyro_x * gyro_scale.X;
gyros[1] = mpu6000_data.gyro_y * gyro_scale.Y;
gyros[2] = mpu6000_data.gyro_z * gyro_scale.Z;
gyros[0] = mpu6000_data.gyro_x * PIOS_MPU6000_GetScale();
gyros[1] = mpu6000_data.gyro_y * PIOS_MPU6000_GetScale();
gyros[2] = mpu6000_data.gyro_z * PIOS_MPU6000_GetScale();
accels[0] = mpu6000_data.accel_x * accel_scale.X;
accels[1] = mpu6000_data.accel_y * accel_scale.Y;
accels[2] = mpu6000_data.accel_z * accel_scale.Z;
accels[0] = mpu6000_data.accel_x * PIOS_MPU6000_GetAccelScale();
accels[1] = mpu6000_data.accel_y * PIOS_MPU6000_GetAccelScale();
accels[2] = mpu6000_data.accel_z * PIOS_MPU6000_GetAccelScale();
float ctemp = mpu6000_data.temperature > temp_calibrated_extent.max ? temp_calibrated_extent.max :
(mpu6000_data.temperature < temp_calibrated_extent.min ? temp_calibrated_extent.min
: mpu6000_data.temperature);
if (apply_gyro_temp) {
gyros[0] -= (gyro_temp_coeff.X + gyro_temp_coeff.X2 * ctemp) * ctemp;
gyros[1] -= (gyro_temp_coeff.Y + gyro_temp_coeff.Y2 * ctemp) * ctemp;
gyros[2] -= (gyro_temp_coeff.Z + gyro_temp_coeff.Z2 * ctemp) * ctemp;
}
if (apply_accel_temp) {
accels[0] -= accel_temp_coeff.X * ctemp;
accels[1] -= accel_temp_coeff.Y * ctemp;
accels[2] -= accel_temp_coeff.Z * ctemp;
}
// gyrosData->temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f;
// accelsData->temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f;
#endif /* if defined(PIOS_INCLUDE_MPU6000) */
@ -437,9 +468,9 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
gyros[2] = vec_out[2];
}
accelStateData->x = accels[0] - accelbias[0] * ACCEL_SCALE; // Applying arbitrary scale here to match CC v1
accelStateData->y = accels[1] - accelbias[1] * ACCEL_SCALE;
accelStateData->z = accels[2] - accelbias[2] * ACCEL_SCALE;
accelStateData->x = accels[0] - accel_bias.X;
accelStateData->y = accels[1] - accel_bias.Y;
accelStateData->z = accels[2] - accel_bias.Z;
gyrosData->x = gyros[0];
gyrosData->y = gyros[1];
@ -590,14 +621,14 @@ static void updateAttitude(AccelStateData *accelStateData, GyroStateData *gyrosD
static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
{
AttitudeSettingsData attitudeSettings;
AccelGyroSettingsData accelGyroSettings;
AttitudeSettingsGet(&attitudeSettings);
AccelGyroSettingsGet(&accelGyroSettings);
accelKp = attitudeSettings.AccelKp;
accelKi = attitudeSettings.AccelKi;
yawBiasRate = attitudeSettings.YawBiasRate;
gyroGain = attitudeSettings.GyroGain;
// Calculate accel filter alpha, in the same way as for gyro data in stabilization module.
const float fakeDt = 0.0025f;
@ -612,13 +643,54 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE;
bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE;
accelbias[0] = attitudeSettings.AccelBias.X;
accelbias[1] = attitudeSettings.AccelBias.Y;
accelbias[2] = attitudeSettings.AccelBias.Z;
memcpy(&gyro_temp_coeff, &accelGyroSettings.gyro_temp_coeff, sizeof(AccelGyroSettingsgyro_temp_coeffData));
memcpy(&accel_temp_coeff, &accelGyroSettings.accel_temp_coeff, sizeof(AccelGyroSettingsaccel_temp_coeffData));
gyro_correct_int[0] = attitudeSettings.GyroBias.X / 100.0f;
gyro_correct_int[1] = attitudeSettings.GyroBias.Y / 100.0f;
gyro_correct_int[2] = attitudeSettings.GyroBias.Z / 100.0f;
apply_gyro_temp = (fabsf(gyro_temp_coeff.X) > 1e-6f ||
fabsf(gyro_temp_coeff.Y) > 1e-6f ||
fabsf(gyro_temp_coeff.Z) > 1e-6f ||
fabsf(gyro_temp_coeff.X2) > 1e-6f ||
fabsf(gyro_temp_coeff.Y2) > 1e-6f ||
fabsf(gyro_temp_coeff.Z2) > 1e-6f);
apply_accel_temp = (fabsf(accel_temp_coeff.X) > 1e-6f ||
fabsf(accel_temp_coeff.Y) > 1e-6f ||
fabsf(accel_temp_coeff.Z) > 1e-6f);
gyro_correct_int[0] = accelGyroSettings.gyro_bias.X;
gyro_correct_int[1] = accelGyroSettings.gyro_bias.Y;
gyro_correct_int[2] = accelGyroSettings.gyro_bias.Z;
temp_calibrated_extent.min = accelGyroSettings.temp_calibrated_extent.min;
temp_calibrated_extent.max = accelGyroSettings.temp_calibrated_extent.max;
if (BOARDISCC3D) {
accel_bias.X = accelGyroSettings.accel_bias.X;
accel_bias.Y = accelGyroSettings.accel_bias.Y;
accel_bias.Z = accelGyroSettings.accel_bias.Z;
gyro_scale.X = accelGyroSettings.gyro_scale.X * PIOS_MPU6000_GetScale();
gyro_scale.Y = accelGyroSettings.gyro_scale.Y * PIOS_MPU6000_GetScale();
gyro_scale.Z = accelGyroSettings.gyro_scale.Z * PIOS_MPU6000_GetScale();
accel_scale.X = accelGyroSettings.accel_scale.X * PIOS_MPU6000_GetAccelScale();
accel_scale.Y = accelGyroSettings.accel_scale.Y * PIOS_MPU6000_GetAccelScale();
accel_scale.Z = accelGyroSettings.accel_scale.Z * PIOS_MPU6000_GetAccelScale();
} else {
// Original CC with analog gyros and ADXL accel
accel_bias.X = accelGyroSettings.accel_bias.X;
accel_bias.Y = accelGyroSettings.accel_bias.Y;
accel_bias.Z = accelGyroSettings.accel_bias.Z;
gyro_scale.X = accelGyroSettings.gyro_scale.X * STD_CC_ANALOG_GYRO_GAIN;
gyro_scale.Y = accelGyroSettings.gyro_scale.Y * STD_CC_ANALOG_GYRO_GAIN;
gyro_scale.Z = accelGyroSettings.gyro_scale.Z * STD_CC_ANALOG_GYRO_GAIN;
accel_scale.X = accelGyroSettings.accel_scale.X * STD_CC_ACCEL_SCALE;
accel_scale.Y = accelGyroSettings.accel_scale.Y * STD_CC_ACCEL_SCALE;
accel_scale.Z = accelGyroSettings.accel_scale.Z * STD_CC_ACCEL_SCALE;
}
// Indicates not to expend cycles on rotation
if (attitudeSettings.BoardRotation.Pitch == 0 && attitudeSettings.BoardRotation.Roll == 0 &&
@ -646,11 +718,11 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
trim_requested = true;
} else if (attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_LOAD) {
trim_requested = false;
attitudeSettings.AccelBias.X = trim_accels[0] / trim_samples;
attitudeSettings.AccelBias.Y = trim_accels[1] / trim_samples;
accelGyroSettings.accel_scale.X = trim_accels[0] / trim_samples;
accelGyroSettings.accel_scale.Y = trim_accels[1] / trim_samples;
// Z should average -grav
attitudeSettings.AccelBias.Z = trim_accels[2] / trim_samples + GRAV / ACCEL_SCALE;
attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL;
accelGyroSettings.accel_scale.Z = trim_accels[2] / trim_samples + GRAV;
attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL;
AttitudeSettingsSet(&attitudeSettings);
} else {
trim_requested = false;

5
flight/modules/Attitude/revolution/attitude.c
View File

@ -650,7 +650,7 @@ static int32_t updateAttitudeComplementary(bool first_run)
if (airspeedSensor.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE) {
// we have airspeed available
airspeed.CalibratedAirspeed = airspeedSensor.CalibratedAirspeed;
airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS(homeLocation.Altitude - positionState.Down);
airspeed.TrueAirspeed = (airspeedSensor.TrueAirspeed < 0.f) ? airspeed.CalibratedAirspeed *IAS2TAS(homeLocation.Altitude - positionState.Down) : airspeedSensor.TrueAirspeed;
AirspeedStateSet(&airspeed);
}
}
@ -1078,7 +1078,8 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
AirspeedStateGet(&airspeed);
airspeed.CalibratedAirspeed = airspeedData.CalibratedAirspeed;
airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS(homeLocation.Altitude - Nav.Pos[2]);
airspeed.TrueAirspeed = (airspeedSensor.TrueAirspeed < 0.f) ? airspeed.CalibratedAirspeed *IAS2TAS(homeLocation.Altitude - positionState.Down) : airspeedSensor.TrueAirspeed;
AirspeedStateSet(&airspeed);
if (!gps_vel_updated && !gps_updated) {

8
flight/modules/Autotune/autotune.c
View File

@ -182,8 +182,8 @@ static void AutotuneTask(__attribute__((unused)) void *parameters)
}
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.Yaw = manualControl.Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW];
stabDesired.Throttle = manualControl.Throttle;
stabDesired.Yaw = manualControl.Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW];
stabDesired.Thrust = manualControl.Thrust;
switch (state) {
case AT_INIT:
@ -191,7 +191,7 @@ static void AutotuneTask(__attribute__((unused)) void *parameters)
lastUpdateTime = xTaskGetTickCount();
// Only start when armed and flying
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && stabDesired.Throttle > 0) {
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && stabDesired.Thrust > 0) {
state = AT_START;
}
break;
@ -236,7 +236,7 @@ static void AutotuneTask(__attribute__((unused)) void *parameters)
case AT_FINISHED:
// Wait until disarmed and landed before updating the settings
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED && stabDesired.Throttle <= 0) {
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED && stabDesired.Thrust <= 0) {
state = AT_SET;
}

1
flight/modules/CameraStab/camerastab.c
View File

@ -128,6 +128,7 @@ int32_t CameraStabInitialize(void)
.obj = AttitudeStateHandle(),
.instId = 0,
.event = 0,
.lowPriority = false,
};
EventPeriodicCallbackCreate(&ev, attitudeUpdated, SAMPLE_PERIOD_MS / portTICK_RATE_MS);

2
flight/modules/FirmwareIAP/firmwareiap.c
View File

@ -100,6 +100,7 @@ int32_t FirmwareIAPInitialize()
PIOS_BL_HELPER_FLASH_Read_Description(data.Description, FIRMWAREIAPOBJ_DESCRIPTION_NUMELEM);
PIOS_SYS_SerialNumberGetBinary(data.CPUSerial);
data.BoardRevision = bdinfo->board_rev;
data.BootloaderRevision = bdinfo->bl_rev;
data.ArmReset = 0;
data.crc = 0;
FirmwareIAPObjSet(&data);
@ -147,6 +148,7 @@ static void FirmwareIAPCallback(UAVObjEvent *ev)
PIOS_BL_HELPER_FLASH_Read_Description(data.Description, FIRMWAREIAPOBJ_DESCRIPTION_NUMELEM);
PIOS_SYS_SerialNumberGetBinary(data.CPUSerial);
data.BoardRevision = bdinfo->board_rev;
data.BootloaderRevision = bdinfo->bl_rev;
data.crc = PIOS_BL_HELPER_CRC_Memory_Calc();
FirmwareIAPObjSet(&data);
}

111
flight/modules/FixedWingPathFollower/fixedwingpathfollower.c
View File

@ -49,7 +49,6 @@
#include "attitudestate.h"
#include "pathdesired.h" // object that will be updated by the module
#include "positionstate.h"
#include "manualcontrol.h"
#include "flightstatus.h"
#include "pathstatus.h"
#include "airspeedstate.h"
@ -182,52 +181,50 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters)
uint8_t result;
// Check the combinations of flightmode and pathdesired mode
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
updatePathVelocity();
result = updateFixedDesiredAttitude();
if (result) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
if (flightStatus.ControlChain.PathFollower == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
if (flightStatus.ControlChain.PathPlanner == FLIGHTSTATUS_CONTROLCHAIN_FALSE) {
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
updatePathVelocity();
result = updateFixedDesiredAttitude();
if (result) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
}
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
}
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
}
break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
pathStatus.UID = pathDesired.UID;
pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
switch (pathDesired.Mode) {
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_FLYCIRCLELEFT:
updatePathVelocity();
result = updateFixedDesiredAttitude();
if (result) {
pathStatus.UID = pathDesired.UID;
pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
switch (pathDesired.Mode) {
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_FLYCIRCLELEFT:
updatePathVelocity();
result = updateFixedDesiredAttitude();
if (result) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
} else {
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
}
break;
case PATHDESIRED_MODE_FIXEDATTITUDE:
updateFixedAttitude(pathDesired.ModeParameters);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
} else {
break;
case PATHDESIRED_MODE_DISARMALARM:
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
break;
default:
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
break;
}
break;
case PATHDESIRED_MODE_FIXEDATTITUDE:
updateFixedAttitude(pathDesired.ModeParameters);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_DISARMALARM:
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
break;
default:
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
break;
}
break;
default:
} else {
// Be cleaner and get rid of global variables
northVelIntegral = 0;
eastVelIntegral = 0;
@ -235,8 +232,6 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters)
courseIntegral = 0;
speedIntegral = 0;
powerIntegral = 0;
break;
}
PathStatusSet(&pathStatus);
}
@ -357,10 +352,10 @@ static void updateFixedAttitude(float *attitude)
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
stabDesired.Roll = attitude[0];
stabDesired.Pitch = attitude[1];
stabDesired.Yaw = attitude[2];
stabDesired.Throttle = attitude[3];
stabDesired.Roll = attitude[0];
stabDesired.Pitch = attitude[1];
stabDesired.Yaw = attitude[2];
stabDesired.Thrust = attitude[3];
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
@ -420,7 +415,7 @@ static uint8_t updateFixedDesiredAttitude()
/**
* Compute speed error (required for throttle and pitch)
* Compute speed error (required for thrust and pitch)
*/
// Current ground speed
@ -474,9 +469,9 @@ static uint8_t updateFixedDesiredAttitude()
}
/**
* Compute desired throttle command
* Compute desired thrust command
*/
// compute saturated integral error throttle response. Make integral leaky for better performance. Approximately 30s time constant.
// compute saturated integral error thrust response. Make integral leaky for better performance. Approximately 30s time constant.
if (fixedwingpathfollowerSettings.PowerPI.Ki > 0) {
powerIntegral = bound(powerIntegral + -descentspeedError * dT,
-fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki,
@ -491,7 +486,7 @@ static uint8_t updateFixedDesiredAttitude()
fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Max
);
// Compute final throttle response
// Compute final thrust response
powerCommand = -descentspeedError * fixedwingpathfollowerSettings.PowerPI.Kp +
powerIntegral * fixedwingpathfollowerSettings.PowerPI.Ki +
speedErrorToPowerCommandComponent;
@ -501,14 +496,14 @@ static uint8_t updateFixedDesiredAttitude()
fixedwingpathfollowerStatus.ErrorInt.Power = powerIntegral;
fixedwingpathfollowerStatus.Command.Power = powerCommand;
// set throttle
stabDesired.Throttle = bound(fixedwingpathfollowerSettings.ThrottleLimit.Neutral + powerCommand,
fixedwingpathfollowerSettings.ThrottleLimit.Min,
fixedwingpathfollowerSettings.ThrottleLimit.Max);
// set thrust
stabDesired.Thrust = bound(fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand,
fixedwingpathfollowerSettings.ThrustLimit.Min,
fixedwingpathfollowerSettings.ThrustLimit.Max);
// Error condition: plane cannot hold altitude at current speed.
fixedwingpathfollowerStatus.Errors.Lowpower = 0;
if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit.Max && // throttle at maximum
if (fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand >= fixedwingpathfollowerSettings.ThrustLimit.Max && // thrust at maximum
velocityState.Down > 0 && // we ARE going down
descentspeedDesired < 0 && // we WANT to go up
airspeedError > 0 && // we are too slow already
@ -516,9 +511,9 @@ static uint8_t updateFixedDesiredAttitude()
fixedwingpathfollowerStatus.Errors.Lowpower = 1;
result = 0;
}
// Error condition: plane keeps climbing despite minimum throttle (opposite of above)
// Error condition: plane keeps climbing despite minimum thrust (opposite of above)
fixedwingpathfollowerStatus.Errors.Highpower = 0;
if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit.Min && // throttle at minimum
if (fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand <= fixedwingpathfollowerSettings.ThrustLimit.Min && // thrust at minimum
velocityState.Down < 0 && // we ARE going up
descentspeedDesired > 0 && // we WANT to go down
airspeedError < 0 && // we are too fast already
@ -560,7 +555,7 @@ static uint8_t updateFixedDesiredAttitude()
// Error condition: high speed dive
fixedwingpathfollowerStatus.Errors.Pitchcontrol = 0;
if (pitchCommand >= fixedwingpathfollowerSettings.PitchLimit.Max && // pitch demand is full up
if (fixedwingpathfollowerSettings.PitchLimit.Neutral + pitchCommand >= fixedwingpathfollowerSettings.PitchLimit.Max && // pitch demand is full up
velocityState.Down > 0 && // we ARE going down
descentspeedDesired < 0 && // we WANT to go up
airspeedError < 0 && // we are too fast already

7
flight/modules/Logging/Logging.c
View File

@ -73,7 +73,12 @@ int32_t LoggingStart(void)
FlightStatusConnectCallback(FlightStatusUpdatedCb);
SettingsUpdatedCb(DebugLogSettingsHandle());
UAVObjEvent ev = { .obj = DebugLogSettingsHandle(), .instId = 0, .event = EV_UPDATED_PERIODIC };
UAVObjEvent ev = {
.obj = DebugLogSettingsHandle(),
.instId = 0,
.event = EV_UPDATED_PERIODIC,
.lowPriority = true,
};
EventPeriodicCallbackCreate(&ev, StatusUpdatedCb, 1000);
// invoke a periodic dispatcher callback - the event struct is a dummy, it could be filled with anything!
StatusUpdatedCb(&ev);

133
flight/modules/ManualControl/altitudehandler.c Normal file
View File

@ -0,0 +1,133 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup ManualControl
* @brief Interpretes the control input in ManualControlCommand
* @{
*
* @file altitudehandler.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
*
* @see The GNU Public License (GPL) Version 3
*
******************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "inc/manualcontrol.h"
#include <manualcontrolcommand.h>
#include <stabilizationbank.h>
#include <altitudeholddesired.h>
#include <altitudeholdsettings.h>
#include <positionstate.h>
#if defined(REVOLUTION)
// Private constants
// Private types
// Private functions
/**
* @brief Handler to control deprecated flight modes controlled by AltitudeHold module
* @input: ManualControlCommand
* @output: AltitudeHoldDesired
*/
void altitudeHandler(bool newinit)
{
const float DEADBAND = 0.20f;
const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2;
const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2;
if (newinit) {
StabilizationBankInitialize();
AltitudeHoldDesiredInitialize();
AltitudeHoldSettingsInitialize();
PositionStateInitialize();
}
// this is the max speed in m/s at the extents of thrust
float thrustRate;
uint8_t thrustExp;
static uint8_t flightMode;
static bool newaltitude = true;
ManualControlCommandData cmd;
ManualControlCommandGet(&cmd);
FlightStatusFlightModeGet(&flightMode);
AltitudeHoldDesiredData altitudeHoldDesiredData;
AltitudeHoldDesiredGet(&altitudeHoldDesiredData);
AltitudeHoldSettingsThrustExpGet(&thrustExp);
AltitudeHoldSettingsThrustRateGet(&thrustRate);
StabilizationBankData stabSettings;
StabilizationBankGet(&stabSettings);
PositionStateData posState;
PositionStateGet(&posState);
altitudeHoldDesiredData.Roll = cmd.Roll * stabSettings.RollMax;
altitudeHoldDesiredData.Pitch = cmd.Pitch * stabSettings.PitchMax;
altitudeHoldDesiredData.Yaw = cmd.Yaw * stabSettings.ManualRate.Yaw;
if (newinit) {
newaltitude = true;
}
uint8_t cutOff;
AltitudeHoldSettingsCutThrustWhenZeroGet(&cutOff);
if (cutOff && cmd.Thrust < 0) {
// Cut thrust if desired
altitudeHoldDesiredData.SetPoint = cmd.Thrust;
altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_THRUST;
newaltitude = true;
} else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd.Thrust > DEADBAND_HIGH) {
// being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1
// then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255
altitudeHoldDesiredData.SetPoint = -((thrustExp * powf((cmd.Thrust - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - thrustExp) * (cmd.Thrust - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * thrustRate);
altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY;
newaltitude = true;
} else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd.Thrust < DEADBAND_LOW) {
altitudeHoldDesiredData.SetPoint = -(-(thrustExp * powf((DEADBAND_LOW - (cmd.Thrust < 0 ? 0 : cmd.Thrust)) / DEADBAND_LOW, 3) + (255 - thrustExp) * (DEADBAND_LOW - cmd.Thrust) / DEADBAND_LOW) / 255 * thrustRate);
altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY;
newaltitude = true;
} else if (newaltitude == true) {
altitudeHoldDesiredData.SetPoint = posState.Down;
altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE;
newaltitude = false;
}
AltitudeHoldDesiredSet(&altitudeHoldDesiredData);
}
#else /* if defined(REVOLUTION) */
void altitudeHandler(__attribute__((unused)) bool newinit)
{
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); // should not be called
}
#endif // REVOLUTION
/**
* @}
* @}
*/

326
flight/modules/ManualControl/armhandler.c Normal file
View File

@ -0,0 +1,326 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup ManualControl
* @brief Interpretes the control input in ManualControlCommand
* @{
*
* @file armhandler.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
*
* @see The GNU Public License (GPL) Version 3
*
******************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "inc/manualcontrol.h"
#include <pios_struct_helper.h>
#include <sanitycheck.h>
#include <manualcontrolcommand.h>
#include <accessorydesired.h>
#include <flightstatus.h>
#include <flightmodesettings.h>
// Private constants
#define ARMED_THRESHOLD 0.50f
// Private types
typedef enum {
ARM_STATE_DISARMED,
ARM_STATE_ARMING_MANUAL,
ARM_STATE_ARMED,
ARM_STATE_DISARMING_MANUAL,
ARM_STATE_DISARMING_TIMEOUT
} ArmState_t;
// Private variables
// Private functions
static void setArmedIfChanged(uint8_t val);
static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time);
static bool okToArm(void);
static bool forcedDisArm(void);
/**
* @brief Handler to interprete Command inputs in regards to arming/disarming
* @input: ManualControlCommand, AccessoryDesired
* @output: FlightStatus.Arming
*/
void armHandler(bool newinit)
{
static ArmState_t armState;
if (newinit) {
AccessoryDesiredInitialize();
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
armState = ARM_STATE_DISARMED;
}
portTickType sysTime = xTaskGetTickCount();
FlightModeSettingsData settings;
FlightModeSettingsGet(&settings);
ManualControlCommandData cmd;
ManualControlCommandGet(&cmd);
AccessoryDesiredData acc;
bool lowThrottle = cmd.Throttle < 0;
bool armSwitch = false;
switch (settings.Arming) {
case FLIGHTMODESETTINGS_ARMING_ACCESSORY0:
AccessoryDesiredInstGet(0, &acc);
armSwitch = true;
break;
case FLIGHTMODESETTINGS_ARMING_ACCESSORY1:
AccessoryDesiredInstGet(1, &acc);
armSwitch = true;
break;
case FLIGHTMODESETTINGS_ARMING_ACCESSORY2:
AccessoryDesiredInstGet(2, &acc);
armSwitch = true;
break;
default:
break;
}
// immediate disarm on switch
if (armSwitch && acc.AccessoryVal <= -ARMED_THRESHOLD) {
lowThrottle = true;
}
if (forcedDisArm()) {
// PathPlanner forces explicit disarming due to error condition (crash, impact, fire, ...)
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
return;
}
if (settings.Arming == FLIGHTMODESETTINGS_ARMING_ALWAYSDISARMED) {
// In this configuration we always disarm
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
return;
}
// The throttle is not low, in case we where arming or disarming, abort
if (!lowThrottle) {
switch (armState) {
case ARM_STATE_DISARMING_MANUAL:
case ARM_STATE_DISARMING_TIMEOUT:
armState = ARM_STATE_ARMED;
break;
case ARM_STATE_ARMING_MANUAL:
armState = ARM_STATE_DISARMED;
break;
default:
// Nothing needs to be done in the other states
break;
}
return;
}
// The rest of these cases throttle is low
if (settings.Arming == FLIGHTMODESETTINGS_ARMING_ALWAYSARMED) {
// In this configuration, we go into armed state as soon as the throttle is low, never disarm
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
return;
}
// When the configuration is not "Always armed" and no "Always disarmed",
// the state will not be changed when the throttle is not low
static portTickType armedDisarmStart;
float armingInputLevel = 0;
// Calc channel see assumptions7
switch (settings.Arming) {
case FLIGHTMODESETTINGS_ARMING_ROLLLEFT:
armingInputLevel = 1.0f * cmd.Roll;
break;
case FLIGHTMODESETTINGS_ARMING_ROLLRIGHT:
armingInputLevel = -1.0f * cmd.Roll;
break;
case FLIGHTMODESETTINGS_ARMING_PITCHFORWARD:
armingInputLevel = 1.0f * cmd.Pitch;
break;
case FLIGHTMODESETTINGS_ARMING_PITCHAFT:
armingInputLevel = -1.0f * cmd.Pitch;
break;
case FLIGHTMODESETTINGS_ARMING_YAWLEFT:
armingInputLevel = 1.0f * cmd.Yaw;
break;
case FLIGHTMODESETTINGS_ARMING_YAWRIGHT:
armingInputLevel = -1.0f * cmd.Yaw;
break;
case FLIGHTMODESETTINGS_ARMING_ACCESSORY0:
case FLIGHTMODESETTINGS_ARMING_ACCESSORY1:
case FLIGHTMODESETTINGS_ARMING_ACCESSORY2:
armingInputLevel = -1.0f * acc.AccessoryVal;
break;
}
bool manualArm = false;
bool manualDisarm = false;
if (armingInputLevel <= -ARMED_THRESHOLD) {
manualArm = true;
} else if (armingInputLevel >= +ARMED_THRESHOLD) {
manualDisarm = true;
}
switch (armState) {
case ARM_STATE_DISARMED:
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
// only allow arming if it's OK too
if (manualArm && okToArm()) {
armedDisarmStart = sysTime;
armState = ARM_STATE_ARMING_MANUAL;
}
break;
case ARM_STATE_ARMING_MANUAL:
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING);
if (manualArm && (timeDifferenceMs(armedDisarmStart, sysTime) > settings.ArmingSequenceTime)) {
armState = ARM_STATE_ARMED;
} else if (!manualArm) {
armState = ARM_STATE_DISARMED;
}
break;
case ARM_STATE_ARMED:
// When we get here, the throttle is low,
// we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled
armedDisarmStart = sysTime;
armState = ARM_STATE_DISARMING_TIMEOUT;
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
break;
case ARM_STATE_DISARMING_TIMEOUT:
// We get here when armed while throttle low, even when the arming timeout is not enabled
if ((settings.ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, sysTime) > settings.ArmedTimeout)) {
armState = ARM_STATE_DISARMED;
}
// Switch to disarming due to manual control when needed
if (manualDisarm) {
armedDisarmStart = sysTime;
armState = ARM_STATE_DISARMING_MANUAL;
}
break;
case ARM_STATE_DISARMING_MANUAL:
// arming switch disarms immediately,
if (manualDisarm && (timeDifferenceMs(armedDisarmStart, sysTime) > settings.DisarmingSequenceTime)) {
armState = ARM_STATE_DISARMED;
} else if (!manualDisarm) {
armState = ARM_STATE_ARMED;
}
break;
} // End Switch
}
static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time)
{
return (end_time - start_time) * portTICK_RATE_MS;
}
/**
* @brief Determine if the aircraft is safe to arm
* @returns True if safe to arm, false otherwise
*/
static bool okToArm(void)
{
// update checks
configuration_check();
// read alarms
SystemAlarmsData alarms;
SystemAlarmsGet(&alarms);
// Check each alarm
for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) {
if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set
if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) {
continue;
}
return false;
}
}
uint8_t flightMode;
FlightStatusFlightModeGet(&flightMode);
switch (flightMode) {
case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
return true;
break;
default:
return false;
break;
}
}
/**
* @brief Determine if the aircraft is forced to disarm by an explicit alarm
* @returns True if safe to arm, false otherwise
*/
static bool forcedDisArm(void)
{
// read alarms
SystemAlarmsAlarmData alarms;
SystemAlarmsAlarmGet(&alarms);
if (alarms.Guidance == SYSTEMALARMS_ALARM_CRITICAL) {
return true;
}
if (alarms.Receiver == SYSTEMALARMS_ALARM_CRITICAL) {
return true;
}
return false;
}
/**
* @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);
}
}
/**
* @}
* @}
*/

133
flight/modules/ManualControl/inc/manualcontrol.h
View File

@ -6,7 +6,7 @@
* @{
*
* @file manualcontrol.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief ManualControl module. Handles safety R/C link and flight mode.
*
* @see The GNU Public License (GPL) Version 3
@ -30,30 +30,57 @@
#ifndef MANUALCONTROL_H
#define MANUALCONTROL_H
#include "manualcontrolcommand.h"
#include <openpilot.h>
#include <flightstatus.h>
typedef enum { FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABILIZED = 2, FLIGHTMODE_GUIDANCE = 3, FLIGHTMODE_TUNING = 4 } flightmode_path;
typedef void (*handlerFunc)(bool newinit);
#define PARSE_FLIGHT_MODE(x) \
( \
(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_ALTITUDEHOLD) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_LAND) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) ? FLIGHTMODE_TUNING : \
(x == FLIGHTSTATUS_FLIGHTMODE_POI) ? FLIGHTMODE_GUIDANCE : \
FLIGHTMODE_UNDEFINED \
)
typedef struct controlHandlerStruct {
FlightStatusControlChainData controlChain;
handlerFunc handler;
} controlHandler;
int32_t ManualControlInitialize();
/**
* @brief Handler to interprete Command inputs in regards to arming/disarming (called in all flight modes)
* @input: ManualControlCommand, AccessoryDesired
* @output: FlightStatus.Arming
*/
void armHandler(bool newinit);
/**
* @brief Handler to control Manual flightmode - input directly steers actuators
* @input: ManualControlCommand
* @output: ActuatorDesired
*/
void manualHandler(bool newinit);
/**
* @brief Handler to control Stabilized flightmodes. FlightControl is governed by "Stabilization"
* @input: ManualControlCommand
* @output: StabilizationDesired
*/
void stabilizedHandler(bool newinit);
/**
* @brief Handler to control deprecated flight modes controlled by AltitudeHold module
* @input: ManualControlCommand
* @output: AltitudeHoldDesired
*/
void altitudeHandler(bool newinit);
/**
* @brief Handler to control Guided flightmodes. FlightControl is governed by PathFollower, control via PathDesired
* @input: NONE: fully automated mode -- TODO recursively call handler for advanced stick commands
* @output: PathDesired
*/
void pathFollowerHandler(bool newinit);
/**
* @brief Handler to control Navigated flightmodes. FlightControl is governed by PathFollower, controlled indirectly via PathPlanner
* @input: NONE: fully automated mode -- TODO recursively call handler for advanced stick commands to affect navigation
* @output: NONE
*/
void pathPlannerHandler(bool newinit);
/*
* These are assumptions we make in the flight code about the order of settings and their consistency between
@ -61,54 +88,46 @@ int32_t ManualControlInitialize();
*/
#define assumptions1 \
( \
((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
)
#define assumptions3 \
( \
((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
)
#define assumptions5 \
( \
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
)
#define assumptions_flightmode \
( \
((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_ALTITUDEHOLD == (int)FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) && \
((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO == (int)FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO) && \
((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int)FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \
((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int)FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) && \
((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER == (int)FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) && \
((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE == (int)FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) && \
((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_LAND == (int)FLIGHTSTATUS_FLIGHTMODE_LAND) && \
((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_AUTOTUNE == (int)FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) && \
((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POI == (int)FLIGHTSTATUS_FLIGHTMODE_POI) \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_MANUAL == (int)FLIGHTSTATUS_FLIGHTMODE_MANUAL) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD == (int)FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO == (int)FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int)FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int)FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER == (int)FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE == (int)FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND == (int)FLIGHTSTATUS_FLIGHTMODE_LAND) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE == (int)FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI == (int)FLIGHTSTATUS_FLIGHTMODE_POI) \
)
#define assumptions_channelcount \
( \
((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM) && \
((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELNUMBER_NUMELEM) && \
((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELMIN_NUMELEM) && \
((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELMAX_NUMELEM) && \
((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELNEUTRAL_NUMELEM))
#endif // MANUALCONTROL_H

1311
flight/modules/ManualControl/manualcontrol.c
View File

@ -11,7 +11,7 @@
* AttitudeDesired object (stabilized mode)
*
* @file manualcontrol.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief ManualControl module. Handles safety R/C link and flight mode.
*
* @see The GNU Public License (GPL) Version 3
@ -33,29 +33,16 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include <openpilot.h>
#include <pios_struct_helper.h>
#include "accessorydesired.h"
#include "actuatordesired.h"
#include "altitudeholddesired.h"
#include "flighttelemetrystats.h"
#include "flightstatus.h"
#include "sanitycheck.h"
#include "manualcontrol.h"
#include "manualcontrolsettings.h"
#include "manualcontrolcommand.h"
#include "positionstate.h"
#include "pathdesired.h"
#include "stabilizationbank.h"
#include "stabilizationdesired.h"
#include "receiveractivity.h"
#include "systemsettings.h"
#include <altitudeholdsettings.h>
#include <taskinfo.h>
#include "inc/manualcontrol.h"
#include <sanitycheck.h>
#include <manualcontrolsettings.h>
#include <manualcontrolcommand.h>
#include <flightmodesettings.h>
#include <flightstatus.h>
#include <systemsettings.h>
#include <stabilizationdesired.h>
#include <callbackinfo.h>
#if defined(PIOS_INCLUDE_USB_RCTX)
#include "pios_usb_rctx.h"
#endif /* PIOS_INCLUDE_USB_RCTX */
// Private constants
#if defined(PIOS_MANUAL_STACK_SIZE)
@ -64,79 +51,98 @@
#define STACK_SIZE_BYTES 1152
#endif
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) // 3 = flight control
#define UPDATE_PERIOD_MS 20
#define THROTTLE_FAILSAFE -0.1f
#define ARMED_THRESHOLD 0.50f
// safe band to allow a bit of calibration error or trim offset (in microseconds)
#define CONNECTION_OFFSET 250
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR
#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
// defined handlers
static controlHandler handler_MANUAL = {
.controlChain = {
.Stabilization = false,
.PathFollower = false,
.PathPlanner = false,
},
.handler = &manualHandler,
};
static controlHandler handler_STABILIZED = {
.controlChain = {
.Stabilization = true,
.PathFollower = false,
.PathPlanner = false,
},
.handler = &stabilizedHandler,
};
// TODO: move the altitude handling into stabi
static controlHandler handler_ALTITUDE = {
.controlChain = {
.Stabilization = true,
.PathFollower = false,
.PathPlanner = false,
},
.handler = &altitudeHandler,
};
static controlHandler handler_AUTOTUNE = {
.controlChain = {
.Stabilization = false,
.PathFollower = false,
.PathPlanner = false,
},
.handler = NULL,
};
static controlHandler handler_PATHFOLLOWER = {
.controlChain = {
.Stabilization = true,
.PathFollower = true,
.PathPlanner = false,
},
.handler = &pathFollowerHandler,
};
static controlHandler handler_PATHPLANNER = {
.controlChain = {
.Stabilization = true,
.PathFollower = true,
.PathPlanner = true,
},
.handler = &pathPlannerHandler,
};
// Private types
typedef enum {
ARM_STATE_DISARMED,
ARM_STATE_ARMING_MANUAL,
ARM_STATE_ARMED,
ARM_STATE_DISARMING_MANUAL,
ARM_STATE_DISARMING_TIMEOUT
} ArmState_t;
// Private variables
static xTaskHandle taskHandle;
static ArmState_t armState;
static portTickType lastSysTime;
#ifdef USE_INPUT_LPF
static portTickType lastSysTimeLPF;
static float inputFiltered[MANUALCONTROLSETTINGS_RESPONSETIME_NUMELEM];
#endif
static DelayedCallbackInfo *callbackHandle;
// Private functions
static void updateActuatorDesired(ManualControlCommandData *cmd);
static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualControlSettingsData *settings);
static void updateLandDesired(ManualControlCommandData *cmd, bool changed);
static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed);
static void updatePathDesired(ManualControlCommandData *cmd, bool changed, bool home);
static void processFlightMode(ManualControlSettingsData *settings, float flightMode, ManualControlCommandData *cmd);
static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData *settings, int8_t armSwitch);
static void setArmedIfChanged(uint8_t val);
static void configurationUpdatedCb(UAVObjEvent *ev);
static void commandUpdatedCb(UAVObjEvent *ev);
static void manualControlTask(void *parameters);
static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral);
static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time);
static bool okToArm(void);
static bool validInputRange(int16_t min, int16_t max, uint16_t value);
static void applyDeadband(float *value, float deadband);
static void manualControlTask(void);
#ifdef USE_INPUT_LPF
static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT);
#endif
#define RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP 12
#define RCVR_ACTIVITY_MONITOR_MIN_RANGE 10
struct rcvr_activity_fsm {
ManualControlSettingsChannelGroupsOptions group;
uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP];
uint8_t sample_count;
};
static struct rcvr_activity_fsm activity_fsm;
static void resetRcvrActivity(struct rcvr_activity_fsm *fsm);
static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm);
#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions_flightmode && assumptions_channelcount)
#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions_flightmode)
/**
* Module starting
*/
int32_t ManualControlStart()
{
// Run this initially to make sure the configuration is checked
configuration_check();
// Whenever the configuration changes, make sure it is safe to fly
SystemSettingsConnectCallback(configurationUpdatedCb);
ManualControlSettingsConnectCallback(configurationUpdatedCb);
ManualControlCommandConnectCallback(commandUpdatedCb);
// clear alarms
AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
// Make sure unarmed on power up
armHandler(true);
// Start main task
xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_MANUALCONTROL, taskHandle);
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL);
#endif
PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
return 0;
}
@ -147,16 +153,15 @@ int32_t ManualControlStart()
int32_t ManualControlInitialize()
{
/* Check the assumptions about uavobject enum's are correct */
if (!assumptions) {
return -1;
}
PIOS_STATIC_ASSERT(assumptions);
AccessoryDesiredInitialize();
ManualControlCommandInitialize();
FlightStatusInitialize();
StabilizationDesiredInitialize();
ReceiverActivityInitialize();
ManualControlSettingsInitialize();
FlightModeSettingsInitialize();
SystemSettingsInitialize();
callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&manualControlTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_MANUALCONTROL, STACK_SIZE_BYTES);
return 0;
}
@ -165,1103 +170,75 @@ MODULE_INITCALL(ManualControlInitialize, ManualControlStart);
/**
* Module task
*/
static void manualControlTask(__attribute__((unused)) void *parameters)
static void manualControlTask(void)
{
ManualControlSettingsData settings;
// Process Arming
armHandler(false);
// Process flight mode
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
ManualControlCommandData cmd;
FlightStatusData flightStatus;
float flightMode = 0;
uint8_t disconnected_count = 0;
uint8_t connected_count = 0;
// For now manual instantiate extra instances of Accessory Desired. In future should be done dynamically
// this includes not even registering it if not used
AccessoryDesiredCreateInstance();
AccessoryDesiredCreateInstance();
// Run this initially to make sure the configuration is checked
configuration_check();
// Whenever the configuration changes, make sure it is safe to fly
SystemSettingsConnectCallback(configurationUpdatedCb);
ManualControlSettingsConnectCallback(configurationUpdatedCb);
// Whenever the configuration changes, make sure it is safe to fly
// Make sure unarmed on power up
ManualControlCommandGet(&cmd);
FlightStatusGet(&flightStatus);
flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
armState = ARM_STATE_DISARMED;
/* Initialize the RcvrActivty FSM */
portTickType lastActivityTime = xTaskGetTickCount();
resetRcvrActivity(&activity_fsm);
FlightModeSettingsData modeSettings;
FlightModeSettingsGet(&modeSettings);
// Main task loop
lastSysTime = xTaskGetTickCount();
float scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM] = { 0 };
while (1) {
// Wait until next update
vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS);
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL);
#endif
// Read settings
ManualControlSettingsGet(&settings);
/* Update channel activity monitor */
if (flightStatus.Armed == ARM_STATE_DISARMED) {
if (updateRcvrActivity(&activity_fsm)) {
/* Reset the aging timer because activity was detected */
lastActivityTime = lastSysTime;
}
}
if (timeDifferenceMs(lastActivityTime, lastSysTime) > 5000) {
resetRcvrActivity(&activity_fsm);
lastActivityTime = lastSysTime;
}
if (ManualControlCommandReadOnly()) {
FlightTelemetryStatsData flightTelemStats;
FlightTelemetryStatsGet(&flightTelemStats);
if (flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
/* trying to fly via GCS and lost connection. fall back to transmitter */
UAVObjMetadata metadata;
ManualControlCommandGetMetadata(&metadata);
UAVObjSetAccess(&metadata, ACCESS_READWRITE);
ManualControlCommandSetMetadata(&metadata);
}
}
if (!ManualControlCommandReadOnly()) {
bool valid_input_detected = true;
// Read channel values in us
for (uint8_t n = 0; n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) {
extern uint32_t pios_rcvr_group_map[];
if (cast_struct_to_array(settings.ChannelGroups, settings.ChannelGroups.Roll)[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
cmd.Channel[n] = PIOS_RCVR_INVALID;
} else {
cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[
cast_struct_to_array(settings.ChannelGroups, settings.ChannelGroups.Pitch)[n]],
cast_struct_to_array(settings.ChannelNumber, settings.ChannelNumber.Pitch)[n]);
}
// If a channel has timed out this is not valid data and we shouldn't update anything
// until we decide to go to failsafe
if (cmd.Channel[n] == (uint16_t)PIOS_RCVR_TIMEOUT) {
valid_input_detected = false;
} else {
scaledChannel[n] = scaleChannel(cmd.Channel[n],
cast_struct_to_array(settings.ChannelMax, settings.ChannelMax.Pitch)[n],
cast_struct_to_array(settings.ChannelMin, settings.ChannelMin.Pitch)[n],
cast_struct_to_array(settings.ChannelNeutral, settings.ChannelNeutral.Pitch)[n]);
}
}
// Check settings, if error raise alarm
if (settings.ChannelGroups.Roll >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups.Pitch >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups.Yaw >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups.Throttle >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
||
// Check all channel mappings are valid
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t)PIOS_RCVR_INVALID
||
// Check the driver exists
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_NODRIVER
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_NODRIVER
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_NODRIVER
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t)PIOS_RCVR_NODRIVER ||
// Check the FlightModeNumber is valid
settings.FlightModeNumber < 1 || settings.FlightModeNumber > MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM ||
// Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care
((settings.FlightModeNumber > 1)
&& (settings.ChannelGroups.FlightMode >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_NODRIVER))) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
ManualControlCommandSet(&cmd);
// Need to do this here since we don't process armed status. Since this shouldn't happen in flight (changed config)
// immediately disarm
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
continue;
}
// decide if we have valid manual input or not
valid_input_detected &= validInputRange(settings.ChannelMin.Throttle,
settings.ChannelMax.Throttle, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE])
&& validInputRange(settings.ChannelMin.Roll,
settings.ChannelMax.Roll, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL])
&& validInputRange(settings.ChannelMin.Yaw,
settings.ChannelMax.Yaw, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW])
&& validInputRange(settings.ChannelMin.Pitch,
settings.ChannelMax.Pitch, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]);
// Implement hysteresis loop on connection status
if (valid_input_detected && (++connected_count > 10)) {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
connected_count = 0;
disconnected_count = 0;
} else if (!valid_input_detected && (++disconnected_count > 10)) {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
connected_count = 0;
disconnected_count = 0;
}
int8_t armSwitch = 0;
if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) {
cmd.Throttle = -1; // Shut down engine with no control
cmd.Roll = 0;
cmd.Yaw = 0;
cmd.Pitch = 0;
cmd.Collective = 0;
if (settings.FailsafeBehavior != MANUALCONTROLSETTINGS_FAILSAFEBEHAVIOR_NONE) {
FlightStatusGet(&flightStatus);
cmd.FlightModeSwitchPosition = (uint8_t)settings.FailsafeBehavior - 1;
flightStatus.FlightMode = settings.FlightModePosition[settings.FailsafeBehavior - 1];
FlightStatusSet(&flightStatus);
}
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
AccessoryDesiredData accessory;
// Set Accessory 0
if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = 0;
if (AccessoryDesiredInstSet(0, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
}
// Set Accessory 1
if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = 0;
if (AccessoryDesiredInstSet(1, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
}
// Set Accessory 2
if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = 0;
if (AccessoryDesiredInstSet(2, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
}
} else if (valid_input_detected) {
AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
// Scale channels to -1 -> +1 range
cmd.Roll = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL];
cmd.Pitch = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH];
cmd.Yaw = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW];
cmd.Throttle = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE];
flightMode = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE];
// Apply deadband for Roll/Pitch/Yaw stick inputs
if (settings.Deadband > 0.0f) {
applyDeadband(&cmd.Roll, settings.Deadband);
applyDeadband(&cmd.Pitch, settings.Deadband);
applyDeadband(&cmd.Yaw, settings.Deadband);
}
#ifdef USE_INPUT_LPF
// Apply Low Pass Filter to input channels, time delta between calls in ms
portTickType thisSysTime = xTaskGetTickCount();
float dT = (thisSysTime > lastSysTimeLPF) ?
(float)((thisSysTime - lastSysTimeLPF) * portTICK_RATE_MS) :
(float)UPDATE_PERIOD_MS;
lastSysTimeLPF = thisSysTime;
applyLPF(&cmd.Roll, MANUALCONTROLSETTINGS_RESPONSETIME_ROLL, &settings, dT);
applyLPF(&cmd.Pitch, MANUALCONTROLSETTINGS_RESPONSETIME_PITCH, &settings, dT);
applyLPF(&cmd.Yaw, MANUALCONTROLSETTINGS_RESPONSETIME_YAW, &settings, dT);
#endif // USE_INPUT_LPF
if (cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_INVALID
&& cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_NODRIVER
&& cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_TIMEOUT) {
cmd.Collective = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE];
}
AccessoryDesiredData accessory;
// Set Accessory 0
if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0];
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT);
#endif
if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ACCESSORY0) {
if (accessory.AccessoryVal > ARMED_THRESHOLD) {
armSwitch = 1;
} else if (accessory.AccessoryVal < -ARMED_THRESHOLD) {
armSwitch = -1;
}
}
if (AccessoryDesiredInstSet(0, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
}
// Set Accessory 1
if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1];
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT);
#endif
if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ACCESSORY1) {
if (accessory.AccessoryVal > ARMED_THRESHOLD) {
armSwitch = 1;
} else if (accessory.AccessoryVal < -ARMED_THRESHOLD) {
armSwitch = -1;
}
}
if (AccessoryDesiredInstSet(1, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
}
// Set Accessory 2
if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2];
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT);
#endif
if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ACCESSORY2) {
if (accessory.AccessoryVal > ARMED_THRESHOLD) {
armSwitch = 1;
} else if (accessory.AccessoryVal < -ARMED_THRESHOLD) {
armSwitch = -1;
}
}
if (AccessoryDesiredInstSet(2, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
}
processFlightMode(&settings, flightMode, &cmd);
}
// Process arming outside conditional so system will disarm when disconnected
processArm(&cmd, &settings, armSwitch);
// Update cmd object
ManualControlCommandSet(&cmd);
#if defined(PIOS_INCLUDE_USB_RCTX)
if (pios_usb_rctx_id) {
PIOS_USB_RCTX_Update(pios_usb_rctx_id,
cmd.Channel,
cast_struct_to_array(settings.ChannelMin, settings.ChannelMin.Roll),
cast_struct_to_array(settings.ChannelMax, settings.ChannelMax.Roll),
NELEMENTS(cmd.Channel));
}
#endif /* PIOS_INCLUDE_USB_RCTX */
} else {
ManualControlCommandGet(&cmd); /* Under GCS control */
}
FlightStatusGet(&flightStatus);
// Depending on the mode update the Stabilization or Actuator objects
static uint8_t lastFlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL;
switch (PARSE_FLIGHT_MODE(flightStatus.FlightMode)) {
case FLIGHTMODE_UNDEFINED:
// This reflects a bug in the code architecture!
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
break;
case FLIGHTMODE_MANUAL:
updateActuatorDesired(&cmd);
break;
case FLIGHTMODE_STABILIZED:
updateStabilizationDesired(&cmd, &settings);
break;
case FLIGHTMODE_TUNING:
// Tuning takes settings directly from manualcontrolcommand. No need to
// call anything else. This just avoids errors.
break;
case FLIGHTMODE_GUIDANCE:
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO:
altitudeHoldDesired(&cmd, lastFlightMode != flightStatus.FlightMode);
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
case FLIGHTSTATUS_FLIGHTMODE_POI:
updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, false);
break;
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, true);
break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
// No need to call anything. This just avoids errors.
break;
case FLIGHTSTATUS_FLIGHTMODE_LAND:
updateLandDesired(&cmd, lastFlightMode != flightStatus.FlightMode);
break;
default:
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
}
break;
}
lastFlightMode = flightStatus.FlightMode;
}
}
static void resetRcvrActivity(struct rcvr_activity_fsm *fsm)
{
ReceiverActivityData data;
bool updated = false;
/* Clear all channel activity flags */
ReceiverActivityGet(&data);
if (data.ActiveGroup != RECEIVERACTIVITY_ACTIVEGROUP_NONE && data.ActiveChannel != 255) {
data.ActiveGroup = RECEIVERACTIVITY_ACTIVEGROUP_NONE;
data.ActiveChannel = 255;
updated = true;
}
if (updated) {
ReceiverActivitySet(&data);
uint8_t position = cmd.FlightModeSwitchPosition;
uint8_t newMode = flightStatus.FlightMode;
if (position < FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM) {
newMode = modeSettings.FlightModePosition[position];
}
/* Reset the FSM state */
fsm->group = 0;
fsm->sample_count = 0;
}
static void updateRcvrActivitySample(uint32_t rcvr_id, uint16_t samples[], uint8_t max_channels)
{
for (uint8_t channel = 1; channel <= max_channels; channel++) {
// Subtract 1 because channels are 1 indexed
samples[channel - 1] = PIOS_RCVR_Read(rcvr_id, channel);
}
}
static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm *fsm)
{
bool activity_updated = false;
/* Compare the current value to the previous sampled value */
for (uint8_t channel = 1; channel <= RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP; channel++) {
uint16_t delta;
uint16_t prev = fsm->prev[channel - 1]; // Subtract 1 because channels are 1 indexed
uint16_t curr = PIOS_RCVR_Read(rcvr_id, channel);
if (curr > prev) {
delta = curr - prev;
} else {
delta = prev - curr;
}
if (delta > RCVR_ACTIVITY_MONITOR_MIN_RANGE) {
/* Mark this channel as active */
ReceiverActivityActiveGroupOptions group;
/* Don't assume manualcontrolsettings and receiveractivity are in the same order. */
switch (fsm->group) {
case MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM:
group = RECEIVERACTIVITY_ACTIVEGROUP_PWM;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM:
group = RECEIVERACTIVITY_ACTIVEGROUP_PPM;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT:
group = RECEIVERACTIVITY_ACTIVEGROUP_DSMMAINPORT;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT:
group = RECEIVERACTIVITY_ACTIVEGROUP_DSMFLEXIPORT;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS:
group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS:
group = RECEIVERACTIVITY_ACTIVEGROUP_GCS;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK:
group = RECEIVERACTIVITY_ACTIVEGROUP_OPLINK;
break;
default:
PIOS_Assert(0);
break;
}
ReceiverActivityActiveGroupSet((uint8_t *)&group);
ReceiverActivityActiveChannelSet(&channel);
activity_updated = true;
}
}
return activity_updated;
}
static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm)
{
bool activity_updated = false;
if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
/* We're out of range, reset things */
resetRcvrActivity(fsm);
}
extern uint32_t pios_rcvr_group_map[];
if (!pios_rcvr_group_map[fsm->group]) {
/* Unbound group, skip it */
goto group_completed;
}
if (fsm->sample_count == 0) {
/* Take a sample of each channel in this group */
updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], fsm->prev, NELEMENTS(fsm->prev));
fsm->sample_count++;
return false;
}
/* Compare with previous sample */
activity_updated = updateRcvrActivityCompare(pios_rcvr_group_map[fsm->group], fsm);
group_completed:
/* Reset the sample counter */
fsm->sample_count = 0;
/* Find the next active group, but limit search so we can't loop forever here */
for (uint8_t i = 0; i < MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE; i++) {
/* Move to the next group */
fsm->group++;
if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
/* Wrap back to the first group */
fsm->group = 0;
}
if (pios_rcvr_group_map[fsm->group]) {
/*
* Found an active group, take a sample here to avoid an
* extra 20ms delay in the main thread so we can speed up
* this algorithm.
*/
updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], fsm->prev, NELEMENTS(fsm->prev));
fsm->sample_count++;
break;
}
}
return activity_updated;
}
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)
{
StabilizationDesiredData stabilization;
StabilizationDesiredGet(&stabilization);
StabilizationBankData stabSettings;
StabilizationBankGet(&stabSettings);
uint8_t *stab_settings;
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
stab_settings = cast_struct_to_array(settings->Stabilization1Settings, settings->Stabilization1Settings.Roll);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
stab_settings = cast_struct_to_array(settings->Stabilization2Settings, settings->Stabilization2Settings.Roll);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
stab_settings = cast_struct_to_array(settings->Stabilization3Settings, settings->Stabilization3Settings.Roll);
break;
default:
// Major error, this should not occur because only enter this block when one of these is true
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
return;
}
stabilization.Roll =
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax :
0; // this is an invalid mode
stabilization.Pitch =
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
0; // this is an invalid mode
// TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
stabilization.StabilizationMode.Roll = stab_settings[0];
stabilization.StabilizationMode.Pitch = stab_settings[1];
// Other axes (yaw) cannot be Rattitude, so use Rate
// Should really do this for Attitude mode as well?
if (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) {
stabilization.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabilization.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw;
} else {
stabilization.StabilizationMode.Yaw = stab_settings[2];
stabilization.Yaw =
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
0; // this is an invalid mode
}
stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
StabilizationDesiredSet(&stabilization);
}
#if defined(REVOLUTION)
// TODO: Need compile flag to exclude this from copter control
/**
* @brief Update the position desired to current location when
* enabled and allow the waypoint to be moved by transmitter
*/
static void updatePathDesired(__attribute__((unused)) ManualControlCommandData *cmd, bool changed, bool home)
{
/*
static portTickType lastSysTime;
portTickType thisSysTime = xTaskGetTickCount();
dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f);
lastSysTime = thisSysTime;
*/
if (home && changed) {
// Simple Return To Base mode - keep altitude the same, fly to home position
PositionStateData positionState;
PositionStateGet(&positionState);
ManualControlSettingsData settings;
ManualControlSettingsGet(&settings);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.Start.North = 0;
pathDesired.Start.East = 0;
pathDesired.Start.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset;
pathDesired.End.North = 0;
pathDesired.End.East = 0;
pathDesired.End.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset;
pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
} else if (changed) {
// After not being in this mode for a while init at current height
PositionStateData positionState;
PositionStateGet(&positionState);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.Start.North = positionState.North;
pathDesired.Start.East = positionState.East;
pathDesired.Start.Down = positionState.Down;
pathDesired.End.North = positionState.North;
pathDesired.End.East = positionState.East;
pathDesired.End.Down = positionState.Down;
pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
/* Disable this section, until such time as proper discussion can be had about how to implement it for all types of crafts.
} else {
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.End[PATHDESIRED_END_NORTH] += dT * -cmd->Pitch;
pathDesired.End[PATHDESIRED_END_EAST] += dT * cmd->Roll;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
*/
}
}
static void updateLandDesired(__attribute__((unused)) ManualControlCommandData *cmd, bool changed)
{
/*
static portTickType lastSysTime;
portTickType thisSysTime;
float dT;
thisSysTime = xTaskGetTickCount();
dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f);
lastSysTime = thisSysTime;
*/
PositionStateData positionState;
PositionStateGet(&positionState);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
if (changed) {
// After not being in this mode for a while init at current height
pathDesired.Start.North = positionState.North;
pathDesired.Start.East = positionState.East;
pathDesired.Start.Down = positionState.Down;
pathDesired.End.North = positionState.North;
pathDesired.End.East = positionState.East;
pathDesired.End.Down = positionState.Down;
pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
}
pathDesired.End.Down = positionState.Down + 5;
PathDesiredSet(&pathDesired);
}
/**
* @brief Update the altitude desired to current altitude when
* enabled and enable altitude mode for stabilization
* @todo: Need compile flag to exclude this from copter control
*/
static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
{
const float DEADBAND = 0.20f;
const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2;
const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2;
// this is the max speed in m/s at the extents of throttle
float throttleRate;
uint8_t throttleExp;
static uint8_t flightMode;
static bool newaltitude = true;
FlightStatusFlightModeGet(&flightMode);
AltitudeHoldDesiredData altitudeHoldDesiredData;
AltitudeHoldDesiredGet(&altitudeHoldDesiredData);
AltitudeHoldSettingsThrottleExpGet(&throttleExp);
AltitudeHoldSettingsThrottleRateGet(&throttleRate);
StabilizationBankData stabSettings;
StabilizationBankGet(&stabSettings);
PositionStateData posState;
PositionStateGet(&posState);
altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax;
altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax;
altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw;
if (changed) {
newaltitude = true;
}
uint8_t cutOff;
AltitudeHoldSettingsCutThrottleWhenZeroGet(&cutOff);
if (cutOff && cmd->Throttle < 0) {
// Cut throttle if desired
altitudeHoldDesiredData.SetPoint = cmd->Throttle;
altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_THROTTLE;
newaltitude = true;
} else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Throttle > DEADBAND_HIGH) {
// being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1
// then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255
altitudeHoldDesiredData.SetPoint = -((throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate);
altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY;
newaltitude = true;
} else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Throttle < DEADBAND_LOW) {
altitudeHoldDesiredData.SetPoint = -(-(throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate);
altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY;
newaltitude = true;
} else if (newaltitude == true) {
altitudeHoldDesiredData.SetPoint = posState.Down;
altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE;
newaltitude = false;
}
AltitudeHoldDesiredSet(&altitudeHoldDesiredData);
}
#else /* if defined(REVOLUTION) */
// TODO: These functions should never be accessible on CC. Any configuration that
// could allow them to be called should already throw an error to prevent this happening
// in flight
static void updatePathDesired(__attribute__((unused)) ManualControlCommandData *cmd,
__attribute__((unused)) bool changed,
__attribute__((unused)) bool home)
{
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
}
static void updateLandDesired(__attribute__((unused)) ManualControlCommandData *cmd,
__attribute__((unused)) bool changed)
{
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
}
static void altitudeHoldDesired(__attribute__((unused)) ManualControlCommandData *cmd,
__attribute__((unused)) bool changed)
{
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
}
#endif /* if defined(REVOLUTION) */
/**
* Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range.
*/
static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral)
{
float valueScaled;
// Scale
if ((max > min && value >= neutral) || (min > max && value <= neutral)) {
if (max != neutral) {
valueScaled = (float)(value - neutral) / (float)(max - neutral);
} else {
valueScaled = 0;
}
} else {
if (min != neutral) {
valueScaled = (float)(value - neutral) / (float)(neutral - min);
} else {
valueScaled = 0;
}
}
// Bound
if (valueScaled > 1.0f) {
valueScaled = 1.0f;
} else if (valueScaled < -1.0f) {
valueScaled = -1.0f;
}
return valueScaled;
}
static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time)
{
return (end_time - start_time) * portTICK_RATE_MS;
}
/**
* @brief Determine if the aircraft is safe to arm
* @returns True if safe to arm, false otherwise
*/
static bool okToArm(void)
{
// update checks
configuration_check();
// read alarms
SystemAlarmsData alarms;
SystemAlarmsGet(&alarms);
// Check each alarm
for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) {
if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set
if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) {
continue;
}
return false;
}
}
uint8_t flightMode;
FlightStatusFlightModeGet(&flightMode);
switch (flightMode) {
// Depending on the mode update the Stabilization or Actuator objects
controlHandler *handler = &handler_MANUAL;
switch (newMode) {
case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
handler = &handler_MANUAL;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
return true;
default:
return false;
}
}
/**
* @brief Determine if the aircraft is forced to disarm by an explicit alarm
* @returns True if safe to arm, false otherwise
*/
static bool forcedDisArm(void)
{
// read alarms
SystemAlarmsData alarms;
SystemAlarmsGet(&alarms);
if (alarms.Alarm.Guidance == SYSTEMALARMS_ALARM_CRITICAL) {
return true;
}
return false;
}
/**
* @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
* @param[in] settings Settings indicating the necessary position
*/
static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData *settings, int8_t armSwitch)
{
bool lowThrottle = cmd->Throttle < 0;
/**
* do NOT check throttle if disarming via switch, must be instant
*/
switch (settings->Arming) {
case MANUALCONTROLSETTINGS_ARMING_ACCESSORY0:
case MANUALCONTROLSETTINGS_ARMING_ACCESSORY1:
case MANUALCONTROLSETTINGS_ARMING_ACCESSORY2:
if (armSwitch < 0) {
lowThrottle = true;
}
handler = &handler_STABILIZED;
break;
default:
case FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL:
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
case FLIGHTSTATUS_FLIGHTMODE_LAND:
case FLIGHTSTATUS_FLIGHTMODE_POI:
handler = &handler_PATHFOLLOWER;
break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
handler = &handler_PATHPLANNER;
break;
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO:
handler = &handler_ALTITUDE;
break;
case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE:
handler = &handler_AUTOTUNE;
break;
// There is no default, so if a flightmode is forgotten the compiler can throw a warning!
}
if (forcedDisArm()) {
// PathPlanner forces explicit disarming due to error condition (crash, impact, fire, ...)
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
return;
}
bool newinit = false;
if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) {
// In this configuration we always disarm
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
} else {
// Not really needed since this function not called when disconnected
if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) {
lowThrottle = true;
}
// FlightMode needs to be set correctly on first run (otherwise ControlChain is invalid)
static bool firstRun = true;
// The throttle is not low, in case we where arming or disarming, abort
if (!lowThrottle) {
switch (armState) {
case ARM_STATE_DISARMING_MANUAL:
case ARM_STATE_DISARMING_TIMEOUT:
armState = ARM_STATE_ARMED;
break;
case ARM_STATE_ARMING_MANUAL:
armState = ARM_STATE_DISARMED;
break;
default:
// Nothing needs to be done in the other states
break;
}
return;
}
// 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
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
return;
}
// When the configuration is not "Always armed" and no "Always disarmed",
// the state will not be changed when the throttle is not low
static portTickType armedDisarmStart;
float armingInputLevel = 0;
// Calc channel see assumptions7
switch (settings->Arming) {
case MANUALCONTROLSETTINGS_ARMING_ROLLLEFT:
armingInputLevel = 1.0f * cmd->Roll;
break;
case MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT:
armingInputLevel = -1.0f * cmd->Roll;
break;
case MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD:
armingInputLevel = 1.0f * cmd->Pitch;
break;
case MANUALCONTROLSETTINGS_ARMING_PITCHAFT:
armingInputLevel = -1.0f * cmd->Pitch;
break;
case MANUALCONTROLSETTINGS_ARMING_YAWLEFT:
armingInputLevel = 1.0f * cmd->Yaw;
break;
case MANUALCONTROLSETTINGS_ARMING_YAWRIGHT:
armingInputLevel = -1.0f * cmd->Yaw;
break;
case MANUALCONTROLSETTINGS_ARMING_ACCESSORY0:
case MANUALCONTROLSETTINGS_ARMING_ACCESSORY1:
case MANUALCONTROLSETTINGS_ARMING_ACCESSORY2:
armingInputLevel = -1.0f * (float)armSwitch;
break;
}
bool manualArm = false;
bool manualDisarm = false;
if (armingInputLevel <= -ARMED_THRESHOLD) {
manualArm = true;
} else if (armingInputLevel >= +ARMED_THRESHOLD) {
manualDisarm = true;
}
switch (armState) {
case ARM_STATE_DISARMED:
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
// only allow arming if it's OK too
if (manualArm && okToArm()) {
armedDisarmStart = lastSysTime;
armState = ARM_STATE_ARMING_MANUAL;
}
break;
case ARM_STATE_ARMING_MANUAL:
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING);
if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmingSequenceTime)) {
armState = ARM_STATE_ARMED;
} else if (!manualArm) {
armState = ARM_STATE_DISARMED;
}
break;
case ARM_STATE_ARMED:
// When we get here, the throttle is low,
// we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled
armedDisarmStart = lastSysTime;
armState = ARM_STATE_DISARMING_TIMEOUT;
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
break;
case ARM_STATE_DISARMING_TIMEOUT:
// We get here when armed while throttle low, even when the arming timeout is not enabled
if ((settings->ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmedTimeout)) {
armState = ARM_STATE_DISARMED;
}
// Switch to disarming due to manual control when needed
if (manualDisarm) {
armedDisarmStart = lastSysTime;
armState = ARM_STATE_DISARMING_MANUAL;
}
break;
case ARM_STATE_DISARMING_MANUAL:
if (manualDisarm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->DisarmingSequenceTime)) {
armState = ARM_STATE_DISARMED;
} else if (!manualDisarm) {
armState = ARM_STATE_ARMED;
}
break;
} // End Switch
}
}
/**
* @brief Determine which of N positions the flight mode switch is in and set flight mode accordingly
* @param[out] cmd Pointer to the command structure to set the flight mode in
* @param[in] settings The settings which indicate which position is which mode
* @param[in] flightMode the value of the switch position
*/
static void processFlightMode(ManualControlSettingsData *settings, float flightMode, ManualControlCommandData *cmd)
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
// Convert flightMode value into the switch position in the range [0..N-1]
uint8_t pos = ((int16_t)(flightMode * 256.0f) + 256) * settings->FlightModeNumber >> 9;
if (pos >= settings->FlightModeNumber) {
pos = settings->FlightModeNumber - 1;
}
cmd->FlightModeSwitchPosition = pos;
uint8_t newMode = settings->FlightModePosition[pos];
if (flightStatus.FlightMode != newMode) {
flightStatus.FlightMode = newMode;
if (flightStatus.FlightMode != newMode || firstRun) {
firstRun = false;
flightStatus.ControlChain = handler->controlChain;
flightStatus.FlightMode = newMode;
FlightStatusSet(&flightStatus);
newinit = true;
}
if (handler->handler) {
handler->handler(newinit);
}
}
/**
* @brief Determine if the manual input value is within acceptable limits
* @returns return TRUE if so, otherwise return FALSE
*/
bool validInputRange(int16_t min, int16_t max, uint16_t value)
{
if (min > max) {
int16_t tmp = min;
min = max;
max = tmp;
}
return value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET;
}
/**
* @brief Apply deadband to Roll/Pitch/Yaw channels
*/
static void applyDeadband(float *value, float deadband)
{
if (fabsf(*value) < deadband) {
*value = 0.0f;
} else if (*value > 0.0f) {
*value -= deadband;
} else {
*value += deadband;
}
}
#ifdef USE_INPUT_LPF
/**
* @brief Apply Low Pass Filter to Throttle/Roll/Pitch/Yaw or Accessory channel
*/
static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT)
{
if (cast_struct_to_array(settings->ResponseTime, settings->ResponseTime.Roll)[channel]) {
float rt = (float)cast_struct_to_array(settings->ResponseTime, settings->ResponseTime.Roll)[channel];
inputFiltered[channel] = ((rt * inputFiltered[channel]) + (dT * (*value))) / (rt + dT);
*value = inputFiltered[channel];
}
}
#endif // USE_INPUT_LPF
/**
* Called whenever a critical configuration component changes
*/
@ -1270,6 +247,14 @@ static void configurationUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
configuration_check();
}
/**
* Called whenever a critical configuration component changes
*/
static void commandUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
}
/**
* @}
* @}

71
flight/modules/ManualControl/manualhandler.c Normal file
View File

@ -0,0 +1,71 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup ManualControl
* @brief Interpretes the control input in ManualControlCommand
* @{
*
* @file manualhandler.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
*
* @see The GNU Public License (GPL) Version 3
*
******************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "inc/manualcontrol.h"
#include <manualcontrolcommand.h>
#include <actuatordesired.h>
// Private constants
// Private types
// Private functions
/**
* @brief Handler to control Manual flightmode - input directly steers actuators
* @input: ManualControlCommand
* @output: ActuatorDesired
*/
void manualHandler(bool newinit)
{
if (newinit) {
ActuatorDesiredInitialize();
}
ManualControlCommandData cmd;
ManualControlCommandGet(&cmd);
ActuatorDesiredData actuator;
ActuatorDesiredGet(&actuator);
actuator.Roll = cmd.Roll;
actuator.Pitch = cmd.Pitch;
actuator.Yaw = cmd.Yaw;
actuator.Thrust = cmd.Thrust;
ActuatorDesiredSet(&actuator);
}
/**
* @}
* @}
*/

131
flight/modules/ManualControl/pathfollowerhandler.c Normal file
View File

@ -0,0 +1,131 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup ManualControl
* @brief Interpretes the control input in ManualControlCommand
* @{
*
* @file pathfollowerhandler.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
*
* @see The GNU Public License (GPL) Version 3
*
******************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "inc/manualcontrol.h"
#include <pathdesired.h>
#include <manualcontrolcommand.h>
#include <flightstatus.h>
#include <positionstate.h>
#include <flightmodesettings.h>
#if defined(REVOLUTION)
// Private constants
// Private types
// Private functions
/**
* @brief Handler to control Guided flightmodes. FlightControl is governed by PathFollower, control via PathDesired
* @input: NONE: fully automated mode -- TODO recursively call handler for advanced stick commands
* @output: PathDesired
*/
void pathFollowerHandler(bool newinit)
{
if (newinit) {
PathDesiredInitialize();
PositionStateInitialize();
}
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if (newinit) {
// After not being in this mode for a while init at current height
PositionStateData positionState;
PositionStateGet(&positionState);
FlightModeSettingsData settings;
FlightModeSettingsGet(&settings);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
// Simple Return To Base mode - keep altitude the same, fly to home position
pathDesired.Start.North = 0;
pathDesired.Start.East = 0;
pathDesired.Start.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset;
pathDesired.End.North = 0;
pathDesired.End.East = 0;
pathDesired.End.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset;
pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
break;
default:
pathDesired.Start.North = positionState.North;
pathDesired.Start.East = positionState.East;
pathDesired.Start.Down = positionState.Down;
pathDesired.End.North = positionState.North;
pathDesired.End.East = positionState.East;
pathDesired.End.Down = positionState.Down;
pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
/* Disable this section, until such time as proper discussion can be had about how to implement it for all types of crafts.
} else {
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.End[PATHDESIRED_END_NORTH] += dT * -cmd->Pitch;
pathDesired.End[PATHDESIRED_END_EAST] += dT * cmd->Roll;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
*/
break;
}
PathDesiredSet(&pathDesired);
}
// special handling of autoland - behaves like positon hold but with slow altitude decrease
if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
PositionStateData positionState;
PositionStateGet(&positionState);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.End.Down = positionState.Down + 5;
PathDesiredSet(&pathDesired);
}
}
#else /* if defined(REVOLUTION) */
void pathFollowerHandler(__attribute__((unused)) bool newinit)
{
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); // should not be called
}
#endif // REVOLUTION
/**
* @}
* @}
*/

64
flight/modules/ManualControl/pathplannerhandler.c Normal file
View File

@ -0,0 +1,64 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup ManualControl
* @brief Interpretes the control input in ManualControlCommand
* @{
*
* @file pathplannerhandler.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
*
* @see The GNU Public License (GPL) Version 3
*
******************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "inc/manualcontrol.h"
// Private constants
// Private types
// Private functions
/**
* @brief Handler to control Navigated flightmodes. FlightControl is governed by PathFollower, controlled indirectly via PathPlanner
* @input: NONE: fully automated mode -- TODO recursively call handler for advanced stick commands to affect navigation
* @output: NONE
*/
void pathPlannerHandler(__attribute__((unused)) bool newinit)
{
/**
*
* TODO: In fully autonomous mode, commands to the navigation facility
* can be encoded through standard input cmd channels, as the pathplanner
* pathfollower generally ignores them
*
* this should be provided by a separate handler invoked here, as the
* handler for pathFollower should likely invoke them as well!!!
* (inputs also ignored)
*
*/
}
/**
* @}
* @}
*/

143
flight/modules/ManualControl/stabilizedhandler.c Normal file
View File

@ -0,0 +1,143 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup ManualControl
* @brief Interpretes the control input in ManualControlCommand
* @{
*
* @file stabilizedhandler.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
*
* @see The GNU Public License (GPL) Version 3
*
******************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "inc/manualcontrol.h"
#include <pios_struct_helper.h>
#include <manualcontrolcommand.h>
#include <stabilizationdesired.h>
#include <flightmodesettings.h>
#include <stabilizationbank.h>
// Private constants
// Private types
// Private functions
/**
* @brief Handler to control Stabilized flightmodes. FlightControl is governed by "Stabilization"
* @input: ManualControlCommand
* @output: StabilizationDesired
*/
void stabilizedHandler(bool newinit)
{
if (newinit) {
StabilizationDesiredInitialize();
StabilizationBankInitialize();
}
ManualControlCommandData cmd;
ManualControlCommandGet(&cmd);
FlightModeSettingsData settings;
FlightModeSettingsGet(&settings);
StabilizationDesiredData stabilization;
StabilizationDesiredGet(&stabilization);
StabilizationBankData stabSettings;
StabilizationBankGet(&stabSettings);
uint8_t *stab_settings;
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
stab_settings = cast_struct_to_array(settings.Stabilization1Settings, settings.Stabilization1Settings.Roll);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
stab_settings = cast_struct_to_array(settings.Stabilization2Settings, settings.Stabilization2Settings.Roll);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
stab_settings = cast_struct_to_array(settings.Stabilization3Settings, settings.Stabilization3Settings.Roll);
break;
default:
// Major error, this should not occur because only enter this block when one of these is true
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
stab_settings = cast_struct_to_array(settings.Stabilization1Settings, settings.Stabilization1Settings.Roll);
return;
}
stabilization.Roll =
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Roll * stabSettings.RollMax :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd.Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd.Roll * stabSettings.RollMax :
0; // this is an invalid mode
stabilization.Pitch =
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Pitch * stabSettings.PitchMax :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd.Pitch * stabSettings.PitchMax :
0; // this is an invalid mode
// TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
stabilization.StabilizationMode.Roll = stab_settings[0];
stabilization.StabilizationMode.Pitch = stab_settings[1];
// Other axes (yaw) cannot be Rattitude, so use Rate
// Should really do this for Attitude mode as well?
if (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) {
stabilization.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabilization.Yaw = cmd.Yaw * stabSettings.ManualRate.Yaw;
} else {
stabilization.StabilizationMode.Yaw = stab_settings[2];
stabilization.Yaw =
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Yaw * stabSettings.YawMax :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd.Yaw * stabSettings.YawMax :
0; // this is an invalid mode
}
stabilization.Thrust = cmd.Thrust;
StabilizationDesiredSet(&stabilization);
}
/**
* @}
* @}
*/

14
flight/modules/OPLink/oplinkmod.c
View File

@ -63,8 +63,6 @@
// Private types
// Private variables
static uint32_t idleCounter;
static uint32_t idleCounterClear;
static xTaskHandle systemTaskHandle;
static bool stackOverflow;
static bool mallocFailed;
@ -130,8 +128,6 @@ static void systemTask(__attribute__((unused)) void *parameters)
}
// Initialize vars
idleCounter = 0;
idleCounterClear = 0;
lastSysTime = xTaskGetTickCount();
// Main system loop
@ -205,15 +201,7 @@ static void systemTask(__attribute__((unused)) void *parameters)
* Called by the RTOS when the CPU is idle, used to measure the CPU idle time.
*/
void vApplicationIdleHook(void)
{
// Called when the scheduler has no tasks to run
if (idleCounterClear == 0) {
++idleCounter;
} else {
idleCounter = 0;
idleCounterClear = 0;
}
}
{}
/**
* Called by the RTOS when a stack overflow is detected.

8
flight/modules/PathPlanner/pathplanner.c
View File

@ -42,7 +42,7 @@
#include "velocitystate.h"
#include "waypoint.h"
#include "waypointactive.h"
#include "manualcontrolsettings.h"
#include "flightmodesettings.h"
#include <pios_struct_helper.h>
#include "paths.h"
@ -139,7 +139,7 @@ static void pathPlannerTask()
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) {
if (flightStatus.ControlChain.PathPlanner != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
pathplanner_active = false;
if (!validPathPlan) {
// unverified path plans are only a warning while we are not in pathplanner mode
@ -171,8 +171,8 @@ static void pathPlannerTask()
// copy pasta: same calculation as in manualcontrol, set return to home coordinates
PositionStateData positionState;
PositionStateGet(&positionState);
ManualControlSettingsData settings;
ManualControlSettingsGet(&settings);
FlightModeSettingsData settings;
FlightModeSettingsGet(&settings);
pathDesired.Start.North = 0;
pathDesired.Start.East = 0;

56
flight/modules/RadioComBridge/RadioComBridge.c
View File

@ -268,6 +268,7 @@ static void registerObject(UAVObjHandle obj)
.obj = obj,
.instId = UAVOBJ_ALL_INSTANCES,
.event = EV_UPDATED_PERIODIC,
.lowPriority = true,
};
// Get metadata
@ -289,34 +290,33 @@ static void updateRadioComBridgeStats()
RadioComBridgeStatsData radioComBridgeStats;
// Get telemetry stats
UAVTalkGetStats(data->telemUAVTalkCon, &telemetryUAVTalkStats);
UAVTalkResetStats(data->telemUAVTalkCon);
UAVTalkGetStats(data->telemUAVTalkCon, &telemetryUAVTalkStats, true);
// Get radio stats
UAVTalkGetStats(data->radioUAVTalkCon, &radioUAVTalkStats);
UAVTalkResetStats(data->radioUAVTalkCon);
UAVTalkGetStats(data->radioUAVTalkCon, &radioUAVTalkStats, true);
// Get stats object data
RadioComBridgeStatsGet(&radioComBridgeStats);
radioComBridgeStats.TelemetryTxRetries = data->telemetryTxRetries;
radioComBridgeStats.RadioTxRetries = data->radioTxRetries;
// Update stats object
radioComBridgeStats.TelemetryTxBytes += telemetryUAVTalkStats.txBytes;
radioComBridgeStats.TelemetryTxFailures += telemetryUAVTalkStats.txErrors;
radioComBridgeStats.TelemetryTxRetries += data->telemetryTxRetries;
radioComBridgeStats.TelemetryRxBytes += telemetryUAVTalkStats.rxBytes;
radioComBridgeStats.TelemetryRxFailures += telemetryUAVTalkStats.rxErrors;
radioComBridgeStats.TelemetryRxSyncErrors += telemetryUAVTalkStats.rxSyncErrors;
radioComBridgeStats.TelemetryRxCrcErrors += telemetryUAVTalkStats.rxCrcErrors;
radioComBridgeStats.RadioTxBytes += radioUAVTalkStats.txBytes;
radioComBridgeStats.RadioTxFailures += radioUAVTalkStats.txErrors;
radioComBridgeStats.RadioTxRetries += data->radioTxRetries;
radioComBridgeStats.RadioTxBytes += radioUAVTalkStats.txBytes;
radioComBridgeStats.RadioTxFailures += radioUAVTalkStats.txErrors;
radioComBridgeStats.RadioRxBytes += radioUAVTalkStats.rxBytes;
radioComBridgeStats.RadioRxFailures += radioUAVTalkStats.rxErrors;
radioComBridgeStats.RadioRxSyncErrors += radioUAVTalkStats.rxSyncErrors;
radioComBridgeStats.RadioRxCrcErrors += radioUAVTalkStats.rxCrcErrors;
radioComBridgeStats.RadioRxBytes += radioUAVTalkStats.rxBytes;
radioComBridgeStats.RadioRxFailures += radioUAVTalkStats.rxErrors;
radioComBridgeStats.RadioRxSyncErrors += radioUAVTalkStats.rxSyncErrors;
radioComBridgeStats.RadioRxCrcErrors += radioUAVTalkStats.rxCrcErrors;
// Update stats object data
RadioComBridgeStatsSet(&radioComBridgeStats);
@ -342,11 +342,11 @@ static void telemetryTxTask(__attribute__((unused)) void *parameters)
updateRadioComBridgeStats();
}
// Send update (with retries)
int32_t ret = -1;
uint32_t retries = 0;
int32_t success = -1;
while (retries < MAX_RETRIES && success == -1) {
success = UAVTalkSendObject(data->telemUAVTalkCon, ev.obj, ev.instId, 0, RETRY_TIMEOUT_MS) == 0;
if (success == -1) {
while (retries <= MAX_RETRIES && ret == -1) {
ret = UAVTalkSendObject(data->telemUAVTalkCon, ev.obj, ev.instId, 0, RETRY_TIMEOUT_MS);
if (ret == -1) {
++retries;
}
}
@ -376,11 +376,11 @@ static void radioTxTask(__attribute__((unused)) void *parameters)
if (xQueueReceive(data->radioEventQueue, &ev, MAX_PORT_DELAY) == pdTRUE) {
if ((ev.event == EV_UPDATED) || (ev.event == EV_UPDATE_REQ)) {
// Send update (with retries)
int32_t ret = -1;
uint32_t retries = 0;
int32_t success = -1;
while (retries < MAX_RETRIES && success == -1) {
success = UAVTalkSendObject(data->radioUAVTalkCon, ev.obj, ev.instId, 0, RETRY_TIMEOUT_MS) == 0;
if (success == -1) {
while (retries <= MAX_RETRIES && ret == -1) {
ret = UAVTalkSendObject(data->radioUAVTalkCon, ev.obj, ev.instId, 0, RETRY_TIMEOUT_MS);
if (ret == -1) {
++retries;
}
}
@ -413,8 +413,8 @@ static void radioRxTask(__attribute__((unused)) void *parameters)
}
} else if (PIOS_COM_TELEMETRY) {
// Send the data straight to the telemetry port.
// FIXME following call can fail (with -2 error code) if buffer is full
// it is the caller responsibility to retry in such cases...
// Following call can fail with -2 error code (buffer full) or -3 error code (could not acquire send mutex)
// It is the caller responsibility to retry in such cases...
int32_t ret = -2;
uint8_t count = 5;
while (count-- > 0 && ret < -1) {
@ -513,8 +513,8 @@ static void serialRxTask(__attribute__((unused)) void *parameters)
if (bytes_to_process > 0) {
// Send the data over the radio link.
// FIXME following call can fail (with -2 error code) if buffer is full
// it is the caller responsibility to retry in such cases...
// Following call can fail with -2 error code (buffer full) or -3 error code (could not acquire send mutex)
// It is the caller responsibility to retry in such cases...
int32_t ret = -2;
uint8_t count = 5;
while (count-- > 0 && ret < -1) {
@ -547,8 +547,8 @@ static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length)
}
#endif /* PIOS_INCLUDE_USB */
if (outputPort) {
// FIXME following call can fail (with -2 error code) if buffer is full
// it is the caller responsibility to retry in such cases...
// Following call can fail with -2 error code (buffer full) or -3 error code (could not acquire send mutex)
// It is the caller responsibility to retry in such cases...
ret = -2;
uint8_t count = 5;
while (count-- > 0 && ret < -1) {
@ -577,8 +577,8 @@ static int32_t RadioSendHandler(uint8_t *buf, int32_t length)
// Don't send any data unless the radio port is available.
if (outputPort && PIOS_COM_Available(outputPort)) {
// FIXME following call can fail (with -2 error code) if buffer is full
// it is the caller responsibility to retry in such cases...
// Following call can fail with -2 error code (buffer full) or -3 error code (could not acquire send mutex)
// It is the caller responsibility to retry in such cases...
int32_t ret = -2;
uint8_t count = 5;
while (count-- > 0 && ret < -1) {

673
flight/modules/Receiver/receiver.c Normal file
View File

@ -0,0 +1,673 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup ReceiverModule Manual Control Module
* @brief Provide manual control or allow it alter flight mode.
* @{
*
* Reads in the ManualControlCommand from receiver then
* pass it to ManualControl
*
* @file receiver.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Receiver module. Handles safety R/C link and flight mode.
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include <openpilot.h>
#include <pios_struct_helper.h>
#include <accessorydesired.h>
#include <manualcontrolsettings.h>
#include <manualcontrolcommand.h>
#include <receiveractivity.h>
#include <flightstatus.h>
#include <flighttelemetrystats.h>
#include <flightmodesettings.h>
#include <systemsettings.h>
#include <taskinfo.h>
#if defined(PIOS_INCLUDE_USB_RCTX)
#include "pios_usb_rctx.h"
#endif /* PIOS_INCLUDE_USB_RCTX */
// Private constants
#if defined(PIOS_RECEIVER_STACK_SIZE)
#define STACK_SIZE_BYTES PIOS_RECEIVER_STACK_SIZE
#else
#define STACK_SIZE_BYTES 1152
#endif
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) // 3 = flight control
#define UPDATE_PERIOD_MS 20
#define THROTTLE_FAILSAFE -0.1f
#define ARMED_THRESHOLD 0.50f
// safe band to allow a bit of calibration error or trim offset (in microseconds)
#define CONNECTION_OFFSET 250
// Private types
// Private variables
static xTaskHandle taskHandle;
static portTickType lastSysTime;
#ifdef USE_INPUT_LPF
static portTickType lastSysTimeLPF;
static float inputFiltered[MANUALCONTROLSETTINGS_RESPONSETIME_NUMELEM];
#endif
// Private functions
static void receiverTask(void *parameters);
static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral);
static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time);
static bool validInputRange(int16_t min, int16_t max, uint16_t value);
static void applyDeadband(float *value, float deadband);
#ifdef USE_INPUT_LPF
static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT);
#endif
#define RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP 12
#define RCVR_ACTIVITY_MONITOR_MIN_RANGE 10
struct rcvr_activity_fsm {
ManualControlSettingsChannelGroupsOptions group;
uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP];
uint8_t sample_count;
};
static struct rcvr_activity_fsm activity_fsm;
static void resetRcvrActivity(struct rcvr_activity_fsm *fsm);
static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm);
#define assumptions \
( \
((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM) && \
((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELNUMBER_NUMELEM) && \
((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELMIN_NUMELEM) && \
((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELMAX_NUMELEM) && \
((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELNEUTRAL_NUMELEM))
/**
* Module starting
*/
int32_t ReceiverStart()
{
// Start main task
xTaskCreate(receiverTask, (signed char *)"Receiver", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_RECEIVER, taskHandle);
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL);
#endif
return 0;
}
/**
* Module initialization
*/
int32_t ReceiverInitialize()
{
/* Check the assumptions about uavobject enum's are correct */
PIOS_STATIC_ASSERT(assumptions);
ManualControlCommandInitialize();
ReceiverActivityInitialize();
ManualControlSettingsInitialize();
return 0;
}
MODULE_INITCALL(ReceiverInitialize, ReceiverStart);
/**
* Module task
*/
static void receiverTask(__attribute__((unused)) void *parameters)
{
ManualControlSettingsData settings;
ManualControlCommandData cmd;
FlightStatusData flightStatus;
uint8_t disconnected_count = 0;
uint8_t connected_count = 0;
// For now manual instantiate extra instances of Accessory Desired. In future should be done dynamically
// this includes not even registering it if not used
AccessoryDesiredCreateInstance();
AccessoryDesiredCreateInstance();
// Whenever the configuration changes, make sure it is safe to fly
ManualControlCommandGet(&cmd);
FlightStatusGet(&flightStatus);
/* Initialize the RcvrActivty FSM */
portTickType lastActivityTime = xTaskGetTickCount();
resetRcvrActivity(&activity_fsm);
// Main task loop
lastSysTime = xTaskGetTickCount();
float scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM] = { 0 };
SystemSettingsThrustControlOptions thrustType;
while (1) {
// Wait until next update
vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS);
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL);
#endif
// Read settings
ManualControlSettingsGet(&settings);
SystemSettingsThrustControlGet(&thrustType);
/* Update channel activity monitor */
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) {
if (updateRcvrActivity(&activity_fsm)) {
/* Reset the aging timer because activity was detected */
lastActivityTime = lastSysTime;
}
}
if (timeDifferenceMs(lastActivityTime, lastSysTime) > 5000) {
resetRcvrActivity(&activity_fsm);
lastActivityTime = lastSysTime;
}
if (ManualControlCommandReadOnly()) {
FlightTelemetryStatsData flightTelemStats;
FlightTelemetryStatsGet(&flightTelemStats);
if (flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
/* trying to fly via GCS and lost connection. fall back to transmitter */
UAVObjMetadata metadata;
ManualControlCommandGetMetadata(&metadata);
UAVObjSetAccess(&metadata, ACCESS_READWRITE);
ManualControlCommandSetMetadata(&metadata);
}
}
bool valid_input_detected = true;
// Read channel values in us
for (uint8_t n = 0; n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) {
extern uint32_t pios_rcvr_group_map[];
if (cast_struct_to_array(settings.ChannelGroups, settings.ChannelGroups.Roll)[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
cmd.Channel[n] = PIOS_RCVR_INVALID;
} else {
cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[
cast_struct_to_array(settings.ChannelGroups, settings.ChannelGroups.Pitch)[n]],
cast_struct_to_array(settings.ChannelNumber, settings.ChannelNumber.Pitch)[n]);
}
// If a channel has timed out this is not valid data and we shouldn't update anything
// until we decide to go to failsafe
if (cmd.Channel[n] == (uint16_t)PIOS_RCVR_TIMEOUT) {
valid_input_detected = false;
} else {
scaledChannel[n] = scaleChannel(cmd.Channel[n],
cast_struct_to_array(settings.ChannelMax, settings.ChannelMax.Pitch)[n],
cast_struct_to_array(settings.ChannelMin, settings.ChannelMin.Pitch)[n],
cast_struct_to_array(settings.ChannelNeutral, settings.ChannelNeutral.Pitch)[n]);
}
}
// Check settings, if error raise alarm
if (settings.ChannelGroups.Roll >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups.Pitch >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups.Yaw >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups.Throttle >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
||
// Check all channel mappings are valid
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t)PIOS_RCVR_INVALID
||
// Check the driver exists
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_NODRIVER
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_NODRIVER
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_NODRIVER
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t)PIOS_RCVR_NODRIVER
||
// Check collective if required
(thrustType == SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE && (
settings.ChannelGroups.Collective >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] == (uint16_t)PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] == (uint16_t)PIOS_RCVR_NODRIVER))
||
// Check the FlightModeNumber is valid
settings.FlightModeNumber < 1 || settings.FlightModeNumber > FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM
||
// Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care
((settings.FlightModeNumber > 1)
&& (settings.ChannelGroups.FlightMode >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_NODRIVER))) {
AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_CRITICAL);
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
ManualControlCommandSet(&cmd);
continue;
}
// decide if we have valid manual input or not
valid_input_detected &= validInputRange(settings.ChannelMin.Throttle,
settings.ChannelMax.Throttle, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE])
&& validInputRange(settings.ChannelMin.Roll,
settings.ChannelMax.Roll, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL])
&& validInputRange(settings.ChannelMin.Yaw,
settings.ChannelMax.Yaw, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW])
&& validInputRange(settings.ChannelMin.Pitch,
settings.ChannelMax.Pitch, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]);
if (settings.ChannelGroups.Collective != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
valid_input_detected &= validInputRange(settings.ChannelMin.Collective,
settings.ChannelMax.Collective, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE]);
}
if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
valid_input_detected &= validInputRange(settings.ChannelMin.Accessory0,
settings.ChannelMax.Accessory0, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0]);
}
if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
valid_input_detected &= validInputRange(settings.ChannelMin.Accessory1,
settings.ChannelMax.Accessory1, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1]);
}
if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
valid_input_detected &= validInputRange(settings.ChannelMin.Accessory2,
settings.ChannelMax.Accessory2, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2]);
}
// Implement hysteresis loop on connection status
if (valid_input_detected && (++connected_count > 10)) {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
connected_count = 0;
disconnected_count = 0;
} else if (!valid_input_detected && (++disconnected_count > 10)) {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
connected_count = 0;
disconnected_count = 0;
}
if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) {
cmd.Throttle = settings.FailsafeChannel.Throttle;
cmd.Roll = settings.FailsafeChannel.Roll;
cmd.Pitch = settings.FailsafeChannel.Pitch;
cmd.Yaw = settings.FailsafeChannel.Yaw;
cmd.Collective = settings.FailsafeChannel.Collective;
switch (thrustType) {
case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE:
cmd.Thrust = cmd.Throttle;
break;
case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE:
cmd.Thrust = cmd.Collective;
break;
default:
break;
}
if (settings.FailsafeFlightModeSwitchPosition >= 0 && settings.FailsafeFlightModeSwitchPosition < settings.FlightModeNumber) {
cmd.FlightModeSwitchPosition = (uint8_t)settings.FailsafeFlightModeSwitchPosition;
}
AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
AccessoryDesiredData accessory;
// Set Accessory 0
if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = settings.FailsafeChannel.Accessory0;
if (AccessoryDesiredInstSet(0, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
}
}
// Set Accessory 1
if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = settings.FailsafeChannel.Accessory1;
if (AccessoryDesiredInstSet(1, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
}
}
// Set Accessory 2
if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = settings.FailsafeChannel.Accessory2;
if (AccessoryDesiredInstSet(2, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
}
}
} else if (valid_input_detected) {
AlarmsClear(SYSTEMALARMS_ALARM_RECEIVER);
// Scale channels to -1 -> +1 range
cmd.Roll = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL];
cmd.Pitch = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH];
cmd.Yaw = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW];
cmd.Throttle = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE];
// Convert flightMode value into the switch position in the range [0..N-1]
cmd.FlightModeSwitchPosition = ((int16_t)(scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] * 256.0f) + 256) * settings.FlightModeNumber >> 9;
if (cmd.FlightModeSwitchPosition >= settings.FlightModeNumber) {
cmd.FlightModeSwitchPosition = settings.FlightModeNumber - 1;
}
// Apply deadband for Roll/Pitch/Yaw stick inputs
if (settings.Deadband > 0.0f) {
applyDeadband(&cmd.Roll, settings.Deadband);
applyDeadband(&cmd.Pitch, settings.Deadband);
applyDeadband(&cmd.Yaw, settings.Deadband);
}
#ifdef USE_INPUT_LPF
// Apply Low Pass Filter to input channels, time delta between calls in ms
portTickType thisSysTime = xTaskGetTickCount();
float dT = (thisSysTime > lastSysTimeLPF) ?
(float)((thisSysTime - lastSysTimeLPF) * portTICK_RATE_MS) :
(float)UPDATE_PERIOD_MS;
lastSysTimeLPF = thisSysTime;
applyLPF(&cmd.Roll, MANUALCONTROLSETTINGS_RESPONSETIME_ROLL, &settings, dT);
applyLPF(&cmd.Pitch, MANUALCONTROLSETTINGS_RESPONSETIME_PITCH, &settings, dT);
applyLPF(&cmd.Yaw, MANUALCONTROLSETTINGS_RESPONSETIME_YAW, &settings, dT);
#endif // USE_INPUT_LPF
if (cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_INVALID
&& cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_NODRIVER
&& cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_TIMEOUT) {
cmd.Collective = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE];
if (settings.Deadband > 0.0f) {
applyDeadband(&cmd.Collective, settings.Deadband);
}
#ifdef USE_INPUT_LPF
applyLPF(&cmd.Collective, MANUALCONTROLSETTINGS_RESPONSETIME_COLLECTIVE, &settings, dT);
#endif // USE_INPUT_LPF
}
switch (thrustType) {
case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE:
cmd.Thrust = cmd.Throttle;
break;
case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE:
cmd.Thrust = cmd.Collective;
break;
default:
break;
}
AccessoryDesiredData accessory;
// Set Accessory 0
if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0];
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT);
#endif
if (AccessoryDesiredInstSet(0, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
}
}
// Set Accessory 1
if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1];
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT);
#endif
if (AccessoryDesiredInstSet(1, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
}
}
// Set Accessory 2
if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2];
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT);
#endif
if (AccessoryDesiredInstSet(2, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
}
}
}
// Update cmd object
ManualControlCommandSet(&cmd);
#if defined(PIOS_INCLUDE_USB_RCTX)
if (pios_usb_rctx_id) {
PIOS_USB_RCTX_Update(pios_usb_rctx_id,
cmd.Channel,
cast_struct_to_array(settings.ChannelMin, settings.ChannelMin.Roll),
cast_struct_to_array(settings.ChannelMax, settings.ChannelMax.Roll),
NELEMENTS(cmd.Channel));
}
#endif /* PIOS_INCLUDE_USB_RCTX */
}
}
static void resetRcvrActivity(struct rcvr_activity_fsm *fsm)
{
ReceiverActivityData data;
bool updated = false;
/* Clear all channel activity flags */
ReceiverActivityGet(&data);
if (data.ActiveGroup != RECEIVERACTIVITY_ACTIVEGROUP_NONE && data.ActiveChannel != 255) {
data.ActiveGroup = RECEIVERACTIVITY_ACTIVEGROUP_NONE;
data.ActiveChannel = 255;
updated = true;
}
if (updated) {
ReceiverActivitySet(&data);
}
/* Reset the FSM state */
fsm->group = 0;
fsm->sample_count = 0;
}
static void updateRcvrActivitySample(uint32_t rcvr_id, uint16_t samples[], uint8_t max_channels)
{
for (uint8_t channel = 1; channel <= max_channels; channel++) {
// Subtract 1 because channels are 1 indexed
samples[channel - 1] = PIOS_RCVR_Read(rcvr_id, channel);
}
}
static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm *fsm)
{
bool activity_updated = false;
/* Compare the current value to the previous sampled value */
for (uint8_t channel = 1; channel <= RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP; channel++) {
uint16_t delta;
uint16_t prev = fsm->prev[channel - 1]; // Subtract 1 because channels are 1 indexed
uint16_t curr = PIOS_RCVR_Read(rcvr_id, channel);
if (curr > prev) {
delta = curr - prev;
} else {
delta = prev - curr;
}
if (delta > RCVR_ACTIVITY_MONITOR_MIN_RANGE) {
/* Mark this channel as active */
ReceiverActivityActiveGroupOptions group;
/* Don't assume manualcontrolsettings and receiveractivity are in the same order. */
switch (fsm->group) {
case MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM:
group = RECEIVERACTIVITY_ACTIVEGROUP_PWM;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM:
group = RECEIVERACTIVITY_ACTIVEGROUP_PPM;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT:
group = RECEIVERACTIVITY_ACTIVEGROUP_DSMMAINPORT;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT:
group = RECEIVERACTIVITY_ACTIVEGROUP_DSMFLEXIPORT;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS:
group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS:
group = RECEIVERACTIVITY_ACTIVEGROUP_GCS;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK:
group = RECEIVERACTIVITY_ACTIVEGROUP_OPLINK;
break;
default:
PIOS_Assert(0);
break;
}
ReceiverActivityActiveGroupSet((uint8_t *)&group);
ReceiverActivityActiveChannelSet(&channel);
activity_updated = true;
}
}
return activity_updated;
}
static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm)
{
bool activity_updated = false;
if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
/* We're out of range, reset things */
resetRcvrActivity(fsm);
}
extern uint32_t pios_rcvr_group_map[];
if (!pios_rcvr_group_map[fsm->group]) {
/* Unbound group, skip it */
goto group_completed;
}
if (fsm->sample_count == 0) {
/* Take a sample of each channel in this group */
updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], fsm->prev, NELEMENTS(fsm->prev));
fsm->sample_count++;
return false;
}
/* Compare with previous sample */
activity_updated = updateRcvrActivityCompare(pios_rcvr_group_map[fsm->group], fsm);
group_completed:
/* Reset the sample counter */
fsm->sample_count = 0;
/* Find the next active group, but limit search so we can't loop forever here */
for (uint8_t i = 0; i < MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE; i++) {
/* Move to the next group */
fsm->group++;
if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
/* Wrap back to the first group */
fsm->group = 0;
}
if (pios_rcvr_group_map[fsm->group]) {
/*
* Found an active group, take a sample here to avoid an
* extra 20ms delay in the main thread so we can speed up
* this algorithm.
*/
updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], fsm->prev, NELEMENTS(fsm->prev));
fsm->sample_count++;
break;
}
}
return activity_updated;
}
/**
* Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range.
*/
static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral)
{
float valueScaled;
// Scale
if ((max > min && value >= neutral) || (min > max && value <= neutral)) {
if (max != neutral) {
valueScaled = (float)(value - neutral) / (float)(max - neutral);
} else {
valueScaled = 0;
}
} else {
if (min != neutral) {
valueScaled = (float)(value - neutral) / (float)(neutral - min);
} else {
valueScaled = 0;
}
}
// Bound
if (valueScaled > 1.0f) {
valueScaled = 1.0f;
} else if (valueScaled < -1.0f) {
valueScaled = -1.0f;
}
return valueScaled;
}
static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time)
{
return (end_time - start_time) * portTICK_RATE_MS;
}
/**
* @brief Determine if the manual input value is within acceptable limits
* @returns return TRUE if so, otherwise return FALSE
*/
bool validInputRange(int16_t min, int16_t max, uint16_t value)
{
if (min > max) {
int16_t tmp = min;
min = max;
max = tmp;
}
return value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET;
}
/**
* @brief Apply deadband to Roll/Pitch/Yaw channels
*/
static void applyDeadband(float *value, float deadband)
{
if (fabsf(*value) < deadband) {
*value = 0.0f;
} else if (*value > 0.0f) {
*value -= deadband;
} else {
*value += deadband;
}
}
#ifdef USE_INPUT_LPF
/**
* @brief Apply Low Pass Filter to Throttle/Roll/Pitch/Yaw or Accessory channel
*/
static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT)
{
if (cast_struct_to_array(settings->ResponseTime, settings->ResponseTime.Roll)[channel]) {
float rt = (float)cast_struct_to_array(settings->ResponseTime, settings->ResponseTime.Roll)[channel];
inputFiltered[channel] = ((rt * inputFiltered[channel]) + (dT * (*value))) / (rt + dT);
*value = inputFiltered[channel];
}
}
#endif // USE_INPUT_LPF
/**
* @}
* @}
*/

91
flight/modules/Sensors/sensors.c
View File

@ -55,6 +55,7 @@
#include <attitudestate.h>
#include <attitudesettings.h>
#include <revocalibration.h>
#include <accelgyrosettings.h>
#include <flightstatus.h>
#include <taskinfo.h>
@ -78,16 +79,15 @@ static void settingsUpdatedCb(UAVObjEvent *objEv);
// Private variables
static xTaskHandle sensorsTaskHandle;
RevoCalibrationData cal;
AccelGyroSettingsData agcal;
// These values are initialized by settings but can be updated by the attitude algorithm
static float mag_bias[3] = { 0, 0, 0 };
static float mag_scale[3] = { 0, 0, 0 };
static float accel_bias[3] = { 0, 0, 0 };
static float accel_scale[3] = { 0, 0, 0 };
static float gyro_staticbias[3] = { 0, 0, 0 };
static float gyro_scale[3] = { 0, 0, 0 };
// temp coefficient to calculate gyro bias
static volatile bool gyro_temp_calibrated = false;
static volatile bool accel_temp_calibrated = false;
static float R[3][3] = {
{ 0 }
};
@ -113,12 +113,13 @@ int32_t SensorsInitialize(void)
MagSensorInitialize();
RevoCalibrationInitialize();
AttitudeSettingsInitialize();
AccelGyroSettingsInitialize();
rotate = 0;
RevoCalibrationConnectCallback(&settingsUpdatedCb);
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
AccelGyroSettingsConnectCallback(&settingsUpdatedCb);
return 0;
}
@ -341,12 +342,24 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
}
// Scale the accels
float accels[3] = { (float)accel_accum[0] / accel_samples,
(float)accel_accum[1] / accel_samples,
(float)accel_accum[2] / accel_samples };
float accels_out[3] = { accels[0] * accel_scaling * accel_scale[0] - accel_bias[0],
accels[1] * accel_scaling * accel_scale[1] - accel_bias[1],
accels[2] * accel_scaling * accel_scale[2] - accel_bias[2] };
float accels[3] = { (float)accel_accum[0] / accel_samples,
(float)accel_accum[1] / accel_samples,
(float)accel_accum[2] / accel_samples };
float accels_out[3] = { accels[0] * accel_scaling * agcal.accel_scale.X - agcal.accel_bias.X,
accels[1] * accel_scaling * agcal.accel_scale.Y - agcal.accel_bias.Y,
accels[2] * accel_scaling * agcal.accel_scale.Z - agcal.accel_bias.Z };
if (accel_temp_calibrated) {
float ctemp = accelSensorData.temperature > agcal.temp_calibrated_extent.max ? agcal.temp_calibrated_extent.max :
(accelSensorData.temperature < agcal.temp_calibrated_extent.min ? agcal.temp_calibrated_extent.min
: accelSensorData.temperature);
accels_out[0] -= agcal.accel_temp_coeff.X * ctemp;
accels_out[1] -= agcal.accel_temp_coeff.Y * ctemp;
accels_out[2] -= agcal.accel_temp_coeff.Z * ctemp;
}
if (rotate) {
rot_mult(R, accels_out, accels);
accelSensorData.x = accels[0];
@ -360,12 +373,23 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
AccelSensorSet(&accelSensorData);
// Scale the gyros
float gyros[3] = { (float)gyro_accum[0] / gyro_samples,
(float)gyro_accum[1] / gyro_samples,
(float)gyro_accum[2] / gyro_samples };
float gyros_out[3] = { gyros[0] * gyro_scaling * gyro_scale[0] - gyro_staticbias[0],
gyros[1] * gyro_scaling * gyro_scale[1] - gyro_staticbias[1],
gyros[2] * gyro_scaling * gyro_scale[2] - gyro_staticbias[2] };
float gyros[3] = { (float)gyro_accum[0] / gyro_samples,
(float)gyro_accum[1] / gyro_samples,
(float)gyro_accum[2] / gyro_samples };
float gyros_out[3] = { gyros[0] * gyro_scaling * agcal.gyro_scale.X - agcal.gyro_bias.X,
gyros[1] * gyro_scaling * agcal.gyro_scale.Y - agcal.gyro_bias.Y,
gyros[2] * gyro_scaling * agcal.gyro_scale.Z - agcal.gyro_bias.Z };
if (gyro_temp_calibrated) {
float ctemp = gyroSensorData.temperature > agcal.temp_calibrated_extent.max ? agcal.temp_calibrated_extent.max :
(gyroSensorData.temperature < agcal.temp_calibrated_extent.min ? agcal.temp_calibrated_extent.min
: gyroSensorData.temperature);
gyros_out[0] -= (agcal.gyro_temp_coeff.X + agcal.gyro_temp_coeff.X2 * ctemp) * ctemp;
gyros_out[1] -= (agcal.gyro_temp_coeff.Y + agcal.gyro_temp_coeff.Y2 * ctemp) * ctemp;
gyros_out[2] -= (agcal.gyro_temp_coeff.Z + agcal.gyro_temp_coeff.Z2 * ctemp) * ctemp;
}
if (rotate) {
rot_mult(R, gyros_out, gyros);
gyroSensorData.x = gyros[0];
@ -421,25 +445,20 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
{
RevoCalibrationGet(&cal);
AccelGyroSettingsGet(&agcal);
mag_bias[0] = cal.mag_bias.X;
mag_bias[1] = cal.mag_bias.Y;
mag_bias[2] = cal.mag_bias.Z;
mag_scale[0] = cal.mag_scale.X;
mag_scale[1] = cal.mag_scale.Y;
mag_scale[2] = cal.mag_scale.Z;
mag_bias[0] = cal.mag_bias.X;
mag_bias[1] = cal.mag_bias.Y;
mag_bias[2] = cal.mag_bias.Z;
mag_scale[0] = cal.mag_scale.X;
mag_scale[1] = cal.mag_scale.Y;
mag_scale[2] = cal.mag_scale.Z;
accel_bias[0] = cal.accel_bias.X;
accel_bias[1] = cal.accel_bias.Y;
accel_bias[2] = cal.accel_bias.Z;
accel_scale[0] = cal.accel_scale.X;
accel_scale[1] = cal.accel_scale.Y;
accel_scale[2] = cal.accel_scale.Z;
gyro_staticbias[0] = cal.gyro_bias.X;
gyro_staticbias[1] = cal.gyro_bias.Y;
gyro_staticbias[2] = cal.gyro_bias.Z;
gyro_scale[0] = cal.gyro_scale.X;
gyro_scale[1] = cal.gyro_scale.Y;
gyro_scale[2] = cal.gyro_scale.Z;
accel_temp_calibrated = (agcal.temp_calibrated_extent.max - agcal.temp_calibrated_extent.min > .1f) &&
(fabsf(agcal.accel_temp_coeff.X) > 1e-9f || fabsf(agcal.accel_temp_coeff.Y) > 1e-9f || fabsf(agcal.accel_temp_coeff.Z) > 1e-9f);
gyro_temp_calibrated = (agcal.temp_calibrated_extent.max - agcal.temp_calibrated_extent.min > .1f) &&
(fabsf(agcal.gyro_temp_coeff.X) > 1e-9f || fabsf(agcal.gyro_temp_coeff.Y) > 1e-9f ||
fabsf(agcal.gyro_temp_coeff.Z) > 1e-9f || fabsf(agcal.gyro_temp_coeff.Z2) > 1e-9f);
AttitudeSettingsData attitudeSettings;

265
flight/modules/Stabilization/stabilization.c
View File

@ -49,7 +49,8 @@
#include "gyrostate.h"
#include "flightstatus.h"
#include "manualcontrolsettings.h"
#include "manualcontrol.h" // Just to get a macro
#include "manualcontrolcommand.h"
#include "flightmodesettings.h"
#include "taskinfo.h"
// Math libraries
@ -75,20 +76,15 @@
#if defined(PIOS_STABILIZATION_STACK_SIZE)
#define STACK_SIZE_BYTES PIOS_STABILIZATION_STACK_SIZE
#else
#define STACK_SIZE_BYTES 840
#define STACK_SIZE_BYTES 860
#endif
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) // FLIGHT CONTROL priority
#define FAILSAFE_TIMEOUT_MS 30
// number of flight mode switch positions
#define NUM_FMS_POSITIONS 6
// The PID_RATE_ROLL set is used by Rate mode and the rate portion of Attitude mode
// The PID_RATE set is used by the attitude portion of Attitude mode
// The PID_RATEA_ROLL set is used by Rattitude mode because it needs to maintain
// - two independant rate PIDs because it does rate and attitude simultaneously
enum { PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_RATEA_ROLL, PID_RATEA_PITCH, PID_RATEA_YAW, PID_MAX };
enum { PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_MAX };
enum { RATE_P, RATE_I, RATE_D, RATE_LIMIT, RATE_OFFSET };
enum { ATT_P, ATT_I, ATT_LIMIT, ATT_OFFSET };
@ -103,21 +99,20 @@ uint8_t max_axislock_rate = 0;
float weak_leveling_kp = 0;
uint8_t weak_leveling_max = 0;
bool lowThrottleZeroIntegral;
bool lowThrottleZeroAxis[MAX_AXES];
float vbar_decay = 0.991f;
struct pid pids[PID_MAX];
int cur_flight_mode = -1;
static uint8_t rattitude_anti_windup;
static float cruise_control_min_throttle;
static float cruise_control_max_throttle;
static float rattitude_mode_transition_stick_position;
static float cruise_control_min_thrust;
static float cruise_control_max_thrust;
static uint8_t cruise_control_max_angle;
static float cruise_control_max_power_factor;
static float cruise_control_power_trim;
static int8_t cruise_control_inverted_power_switch;
static float cruise_control_neutral_thrust;
static uint8_t cruise_control_flight_mode_switch_pos_enable[NUM_FMS_POSITIONS];
static uint8_t cruise_control_flight_mode_switch_pos_enable[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
// Private functions
static void stabilizationTask(void *parameters);
@ -127,9 +122,6 @@ static void SettingsUpdatedCb(UAVObjEvent *ev);
static void BankUpdatedCb(UAVObjEvent *ev);
static void SettingsBankUpdatedCb(UAVObjEvent *ev);
static float stab_log2f(float x);
static float stab_powf(float x, uint8_t p);
/**
* Module initialization
*/
@ -167,10 +159,11 @@ int32_t StabilizationStart()
*/
int32_t StabilizationInitialize()
{
// stop the compile if the number of switch positions changes, but has not been changed here
PIOS_STATIC_ASSERT(NUM_FMS_POSITIONS == sizeof(((ManualControlSettingsData *)0)->FlightModePosition) / sizeof((((ManualControlSettingsData *)0)->FlightModePosition)[0]));
// Initialize variables
ManualControlCommandInitialize();
ManualControlSettingsInitialize();
FlightStatusInitialize();
StabilizationDesiredInitialize();
StabilizationSettingsInitialize();
StabilizationBankInitialize();
StabilizationSettingsBank1Initialize();
@ -205,6 +198,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
ActuatorDesiredData actuatorDesired;
StabilizationDesiredData stabDesired;
float throttleDesired;
RateDesiredData rateDesired;
AttitudeStateData attitudeState;
GyroStateData gyroStateData;
@ -236,6 +230,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
FlightStatusGet(&flightStatus);
StabilizationDesiredGet(&stabDesired);
ManualControlCommandThrottleGet(&throttleDesired);
AttitudeStateGet(&attitudeState);
GyroStateGet(&gyroStateData);
StabilizationBankGet(&stabBank);
@ -382,7 +377,6 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
if (reinit) {
pids[PID_ROLL + i].iAccumulator = 0;
pids[PID_RATE_ROLL + i].iAccumulator = 0;
pids[PID_RATEA_ROLL + i].iAccumulator = 0;
}
// Compute what Rate mode would give for this stick angle's rate
@ -405,8 +399,8 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
float rateDesiredAxisAttitude;
rateDesiredAxisAttitude = pid_apply(&pids[PID_ROLL + i], attitude_error, dT);
rateDesiredAxisAttitude = bound(rateDesiredAxisAttitude,
cast_struct_to_array(stabBank.MaximumRate,
stabBank.MaximumRate.Roll)[i]);
cast_struct_to_array(stabBank.ManualRate,
stabBank.ManualRate.Roll)[i]);
// Compute the weighted average rate desired
// Using max() rather than sqrt() for cpu speed;
@ -416,81 +410,54 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
// magnitude = sqrt(stabDesired.Roll*stabDesired.Roll + stabDesired.Pitch*stabDesired.Pitch);
float magnitude;
magnitude = fmaxf(fabsf(stabDesired.Roll), fabsf(stabDesired.Pitch));
// modify magnitude to move the Att to Rate transition to the place
// specified by the user
// we are looking for where the stick angle == transition angle
// and the Att rate equals the Rate rate
// that's where Rate x (1-StickAngle) [Attitude pulling down max X Ratt proportion]
// == Rate x StickAngle [Rate pulling up according to stick angle]
// * StickAngle [X Ratt proportion]
// so 1-x == x*x or x*x+x-1=0 where xE(0,1)
// (-1+-sqrt(1+4))/2 = (-1+sqrt(5))/2
// and quadratic formula says that is 0.618033989f
// I tested 14.01 and came up with .61 without even remembering this number
// I thought that moving the P,I, and maxangle terms around would change this value
// and that I would have to take these into account, but varying
// all P's and I's by factors of 1/2 to 2 didn't change it noticeably
// and varying maxangle from 4 to 120 didn't either.
// so for now I'm not taking these into account
// while working with this, it occurred to me that Attitude mode,
// set up with maxangle=190 would be similar to Ratt, and it is.
#define STICK_VALUE_AT_MODE_TRANSITION 0.618033989f
// the following assumes the transition would otherwise be at 0.618033989f
// and THAT assumes that Att ramps up to max roll rate
// when a small number of degrees off of where it should be
// if below the transition angle (still in attitude mode)
// '<=' instead of '<' keeps rattitude_mode_transition_stick_position==1.0 from causing DZ
if (magnitude <= rattitude_mode_transition_stick_position) {
magnitude *= STICK_VALUE_AT_MODE_TRANSITION / rattitude_mode_transition_stick_position;
} else {
magnitude = (magnitude - rattitude_mode_transition_stick_position)
* (1.0f - STICK_VALUE_AT_MODE_TRANSITION)
/ (1.0f - rattitude_mode_transition_stick_position)
+ STICK_VALUE_AT_MODE_TRANSITION;
}
rateDesiredAxis[i] = (1.0f - magnitude) * rateDesiredAxisAttitude
+ magnitude * rateDesiredAxisRate;
// Compute the inner loop for both Rate mode and Attitude mode
// actuatorDesiredAxis[i] is the weighted average of the two PIDs from the two rates
actuatorDesiredAxis[i] =
(1.0f - magnitude) * pid_apply_setpoint(&pids[PID_RATEA_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT)
+ magnitude * pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
// Compute the inner loop for the averaged rate
// actuatorDesiredAxis[i] is the weighted average
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor,
rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
// settings.RattitudeAntiWindup controls the iAccumulator zeroing
// - so both iAccs don't wind up too far;
// - nor do both iAccs get held too close to zero at mid stick
// I suspect that there would be windup without it
// - since rate and attitude fight each other here
// - rate trying to pull it over the top and attitude trying to pull it back down
// Wind-up increases linearly with cycles for a fixed error.
// We must never increase the iAcc or we risk oscillation.
// Use the powf() function to make two anti-windup curves
// - one for zeroing rate close to center stick
// - the other for zeroing attitude close to max stick
// the bigger the dT the more anti windup needed
// the bigger the PID[].i the more anti windup needed
// more anti windup is achieved with a lower powf() power
// a doubling of e.g. PID[].i should cause the runtime anti windup factor
// to get twice as far away from 1.0 (towards zero)
// e.g. from .90 to .80
// magic numbers
// these generate the inverted parabola like curves that go through [0,1] and [1,0]
// the higher the power, the closer the curve is to a straight line
// from [0,1] to [1,1] to [1,0] and the less anti windup is applied
// the UAVO RattitudeAntiWindup varies from 0 to 31
// 0 turns off anti windup
// 1 gives very little anti-windup because the power given to powf() is 31
// 31 gives a lot of anti-windup because the power given to powf() is 1
// the 32.1 is what does this
// the 7.966 and 17.668 cancel the default PID value and dT given to log2f()
// if these are non-default, tweaking is thus done so the user doesn't have to readjust
// the default value of 10 for UAVO RattitudeAntiWindup gives a power of 22
// these calculations are for magnitude = 0.5, so 22 corresponds to the number of bits
// used in the mantissa of the float
// i.e. 1.0-(0.5^22) almost underflows
// This may only be useful for aircraft with large Ki values and limits
if (dT > 0.0f && rattitude_anti_windup > 0.0f) {
float factor;
// At magnitudes close to one, the Attitude accumulators gets zeroed
if (pids[PID_ROLL + i].i > 0.0f) {
factor = 1.0f - stab_powf(magnitude, ((uint8_t)(32.1f - 7.966f - stab_log2f(dT * pids[PID_ROLL + i].i))) - rattitude_anti_windup);
pids[PID_ROLL + i].iAccumulator *= factor;
}
if (pids[PID_RATEA_ROLL + i].i > 0.0f) {
factor = 1.0f - stab_powf(magnitude, ((uint8_t)(32.1f - 17.668f - stab_log2f(dT * pids[PID_RATEA_ROLL + i].i))) - rattitude_anti_windup);
pids[PID_RATEA_ROLL + i].iAccumulator *= factor;
}
// At magnitudes close to zero, the Rate accumulator gets zeroed
if (pids[PID_RATE_ROLL + i].i > 0.0f) {
factor = 1.0f - stab_powf(1.0f - magnitude, ((uint8_t)(32.1f - 17.668f - stab_log2f(dT * pids[PID_RATE_ROLL + i].i))) - rattitude_anti_windup);
pids[PID_RATE_ROLL + i].iAccumulator *= factor;
}
}
break;
}
case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR:
// Store for debugging output
rateDesiredAxis[i] = stabDesiredAxis[i];
@ -592,28 +559,13 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
// Save dT
actuatorDesired.UpdateTime = dT * 1000;
actuatorDesired.Throttle = stabDesired.Throttle;
actuatorDesired.Thrust = stabDesired.Thrust;
// Suppress desired output while disarmed or throttle low, for configured axis
if (flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED || stabDesired.Throttle < 0) {
if (lowThrottleZeroAxis[ROLL]) {
actuatorDesired.Roll = 0.0f;
}
if (lowThrottleZeroAxis[PITCH]) {
actuatorDesired.Pitch = 0.0f;
}
if (lowThrottleZeroAxis[YAW]) {
actuatorDesired.Yaw = 0.0f;
}
}
// modify throttle according to 1/cos(bank angle)
// modify thrust according to 1/cos(bank angle)
// to maintain same altitdue with changing bank angle
// but without manually adjusting throttle
// but without manually adjusting thrust
// do it here and all the various flight modes (e.g. Altitude Hold) can use it
if (flight_mode_switch_position < NUM_FMS_POSITIONS
if (flight_mode_switch_position < FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM
&& cruise_control_flight_mode_switch_pos_enable[flight_mode_switch_position] != (uint8_t)0
&& cruise_control_max_power_factor > 0.0001f) {
static uint8_t toggle;
@ -645,7 +597,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
// if inverted and they want negative boost
if (angle > 90.0f && cruise_control_inverted_power_switch == (int8_t)-1) {
factor = -factor;
// as long as throttle is getting reversed
// as long as thrust is getting reversed
// we may as well do pitch and yaw for a complete "invert switch"
actuatorDesired.Pitch = -actuatorDesired.Pitch;
actuatorDesired.Yaw = -actuatorDesired.Yaw;
@ -653,20 +605,20 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
}
}
// also don't adjust throttle if <= 0, leaves neg alone and zero throttle stops motors
if (actuatorDesired.Throttle > cruise_control_min_throttle) {
// also don't adjust thrust if <= 0, leaves neg alone and zero thrust stops motors
if (actuatorDesired.Thrust > cruise_control_min_thrust) {
// quad example factor of 2 at hover power of 40%: (0.4 - 0.0) * 2.0 + 0.0 = 0.8
// CP heli example factor of 2 at hover stick of 60%: (0.6 - 0.5) * 2.0 + 0.5 = 0.7
actuatorDesired.Throttle = (actuatorDesired.Throttle - cruise_control_neutral_thrust) * factor + cruise_control_neutral_thrust;
if (actuatorDesired.Throttle > cruise_control_max_throttle) {
actuatorDesired.Throttle = cruise_control_max_throttle;
} else if (actuatorDesired.Throttle < cruise_control_min_throttle) {
actuatorDesired.Throttle = cruise_control_min_throttle;
actuatorDesired.Thrust = (actuatorDesired.Thrust - cruise_control_neutral_thrust) * factor + cruise_control_neutral_thrust;
if (actuatorDesired.Thrust > cruise_control_max_thrust) {
actuatorDesired.Thrust = cruise_control_max_thrust;
} else if (actuatorDesired.Thrust < cruise_control_min_thrust) {
actuatorDesired.Thrust = cruise_control_min_thrust;
}
}
}
if (PARSE_FLIGHT_MODE(flightStatus.FlightMode) != FLIGHTMODE_MANUAL) {
if (flightStatus.ControlChain.Stabilization == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
ActuatorDesiredSet(&actuatorDesired);
} else {
// Force all axes to reinitialize when engaged
@ -676,7 +628,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
}
if (flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED ||
(lowThrottleZeroIntegral && stabDesired.Throttle < 0)) {
(lowThrottleZeroIntegral && throttleDesired < 0)) {
// Force all axes to reinitialize when engaged
for (uint8_t i = 0; i < MAX_AXES; i++) {
previous_mode[i] = 255;
@ -724,44 +676,9 @@ static float bound(float val, float range)
}
// x small (0.0 < x < .01) so interpolation of fractional part is reasonable
static float stab_log2f(float x)
{
union {
volatile float f;
volatile uint32_t i;
volatile unsigned char c[4];
} __attribute__((packed)) u1, u2;
u2.f = u1.f = x;
u1.i <<= 1;
u2.i &= 0xff800000;
// get and unbias the exponent, add in a linear interpolation of the fractional part
return (float)(u1.c[3] - 127) + (x / u2.f) - 1.0f;
}
// 0<=x<=1, 0<=p<=31
static float stab_powf(float x, uint8_t p)
{
float retval = 1.0f;
while (p) {
if (p & 1) {
retval *= x;
}
x *= x;
p >>= 1;
}
return retval;
}
static void SettingsBankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
if (cur_flight_mode < 0 || cur_flight_mode >= NUM_FMS_POSITIONS) {
if (cur_flight_mode < 0 || cur_flight_mode >= FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM) {
return;
}
if ((ev) && ((settings.FlightModeMap[cur_flight_mode] == 0 && ev->obj != StabilizationSettingsBank1Handle()) ||
@ -867,27 +784,6 @@ static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
bank.YawPI.Ki,
0,
bank.YawPI.ILimit);
// Set the Rattitude roll rate PID constants
pid_configure(&pids[PID_RATEA_ROLL],
bank.RollRatePID.Kp,
bank.RollRatePID.Ki,
bank.RollRatePID.Kd,
bank.RollRatePID.ILimit);
// Set the Rattitude pitch rate PID constants
pid_configure(&pids[PID_RATEA_PITCH],
bank.PitchRatePID.Kp,
bank.PitchRatePID.Ki,
bank.PitchRatePID.Kd,
bank.PitchRatePID.ILimit);
// Set the Rattitude yaw rate PID constants
pid_configure(&pids[PID_RATEA_YAW],
bank.YawRatePID.Kp,
bank.YawRatePID.Ki,
bank.YawRatePID.Kd,
bank.YawRatePID.ILimit);
}
@ -906,13 +802,8 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
weak_leveling_kp = settings.WeakLevelingKp;
weak_leveling_max = settings.MaxWeakLevelingRate;
// Whether to zero the PID integrals while throttle is low
lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE;
// Whether to suppress (zero) the StabilizationDesired output for each axis while disarmed or throttle is low
lowThrottleZeroAxis[ROLL] = settings.LowThrottleZeroAxis.Roll == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE;
lowThrottleZeroAxis[PITCH] = settings.LowThrottleZeroAxis.Pitch == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE;
lowThrottleZeroAxis[YAW] = settings.LowThrottleZeroAxis.Yaw == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE;
// Whether to zero the PID integrals while thrust is low
lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE;
// The dT has some jitter iteration to iteration that we don't want to
// make thie result unpredictable. Still, it's nicer to specify the constant
@ -932,11 +823,15 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
// force flight mode update
cur_flight_mode = -1;
// Rattitude flight mode anti-windup factor
rattitude_anti_windup = settings.RattitudeAntiWindup;
// Rattitude stick angle where the attitude to rate transition happens
if (settings.RattitudeModeTransition < (uint8_t)10) {
rattitude_mode_transition_stick_position = 10.0f / 100.0f;
} else {
rattitude_mode_transition_stick_position = (float)settings.RattitudeModeTransition / 100.0f;
}
cruise_control_min_throttle = (float)settings.CruiseControlMinThrottle / 100.0f;
cruise_control_max_throttle = (float)settings.CruiseControlMaxThrottle / 100.0f;
cruise_control_min_thrust = (float)settings.CruiseControlMinThrust / 100.0f;
cruise_control_max_thrust = (float)settings.CruiseControlMaxThrust / 100.0f;
cruise_control_max_angle = settings.CruiseControlMaxAngle;
cruise_control_max_power_factor = settings.CruiseControlMaxPowerFactor;
cruise_control_power_trim = settings.CruiseControlPowerTrim / 100.0f;

2
flight/modules/StateEstimation/filterair.c
View File

@ -78,7 +78,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
this->altitude = state->baro[0];
}
// calculate true airspeed estimation
if (IS_SET(state->updated, SENSORUPDATES_airspeed)) {
if (IS_SET(state->updated, SENSORUPDATES_airspeed) && (state->airspeed[1] < 0.f)) {
state->airspeed[1] = state->airspeed[0] * IAS2TAS(this->altitude);
}

54
flight/modules/StateEstimation/stateestimation.c
View File

@ -40,6 +40,7 @@
#include <airspeedsensor.h>
#include <gpspositionsensor.h>
#include <gpsvelocitysensor.h>
#include <homelocation.h>
#include <gyrostate.h>
#include <accelstate.h>
@ -55,10 +56,14 @@
#include "CoordinateConversions.h"
// Private constants
#define STACK_SIZE_BYTES 256
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR
#define TASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
#define TIMEOUT_MS 10
#define STACK_SIZE_BYTES 256
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR
#define TASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
#define TIMEOUT_MS 10
// Private filter init const
#define FILTER_INIT_FORCE -1
#define FILTER_INIT_IF_POSSIBLE -2
// local macros, ONLY to be used in the middle of StateEstimationCb in section RUNSTATE_LOAD after the update of states updated!
#define FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(sensorname, shortname, a1, a2, a3) \
@ -74,6 +79,7 @@
UNSET_MASK(states.updated, SENSORUPDATES_##shortname); \
} \
}
#define FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(sensorname, shortname, a1, EXTRACHECK) \
if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \
sensorname##Data s; \
@ -86,6 +92,19 @@
} \
}
#define FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_2_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(sensorname, shortname, a1, a2, EXTRACHECK) \
if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \
sensorname##Data s; \
sensorname##Get(&s); \
if (IS_REAL(s.a1) && IS_REAL(s.a2) && EXTRACHECK) { \
states.shortname[0] = s.a1; \
states.shortname[1] = s.a2; \
} \
else { \
UNSET_MASK(states.updated, SENSORUPDATES_##shortname); \
} \
}
// local macros, ONLY to be used in the middle of StateEstimationCb in section RUNSTATE_SAVE before the check of alarms!
#define EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(statename, shortname, a1, a2, a3) \
if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \
@ -96,6 +115,7 @@
s.a3 = states.shortname[2]; \
statename##Set(&s); \
}
#define EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_2_DIMENSIONS(statename, shortname, a1, a2) \
if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \
statename##Data s; \
@ -105,8 +125,8 @@
statename##Set(&s); \
}
// Private types
// Private types
struct filterPipelineStruct;
typedef const struct filterPipelineStruct {
@ -119,7 +139,7 @@ static DelayedCallbackInfo *stateEstimationCallback;
static volatile RevoSettingsData revoSettings;
static volatile sensorUpdates updatedSensors;
static int32_t fusionAlgorithm = -1;
static volatile int32_t fusionAlgorithm = -1;
static filterPipeline *filterChain = NULL;
// different filters available to state estimation
@ -213,6 +233,7 @@ static const filterPipeline *ekf13Queue = &(filterPipeline) {
static void settingsUpdatedCb(UAVObjEvent *objEv);
static void sensorUpdatedCb(UAVObjEvent *objEv);
static void homeLocationUpdatedCb(UAVObjEvent *objEv);
static void StateEstimationCb(void);
static inline int32_t maxint32_t(int32_t a, int32_t b)
@ -238,6 +259,8 @@ int32_t StateEstimationInitialize(void)
GPSVelocitySensorInitialize();
GPSPositionSensorInitialize();
HomeLocationInitialize();
GyroStateInitialize();
AccelStateInitialize();
MagStateInitialize();
@ -247,6 +270,8 @@ int32_t StateEstimationInitialize(void)
RevoSettingsConnectCallback(&settingsUpdatedCb);
HomeLocationConnectCallback(&homeLocationUpdatedCb);
GyroSensorConnectCallback(&sensorUpdatedCb);
AccelSensorConnectCallback(&sensorUpdatedCb);
MagSensorConnectCallback(&sensorUpdatedCb);
@ -329,7 +354,7 @@ static void StateEstimationCb(void)
if (fusionAlgorithm != revoSettings.FusionAlgorithm) {
FlightStatusData fs;
FlightStatusGet(&fs);
if (fs.Armed == FLIGHTSTATUS_ARMED_DISARMED || fusionAlgorithm == -1) {
if (fs.Armed == FLIGHTSTATUS_ARMED_DISARMED || fusionAlgorithm == FILTER_INIT_FORCE) {
const filterPipeline *newFilterChain;
switch (revoSettings.FusionAlgorithm) {
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY:
@ -379,8 +404,8 @@ static void StateEstimationCb(void)
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(MagSensor, mag, x, y, z);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GPSVelocitySensor, vel, North, East, Down);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(BaroSensor, baro, Altitude, true);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(AirspeedSensor, airspeed, CalibratedAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE);
states.airspeed[1] = 0.0f; // sensor does not provide true airspeed, needs to be calculated by filter, set to zero for now
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_2_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(AirspeedSensor, airspeed, CalibratedAirspeed, TrueAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE);
// GPS position data (LLA) is not fetched here since it does not contain floats. The filter must do all checks itself
// at this point sensor state is stored in "states" with some rudimentary filtering applied
@ -477,6 +502,17 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
RevoSettingsGet((RevoSettingsData *)&revoSettings);
}
/**
* Callback for eventdispatcher when HomeLocation has been updated
*/
static void homeLocationUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
// Ask for a filter init (necessary for LLA filter)
// Only possible if disarmed
fusionAlgorithm = FILTER_INIT_IF_POSSIBLE;
}
/**
* Callback for eventdispatcher when any sensor UAVObject has been updated
* updates the list of "recently updated UAVObjects" and dispatches the state estimator callback

41
flight/modules/System/systemmod.c
View File

@ -92,8 +92,6 @@
// Private types
// Private variables
static uint32_t idleCounter;
static uint32_t idleCounterClear;
static xTaskHandle systemTaskHandle;
static xQueueHandle objectPersistenceQueue;
static enum { STACKOVERFLOW_NONE = 0, STACKOVERFLOW_WARNING = 1, STACKOVERFLOW_CRITICAL = 3 } stackOverflow;
@ -132,6 +130,7 @@ int32_t SystemModStart(void)
// Register task
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SYSTEM, systemTaskHandle);
return 0;
}
@ -186,15 +185,12 @@ static void systemTask(__attribute__((unused)) void *parameters)
*/
PIOS_SYS_Reset();
}
#if defined(PIOS_INCLUDE_IAP)
/* Record a successful boot */
PIOS_IAP_WriteBootCount(0);
#endif
// Initialize vars
idleCounter = 0;
idleCounterClear = 0;
// Listen for SettingPersistance object updates, connect a callback function
ObjectPersistenceConnectQueue(objectPersistenceQueue);
@ -212,9 +208,10 @@ static void systemTask(__attribute__((unused)) void *parameters)
// Main system loop
while (1) {
// Update the system statistics
updateStats();
cycleCount = cycleCount > 0 ? cycleCount - 1 : 7;
cycleCount = cycleCount > 0 ? cycleCount - 1 : 7;
// if(cycleCount == 1){
updateStats();
// Update the system alarms
updateSystemAlarms();
#ifdef DIAG_I2C_WDG_STATS
@ -227,10 +224,12 @@ static void systemTask(__attribute__((unused)) void *parameters)
PIOS_TASK_MONITOR_ForEachTask(taskMonitorForEachCallback, &taskInfoData);
TaskInfoSet(&taskInfoData);
// Update the callback status object
// if(FALSE){
PIOS_CALLBACKSCHEDULER_ForEachCallback(callbackSchedulerForEachCallback, &callbackInfoData);
CallbackInfoSet(&callbackInfoData);
// }
#endif
// }
// Flash the heartbeat LED
#if defined(PIOS_LED_HEARTBEAT)
uint8_t armingStatus;
@ -541,7 +540,6 @@ static uint16_t GetFreeIrqStackSize(void)
*/
static void updateStats()
{
static portTickType lastTickCount = 0;
SystemStatsData stats;
// Get stats and update
@ -559,10 +557,6 @@ static void updateStats()
// Get Irq stack status
stats.IRQStackRemaining = GetFreeIrqStackSize();
// When idleCounterClear was not reset by the idle-task, it means the idle-task did not run
if (idleCounterClear) {
idleCounter = 0;
}
#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32)
if (pios_uavo_settings_fs_id) {
PIOS_FLASHFS_GetStats(pios_uavo_settings_fs_id, &fsStats);
@ -575,16 +569,11 @@ static void updateStats()
stats.UsrSlotsActive = fsStats.num_active_slots;
}
#endif
portTickType now = xTaskGetTickCount();
if (now > lastTickCount) {
uint32_t dT = (xTaskGetTickCount() - lastTickCount) * portTICK_RATE_MS; // in ms
stats.CPULoad = 100 - (uint8_t)roundf(100.0f * ((float)idleCounter / ((float)dT / 1000.0f)) / (float)IDLE_COUNTS_PER_SEC_AT_NO_LOAD);
} // else: TickCount has wrapped, do not calc now
lastTickCount = now;
idleCounterClear = 1;
stats.CPULoad = 100 - PIOS_TASK_MONITOR_GetIdlePercentage();
#if defined(PIOS_INCLUDE_ADC) && defined(PIOS_ADC_USE_TEMP_SENSOR)
float temp_voltage = PIOS_ADC_PinGetVolt(PIOS_ADC_TEMPERATURE_PIN);
stats.CPUTemp = PIOS_CONVERT_VOLT_TO_CPU_TEMP(temp_voltage);;
stats.CPUTemp = PIOS_CONVERT_VOLT_TO_CPU_TEMP(temp_voltage);;
#endif
SystemStatsSet(&stats);
}
@ -663,15 +652,7 @@ static void updateSystemAlarms()
* Called by the RTOS when the CPU is idle, used to measure the CPU idle time.
*/
void vApplicationIdleHook(void)
{
// Called when the scheduler has no tasks to run
if (idleCounterClear == 0) {
++idleCounter;
} else {
idleCounter = 0;
idleCounterClear = 0;
}
}
{}
/**
* Called by the RTOS when a stack overflow is detected.

73
flight/modules/Telemetry/telemetry.c
View File

@ -54,6 +54,9 @@
// Private variables
static uint32_t telemetryPort;
#ifdef PIOS_INCLUDE_RFM22B
static uint32_t radioPort;
#endif
static xQueueHandle queue;
#if defined(PIOS_TELEM_PRIORITY_QUEUE)
@ -80,6 +83,7 @@ static void telemetryTxTask(void *parameters);
static void telemetryRxTask(void *parameters);
#ifdef PIOS_INCLUDE_RFM22B
static void radioRxTask(void *parameters);
static int32_t transmitRadioData(uint8_t *data, int32_t length);
#endif
static int32_t transmitData(uint8_t *data, int32_t length);
static void registerObject(UAVObjHandle obj);
@ -140,13 +144,16 @@ int32_t TelemetryInitialize(void)
// Update telemetry settings
telemetryPort = PIOS_COM_TELEM_RF;
#ifdef PIOS_INCLUDE_RFM22B
radioPort = PIOS_COM_RF;
#endif
HwSettingsInitialize();
updateSettings();
// Initialise UAVTalk
uavTalkCon = UAVTalkInitialize(&transmitData);
#ifdef PIOS_INCLUDE_RFM22B
radioUavTalkCon = UAVTalkInitialize(&transmitData);
radioUavTalkCon = UAVTalkInitialize(&transmitRadioData);
#endif
// Create periodic event that will be used to update the telemetry stats
@ -375,17 +382,28 @@ static void telemetryTxTask(__attribute__((unused)) void *parameters)
* Tries to empty the high priority queue before handling any standard priority item
*/
#if defined(PIOS_TELEM_PRIORITY_QUEUE)
// Loop forever
while (xQueueReceive(priorityQueue, &ev, 1) == pdTRUE) {
// empty priority queue, non-blocking
while (xQueueReceive(priorityQueue, &ev, 0) == pdTRUE) {
// Process event
processObjEvent(&ev);
}
#endif
// Wait for queue message
// check regular queue and process update - non-blocking
if (xQueueReceive(queue, &ev, 0) == pdTRUE) {
// Process event
processObjEvent(&ev);
// if both queues are empty, wait on priority queue for updates (1 tick) then repeat cycle
} else if (xQueueReceive(priorityQueue, &ev, 1) == pdTRUE) {
// Process event
processObjEvent(&ev);
}
#else
// wait on queue for updates (1 tick) then repeat cycle
if (xQueueReceive(queue, &ev, 1) == pdTRUE) {
// Process event
processObjEvent(&ev);
}
#endif /* if defined(PIOS_TELEM_PRIORITY_QUEUE) */
}
}
@ -424,12 +442,12 @@ static void radioRxTask(__attribute__((unused)) void *parameters)
{
// Task loop
while (1) {
if (telemetryPort) {
if (radioPort) {
// Block until data are available
uint8_t serial_data[1];
uint16_t bytes_to_process;
bytes_to_process = PIOS_COM_ReceiveBuffer(telemetryPort, serial_data, sizeof(serial_data), 500);
bytes_to_process = PIOS_COM_ReceiveBuffer(radioPort, serial_data, sizeof(serial_data), 500);
if (bytes_to_process > 0) {
for (uint8_t i = 0; i < bytes_to_process; i++) {
UAVTalkProcessInputStream(radioUavTalkCon, serial_data[i]);
@ -440,6 +458,23 @@ static void radioRxTask(__attribute__((unused)) void *parameters)
}
}
}
/**
* Transmit data buffer to the radioport.
* \param[in] data Data buffer to send
* \param[in] length Length of buffer
* \return -1 on failure
* \return number of bytes transmitted on success
*/
static int32_t transmitRadioData(uint8_t *data, int32_t length)
{
if (radioPort) {
return PIOS_COM_SendBuffer(radioPort, data, length);
}
return -1;
}
#endif /* PIOS_INCLUDE_RFM22B */
/**
@ -476,6 +511,7 @@ static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs)
ev.obj = obj;
ev.instId = UAVOBJ_ALL_INSTANCES;
ev.event = EV_UPDATED_PERIODIC;
ev.lowPriority = true;
xQueueHandle targetQueue = UAVObjIsPriority(obj) ? priorityQueue : queue;
@ -502,6 +538,7 @@ static int32_t setLoggingPeriod(UAVObjHandle obj, int32_t updatePeriodMs)
ev.obj = obj;
ev.instId = UAVOBJ_ALL_INSTANCES;
ev.event = EV_LOGGING_PERIODIC;
ev.lowPriority = true;
xQueueHandle targetQueue = UAVObjIsPriority(obj) ? priorityQueue : queue;
@ -542,12 +579,10 @@ static void updateTelemetryStats()
uint32_t timeNow;
// Get stats
UAVTalkGetStats(uavTalkCon, &utalkStats);
UAVTalkGetStats(uavTalkCon, &utalkStats, true);
#ifdef PIOS_INCLUDE_RFM22B
UAVTalkAddStats(radioUavTalkCon, &utalkStats);
UAVTalkResetStats(radioUavTalkCon);
UAVTalkAddStats(radioUavTalkCon, &utalkStats, true);
#endif
UAVTalkResetStats(uavTalkCon);
// Get object data
FlightTelemetryStatsGet(&flightStats);
@ -676,17 +711,25 @@ static void updateSettings()
* Determine input/output com port as highest priority available
* @param[in] input Returns the approproate input com port if true, else the appropriate output com port
*/
#ifdef PIOS_INCLUDE_RFM22B
static uint32_t getComPort(bool input)
{
#ifndef PIOS_INCLUDE_RFM22B
input = false;
#else
static uint32_t getComPort(__attribute__((unused)) bool input)
#endif
{
#if defined(PIOS_INCLUDE_USB)
// if USB is connected, USB takes precedence for telemetry
if (PIOS_COM_Available(PIOS_COM_TELEM_USB)) {
return PIOS_COM_TELEM_USB;
} else
#endif /* PIOS_INCLUDE_USB */
if (!input && PIOS_COM_Available(telemetryPort)) {
#ifdef PIOS_INCLUDE_RFM22B
// PIOS_COM_RF input is handled by a separate RX thread and therefore must be ignored
if (input && telemetryPort == PIOS_COM_RF) {
return 0;
} else
#endif /* PIOS_INCLUDE_RFM22B */
if (PIOS_COM_Available(telemetryPort)) {
return telemetryPort;
} else {
return 0;

3
flight/modules/TxPID/txpid.c
View File

@ -111,6 +111,7 @@ int32_t TxPIDInitialize(void)
.obj = AccessoryDesiredHandle(),
.instId = 0,
.event = 0,
.lowPriority = false,
};
EventPeriodicCallbackCreate(&ev, updatePIDs, SAMPLE_PERIOD_MS / portTICK_RATE_MS);
@ -326,7 +327,7 @@ static void updatePIDs(UAVObjEvent *ev)
break;
case 2:
StabilizationSettingsBank2Set((StabilizationSettingsBank2Data *)&bank);
StabilizationSettingsBank3Set((StabilizationSettingsBank3Data *)&bank);
break;
default:

128
flight/modules/VtolPathFollower/vtolpathfollower.c
View File

@ -55,7 +55,7 @@
#include "hwsettings.h"
#include "pathdesired.h" // object that will be updated by the module
#include "positionstate.h"
#include "manualcontrol.h"
#include "manualcontrolcommand.h"
#include "flightstatus.h"
#include "pathstatus.h"
#include "gpsvelocitysensor.h"
@ -158,7 +158,7 @@ static float northPosIntegral = 0;
static float eastPosIntegral = 0;
static float downPosIntegral = 0;
static float throttleOffset = 0;
static float thrustOffset = 0;
/**
* Module thread, should not return.
*/
@ -206,55 +206,53 @@ static void vtolPathFollowerTask(__attribute__((unused)) void *parameters)
PathDesiredGet(&pathDesired);
// Check the combinations of flightmode and pathdesired mode
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_LAND:
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
updateEndpointVelocity();
updateVtolDesiredAttitude(false);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
if (flightStatus.ControlChain.PathFollower == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
if (flightStatus.ControlChain.PathPlanner == FLIGHTSTATUS_CONTROLCHAIN_FALSE) {
if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POI) {
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
updateEndpointVelocity();
updateVtolDesiredAttitude(true);
updatePOIBearing();
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
}
} else {
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
updateEndpointVelocity();
updateVtolDesiredAttitude(false);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
}
}
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
pathStatus.UID = pathDesired.UID;
pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
switch (pathDesired.Mode) {
// TODO: Make updateVtolDesiredAttitude and velocity report success and update PATHSTATUS_STATUS accordingly
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_FLYCIRCLELEFT:
updatePathVelocity();
updateVtolDesiredAttitude(false);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_FIXEDATTITUDE:
updateFixedAttitude(pathDesired.ModeParameters);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_DISARMALARM:
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
break;
default:
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
break;
}
PathStatusSet(&pathStatus);
}
break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
pathStatus.UID = pathDesired.UID;
pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
switch (pathDesired.Mode) {
// TODO: Make updateVtolDesiredAttitude and velocity report success and update PATHSTATUS_STATUS accordingly
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_FLYCIRCLELEFT:
updatePathVelocity();
updateVtolDesiredAttitude(false);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_FIXEDATTITUDE:
updateFixedAttitude(pathDesired.ModeParameters);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_DISARMALARM:
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
break;
default:
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
break;
}
PathStatusSet(&pathStatus);
break;
case FLIGHTSTATUS_FLIGHTMODE_POI:
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
updateEndpointVelocity();
updateVtolDesiredAttitude(true);
updatePOIBearing();
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
}
break;
default:
} else {
// Be cleaner and get rid of global variables
northVelIntegral = 0;
eastVelIntegral = 0;
@ -263,12 +261,10 @@ static void vtolPathFollowerTask(__attribute__((unused)) void *parameters)
eastPosIntegral = 0;
downPosIntegral = 0;
// Track throttle before engaging this mode. Cheap system ident
// Track thrust before engaging this mode. Cheap system ident
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
throttleOffset = stabDesired.Throttle;
break;
thrustOffset = stabDesired.Thrust;
}
AlarmsClear(SYSTEMALARMS_ALARM_GUIDANCE);
@ -550,10 +546,10 @@ static void updateFixedAttitude(float *attitude)
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
stabDesired.Roll = attitude[0];
stabDesired.Pitch = attitude[1];
stabDesired.Yaw = attitude[2];
stabDesired.Throttle = attitude[3];
stabDesired.Roll = attitude[0];
stabDesired.Pitch = attitude[1];
stabDesired.Yaw = attitude[2];
stabDesired.Thrust = attitude[3];
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
@ -653,13 +649,13 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
downError = velocityDesired.Down - downVel;
// Must flip this sign
downError = -downError;
downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID.Ki,
-vtolpathfollowerSettings.VerticalVelPID.ILimit,
vtolpathfollowerSettings.VerticalVelPID.ILimit);
downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID.Kp + downVelIntegral
- nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID.Kd);
downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID.Ki,
-vtolpathfollowerSettings.VerticalVelPID.ILimit,
vtolpathfollowerSettings.VerticalVelPID.ILimit);
downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID.Kp + downVelIntegral
- nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID.Kd);
stabDesired.Throttle = bound(downCommand + throttleOffset, 0, 1);
stabDesired.Thrust = bound(downCommand + thrustOffset, 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
@ -670,11 +666,11 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
eastCommand * cosf(DEG2RAD(attitudeState.Yaw)),
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
if (vtolpathfollowerSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) {
// For now override throttle with manual control. Disable at your risk, quad goes to China.
if (vtolpathfollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_FALSE) {
// For now override thrust with manual control. Disable at your risk, quad goes to China.
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
stabDesired.Throttle = manualControl.Throttle;
stabDesired.Thrust = manualControl.Thrust;
}
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;

126
flight/pios/common/pios_ms4525do.c Normal file
View File

@ -0,0 +1,126 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_MS4525DO MS4525DO Functions
* @brief Hardware functions to deal with the PixHawk Airspeed Sensor based on MS4525DO
* @{
*
* @file pios_ms4525do.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief PixHawk MS4525DO Airspeed Sensor Driver
* @see The GNU Public License (GPL) Version 3
*
******************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "pios.h"
#ifdef PIOS_INCLUDE_MS4525DO
/* Local Defs and Variables */
static int8_t PIOS_MS4525DO_ReadI2C(uint8_t *buffer, uint8_t len);
static int8_t PIOS_MS4525DO_WriteI2C(uint8_t *buffer, uint8_t len);
static bool pios_ms4525do_requested = false;
static int8_t PIOS_MS4525DO_ReadI2C(uint8_t *buffer, uint8_t len)
{
const struct pios_i2c_txn txn_list[] = {
{
.info = __func__,
.addr = MS4525DO_I2C_ADDR,
.rw = PIOS_I2C_TXN_READ,
.len = len,
.buf = buffer,
}
};
return PIOS_I2C_Transfer(PIOS_I2C_MS4525DO_ADAPTER, txn_list, NELEMENTS(txn_list));
}
static int8_t PIOS_MS4525DO_WriteI2C(uint8_t *buffer, uint8_t len)
{
const struct pios_i2c_txn txn_list[] = {
{
.info = __func__,
.addr = MS4525DO_I2C_ADDR,
.rw = PIOS_I2C_TXN_WRITE,
.len = len,
.buf = buffer,
}
};
return PIOS_I2C_Transfer(PIOS_I2C_MS4525DO_ADAPTER, txn_list, NELEMENTS(txn_list));
}
int8_t PIOS_MS4525DO_Request(void)
{
// MS4525DO expects a zero length write.
// Sending one byte is a workaround that works for the moment
uint8_t data = 0;
int8_t retVal = PIOS_MS4525DO_WriteI2C(&data, sizeof(data));
/* requested only when transfer worked */
pios_ms4525do_requested = (retVal == 0);
return retVal;
}
// values has to ba an arrray with two elements
// values stay untouched on error
int8_t PIOS_MS4525DO_Read(uint16_t *values)
{
if (!pios_ms4525do_requested) {
/* Do not try to read when not requested */
/* else probably stale data will be obtained */
return -4;
}
uint8_t data[4];
int8_t retVal = PIOS_MS4525DO_ReadI2C(data, sizeof(data));
uint8_t status = data[0] & 0xC0;
if (status == 0x80) {
/* stale data */
return -5;
} else if (status == 0xC0) {
/* device probably broken */
return -6;
}
/* differential pressure */
values[0] = (data[0] << 8);
values[0] += data[1];
/* temperature */
values[1] = (data[2] << 8);
values[1] += data[3];
values[1] = (values[1] >> 5);
/* not requested anymore, only when transfer worked */
pios_ms4525do_requested = !(retVal == 0);
return retVal;
}
#endif /* PIOS_INCLUDE_MS4525DO */

34
flight/pios/common/pios_task_monitor.c
View File

@ -33,6 +33,7 @@
static xSemaphoreHandle mLock;
static xTaskHandle *mTaskHandles;
static uint32_t mLastMonitorTime;
static uint32_t mLastIdleMonitorTime;
static uint16_t mMaxTasks;
/**
@ -53,9 +54,11 @@ int32_t PIOS_TASK_MONITOR_Initialize(uint16_t max_tasks)
mMaxTasks = max_tasks;
#if (configGENERATE_RUN_TIME_STATS == 1)
mLastMonitorTime = portGET_RUN_TIME_COUNTER_VALUE();
mLastMonitorTime = portGET_RUN_TIME_COUNTER_VALUE();
mLastIdleMonitorTime = portGET_RUN_TIME_COUNTER_VALUE();
#else
mLastMonitorTime = 0;
mLastMonitorTime = 0;
mLastIdleMonitorTime = 0;
#endif
return 0;
}
@ -146,4 +149,31 @@ void PIOS_TASK_MONITOR_ForEachTask(TaskMonitorTaskInfoCallback callback, void *c
xSemaphoreGiveRecursive(mLock);
}
uint8_t PIOS_TASK_MONITOR_GetIdlePercentage()
{
#if defined(ARCH_POSIX) || defined(ARCH_WIN32)
return 50;
#elif (configGENERATE_RUN_TIME_STATS == 1)
xSemaphoreTakeRecursive(mLock, portMAX_DELAY);
uint32_t currentTime = portGET_RUN_TIME_COUNTER_VALUE();
/* avoid divide-by-zero if the interval is too small */
uint32_t deltaTime = ((currentTime - mLastIdleMonitorTime) / 100) ? : 1;
mLastIdleMonitorTime = currentTime;
uint8_t running_time_percentage = 0;
/* Generate idle time percentage stats */
running_time_percentage = uxTaskGetRunTime(xTaskGetIdleTaskHandle()) / deltaTime;
xSemaphoreGiveRecursive(mLock);
return running_time_percentage;
#else
return 0;
#endif
}
#endif // PIOS_INCLUDE_TASK_MONITOR

45
flight/pios/inc/pios_ms4525do.h Normal file
View File

@ -0,0 +1,45 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_MS4525DO MS4525DO Functions
* @brief Hardware functions to deal with the PixHawk Airspeed Sensor based on MS4525DO
* @{
*
* @file pios_ms4525do.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief PixHawk MS4525DO Airspeed Sensor Driver
* @see The GNU Public License (GPL) Version 3
*
******************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_MS4525DO_H
#define PIOS_MS4525DO_H
// Interface Type I chip
#define MS4525DO_I2C_ADDR 0x28
// Interface Type J chip
/* #define MS4525DO_I2C_ADDR 0x36 */
// Interface Type K chip
/* #define MS4525DO_I2C_ADDR 0x46 */
extern int8_t PIOS_MS4525DO_Request(void);
extern int8_t PIOS_MS4525DO_Read(uint16_t *values);
#endif /* PIOS_MS4525DO_H */

5
flight/pios/inc/pios_task_monitor.h
View File

@ -104,4 +104,9 @@ typedef void (*TaskMonitorTaskInfoCallback)(uint16_t task_id, const struct pios_
*/
extern void PIOS_TASK_MONITOR_ForEachTask(TaskMonitorTaskInfoCallback callback, void *context);
/**
* Return the idle task running time percentage.
*/
extern uint8_t PIOS_TASK_MONITOR_GetIdlePercentage();
#endif // PIOS_TASK_MONITOR_H

6
flight/pios/pios.h
View File

@ -229,6 +229,12 @@
#include <pios_etasv3.h>
#endif
#ifdef PIOS_INCLUDE_MS4525DO
/* PixHawk Airspeed Sensor based on MS4525DO */
#include <pios_ms4525do.h>
#endif
#ifdef PIOS_INCLUDE_HCSR04
/* HC-SR04 Ultrasonic Sensor */
#include <pios_hcsr04.h>

13
flight/pios/stm32f10x/pios_i2c.c
View File

@ -404,8 +404,17 @@ static void go_starting(struct pios_i2c_adapter *i2c_adapter)
PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn);
PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn);
i2c_adapter->active_byte = &(i2c_adapter->active_txn->buf[0]);
i2c_adapter->last_byte = &(i2c_adapter->active_txn->buf[i2c_adapter->active_txn->len - 1]);
// check for an empty read/write
if (i2c_adapter->active_txn->buf != NULL && i2c_adapter->active_txn->len != 0) {
// Data available
i2c_adapter->active_byte = &(i2c_adapter->active_txn->buf[0]);
i2c_adapter->last_byte = &(i2c_adapter->active_txn->buf[i2c_adapter->active_txn->len - 1]);
} else {
// No Data available => Empty read/write
i2c_adapter->last_byte = NULL;
i2c_adapter->active_byte = i2c_adapter->last_byte + 1;
}
I2C_GenerateSTART(i2c_adapter->cfg->regs, ENABLE);
if (i2c_adapter->active_txn->rw == PIOS_I2C_TXN_READ) {

23
flight/pios/stm32f4xx/pios_i2c.c
View File

@ -94,26 +94,19 @@ static void go_bus_error(struct pios_i2c_adapter *i2c_adapter);
static void go_stopping(struct pios_i2c_adapter *i2c_adapter);
static void go_stopped(struct pios_i2c_adapter *i2c_adapter);
static void go_starting(struct pios_i2c_adapter *i2c_adapter);
static void go_r_any_txn_addr(struct pios_i2c_adapter *i2c_adapter);
static void go_r_more_txn_pre_one(struct pios_i2c_adapter *i2c_adapter);
static void go_r_any_txn_pre_first(struct pios_i2c_adapter *i2c_adapter);
static void go_r_any_txn_pre_middle(struct pios_i2c_adapter *i2c_adapter);
static void go_r_more_txn_pre_last(struct pios_i2c_adapter *i2c_adapter);
static void go_r_any_txn_post_last(struct pios_i2c_adapter *i2c_adapter);
static void go_r_any_txn_addr(struct pios_i2c_adapter *i2c_adapter);
static void go_r_more_txn_pre_one(struct pios_i2c_adapter *i2c_adapter);
static void go_r_last_txn_pre_one(struct pios_i2c_adapter *i2c_adapter);
static void go_r_any_txn_pre_first(struct pios_i2c_adapter *i2c_adapter);
static void go_r_any_txn_pre_middle(struct pios_i2c_adapter *i2c_adapter);
static void go_r_last_txn_pre_last(struct pios_i2c_adapter *i2c_adapter);
static void go_r_more_txn_pre_last(struct pios_i2c_adapter *i2c_adapter);
static void go_r_any_txn_post_last(struct pios_i2c_adapter *i2c_adapter);
static void go_w_any_txn_addr(struct pios_i2c_adapter *i2c_adapter);
static void go_w_any_txn_middle(struct pios_i2c_adapter *i2c_adapter);
static void go_w_more_txn_last(struct pios_i2c_adapter *i2c_adapter);
static void go_w_any_txn_addr(struct pios_i2c_adapter *i2c_adapter);
static void go_w_any_txn_middle(struct pios_i2c_adapter *i2c_adapter);
static void go_w_last_txn_last(struct pios_i2c_adapter *i2c_adapter);
static void go_nack(struct pios_i2c_adapter *i2c_adapter);
@ -414,8 +407,16 @@ static void go_starting(struct pios_i2c_adapter *i2c_adapter)
PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn);
PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn);
i2c_adapter->active_byte = &(i2c_adapter->active_txn->buf[0]);
i2c_adapter->last_byte = &(i2c_adapter->active_txn->buf[i2c_adapter->active_txn->len - 1]);
// check for an empty read/write
if (i2c_adapter->active_txn->buf != NULL && i2c_adapter->active_txn->len != 0) {
// Data available
i2c_adapter->active_byte = &(i2c_adapter->active_txn->buf[0]);
i2c_adapter->last_byte = &(i2c_adapter->active_txn->buf[i2c_adapter->active_txn->len - 1]);
} else {
// No Data available => Empty read/write
i2c_adapter->last_byte = NULL;
i2c_adapter->active_byte = i2c_adapter->last_byte + 1;
}
I2C_GenerateSTART(i2c_adapter->cfg->regs, ENABLE);
if (i2c_adapter->active_txn->rw == PIOS_I2C_TXN_READ) {

2
flight/pios/stm32f4xx/pios_wdg.c
View File

@ -73,7 +73,7 @@ uint16_t PIOS_WDG_Init()
delay = 0x0fff;
}
#if defined(PIOS_INCLUDE_WDG)
DBGMCU_Config(DBGMCU_IWDG_STOP, ENABLE); // make the watchdog stop counting in debug mode
DBGMCU_APB1PeriphConfig(DBGMCU_IWDG_STOP, ENABLE); // OP-1272 : write in APB1 register
IWDG_WriteAccessCmd(IWDG_WriteAccess_Enable);
IWDG_SetPrescaler(IWDG_Prescaler_16);
IWDG_SetReload(delay);

3
flight/targets/boards/coptercontrol/firmware/Makefile
View File

@ -34,6 +34,7 @@ ERASE_FLASH ?= NO
MODULES += Attitude
MODULES += Stabilization
MODULES += Actuator
MODULES += Receiver
MODULES += ManualControl
MODULES += FirmwareIAP
MODULES += Telemetry
@ -91,12 +92,14 @@ ifndef TESTAPP
SRC += $(OPUAVSYNTHDIR)/actuatordesired.c
SRC += $(OPUAVSYNTHDIR)/actuatorsettings.c
SRC += $(OPUAVSYNTHDIR)/accelstate.c
SRC += $(OPUAVSYNTHDIR)/accelgyrosettings.c
SRC += $(OPUAVSYNTHDIR)/gyrostate.c
SRC += $(OPUAVSYNTHDIR)/attitudestate.c
SRC += $(OPUAVSYNTHDIR)/manualcontrolcommand.c
SRC += $(OPUAVSYNTHDIR)/i2cstats.c
SRC += $(OPUAVSYNTHDIR)/watchdogstatus.c
SRC += $(OPUAVSYNTHDIR)/manualcontrolsettings.c
SRC += $(OPUAVSYNTHDIR)/flightmodesettings.c
SRC += $(OPUAVSYNTHDIR)/mixersettings.c
SRC += $(OPUAVSYNTHDIR)/firmwareiapobj.c
SRC += $(OPUAVSYNTHDIR)/attitudesettings.c

7
flight/targets/boards/coptercontrol/firmware/inc/FreeRTOSConfig.h
View File

@ -75,11 +75,9 @@
#endif
/* Enable run time stats collection */
#ifdef DIAG_TASKS
#define configCHECK_FOR_STACK_OVERFLOW 2
#define configGENERATE_RUN_TIME_STATS 1
#define INCLUDE_uxTaskGetRunTime 1
#define INCLUDE_xTaskGetIdleTaskHandle 1
#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \
do { \
(*(unsigned long *)0xe000edfc) |= (1 << 24); /* DEMCR |= DEMCR_TRCENA */ \
@ -87,6 +85,9 @@
} \
while (0)
#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */
#ifdef DIAG_TASKS
#define configCHECK_FOR_STACK_OVERFLOW 2
#else
#define configCHECK_FOR_STACK_OVERFLOW 1
#endif

1
flight/targets/boards/coptercontrol/firmware/inc/pios_config.h
View File

@ -164,7 +164,6 @@
#else
#define PIOS_SYSTEM_STACK_SIZE 660
#endif
#define PIOS_STABILIZATION_STACK_SIZE 790
#define PIOS_TELEM_STACK_SIZE 540
#define PIOS_EVENTDISPATCHER_STACK_SIZE 160

7
flight/targets/boards/oplinkmini/firmware/inc/FreeRTOSConfig.h
View File

@ -75,11 +75,9 @@
#endif
/* Enable run time stats collection */
#ifdef DIAG_TASKS
#define configCHECK_FOR_STACK_OVERFLOW 2
#define configGENERATE_RUN_TIME_STATS 1
#define INCLUDE_uxTaskGetRunTime 1
#define INCLUDE_xTaskGetIdleTaskHandle 1
#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \
do { \
(*(unsigned long *)0xe000edfc) |= (1 << 24); /* DEMCR |= DEMCR_TRCENA */ \
@ -87,6 +85,9 @@
} \
while (0)
#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */
#ifdef DIAG_TASKS
#define configCHECK_FOR_STACK_OVERFLOW 2
#else
#define configCHECK_FOR_STACK_OVERFLOW 1
#endif

1
flight/targets/boards/osd/firmware/inc/FreeRTOSConfig.h
View File

@ -79,6 +79,7 @@
/* Enable run time stats collection */
#define configGENERATE_RUN_TIME_STATS 1
#define INCLUDE_uxTaskGetRunTime 1
#define INCLUDE_xTaskGetIdleTaskHandle 1
/*
* Once we move to CMSIS2 we can at least use:

1
flight/targets/boards/revolution/firmware/Makefile
View File

@ -37,6 +37,7 @@ MODULES += AltitudeHold
MODULES += Stabilization
MODULES += VtolPathFollower
MODULES += ManualControl
MODULES += Receiver
MODULES += Actuator
MODULES += GPS
MODULES += TxPID

2
flight/targets/boards/revolution/firmware/UAVObjects.inc
View File

@ -19,6 +19,7 @@
# These are the UAVObjects supposed to be build as part of the OpenPilot target
# (all architectures)
UAVOBJSRCFILENAMES =
UAVOBJSRCFILENAMES += accelgyrosettings
UAVOBJSRCFILENAMES += accessorydesired
UAVOBJSRCFILENAMES += actuatorcommand
UAVOBJSRCFILENAMES += actuatordesired
@ -60,6 +61,7 @@ UAVOBJSRCFILENAMES += homelocation
UAVOBJSRCFILENAMES += i2cstats
UAVOBJSRCFILENAMES += manualcontrolcommand
UAVOBJSRCFILENAMES += manualcontrolsettings
UAVOBJSRCFILENAMES += flightmodesettings
UAVOBJSRCFILENAMES += mixersettings
UAVOBJSRCFILENAMES += mixerstatus
UAVOBJSRCFILENAMES += nedaccel

1
flight/targets/boards/revolution/firmware/inc/FreeRTOSConfig.h
View File

@ -64,6 +64,7 @@
#define INCLUDE_xTaskGetSchedulerState 1
#define INCLUDE_xTaskGetCurrentTaskHandle 1
#define INCLUDE_uxTaskGetStackHighWaterMark 1
#define INCLUDE_xTaskGetIdleTaskHandle 1
/* This is the raw value as per the Cortex-M3 NVIC. Values can be 255
(lowest) to 1 (highest maskable) to 0 (highest non-maskable). */

2
flight/targets/boards/revolution/firmware/inc/pios_config.h
View File

@ -87,6 +87,8 @@
#define PIOS_INCLUDE_MS5611
#define PIOS_INCLUDE_MPXV
#define PIOS_INCLUDE_ETASV3
#define PIOS_INCLUDE_MS4525DO
/* #define PIOS_INCLUDE_HCSR04 */
/* PIOS receiver drivers */

9
flight/targets/boards/revolution/firmware/pios_board.c
View File

@ -224,6 +224,7 @@ uint32_t pios_com_debug_id;
uint32_t pios_com_gps_id = 0;
uint32_t pios_com_telem_usb_id = 0;
uint32_t pios_com_telem_rf_id = 0;
uint32_t pios_com_rf_id = 0;
uint32_t pios_com_bridge_id = 0;
uint32_t pios_com_overo_id = 0;
uint32_t pios_com_hkosd_id = 0;
@ -231,7 +232,7 @@ uint32_t pios_com_hkosd_id = 0;
uint32_t pios_com_vcp_id = 0;
#if defined(PIOS_INCLUDE_RFM22B)
uint32_t pios_rfm22b_id = 0;
uint32_t pios_rfm22b_id = 0;
#endif
uintptr_t pios_uavo_settings_fs_id;
@ -753,11 +754,15 @@ void PIOS_Board_Init(void)
uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN);
PIOS_Assert(rx_buffer);
PIOS_Assert(tx_buffer);
if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_rfm22b_com_driver, pios_rfm22b_id,
if (PIOS_COM_Init(&pios_com_rf_id, &pios_rfm22b_com_driver, pios_rfm22b_id,
rx_buffer, PIOS_COM_RFM22B_RF_RX_BUF_LEN,
tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) {
PIOS_Assert(0);
}
/* Set Telemetry to use OPLinkMini if no other telemetry is configured (USB always overrides anyway) */
if (!pios_com_telem_rf_id) {
pios_com_telem_rf_id = pios_com_rf_id;
}
oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_ENABLED;
// Set the RF data rate on the modem to ~2X the selected buad rate because the modem is half duplex.

11
flight/targets/boards/revolution/pios_board.h
View File

@ -119,12 +119,13 @@
// PIOS_I2C
// See also pios_board.c
// ------------------------
#define PIOS_I2C_MAX_DEVS 3
#define PIOS_I2C_MAX_DEVS 3
extern uint32_t pios_i2c_mag_pressure_adapter_id;
#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_mag_pressure_adapter_id)
#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_mag_pressure_adapter_id)
extern uint32_t pios_i2c_flexiport_adapter_id;
#define PIOS_I2C_FLEXI_ADAPTER (pios_i2c_flexiport_adapter_id)
#define PIOS_I2C_ETASV3_ADAPTER (PIOS_I2C_FLEXI_ADAPTER)
#define PIOS_I2C_FLEXI_ADAPTER (pios_i2c_flexiport_adapter_id)
#define PIOS_I2C_ETASV3_ADAPTER (PIOS_I2C_FLEXI_ADAPTER)
#define PIOS_I2C_MS4525DO_ADAPTER (PIOS_I2C_FLEXI_ADAPTER)
// -------------------------
// PIOS_USART
@ -140,6 +141,7 @@ extern uint32_t pios_i2c_flexiport_adapter_id;
// -------------------------
#define PIOS_COM_MAX_DEVS 4
extern uint32_t pios_com_telem_rf_id;
extern uint32_t pios_com_rf_id;
extern uint32_t pios_com_gps_id;
extern uint32_t pios_com_telem_usb_id;
extern uint32_t pios_com_bridge_id;
@ -148,6 +150,7 @@ extern uint32_t pios_com_hkosd_id;
#define PIOS_COM_GPS (pios_com_gps_id)
#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id)
#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id)
#define PIOS_COM_RF (pios_com_rf_id)
#define PIOS_COM_BRIDGE (pios_com_bridge_id)
#define PIOS_COM_VCP (pios_com_vcp_id)
#define PIOS_COM_OSDHK (pios_com_hkosd_id)

14
flight/targets/boards/revoproto/firmware/Makefile
View File

@ -29,24 +29,26 @@ USE_DSP_LIB ?= NO
# List of mandatory modules to include
MODULES += Sensors
MODULES += Attitude/revolution
#MODULES += StateEstimation # use instead of Attitude
#MODULES += Attitude/revolution
MODULES += StateEstimation # use instead of Attitude
MODULES += Altitude/revolution
MODULES += Airspeed
MODULES += AltitudeHold
MODULES += Stabilization
MODULES += VtolPathFollower
MODULES += ManualControl
MODULES += Receiver
MODULES += Actuator
MODULES += GPS
MODULES += TxPID
MODULES += CameraStab
MODULES += Battery
MODULES += FirmwareIAP
MODULES += PathPlanner
MODULES += FixedWingPathFollower
#MODULES += OveroSync ## OveroSync disabled until OP292 is fixed
MODULES += Osd/osdoutout
MODULES += Logging
MODULES += Telemetry
#MODULES += VtolPathFollower ## OP-700: VtolPathFollower disabled because its currently unsafe - remove this line once Sambas code has been merged
#MODULES += Battery
#MODULES += TxPID
OPTMODULES += ComUsbBridge

21
flight/targets/boards/revoproto/firmware/UAVObjects.inc
View File

@ -1,9 +1,5 @@
#####
# Project: OpenPilot
#
# Makefile for OpenPilot UAVObject code
#
# The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2011.
# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org
#
# This program is free software; you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
@ -18,12 +14,12 @@
# You should have received a copy of the GNU General Public License along
# with this program; if not, write to the Free Software Foundation, Inc.,
# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
#####
#
# These are the UAVObjects supposed to be build as part of the OpenPilot target
# (all architectures)
UAVOBJSRCFILENAMES =
UAVOBJSRCFILENAMES += accelgyrosettings
UAVOBJSRCFILENAMES += accessorydesired
UAVOBJSRCFILENAMES += actuatorcommand
UAVOBJSRCFILENAMES += actuatordesired
@ -34,12 +30,16 @@ UAVOBJSRCFILENAMES += gyrostate
UAVOBJSRCFILENAMES += gyrosensor
UAVOBJSRCFILENAMES += accelstate
UAVOBJSRCFILENAMES += accelsensor
UAVOBJSRCFILENAMES += magstate
UAVOBJSRCFILENAMES += magsensor
UAVOBJSRCFILENAMES += magstate
UAVOBJSRCFILENAMES += barosensor
UAVOBJSRCFILENAMES += airspeedsensor
UAVOBJSRCFILENAMES += airspeedsettings
UAVOBJSRCFILENAMES += airspeedstate
UAVOBJSRCFILENAMES += debuglogsettings
UAVOBJSRCFILENAMES += debuglogcontrol
UAVOBJSRCFILENAMES += debuglogstatus
UAVOBJSRCFILENAMES += debuglogentry
UAVOBJSRCFILENAMES += flightbatterysettings
UAVOBJSRCFILENAMES += firmwareiapobj
UAVOBJSRCFILENAMES += flightbatterystate
@ -61,10 +61,12 @@ UAVOBJSRCFILENAMES += homelocation
UAVOBJSRCFILENAMES += i2cstats
UAVOBJSRCFILENAMES += manualcontrolcommand
UAVOBJSRCFILENAMES += manualcontrolsettings
UAVOBJSRCFILENAMES += flightmodesettings
UAVOBJSRCFILENAMES += mixersettings
UAVOBJSRCFILENAMES += mixerstatus
UAVOBJSRCFILENAMES += nedaccel
UAVOBJSRCFILENAMES += objectpersistence
UAVOBJSRCFILENAMES += oplinkreceiver
UAVOBJSRCFILENAMES += overosyncstats
UAVOBJSRCFILENAMES += overosyncsettings
UAVOBJSRCFILENAMES += pathaction
@ -100,6 +102,8 @@ UAVOBJSRCFILENAMES += receiveractivity
UAVOBJSRCFILENAMES += cameradesired
UAVOBJSRCFILENAMES += camerastabsettings
UAVOBJSRCFILENAMES += altitudeholdsettings
UAVOBJSRCFILENAMES += oplinksettings
UAVOBJSRCFILENAMES += oplinkstatus
UAVOBJSRCFILENAMES += altitudefiltersettings
UAVOBJSRCFILENAMES += altitudeholddesired
UAVOBJSRCFILENAMES += altitudeholdstatus
@ -108,6 +112,7 @@ UAVOBJSRCFILENAMES += waypointactive
UAVOBJSRCFILENAMES += poilocation
UAVOBJSRCFILENAMES += poilearnsettings
UAVOBJSRCFILENAMES += mpu6000settings
UAVOBJSRCFILENAMES += txpidsettings
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(OPUAVSYNTHDIR)/$(UAVOBJSRCFILE).c )
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )

1
flight/targets/boards/revoproto/firmware/inc/FreeRTOSConfig.h
View File

@ -79,6 +79,7 @@
/* Enable run time stats collection */
#define configGENERATE_RUN_TIME_STATS 1
#define INCLUDE_uxTaskGetRunTime 1
#define INCLUDE_xTaskGetIdleTaskHandle 1
/*
* Once we move to CMSIS2 we can at least use:

1
flight/targets/boards/simposix/firmware/UAVObjects.inc
View File

@ -65,6 +65,7 @@ UAVOBJSRCFILENAMES += homelocation
UAVOBJSRCFILENAMES += i2cstats
UAVOBJSRCFILENAMES += manualcontrolcommand
UAVOBJSRCFILENAMES += manualcontrolsettings
UAVOBJSRCFILENAMES += flightmodesettings
UAVOBJSRCFILENAMES += mixersettings
UAVOBJSRCFILENAMES += mixerstatus
UAVOBJSRCFILENAMES += nedaccel

3
flight/uavobjects/eventdispatcher.c
View File

@ -55,6 +55,7 @@ typedef struct {
UAVObjEvent ev; /** The actual event */
UAVObjEventCallback cb; /** The callback function, or zero if none */
xQueueHandle queue; /** The queue or zero if none */
bool lowpriority; /** set to true for telemetry and other low priority stuffs, prevent raising warning */
} EventCallbackInfo;
/**
@ -341,7 +342,7 @@ static int32_t processPeriodicUpdates()
}
// Push event to queue, if one
if (objEntry->evInfo.queue != 0) {
if (xQueueSend(objEntry->evInfo.queue, &objEntry->evInfo.ev, 0) != pdTRUE) { // do not block if queue is full
if (xQueueSend(objEntry->evInfo.queue, &objEntry->evInfo.ev, 0) != pdTRUE && !objEntry->evInfo.ev.lowPriority) { // do not block if queue is full
if (objEntry->evInfo.ev.obj != NULL) {
mStats.lastErrorID = UAVObjGetID(objEntry->evInfo.ev.obj);
}

1
flight/uavobjects/inc/uavobjectmanager.h
View File

@ -120,6 +120,7 @@ typedef struct {
UAVObjHandle obj;
uint16_t instId;
UAVObjEventType event;
bool lowPriority; /* if true prevents raising warnings */
} UAVObjEvent;
/**

1
flight/uavobjects/uavobjectmanager.c
View File

@ -1739,6 +1739,7 @@ static int32_t sendEvent(struct UAVOBase *obj, uint16_t instId, UAVObjEventType
.obj = (UAVObjHandle)obj,
.event = triggered_event,
.instId = instId,
.lowPriority = false,
};
// Go through each object and push the event message in the queue (if event is activated for the queue)

6
flight/uavtalk/inc/uavtalk.h
View File

@ -59,10 +59,10 @@ int32_t UAVTalkSendObjectTimestamped(UAVTalkConnection connectionHandle, UAVObjH
int32_t UAVTalkSendObjectRequest(UAVTalkConnection connection, UAVObjHandle obj, uint16_t instId, int32_t timeoutMs);
UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connection, uint8_t rxbyte);
UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connection, uint8_t rxbyte);
UAVTalkRxState UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle);
int32_t UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle);
int32_t UAVTalkReceiveObject(UAVTalkConnection connectionHandle);
void UAVTalkGetStats(UAVTalkConnection connection, UAVTalkStats *stats);
void UAVTalkAddStats(UAVTalkConnection connection, UAVTalkStats *stats);
void UAVTalkGetStats(UAVTalkConnection connection, UAVTalkStats *stats, bool reset);
void UAVTalkAddStats(UAVTalkConnection connection, UAVTalkStats *stats, bool reset);
void UAVTalkResetStats(UAVTalkConnection connection);
void UAVTalkGetLastTimestamp(UAVTalkConnection connection, uint16_t *timestamp);
uint32_t UAVTalkGetPacketObjId(UAVTalkConnection connection);

33
flight/uavtalk/uavtalk.c
View File

@ -133,7 +133,7 @@ UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection connectionHandle)
* \param[in] connection UAVTalkConnection to be used
* @param[out] statsOut Statistics counters
*/
void UAVTalkGetStats(UAVTalkConnection connectionHandle, UAVTalkStats *statsOut)
void UAVTalkGetStats(UAVTalkConnection connectionHandle, UAVTalkStats *statsOut, bool reset)
{
UAVTalkConnectionData *connection;
@ -145,6 +145,11 @@ void UAVTalkGetStats(UAVTalkConnection connectionHandle, UAVTalkStats *statsOut)
// Copy stats
memcpy(statsOut, &connection->stats, sizeof(UAVTalkStats));
if (reset) {
// Clear stats
memset(&connection->stats, 0, sizeof(UAVTalkStats));
}
// Release lock
xSemaphoreGiveRecursive(connection->lock);
}
@ -154,7 +159,7 @@ void UAVTalkGetStats(UAVTalkConnection connectionHandle, UAVTalkStats *statsOut)
* \param[in] connection UAVTalkConnection to be used
* @param[out] statsOut Statistics counters
*/
void UAVTalkAddStats(UAVTalkConnection connectionHandle, UAVTalkStats *statsOut)
void UAVTalkAddStats(UAVTalkConnection connectionHandle, UAVTalkStats *statsOut, bool reset)
{
UAVTalkConnectionData *connection;
@ -175,6 +180,11 @@ void UAVTalkAddStats(UAVTalkConnection connectionHandle, UAVTalkStats *statsOut)
statsOut->rxSyncErrors += connection->stats.rxSyncErrors;
statsOut->rxCrcErrors += connection->stats.rxCrcErrors;
if (reset) {
// Clear stats
memset(&connection->stats, 0, sizeof(UAVTalkStats));
}
// Release lock
xSemaphoreGiveRecursive(connection->lock);
}
@ -589,7 +599,7 @@ UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uin
* \return 0 Success
* \return -1 Failure
*/
UAVTalkRxState UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle)
int32_t UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle)
{
UAVTalkConnectionData *inConnection;
@ -656,17 +666,17 @@ UAVTalkRxState UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkC
outConnection->stats.txBytes += (rc > 0) ? rc : 0;
// evaluate return value before releasing the lock
UAVTalkRxState rxState = 0;
int32_t ret = 0;
if (rc != (int32_t)(headerLength + inIproc->length + UAVTALK_CHECKSUM_LENGTH)) {
outConnection->stats.txErrors++;
rxState = -1;
ret = -1;
}
// Release lock
xSemaphoreGiveRecursive(outConnection->lock);
// Done
return rxState;
return ret;
}
/**
@ -862,7 +872,7 @@ static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint3
{
uint32_t numInst;
uint32_t n;
uint32_t ret = -1;
int32_t ret = -1;
// Important note : obj can be null (when type is NACK for example) so protect all obj dereferences.
@ -880,8 +890,8 @@ static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint3
// This allows the receiver to detect when the last object has been received (i.e. when instance 0 is received)
ret = 0;
for (n = 0; n < numInst; ++n) {
if (sendSingleObject(connection, type, objId, numInst - n - 1, obj) == -1) {
ret = -1;
ret = sendSingleObject(connection, type, objId, numInst - n - 1, obj);
if (ret == -1) {
break;
}
}
@ -904,8 +914,7 @@ static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint3
* \param[in] connection UAVTalkConnection to be used
* \param[in] type Transaction type
* \param[in] objId The object ID
* \param[in] instId The instance ID (can NOT be UAVOBJ_ALL_INSTANCES, use
() instead)
* \param[in] instId The instance ID (can NOT be UAVOBJ_ALL_INSTANCES, use () instead)
* \param[in] obj Object handle to send (null when type is NACK)
* \return 0 Success
* \return -1 Failure
@ -982,7 +991,7 @@ static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type,
connection->stats.txBytes += tx_msg_len;
} else {
connection->stats.txErrors++;
// TDOD rc == -1 connection not open, -2 buffer full should retry
// TODO rc == -1 connection not open, -2 buffer full should retry
connection->stats.txBytes += (rc > 0) ? rc : 0;
return -1;
}

5
ground/openpilotgcs/copydata.pro
View File

@ -32,7 +32,8 @@ GCS_LIBRARY_PATH
libQt5QuickParticles.so.5 \
libicui18n.so.51 \
libicuuc.so.51 \
libicudata.so.51
libicudata.so.51 \
libqgsttools_p.so.1
data_copy.commands += -@$(MKDIR) $$targetPath(\"$$GCS_QT_LIBRARY_PATH\") $$addNewline()
for(lib, QT_LIBS) {
@ -55,6 +56,8 @@ GCS_LIBRARY_PATH
imageformats/libqmng.so \
imageformats/libqsvg.so \
imageformats/libqtiff.so \
mediaservice/libgstaudiodecoder.so \
mediaservice/libgstmediaplayer.so \
platforms/libqxcb.so \
sqldrivers/libqsqlite.so
for(lib, QT_PLUGIN_LIBS) {

297
ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg
View File

@ -14,8 +14,8 @@
height="79.57505"
id="svg3604"
version="1.1"
inkscape:version="0.48.2 r9819"
sodipodi:docname="system-health.svg"
inkscape:version="0.48.4 r9939"
sodipodi:docname="system-health-mod.svg"
inkscape:export-filename="C:\NoBackup\OpenPilot\mainboard-health.png"
inkscape:export-xdpi="269.53"
inkscape:export-ydpi="269.53"
@ -27,16 +27,16 @@
borderopacity="1.0"
inkscape:pageopacity="0.0"
inkscape:pageshadow="2"
inkscape:zoom="9.3551549"
inkscape:cx="42.866204"
inkscape:cy="35.940147"
inkscape:current-layer="g4794"
inkscape:zoom="10.554213"
inkscape:cx="47.1292"
inkscape:cy="39.787519"
inkscape:current-layer="svg3604"
id="namedview3608"
showgrid="false"
inkscape:window-width="1920"
inkscape:window-height="1178"
inkscape:window-x="0"
inkscape:window-y="0"
inkscape:window-height="1025"
inkscape:window-x="-2"
inkscape:window-y="-3"
inkscape:window-maximized="1"
showguides="true"
inkscape:guide-bbox="true">
@ -656,39 +656,48 @@
<path
id="path4061"
style="font-size:9px;fill:#ffffff"
d="m 524.3844,367.59399 1.32275,0 1.67432,4.46485 1.68311,-4.46485 1.32275,0 0,6.56104 -0.86572,0 0,-5.76123 -1.6919,4.5 -0.89209,0 -1.69189,-4.5 0,5.76123 -0.86133,0 0,-6.56104" />
d="m 524.3844,367.59399 1.32275,0 1.67432,4.46485 1.68311,-4.46485 1.32275,0 0,6.56104 -0.86572,0 0,-5.76123 -1.6919,4.5 -0.89209,0 -1.69189,-4.5 0,5.76123 -0.86133,0 0,-6.56104"
inkscape:connector-curvature="0" />
<path
id="path4063"
style="font-size:9px;fill:#ffffff"
d="m 534.35559,371.68091 c -0.65332,0 -1.10596,0.0747 -1.35791,0.22412 -0.25195,0.14942 -0.37793,0.4043 -0.37793,0.76465 0,0.28711 0.0937,0.51562 0.28125,0.68554 0.19043,0.167 0.44824,0.25049 0.77344,0.25049 0.44824,0 0.80712,-0.1582 1.07666,-0.47461 0.27246,-0.31933 0.40869,-0.74267 0.40869,-1.27002 l 0,-0.18017 -0.8042,0 m 1.61279,-0.33399 0,2.80811 -0.80859,0 0,-0.74707 c -0.18457,0.29883 -0.41455,0.52002 -0.68994,0.66357 -0.27539,0.14063 -0.61231,0.21094 -1.01074,0.21094 -0.50391,0 -0.90528,-0.14062 -1.20411,-0.42187 -0.29589,-0.28418 -0.44384,-0.66358 -0.44384,-1.13819 0,-0.55371 0.18457,-0.97119 0.55371,-1.25244 0.37207,-0.28125 0.92578,-0.42187 1.66113,-0.42187 l 1.13379,0 0,-0.0791 c 0,-0.37206 -0.12305,-0.65917 -0.36914,-0.86132 -0.24317,-0.20508 -0.58594,-0.30762 -1.02832,-0.30762 -0.28125,0 -0.55518,0.0337 -0.82178,0.10107 -0.2666,0.0674 -0.52295,0.16846 -0.76904,0.30323 l 0,-0.74707 c 0.2959,-0.11426 0.58301,-0.19922 0.86133,-0.25489 0.27832,-0.0586 0.54931,-0.0879 0.81299,-0.0879 0.71191,10e-6 1.24364,0.18458 1.59521,0.55371 0.35156,0.36915 0.52734,0.92872 0.52734,1.67871" />
d="m 534.35559,371.68091 c -0.65332,0 -1.10596,0.0747 -1.35791,0.22412 -0.25195,0.14942 -0.37793,0.4043 -0.37793,0.76465 0,0.28711 0.0937,0.51562 0.28125,0.68554 0.19043,0.167 0.44824,0.25049 0.77344,0.25049 0.44824,0 0.80712,-0.1582 1.07666,-0.47461 0.27246,-0.31933 0.40869,-0.74267 0.40869,-1.27002 l 0,-0.18017 -0.8042,0 m 1.61279,-0.33399 0,2.80811 -0.80859,0 0,-0.74707 c -0.18457,0.29883 -0.41455,0.52002 -0.68994,0.66357 -0.27539,0.14063 -0.61231,0.21094 -1.01074,0.21094 -0.50391,0 -0.90528,-0.14062 -1.20411,-0.42187 -0.29589,-0.28418 -0.44384,-0.66358 -0.44384,-1.13819 0,-0.55371 0.18457,-0.97119 0.55371,-1.25244 0.37207,-0.28125 0.92578,-0.42187 1.66113,-0.42187 l 1.13379,0 0,-0.0791 c 0,-0.37206 -0.12305,-0.65917 -0.36914,-0.86132 -0.24317,-0.20508 -0.58594,-0.30762 -1.02832,-0.30762 -0.28125,0 -0.55518,0.0337 -0.82178,0.10107 -0.2666,0.0674 -0.52295,0.16846 -0.76904,0.30323 l 0,-0.74707 c 0.2959,-0.11426 0.58301,-0.19922 0.86133,-0.25489 0.27832,-0.0586 0.54931,-0.0879 0.81299,-0.0879 0.71191,10e-6 1.24364,0.18458 1.59521,0.55371 0.35156,0.36915 0.52734,0.92872 0.52734,1.67871"
inkscape:connector-curvature="0" />
<path
id="path4065"
style="font-size:9px;fill:#ffffff"
d="m 537.63831,369.23315 0.80859,0 0,4.92188 -0.80859,0 0,-4.92188 m 0,-1.91601 0.80859,0 0,1.02392 -0.80859,0 0,-1.02392" />
d="m 537.63831,369.23315 0.80859,0 0,4.92188 -0.80859,0 0,-4.92188 m 0,-1.91601 0.80859,0 0,1.02392 -0.80859,0 0,-1.02392"
inkscape:connector-curvature="0" />
<path
id="path4067"
style="font-size:9px;fill:#ffffff"
d="m 544.22571,371.18433 0,2.9707 -0.8086,0 0,-2.94434 c 0,-0.46581 -0.0908,-0.81445 -0.27246,-1.0459 -0.18164,-0.23144 -0.4541,-0.34716 -0.81738,-0.34716 -0.43653,0 -0.78076,0.13916 -1.03271,0.41748 -0.25196,0.27832 -0.37794,0.65772 -0.37793,1.13818 l 0,2.78174 -0.81299,0 0,-4.92188 0.81299,0 0,0.76465 c 0.19335,-0.29589 0.4204,-0.51708 0.68115,-0.66357 0.26367,-0.14648 0.56689,-0.21972 0.90967,-0.21973 0.56542,10e-6 0.99316,0.17579 1.2832,0.52735 0.29003,0.34863 0.43505,0.86279 0.43506,1.54248" />
d="m 544.22571,371.18433 0,2.9707 -0.8086,0 0,-2.94434 c 0,-0.46581 -0.0908,-0.81445 -0.27246,-1.0459 -0.18164,-0.23144 -0.4541,-0.34716 -0.81738,-0.34716 -0.43653,0 -0.78076,0.13916 -1.03271,0.41748 -0.25196,0.27832 -0.37794,0.65772 -0.37793,1.13818 l 0,2.78174 -0.81299,0 0,-4.92188 0.81299,0 0,0.76465 c 0.19335,-0.29589 0.4204,-0.51708 0.68115,-0.66357 0.26367,-0.14648 0.56689,-0.21972 0.90967,-0.21973 0.56542,10e-6 0.99316,0.17579 1.2832,0.52735 0.29003,0.34863 0.43505,0.86279 0.43506,1.54248"
inkscape:connector-curvature="0" />
<path
id="path4069"
style="font-size:9px;fill:#ffffff"
d="m 549.38049,371.69849 c 0,-0.59473 -0.12305,-1.06055 -0.36914,-1.39746 -0.24317,-0.33984 -0.57861,-0.50977 -1.00635,-0.50977 -0.42773,0 -0.76465,0.16993 -1.01074,0.50977 -0.24316,0.33691 -0.36475,0.80273 -0.36474,1.39746 -10e-6,0.59472 0.12158,1.06201 0.36474,1.40185 0.24609,0.33692 0.58301,0.50537 1.01074,0.50537 0.42774,0 0.76318,-0.16845 1.00635,-0.50537 0.24609,-0.33984 0.36914,-0.80713 0.36914,-1.40185 m -2.75097,-1.71827 c 0.16992,-0.29296 0.38378,-0.50976 0.6416,-0.65039 0.26074,-0.14355 0.57128,-0.21532 0.93164,-0.21533 0.59765,10e-6 1.08251,0.23731 1.45459,0.71192 0.37499,0.47461 0.56249,1.09863 0.5625,1.87207 -10e-6,0.77344 -0.18751,1.39746 -0.5625,1.87207 -0.37208,0.47461 -0.85694,0.71191 -1.45459,0.71191 -0.36036,0 -0.6709,-0.0703 -0.93164,-0.21094 -0.25782,-0.14355 -0.47168,-0.36181 -0.6416,-0.65478 l 0,0.73828 -0.81299,0 0,-6.83789 0.81299,0 0,2.66308" />
d="m 549.38049,371.69849 c 0,-0.59473 -0.12305,-1.06055 -0.36914,-1.39746 -0.24317,-0.33984 -0.57861,-0.50977 -1.00635,-0.50977 -0.42773,0 -0.76465,0.16993 -1.01074,0.50977 -0.24316,0.33691 -0.36475,0.80273 -0.36474,1.39746 -10e-6,0.59472 0.12158,1.06201 0.36474,1.40185 0.24609,0.33692 0.58301,0.50537 1.01074,0.50537 0.42774,0 0.76318,-0.16845 1.00635,-0.50537 0.24609,-0.33984 0.36914,-0.80713 0.36914,-1.40185 m -2.75097,-1.71827 c 0.16992,-0.29296 0.38378,-0.50976 0.6416,-0.65039 0.26074,-0.14355 0.57128,-0.21532 0.93164,-0.21533 0.59765,10e-6 1.08251,0.23731 1.45459,0.71192 0.37499,0.47461 0.56249,1.09863 0.5625,1.87207 -10e-6,0.77344 -0.18751,1.39746 -0.5625,1.87207 -0.37208,0.47461 -0.85694,0.71191 -1.45459,0.71191 -0.36036,0 -0.6709,-0.0703 -0.93164,-0.21094 -0.25782,-0.14355 -0.47168,-0.36181 -0.6416,-0.65478 l 0,0.73828 -0.81299,0 0,-6.83789 0.81299,0 0,2.66308"
inkscape:connector-curvature="0" />
<path
id="path4071"
style="font-size:9px;fill:#ffffff"
d="m 553.46741,369.80005 c -0.4336,0 -0.77637,0.16992 -1.02832,0.50976 -0.25196,0.33692 -0.37793,0.79981 -0.37793,1.38868 0,0.58887 0.12451,1.05322 0.37353,1.39306 0.25195,0.33692 0.59619,0.50537 1.03272,0.50537 0.43066,0 0.77197,-0.16992 1.02392,-0.50976 0.25195,-0.33984 0.37793,-0.80273 0.37793,-1.38867 0,-0.58301 -0.12598,-1.04443 -0.37793,-1.38428 -0.25195,-0.34277 -0.59326,-0.51416 -1.02392,-0.51416 m 0,-0.68555 c 0.70312,10e-6 1.25536,0.22852 1.65674,0.68555 0.40136,0.45703 0.60204,1.08985 0.60205,1.89844 -10e-6,0.80566 -0.20069,1.43847 -0.60205,1.89843 -0.40138,0.45704 -0.95362,0.68555 -1.65674,0.68555 -0.70606,0 -1.25977,-0.22851 -1.66114,-0.68555 -0.39843,-0.45996 -0.59765,-1.09277 -0.59765,-1.89843 0,-0.80859 0.19922,-1.44141 0.59765,-1.89844 0.40137,-0.45703 0.95508,-0.68554 1.66114,-0.68555" />
d="m 553.46741,369.80005 c -0.4336,0 -0.77637,0.16992 -1.02832,0.50976 -0.25196,0.33692 -0.37793,0.79981 -0.37793,1.38868 0,0.58887 0.12451,1.05322 0.37353,1.39306 0.25195,0.33692 0.59619,0.50537 1.03272,0.50537 0.43066,0 0.77197,-0.16992 1.02392,-0.50976 0.25195,-0.33984 0.37793,-0.80273 0.37793,-1.38867 0,-0.58301 -0.12598,-1.04443 -0.37793,-1.38428 -0.25195,-0.34277 -0.59326,-0.51416 -1.02392,-0.51416 m 0,-0.68555 c 0.70312,10e-6 1.25536,0.22852 1.65674,0.68555 0.40136,0.45703 0.60204,1.08985 0.60205,1.89844 -10e-6,0.80566 -0.20069,1.43847 -0.60205,1.89843 -0.40138,0.45704 -0.95362,0.68555 -1.65674,0.68555 -0.70606,0 -1.25977,-0.22851 -1.66114,-0.68555 -0.39843,-0.45996 -0.59765,-1.09277 -0.59765,-1.89843 0,-0.80859 0.19922,-1.44141 0.59765,-1.89844 0.40137,-0.45703 0.95508,-0.68554 1.66114,-0.68555"
inkscape:connector-curvature="0" />
<path
id="path4073"
style="font-size:9px;fill:#ffffff"
d="m 559.29895,371.68091 c -0.65332,0 -1.10596,0.0747 -1.35791,0.22412 -0.25195,0.14942 -0.37793,0.4043 -0.37793,0.76465 0,0.28711 0.0937,0.51562 0.28125,0.68554 0.19043,0.167 0.44824,0.25049 0.77344,0.25049 0.44824,0 0.80712,-0.1582 1.07666,-0.47461 0.27246,-0.31933 0.40869,-0.74267 0.40869,-1.27002 l 0,-0.18017 -0.8042,0 m 1.61279,-0.33399 0,2.80811 -0.80859,0 0,-0.74707 c -0.18457,0.29883 -0.41455,0.52002 -0.68994,0.66357 -0.2754,0.14063 -0.61231,0.21094 -1.01074,0.21094 -0.50391,0 -0.90528,-0.14062 -1.20411,-0.42187 -0.29589,-0.28418 -0.44384,-0.66358 -0.44384,-1.13819 0,-0.55371 0.18457,-0.97119 0.55371,-1.25244 0.37207,-0.28125 0.92578,-0.42187 1.66113,-0.42187 l 1.13379,0 0,-0.0791 c 0,-0.37206 -0.12305,-0.65917 -0.36914,-0.86132 -0.24317,-0.20508 -0.58594,-0.30762 -1.02832,-0.30762 -0.28125,0 -0.55518,0.0337 -0.82178,0.10107 -0.2666,0.0674 -0.52295,0.16846 -0.76904,0.30323 l 0,-0.74707 c 0.2959,-0.11426 0.583,-0.19922 0.86133,-0.25489 0.27831,-0.0586 0.54931,-0.0879 0.81298,-0.0879 0.71192,10e-6 1.24365,0.18458 1.59522,0.55371 0.35156,0.36915 0.52734,0.92872 0.52734,1.67871" />
d="m 559.29895,371.68091 c -0.65332,0 -1.10596,0.0747 -1.35791,0.22412 -0.25195,0.14942 -0.37793,0.4043 -0.37793,0.76465 0,0.28711 0.0937,0.51562 0.28125,0.68554 0.19043,0.167 0.44824,0.25049 0.77344,0.25049 0.44824,0 0.80712,-0.1582 1.07666,-0.47461 0.27246,-0.31933 0.40869,-0.74267 0.40869,-1.27002 l 0,-0.18017 -0.8042,0 m 1.61279,-0.33399 0,2.80811 -0.80859,0 0,-0.74707 c -0.18457,0.29883 -0.41455,0.52002 -0.68994,0.66357 -0.2754,0.14063 -0.61231,0.21094 -1.01074,0.21094 -0.50391,0 -0.90528,-0.14062 -1.20411,-0.42187 -0.29589,-0.28418 -0.44384,-0.66358 -0.44384,-1.13819 0,-0.55371 0.18457,-0.97119 0.55371,-1.25244 0.37207,-0.28125 0.92578,-0.42187 1.66113,-0.42187 l 1.13379,0 0,-0.0791 c 0,-0.37206 -0.12305,-0.65917 -0.36914,-0.86132 -0.24317,-0.20508 -0.58594,-0.30762 -1.02832,-0.30762 -0.28125,0 -0.55518,0.0337 -0.82178,0.10107 -0.2666,0.0674 -0.52295,0.16846 -0.76904,0.30323 l 0,-0.74707 c 0.2959,-0.11426 0.583,-0.19922 0.86133,-0.25489 0.27831,-0.0586 0.54931,-0.0879 0.81298,-0.0879 0.71192,10e-6 1.24365,0.18458 1.59522,0.55371 0.35156,0.36915 0.52734,0.92872 0.52734,1.67871"
inkscape:connector-curvature="0" />
<path
id="path4075"
style="font-size:9px;fill:#ffffff"
d="m 565.43372,369.98901 c -0.0908,-0.0527 -0.19044,-0.0908 -0.29883,-0.11425 -0.10547,-0.0264 -0.22266,-0.0395 -0.35156,-0.0395 -0.45704,0 -0.8086,0.14941 -1.05469,0.44824 -0.24317,0.2959 -0.36475,0.72217 -0.36475,1.27881 l 0,2.59277 -0.81299,0 0,-4.92188 0.81299,0 0,0.76465 c 0.16992,-0.29882 0.39111,-0.52001 0.66358,-0.66357 0.27245,-0.14648 0.60351,-0.21972 0.99316,-0.21973 0.0557,10e-6 0.11718,0.004 0.18457,0.0132 0.0674,0.006 0.14209,0.0161 0.22412,0.0308 l 0.004,0.83056" />
d="m 565.43372,369.98901 c -0.0908,-0.0527 -0.19044,-0.0908 -0.29883,-0.11425 -0.10547,-0.0264 -0.22266,-0.0395 -0.35156,-0.0395 -0.45704,0 -0.8086,0.14941 -1.05469,0.44824 -0.24317,0.2959 -0.36475,0.72217 -0.36475,1.27881 l 0,2.59277 -0.81299,0 0,-4.92188 0.81299,0 0,0.76465 c 0.16992,-0.29882 0.39111,-0.52001 0.66358,-0.66357 0.27245,-0.14648 0.60351,-0.21972 0.99316,-0.21973 0.0557,10e-6 0.11718,0.004 0.18457,0.0132 0.0674,0.006 0.14209,0.0161 0.22412,0.0308 l 0.004,0.83056"
inkscape:connector-curvature="0" />
<path
id="path4077"
style="font-size:9px;fill:#ffffff"
d="m 569.37122,369.98022 0,-2.66308 0.80859,0 0,6.83789 -0.80859,0 0,-0.73828 c -0.16993,0.29297 -0.38526,0.51123 -0.646,0.65478 -0.25782,0.14063 -0.56836,0.21094 -0.93164,0.21094 -0.59473,0 -1.07959,-0.2373 -1.45459,-0.71191 -0.37207,-0.47461 -0.55811,-1.09863 -0.55811,-1.87207 0,-0.77344 0.18604,-1.39746 0.55811,-1.87207 0.375,-0.47461 0.85986,-0.71191 1.45459,-0.71192 0.36328,10e-6 0.67382,0.0718 0.93164,0.21533 0.26074,0.14063 0.47607,0.35743 0.646,0.65039 m -2.75538,1.71827 c 0,0.59472 0.12159,1.06201 0.36475,1.40185 0.24609,0.33692 0.58301,0.50537 1.01074,0.50537 0.42773,0 0.76465,-0.16845 1.01075,-0.50537 0.24608,-0.33984 0.36913,-0.80713 0.36914,-1.40185 -10e-6,-0.59473 -0.12306,-1.06055 -0.36914,-1.39746 -0.2461,-0.33984 -0.58302,-0.50977 -1.01075,-0.50977 -0.42773,0 -0.76465,0.16993 -1.01074,0.50977 -0.24316,0.33691 -0.36475,0.80273 -0.36475,1.39746" />
d="m 569.37122,369.98022 0,-2.66308 0.80859,0 0,6.83789 -0.80859,0 0,-0.73828 c -0.16993,0.29297 -0.38526,0.51123 -0.646,0.65478 -0.25782,0.14063 -0.56836,0.21094 -0.93164,0.21094 -0.59473,0 -1.07959,-0.2373 -1.45459,-0.71191 -0.37207,-0.47461 -0.55811,-1.09863 -0.55811,-1.87207 0,-0.77344 0.18604,-1.39746 0.55811,-1.87207 0.375,-0.47461 0.85986,-0.71191 1.45459,-0.71192 0.36328,10e-6 0.67382,0.0718 0.93164,0.21533 0.26074,0.14063 0.47607,0.35743 0.646,0.65039 m -2.75538,1.71827 c 0,0.59472 0.12159,1.06201 0.36475,1.40185 0.24609,0.33692 0.58301,0.50537 1.01074,0.50537 0.42773,0 0.76465,-0.16845 1.01075,-0.50537 0.24608,-0.33984 0.36913,-0.80713 0.36914,-1.40185 -10e-6,-0.59473 -0.12306,-1.06055 -0.36914,-1.39746 -0.2461,-0.33984 -0.58302,-0.50977 -1.01075,-0.50977 -0.42773,0 -0.76465,0.16993 -1.01074,0.50977 -0.24316,0.33691 -0.36475,0.80273 -0.36475,1.39746"
inkscape:connector-curvature="0" />
</g>
<rect
inkscape:label="#rect4000-8-5"
@ -701,7 +710,7 @@
<rect
inkscape:label="#rect4000-8-0-9"
style="fill:#332d2d;fill-opacity:1;stroke:#ffffff;stroke-width:0.53149605;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
id="ManualControl"
id="Receiver"
width="13.893178"
height="56.637238"
x="576.71594"
@ -712,7 +721,7 @@
id="Power"
width="18.023544"
height="8.2846708"
x="501.80048"
x="499.24579"
y="346.10165" />
<rect
inkscape:label="#rect4000-7-6-2-2111"
@ -720,29 +729,37 @@
id="Battery"
width="18.023544"
height="8.2846708"
x="501.69357"
x="499.13889"
y="355.57822" />
<rect
style="fill:#332d2d;fill-opacity:1;stroke:#ffffff;stroke-width:0.53149605;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
id="Sensors"
width="9.741457"
width="8"
height="20.208858"
x="556.2887"
x="551.89978"
y="345.96741"
inkscape:label="#Sensors" />
<rect
style="fill:#332d2d;fill-opacity:1;stroke:#ffffff;stroke-width:0.53149605;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline"
id="Airspeed"
width="8"
height="20.208858"
x="562.37372"
y="345.95883"
inkscape:label="#Airspeed" />
<rect
style="fill:#332d2d;fill-opacity:1;stroke:#ffffff;stroke-width:0.53149605;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
id="I2C"
width="18.023544"
height="8.0567961"
x="569.6358"
y="346.10165" />
x="572.51758"
y="345.79932" />
<rect
style="fill:#332d2d;fill-opacity:1;stroke:#ffffff;stroke-width:0.51991701;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
id="Temp-Baro"
width="18.036383"
height="8.0690594"
x="569.63025"
x="572.60791"
y="355.2207" />
<rect
style="fill:#332d2d;fill-opacity:1;stroke:#ffffff;stroke-width:0.53149605;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
@ -812,19 +829,18 @@
style="fill:#332d2d;fill-opacity:1;stroke:#ffffff;stroke-width:0.53149605;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
inkscape:label="#rect4000-7-6-7-1" />
<rect
transform="translate(497.66563,344.28037)"
style="fill:#332d2d;fill-opacity:1;stroke:#ffffff;stroke-width:0.55302167;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline"
id="USARTs"
width="30.08411"
height="20.040594"
x="25.010578"
y="1.9175365" />
x="519.2749"
y="346.19791" />
<rect
style="fill:none;stroke:#ffffff;stroke-width:0.29055119;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline"
id="Telemetry"
width="23.347105"
height="5.6002355"
x="523.76398"
x="520.36267"
y="347.41809"
inkscape:label="#rect4552-27" />
<rect
@ -849,7 +865,7 @@
id="GPS"
width="23.347105"
height="5.6002355"
x="523.76398"
x="520.36267"
y="353.41809" />
<rect
y="375.55762"
@ -951,8 +967,8 @@
id="I2C-OK"
width="18.153212"
height="8.1216154"
x="71.970177"
y="1.8212841"
x="74.851883"
y="1.5189452"
inkscape:label="#rect8374" />
</g>
<g
@ -961,8 +977,8 @@
inkscape:label="I2C-Warning"
style="display:none">
<rect
y="1.8212841"
x="71.970177"
y="1.5189452"
x="74.851883"
height="8.1216154"
width="18.153212"
id="I2C-Warning"
@ -977,17 +993,19 @@
<g
style="stroke:#cf0e0e;stroke-width:2;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline"
id="I2C-Error"
transform="translate(33.813538,-31.144609)"
transform="translate(36.695254,-31.446948)"
inkscape:label="#g4084">
<path
style="fill:none;stroke:#cf0e0e;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
d="M 38.076545,33.292974 56.14311,41.28257"
id="path3156" />
id="path3156"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#cf0e0e;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
d="m 38.220502,41.354548 17.85063,-8.061574"
id="path3158"
inkscape:label="#path4088-8" />
inkscape:label="#path4088-8"
inkscape:connector-curvature="0" />
</g>
</g>
<g
@ -1000,8 +1018,8 @@
id="I2C-Critical"
width="18.153212"
height="8.1216154"
x="71.970177"
y="1.8212841"
x="74.851883"
y="1.5189452"
inkscape:label="#rect4080" />
</g>
<g
@ -1012,7 +1030,7 @@
<rect
inkscape:label="#rect8374"
y="9.1076469"
x="26.199663"
x="22.798349"
height="5.6002355"
width="23.347105"
id="GPS-OK"
@ -1029,7 +1047,7 @@
id="GPS-Warning"
width="23.347105"
height="5.6002355"
x="26.199663"
x="22.798349"
y="9.1076469" />
</g>
<g
@ -1039,18 +1057,20 @@
inkscape:groupmode="layer">
<g
inkscape:label="#g4084"
transform="matrix(1.2853162,0,0,0.64013573,-22.75,-11.845582)"
id="GPS-Error"
style="stroke:#cf0e0e;stroke-width:2;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline">
style="stroke:#cf0e0e;stroke-width:2;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline"
transform="matrix(1.2853162,0,0,0.64013573,-26.151312,-11.845582)">
<path
id="path3176"
d="M 38.076545,33.292974 56.14311,41.28257"
style="fill:none;stroke:#cf0e0e;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none" />
d="m 38,33.292974 17.85,8"
style="fill:none;stroke:#cf0e0e;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
inkscape:connector-curvature="0" />
<path
inkscape:label="#path4088-8"
id="path3178"
d="m 38.220502,41.354548 17.85063,-8.061574"
style="fill:none;stroke:#cf0e0e;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none" />
d="m 38,41.354548 17.85,-8"
style="fill:none;stroke:#cf0e0e;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
inkscape:connector-curvature="0" />
</g>
</g>
<g
@ -1061,12 +1081,78 @@
<rect
inkscape:label="#rect4080"
y="9.1076469"
x="26.199663"
x="22.798349"
height="5.6002355"
width="23.347105"
id="GPS-Critical"
style="fill:#cf0e0e;fill-opacity:1;stroke:#ffffff;stroke-width:0.30000001;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
</g>
<g
inkscape:groupmode="layer"
id="layer50"
inkscape:label="Airspeed-OK"
style="display:none">
<rect
style="fill:#04b629;fill-opacity:1;stroke:#ffffff;stroke-width:0.40371776;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline"
id="Airspeed-OK"
width="8"
height="20.241537"
x="64.72757"
y="1.6625173"
inkscape:label="#rect4552-27" />
</g>
<g
inkscape:groupmode="layer"
id="layer20"
inkscape:label="Airspeed-Warning"
style="display:none">
<rect
inkscape:label="#rect4552-27"
y="1.7688711"
x="64.727081"
height="20.135679"
width="8"
id="Airspeed-Warning"
style="fill:#f1b907;fill-opacity:1;stroke:#ffffff;stroke-width:0.4026823;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
</g>
<g
inkscape:groupmode="layer"
id="layer11"
inkscape:label="Airspeed-Error"
style="display:none">
<g
inkscape:label="#g5707"
transform="matrix(0.98237131,0,0,1.3895115,61.95219,0.4046338)"
id="Airspeed-Error"
style="stroke:#cf0e0e;stroke-width:2;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline">
<path
sodipodi:nodetypes="cc"
id="path5709-4"
d="M 4.2641874,1.9700434 9.4,14.522849"
style="fill:none;stroke:#cf0e0e;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
inkscape:connector-curvature="0" />
<path
sodipodi:nodetypes="cc"
id="path5711-6"
d="M 4.5521007,14.319264 9.4,1.9402289"
style="fill:none;stroke:#cf0e0e;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
inkscape:connector-curvature="0" />
</g>
</g>
<g
inkscape:groupmode="layer"
id="layer6"
inkscape:label="Airspeed-Critical"
style="display:none">
<rect
style="fill:#cf0e0e;fill-opacity:1;stroke:#ffffff;stroke-width:0.40371776;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline"
id="Airspeed-Critical"
width="8"
height="20.241537"
x="64.72757"
y="1.6625173"
inkscape:label="#rect4552-27" />
</g>
<g
style="display:none"
inkscape:label="Sensors-OK"
@ -1075,9 +1161,9 @@
<rect
style="fill:#04b629;fill-opacity:1;stroke:#ffffff;stroke-width:0.40371776;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline"
id="Sensors-OK"
width="9.6456022"
width="8"
height="20.241537"
x="58.617558"
x="54.228596"
y="1.586597"
inkscape:label="#rect4552-27" />
</g>
@ -1089,9 +1175,9 @@
<rect
inkscape:label="#rect4552-27"
y="1.6929722"
x="58.617039"
x="54.228077"
height="20.135679"
width="9.6466379"
width="8"
id="Sensors-Warning"
style="fill:#f1b907;fill-opacity:1;stroke:#ffffff;stroke-width:0.4026823;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
</g>
@ -1102,19 +1188,19 @@
inkscape:groupmode="layer">
<g
inkscape:label="#g5707"
transform="matrix(0.98237131,0,0,1.3895115,55.842113,0.32873234)"
transform="matrix(0.98237131,0,0,1.3895115,51.453151,0.32873234)"
id="Sensors-Error"
style="stroke:#cf0e0e;stroke-width:2;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline">
<path
sodipodi:nodetypes="cc"
id="path5709"
d="M 4.2641874,1.9700434 11.103735,14.522849"
d="M 4.2641874,1.9700434 9.4,14.522849"
style="fill:none;stroke:#cf0e0e;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
inkscape:connector-curvature="0" />
<path
sodipodi:nodetypes="cc"
id="path5711"
d="M 4.5521007,14.319264 11.235342,1.9402289"
d="M 4.5521007,14.319264 9.4,1.9402289"
style="fill:none;stroke:#cf0e0e;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
inkscape:connector-curvature="0" />
</g>
@ -1127,15 +1213,15 @@
<rect
style="fill:#cf0e0e;fill-opacity:1;stroke:#ffffff;stroke-width:0.40371776;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline"
id="Sensors-Critical"
width="9.6456032"
width="8"
height="20.241537"
x="58.617558"
x="54.228596"
y="1.586597"
inkscape:label="#rect4552-27" />
</g>
<g
style="display:none"
inkscape:label="ManualControl-OK"
inkscape:label="Receiver-OK"
id="layer36"
inkscape:groupmode="layer">
<rect
@ -1143,14 +1229,14 @@
x="576.71594"
height="56.637238"
width="13.893178"
id="ManualControl-OK"
id="Receiver-OK"
style="fill:#04b629;fill-opacity:1;stroke:#ffffff;stroke-width:0.53149605;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline"
inkscape:label="#rect4000-8-0-9"
transform="translate(-497.66563,-344.28037)" />
</g>
<g
style="display:none"
inkscape:label="ManualControl-Warning"
inkscape:label="Receiver-Warning"
id="layer33"
inkscape:groupmode="layer">
<rect
@ -1158,19 +1244,19 @@
x="576.71594"
height="56.637238"
width="13.893178"
id="ManualControl-Warning"
id="Receiver-Warning"
style="fill:#f1b907;fill-opacity:1;stroke:#ffffff;stroke-width:0.53149605;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline"
inkscape:label="#rect4000-8-0-9"
transform="translate(-497.66563,-344.28037)" />
</g>
<g
style="display:none"
inkscape:label="ManualControl-Error"
inkscape:label="Receiver-Error"
id="layer34"
inkscape:groupmode="layer">
<g
transform="translate(78,0)"
id="ManualControl-Error"
id="Receiver-Error"
style="stroke:#cf0e0e;stroke-opacity:1;display:inline"
inkscape:label="#g3878">
<path
@ -1185,14 +1271,14 @@
</g>
<g
style="display:none"
inkscape:label="ManualControl-Critical"
inkscape:label="Receiver-Critical"
id="layer35"
inkscape:groupmode="layer">
<rect
transform="translate(-497.66563,-344.28037)"
inkscape:label="#rect4000-8-0-9"
style="fill:#cf0e0e;fill-opacity:1;stroke:#ffffff;stroke-width:0.53149605;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline"
id="ManualControl-Critical"
id="Receiver-Critical"
width="13.893178"
height="56.637238"
x="576.71594"
@ -1332,7 +1418,7 @@
<rect
inkscape:label="#rect4552-27"
y="3.1377156"
x="26.098318"
x="22.697004"
height="5.6002355"
width="23.347105"
id="Telemetry-OK"
@ -1348,7 +1434,7 @@
id="Telemetry-Warning"
width="23.347105"
height="5.6002355"
x="26.098318"
x="22.697004"
y="3.1377156"
inkscape:label="#rect4552-27" />
</g>
@ -1360,15 +1446,18 @@
<g
inkscape:label="#g4357"
style="stroke:#cf0e0e;stroke-opacity:1;display:inline"
id="Telemetry-Error">
id="Telemetry-Error"
transform="translate(-3.4013125,0)">
<path
style="fill:none;stroke:#cf0e0e;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 26.488031,3.4219598 49.017254,8.3164874"
id="path4353" />
id="path4353"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#cf0e0e;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 49.089232,3.3499815 26.56001,8.3884657"
id="path4355" />
id="path4355"
inkscape:connector-curvature="0" />
</g>
</g>
<g
@ -1379,7 +1468,7 @@
<rect
inkscape:label="#rect4552-27"
y="3.1377156"
x="26.098318"
x="22.697004"
height="5.6002355"
width="23.347105"
id="Telemetry-Critical"
@ -1592,7 +1681,7 @@
<rect
inkscape:label="#rect7370"
y="1.8212841"
x="4.1348462"
x="1.5801588"
height="8.2846708"
width="18.023544"
id="Power-OK"
@ -1609,7 +1698,7 @@
id="Power-Warning"
width="18.023544"
height="8.2846708"
x="4.1348462"
x="1.5801588"
y="1.8212841" />
</g>
<g
@ -1620,7 +1709,8 @@
<g
inkscape:label="#g4026"
style="stroke:#cf0e0e;stroke-width:2;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
id="Power-Error">
id="Power-Error"
transform="translate(-1.9500095,0)">
<path
style="fill:none;stroke:#cf0e0e;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
d="M 3.9588091,1.7664579 22.097352,10.043968"
@ -1641,7 +1731,7 @@
<rect
inkscape:label="#rect7437"
y="1.8212841"
x="4.1348462"
x="1.5801588"
height="8.2846708"
width="18.023544"
id="Power-Critical"
@ -1657,7 +1747,7 @@
id="Battery-OK"
width="18.023544"
height="8.2846708"
x="4.0279531"
x="1.4732658"
y="11.334756"
inkscape:label="#rect7370" />
</g>
@ -1668,7 +1758,7 @@
style="display:none">
<rect
y="11.334756"
x="4.0279531"
x="1.4732658"
height="8.2846708"
width="18.023544"
id="Battery-Warning"
@ -1685,7 +1775,7 @@
id="Battery-Critical"
width="18.023544"
height="8.2846708"
x="4.1348462"
x="1.5801588"
y="11.344959"
inkscape:label="#rect7437" />
</g>
@ -1698,7 +1788,7 @@
id="Battery-Error"
style="stroke:#cf0e0e;stroke-width:2;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
inkscape:label="#g4026"
transform="translate(0,9.5134715)">
transform="translate(-1.9500095,9.5134715)">
<path
inkscape:connector-curvature="0"
id="path4708"
@ -1968,7 +2058,7 @@
<g
id="text4522-4"
style="font-size:40px;font-style:normal;font-weight:normal;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Bitstream Vera Sans"
transform="translate(0.21378587,-0.20358251)">
transform="translate(-2.3409014,-0.20358251)">
<path
inkscape:connector-curvature="0"
id="path3980"
@ -1997,7 +2087,8 @@
</g>
<g
id="I2C-Text"
style="font-size:40px;font-style:normal;font-weight:normal;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Bitstream Vera Sans">
style="font-size:40px;font-style:normal;font-weight:normal;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Bitstream Vera Sans"
transform="translate(2.8817153,-0.30233889)">
<path
inkscape:connector-curvature="0"
id="path3924"
@ -2016,7 +2107,8 @@
</g>
<g
id="text4647-0-2-6"
style="font-size:40px;font-style:normal;font-weight:normal;text-align:center;text-anchor:middle;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Bitstream Vera Sans">
style="font-size:40px;font-style:normal;font-weight:normal;text-align:center;text-anchor:middle;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Bitstream Vera Sans"
transform="translate(2.9776741,0)">
<path
inkscape:connector-curvature="0"
id="path3905"
@ -2858,7 +2950,8 @@
</g>
<g
id="text4548-7"
style="font-size:40px;font-style:normal;font-weight:normal;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans">
style="font-size:40px;font-style:normal;font-weight:normal;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans"
transform="translate(-3.4013125,0)">
<path
inkscape:connector-curvature="0"
id="path3947"
@ -2887,7 +2980,8 @@
</g>
<g
id="text4554-61"
style="font-size:35.79270172px;font-style:normal;font-weight:normal;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans">
style="font-size:35.79270172px;font-style:normal;font-weight:normal;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans"
transform="translate(-3.4013125,0)">
<path
inkscape:connector-curvature="0"
id="path3991"
@ -2946,7 +3040,8 @@
</g>
<g
id="text4554-7-2"
style="font-size:40px;font-style:normal;font-weight:normal;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans">
style="font-size:40px;font-style:normal;font-weight:normal;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans"
transform="translate(-3.4013125,0)">
<path
inkscape:connector-curvature="0"
id="path3969"
@ -2975,14 +3070,15 @@
</g>
<rect
y="15.137716"
x="26.098318"
x="22.697004"
height="5.6002355"
width="23.347105"
id="rect4122"
style="fill:none;stroke:#ffffff;stroke-width:0.29055119;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none" />
<g
id="text4124"
style="font-size:40px;font-style:normal;font-weight:normal;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans">
style="font-size:40px;font-style:normal;font-weight:normal;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans"
transform="translate(-3.4013125,0)">
<path
inkscape:connector-curvature="0"
id="path3958"
@ -3249,28 +3345,41 @@
<text
xml:space="preserve"
style="font-size:4px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:125%;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans;-inkscape-font-specification:Bitstream Vera Sans"
x="5.6416268"
x="3.0869398"
y="16.60899"
id="text6573"
sodipodi:linespacing="125%"><tspan
id="tspan6575"
sodipodi:role="line"
x="5.6416268"
x="3.0869398"
y="16.60899"
style="font-size:4px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:125%;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;font-family:Bitstream Vera Sans;-inkscape-font-specification:Bitstream Vera Sans">Battery</tspan></text>
<text
xml:space="preserve"
style="font-size:18px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:80.00000119%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#000000;fill-opacity:1;stroke:none;font-family:Arial;-inkscape-font-specification:Sans Bold"
x="4.2995572"
y="-62.471176"
x="4.3751421"
y="-57.099613"
id="text3565"
sodipodi:linespacing="80.000001%"
transform="matrix(0,1,-1,0,0,0)"><tspan
sodipodi:role="line"
id="tspan3567"
x="4.2995572"
y="-62.471176"
x="4.3751421"
y="-57.099613"
style="font-size:4px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:80.00000119%;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;font-family:Arial;-inkscape-font-specification:Arial">Sensors</tspan></text>
<text
xml:space="preserve"
style="font-size:18px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:80.00000119%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#000000;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Sans Bold"
x="4.1748462"
y="-67.598602"
id="text3565-9"
sodipodi:linespacing="80.000001%"
transform="matrix(0,1,-1,0,0,0)"><tspan
sodipodi:role="line"
id="tspan3567-3"
x="4.1748462"
y="-67.598602"
style="font-size:4px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:80.00000119%;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;font-family:Arial;-inkscape-font-specification:Arial">Airspeed</tspan></text>
</g>
<g
style="display:none"

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 235 KiB

After

Width:  |  Height:  |  Size: 239 KiB

4
ground/openpilotgcs/src/libs/eigen/.hg_archival.txt Normal file
View File

@ -0,0 +1,4 @@
repo: 8a21fd850624c931e448cbcfb38168cb2717c790
node: ffa86ffb557094721ca71dcea6aed2651b9fd610
branch: 3.2
tag: 3.2.0

8
ground/openpilotgcs/src/libs/eigen/.hgeol Normal file
View File

@ -0,0 +1,8 @@
[patterns]
scripts/*.in = LF
debug/msvc/*.dat = CRLF
unsupported/test/mpreal/*.* = CRLF
** = native
[repository]
native = LF

32
ground/openpilotgcs/src/libs/eigen/.hgignore Normal file
View File

@ -0,0 +1,32 @@
syntax: glob
qrc_*cxx
*.orig
*.pyc
*.diff
diff
*.save
save
*.old
*.gmo
*.qm
core
core.*
*.bak
*~
build*
*.moc.*
*.moc
ui_*
CMakeCache.txt
tags
.*.swp
activity.png
*.out
*.php*
*.log
*.orig
*.rej
log
patch
a
a.*

25
ground/openpilotgcs/src/libs/eigen/.hgtags Normal file
View File

@ -0,0 +1,25 @@
2db9468678c6480c9633b6272ff0e3599d1e11a3 2.0-beta3
375224817dce669b6fa31d920d4c895a63fabf32 2.0-beta1
3b8120f077865e2a072e10f5be33e1d942b83a06 2.0-rc1
19dfc0e7666bcee26f7a49eb42f39a0280a3485e 2.0-beta5
7a7d8a9526f003ffa2430dfb0c2c535b5add3023 2.0-beta4
7d14ad088ac23769c349518762704f0257f6a39b 2.0.1
b9d48561579fd7d4c05b2aa42235dc9de6484bf2 2.0-beta6
e17630a40408243cb1a51ad0fe3a99beb75b7450 before-hg-migration
eda654d4cda2210ce80719addcf854773e6dec5a 2.0.0
ee9a7c468a9e73fab12f38f02bac24b07f29ed71 2.0-beta2
d49097c25d8049e730c254a2fed725a240ce4858 after-hg-migration
655348878731bcb5d9bbe0854077b052e75e5237 actual-start-from-scratch
12a658962d4e6dfdc9a1c350fe7b69e36e70675c 3.0-beta1
5c4180ad827b3f869b13b1d82f5a6ce617d6fcee 3.0-beta2
7ae24ca6f3891d5ac58ddc7db60ad413c8d6ec35 3.0-beta3
c40708b9088d622567fecc9208ad4a426621d364 3.0-beta4
b6456624eae74f49ae8683d8e7b2882a2ca0342a 3.0-rc1
a810d5dbab47acfe65b3350236efdd98f67d4d8a 3.1.0-alpha1
304c88ca3affc16dd0b008b1104873986edd77af 3.1.0-alpha2
920fc730b5930daae0a6dbe296d60ce2e3808215 3.1.0-beta1
8383e883ebcc6f14695ff0b5e20bb631abab43fb 3.1.0-rc1
bf4cb8c934fa3a79f45f1e629610f0225e93e493 3.1.0-rc2
da195914abcc1d739027cbee7c52077aab30b336 3.2-beta1
4b687cad1d23066f66863f4f87298447298443df 3.2-rc1
1eeda7b1258bcd306018c0738e2b6a8543661141 3.2-rc2

420
ground/openpilotgcs/src/libs/eigen/CMakeLists.txt Normal file
View File

@ -0,0 +1,420 @@
project(Eigen)
cmake_minimum_required(VERSION 2.8.2)
# guard against in-source builds
if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR})
message(FATAL_ERROR "In-source builds not allowed. Please make a new directory (called a build directory) and run CMake from there. You may need to remove CMakeCache.txt. ")
endif()
# guard against bad build-type strings
if (NOT CMAKE_BUILD_TYPE)
set(CMAKE_BUILD_TYPE "Release")
endif()
string(TOLOWER "${CMAKE_BUILD_TYPE}" cmake_build_type_tolower)
if( NOT cmake_build_type_tolower STREQUAL "debug"
AND NOT cmake_build_type_tolower STREQUAL "release"
AND NOT cmake_build_type_tolower STREQUAL "relwithdebinfo")
message(FATAL_ERROR "Unknown build type \"${CMAKE_BUILD_TYPE}\". Allowed values are Debug, Release, RelWithDebInfo (case-insensitive).")
endif()
#############################################################################
# retrieve version infomation #
#############################################################################
# automatically parse the version number
file(READ "${PROJECT_SOURCE_DIR}/Eigen/src/Core/util/Macros.h" _eigen_version_header)
string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen_world_version_match "${_eigen_version_header}")
set(EIGEN_WORLD_VERSION "${CMAKE_MATCH_1}")
string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen_major_version_match "${_eigen_version_header}")
set(EIGEN_MAJOR_VERSION "${CMAKE_MATCH_1}")
string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen_minor_version_match "${_eigen_version_header}")
set(EIGEN_MINOR_VERSION "${CMAKE_MATCH_1}")
set(EIGEN_VERSION_NUMBER ${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION})
# if the mercurial program is absent, this will leave the EIGEN_HG_CHANGESET string empty,
# but won't stop CMake.
execute_process(COMMAND hg tip -R ${CMAKE_SOURCE_DIR} OUTPUT_VARIABLE EIGEN_HGTIP_OUTPUT)
execute_process(COMMAND hg branch -R ${CMAKE_SOURCE_DIR} OUTPUT_VARIABLE EIGEN_BRANCH_OUTPUT)
# if this is the default (aka development) branch, extract the mercurial changeset number from the hg tip output...
if(EIGEN_BRANCH_OUTPUT MATCHES "default")
string(REGEX MATCH "^changeset: *[0-9]*:([0-9;a-f]+).*" EIGEN_HG_CHANGESET_MATCH "${EIGEN_HGTIP_OUTPUT}")
set(EIGEN_HG_CHANGESET "${CMAKE_MATCH_1}")
endif(EIGEN_BRANCH_OUTPUT MATCHES "default")
#...and show it next to the version number
if(EIGEN_HG_CHANGESET)
set(EIGEN_VERSION "${EIGEN_VERSION_NUMBER} (mercurial changeset ${EIGEN_HG_CHANGESET})")
else(EIGEN_HG_CHANGESET)
set(EIGEN_VERSION "${EIGEN_VERSION_NUMBER}")
endif(EIGEN_HG_CHANGESET)
include(CheckCXXCompilerFlag)
set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
#############################################################################
# find how to link to the standard libraries #
#############################################################################
find_package(StandardMathLibrary)
set(EIGEN_TEST_CUSTOM_LINKER_FLAGS "" CACHE STRING "Additional linker flags when linking unit tests.")
set(EIGEN_TEST_CUSTOM_CXX_FLAGS "" CACHE STRING "Additional compiler flags when compiling unit tests.")
set(EIGEN_STANDARD_LIBRARIES_TO_LINK_TO "")
if(NOT STANDARD_MATH_LIBRARY_FOUND)
message(FATAL_ERROR
"Can't link to the standard math library. Please report to the Eigen developers, telling them about your platform.")
else()
if(EIGEN_STANDARD_LIBRARIES_TO_LINK_TO)
set(EIGEN_STANDARD_LIBRARIES_TO_LINK_TO "${EIGEN_STANDARD_LIBRARIES_TO_LINK_TO} ${STANDARD_MATH_LIBRARY}")
else()
set(EIGEN_STANDARD_LIBRARIES_TO_LINK_TO "${STANDARD_MATH_LIBRARY}")
endif()
endif()
if(EIGEN_STANDARD_LIBRARIES_TO_LINK_TO)
message(STATUS "Standard libraries to link to explicitly: ${EIGEN_STANDARD_LIBRARIES_TO_LINK_TO}")
else()
message(STATUS "Standard libraries to link to explicitly: none")
endif()
option(EIGEN_BUILD_BTL "Build benchmark suite" OFF)
if(NOT WIN32)
option(EIGEN_BUILD_PKGCONFIG "Build pkg-config .pc file for Eigen" ON)
endif(NOT WIN32)
set(CMAKE_INCLUDE_CURRENT_DIR ON)
option(EIGEN_SPLIT_LARGE_TESTS "Split large tests into smaller executables" ON)
option(EIGEN_DEFAULT_TO_ROW_MAJOR "Use row-major as default matrix storage order" OFF)
if(EIGEN_DEFAULT_TO_ROW_MAJOR)
add_definitions("-DEIGEN_DEFAULT_TO_ROW_MAJOR")
endif()
set(EIGEN_TEST_MAX_SIZE "320" CACHE STRING "Maximal matrix/vector size, default is 320")
macro(ei_add_cxx_compiler_flag FLAG)
string(REGEX REPLACE "-" "" SFLAG ${FLAG})
check_cxx_compiler_flag(${FLAG} COMPILER_SUPPORT_${SFLAG})
if(COMPILER_SUPPORT_${SFLAG})
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${FLAG}")
endif()
endmacro(ei_add_cxx_compiler_flag)
if(NOT MSVC)
# We assume that other compilers are partly compatible with GNUCC
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fexceptions")
set(CMAKE_CXX_FLAGS_DEBUG "-g3")
set(CMAKE_CXX_FLAGS_RELEASE "-g0 -O2")
# clang outputs some warnings for unknwon flags that are not caught by check_cxx_compiler_flag
# adding -Werror turns such warnings into errors
check_cxx_compiler_flag("-Werror" COMPILER_SUPPORT_WERROR)
if(COMPILER_SUPPORT_WERROR)
set(CMAKE_REQUIRED_FLAGS "-Werror")
endif()
ei_add_cxx_compiler_flag("-pedantic")
ei_add_cxx_compiler_flag("-Wall")
ei_add_cxx_compiler_flag("-Wextra")
#ei_add_cxx_compiler_flag("-Weverything") # clang
ei_add_cxx_compiler_flag("-Wundef")
ei_add_cxx_compiler_flag("-Wcast-align")
ei_add_cxx_compiler_flag("-Wchar-subscripts")
ei_add_cxx_compiler_flag("-Wnon-virtual-dtor")
ei_add_cxx_compiler_flag("-Wunused-local-typedefs")
ei_add_cxx_compiler_flag("-Wpointer-arith")
ei_add_cxx_compiler_flag("-Wwrite-strings")
ei_add_cxx_compiler_flag("-Wformat-security")
ei_add_cxx_compiler_flag("-Wno-psabi")
ei_add_cxx_compiler_flag("-Wno-variadic-macros")
ei_add_cxx_compiler_flag("-Wno-long-long")
ei_add_cxx_compiler_flag("-fno-check-new")
ei_add_cxx_compiler_flag("-fno-common")
ei_add_cxx_compiler_flag("-fstrict-aliasing")
ei_add_cxx_compiler_flag("-wd981") # disable ICC's "operands are evaluated in unspecified order" remark
ei_add_cxx_compiler_flag("-wd2304") # disbale ICC's "warning #2304: non-explicit constructor with single argument may cause implicit type conversion" produced by -Wnon-virtual-dtor
# The -ansi flag must be added last, otherwise it is also used as a linker flag by check_cxx_compiler_flag making it fails
# Moreover we should not set both -strict-ansi and -ansi
check_cxx_compiler_flag("-strict-ansi" COMPILER_SUPPORT_STRICTANSI)
ei_add_cxx_compiler_flag("-Qunused-arguments") # disable clang warning: argument unused during compilation: '-ansi'
if(COMPILER_SUPPORT_STRICTANSI)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -strict-ansi")
else()
ei_add_cxx_compiler_flag("-ansi")
endif()
set(CMAKE_REQUIRED_FLAGS "")
option(EIGEN_TEST_SSE2 "Enable/Disable SSE2 in tests/examples" OFF)
if(EIGEN_TEST_SSE2)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -msse2")
message(STATUS "Enabling SSE2 in tests/examples")
endif()
option(EIGEN_TEST_SSE3 "Enable/Disable SSE3 in tests/examples" OFF)
if(EIGEN_TEST_SSE3)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -msse3")
message(STATUS "Enabling SSE3 in tests/examples")
endif()
option(EIGEN_TEST_SSSE3 "Enable/Disable SSSE3 in tests/examples" OFF)
if(EIGEN_TEST_SSSE3)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mssse3")
message(STATUS "Enabling SSSE3 in tests/examples")
endif()
option(EIGEN_TEST_SSE4_1 "Enable/Disable SSE4.1 in tests/examples" OFF)
if(EIGEN_TEST_SSE4_1)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -msse4.1")
message(STATUS "Enabling SSE4.1 in tests/examples")
endif()
option(EIGEN_TEST_SSE4_2 "Enable/Disable SSE4.2 in tests/examples" OFF)
if(EIGEN_TEST_SSE4_2)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -msse4.2")
message(STATUS "Enabling SSE4.2 in tests/examples")
endif()
option(EIGEN_TEST_ALTIVEC "Enable/Disable AltiVec in tests/examples" OFF)
if(EIGEN_TEST_ALTIVEC)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -maltivec -mabi=altivec")
message(STATUS "Enabling AltiVec in tests/examples")
endif()
option(EIGEN_TEST_NEON "Enable/Disable Neon in tests/examples" OFF)
if(EIGEN_TEST_NEON)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon -mcpu=cortex-a"8)
message(STATUS "Enabling NEON in tests/examples")
endif()
check_cxx_compiler_flag("-fopenmp" COMPILER_SUPPORT_OPENMP)
if(COMPILER_SUPPORT_OPENMP)
option(EIGEN_TEST_OPENMP "Enable/Disable OpenMP in tests/examples" OFF)
if(EIGEN_TEST_OPENMP)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp")
message(STATUS "Enabling OpenMP in tests/examples")
endif()
endif()
else(NOT MSVC)
# C4127 - conditional expression is constant
# C4714 - marked as __forceinline not inlined (I failed to deactivate it selectively)
# We can disable this warning in the unit tests since it is clear that it occurs
# because we are oftentimes returning objects that have a destructor or may
# throw exceptions - in particular in the unit tests we are throwing extra many
# exceptions to cover indexing errors.
# C4505 - unreferenced local function has been removed (impossible to deactive selectively)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /EHsc /wd4127 /wd4505 /wd4714")
# replace all /Wx by /W4
string(REGEX REPLACE "/W[0-9]" "/W4" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
check_cxx_compiler_flag("/openmp" COMPILER_SUPPORT_OPENMP)
if(COMPILER_SUPPORT_OPENMP)
option(EIGEN_TEST_OPENMP "Enable/Disable OpenMP in tests/examples" OFF)
if(EIGEN_TEST_OPENMP)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /openmp")
message(STATUS "Enabling OpenMP in tests/examples")
endif()
endif()
option(EIGEN_TEST_SSE2 "Enable/Disable SSE2 in tests/examples" OFF)
if(EIGEN_TEST_SSE2)
if(NOT CMAKE_CL_64)
# arch is not supported on 64 bit systems, SSE is enabled automatically.
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /arch:SSE2")
endif(NOT CMAKE_CL_64)
message(STATUS "Enabling SSE2 in tests/examples")
endif(EIGEN_TEST_SSE2)
endif(NOT MSVC)
option(EIGEN_TEST_NO_EXPLICIT_VECTORIZATION "Disable explicit vectorization in tests/examples" OFF)
option(EIGEN_TEST_X87 "Force using X87 instructions. Implies no vectorization." OFF)
option(EIGEN_TEST_32BIT "Force generating 32bit code." OFF)
if(EIGEN_TEST_X87)
set(EIGEN_TEST_NO_EXPLICIT_VECTORIZATION ON)
if(CMAKE_COMPILER_IS_GNUCXX)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpmath=387")
message(STATUS "Forcing use of x87 instructions in tests/examples")
else()
message(STATUS "EIGEN_TEST_X87 ignored on your compiler")
endif()
endif()
if(EIGEN_TEST_32BIT)
if(CMAKE_COMPILER_IS_GNUCXX)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -m32")
message(STATUS "Forcing generation of 32-bit code in tests/examples")
else()
message(STATUS "EIGEN_TEST_32BIT ignored on your compiler")
endif()
endif()
if(EIGEN_TEST_NO_EXPLICIT_VECTORIZATION)
add_definitions(-DEIGEN_DONT_VECTORIZE=1)
message(STATUS "Disabling vectorization in tests/examples")
endif()
option(EIGEN_TEST_NO_EXPLICIT_ALIGNMENT "Disable explicit alignment (hence vectorization) in tests/examples" OFF)
if(EIGEN_TEST_NO_EXPLICIT_ALIGNMENT)
add_definitions(-DEIGEN_DONT_ALIGN=1)
message(STATUS "Disabling alignment in tests/examples")
endif()
option(EIGEN_TEST_C++0x "Enables all C++0x features." OFF)
include_directories(${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_BINARY_DIR})
# the user modifiable install path for header files
set(EIGEN_INCLUDE_INSTALL_DIR ${EIGEN_INCLUDE_INSTALL_DIR} CACHE PATH "The directory where we install the header files (optional)")
# set the internal install path for header files which depends on wether the user modifiable
# EIGEN_INCLUDE_INSTALL_DIR has been set by the user or not.
if(EIGEN_INCLUDE_INSTALL_DIR)
set(INCLUDE_INSTALL_DIR
${EIGEN_INCLUDE_INSTALL_DIR}
CACHE INTERNAL
"The directory where we install the header files (internal)"
)
else()
set(INCLUDE_INSTALL_DIR
"${CMAKE_INSTALL_PREFIX}/include/eigen3"
CACHE INTERNAL
"The directory where we install the header files (internal)"
)
endif()
# similar to set_target_properties but append the property instead of overwriting it
macro(ei_add_target_property target prop value)
get_target_property(previous ${target} ${prop})
# if the property wasn't previously set, ${previous} is now "previous-NOTFOUND" which cmake allows catching with plain if()
if(NOT previous)
set(previous "")
endif(NOT previous)
set_target_properties(${target} PROPERTIES ${prop} "${previous} ${value}")
endmacro(ei_add_target_property)
install(FILES
signature_of_eigen3_matrix_library
DESTINATION ${INCLUDE_INSTALL_DIR} COMPONENT Devel
)
if(EIGEN_BUILD_PKGCONFIG)
SET(path_separator ":")
STRING(REPLACE ${path_separator} ";" pkg_config_libdir_search "$ENV{PKG_CONFIG_LIBDIR}")
message(STATUS "searching for 'pkgconfig' directory in PKG_CONFIG_LIBDIR ( $ENV{PKG_CONFIG_LIBDIR} ), ${CMAKE_INSTALL_PREFIX}/share, and ${CMAKE_INSTALL_PREFIX}/lib")
FIND_PATH(pkg_config_libdir pkgconfig ${pkg_config_libdir_search} ${CMAKE_INSTALL_PREFIX}/share ${CMAKE_INSTALL_PREFIX}/lib ${pkg_config_libdir_search})
if(pkg_config_libdir)
SET(pkg_config_install_dir ${pkg_config_libdir})
message(STATUS "found ${pkg_config_libdir}/pkgconfig" )
else(pkg_config_libdir)
SET(pkg_config_install_dir ${CMAKE_INSTALL_PREFIX}/share)
message(STATUS "pkgconfig not found; installing in ${pkg_config_install_dir}" )
endif(pkg_config_libdir)
configure_file(eigen3.pc.in eigen3.pc)
install(FILES ${CMAKE_CURRENT_BINARY_DIR}/eigen3.pc
DESTINATION ${pkg_config_install_dir}/pkgconfig
)
endif(EIGEN_BUILD_PKGCONFIG)
add_subdirectory(Eigen)
add_subdirectory(doc EXCLUDE_FROM_ALL)
include(EigenConfigureTesting)
# fixme, not sure this line is still needed:
enable_testing() # must be called from the root CMakeLists, see man page
if(EIGEN_LEAVE_TEST_IN_ALL_TARGET)
add_subdirectory(test) # can't do EXCLUDE_FROM_ALL here, breaks CTest
else()
add_subdirectory(test EXCLUDE_FROM_ALL)
endif()
if(EIGEN_LEAVE_TEST_IN_ALL_TARGET)
add_subdirectory(blas)
add_subdirectory(lapack)
else()
add_subdirectory(blas EXCLUDE_FROM_ALL)
add_subdirectory(lapack EXCLUDE_FROM_ALL)
endif()
add_subdirectory(unsupported)
add_subdirectory(demos EXCLUDE_FROM_ALL)
# must be after test and unsupported, for configuring buildtests.in
add_subdirectory(scripts EXCLUDE_FROM_ALL)
# TODO: consider also replacing EIGEN_BUILD_BTL by a custom target "make btl"?
if(EIGEN_BUILD_BTL)
add_subdirectory(bench/btl EXCLUDE_FROM_ALL)
endif(EIGEN_BUILD_BTL)
if(NOT WIN32)
add_subdirectory(bench/spbench EXCLUDE_FROM_ALL)
endif(NOT WIN32)
configure_file(scripts/cdashtesting.cmake.in cdashtesting.cmake @ONLY)
ei_testing_print_summary()
message(STATUS "")
message(STATUS "Configured Eigen ${EIGEN_VERSION_NUMBER}")
message(STATUS "")
option(EIGEN_FAILTEST "Enable failtests." OFF)
if(EIGEN_FAILTEST)
add_subdirectory(failtest)
endif()
string(TOLOWER "${CMAKE_GENERATOR}" cmake_generator_tolower)
if(cmake_generator_tolower MATCHES "makefile")
message(STATUS "Some things you can do now:")
message(STATUS "--------------+--------------------------------------------------------------")
message(STATUS "Command | Description")
message(STATUS "--------------+--------------------------------------------------------------")
message(STATUS "make install | Install to ${CMAKE_INSTALL_PREFIX}. To change that:")
message(STATUS " | cmake . -DCMAKE_INSTALL_PREFIX=yourpath")
message(STATUS " | Eigen headers will then be installed to:")
message(STATUS " | ${INCLUDE_INSTALL_DIR}")
message(STATUS " | To install Eigen headers to a separate location, do:")
message(STATUS " | cmake . -DEIGEN_INCLUDE_INSTALL_DIR=yourpath")
message(STATUS "make doc | Generate the API documentation, requires Doxygen & LaTeX")
message(STATUS "make check | Build and run the unit-tests. Read this page:")
message(STATUS " | http://eigen.tuxfamily.org/index.php?title=Tests")
message(STATUS "make blas | Build BLAS library (not the same thing as Eigen)")
message(STATUS "--------------+--------------------------------------------------------------")
else()
message(STATUS "To build/run the unit tests, read this page:")
message(STATUS " http://eigen.tuxfamily.org/index.php?title=Tests")
endif()
message(STATUS "")

26
ground/openpilotgcs/src/libs/eigen/COPYING.BSD Normal file
View File

@ -0,0 +1,26 @@
/*
Copyright (c) 2011, Intel Corporation. All rights reserved.
Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of Intel Corporation nor the names of its contributors may
be used to endorse or promote products derived from this software without
specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

0
ground/openpilotgcs/src/libs/eigen/COPYING → ground/openpilotgcs/src/libs/eigen/COPYING.GPL
View File

165
ground/openpilotgcs/src/libs/eigen/COPYING.LESSER
View File

@ -1,165 +0,0 @@
GNU LESSER GENERAL PUBLIC LICENSE
Version 3, 29 June 2007
Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>
Everyone is permitted to copy and distribute verbatim copies
of this license document, but changing it is not allowed.
This version of the GNU Lesser General Public License incorporates
the terms and conditions of version 3 of the GNU General Public
License, supplemented by the additional permissions listed below.
0. Additional Definitions.
As used herein, "this License" refers to version 3 of the GNU Lesser
General Public License, and the "GNU GPL" refers to version 3 of the GNU
General Public License.
"The Library" refers to a covered work governed by this License,
other than an Application or a Combined Work as defined below.
An "Application" is any work that makes use of an interface provided
by the Library, but which is not otherwise based on the Library.
Defining a subclass of a class defined by the Library is deemed a mode
of using an interface provided by the Library.
A "Combined Work" is a work produced by combining or linking an
Application with the Library. The particular version of the Library
with which the Combined Work was made is also called the "Linked
Version".
The "Minimal Corresponding Source" for a Combined Work means the
Corresponding Source for the Combined Work, excluding any source code
for portions of the Combined Work that, considered in isolation, are
based on the Application, and not on the Linked Version.
The "Corresponding Application Code" for a Combined Work means the
object code and/or source code for the Application, including any data
and utility programs needed for reproducing the Combined Work from the
Application, but excluding the System Libraries of the Combined Work.
1. Exception to Section 3 of the GNU GPL.
You may convey a covered work under sections 3 and 4 of this License
without being bound by section 3 of the GNU GPL.
2. Conveying Modified Versions.
If you modify a copy of the Library, and, in your modifications, a
facility refers to a function or data to be supplied by an Application
that uses the facility (other than as an argument passed when the
facility is invoked), then you may convey a copy of the modified
version:
a) under this License, provided that you make a good faith effort to
ensure that, in the event an Application does not supply the
function or data, the facility still operates, and performs
whatever part of its purpose remains meaningful, or
b) under the GNU GPL, with none of the additional permissions of
this License applicable to that copy.
3. Object Code Incorporating Material from Library Header Files.
The object code form of an Application may incorporate material from
a header file that is part of the Library. You may convey such object
code under terms of your choice, provided that, if the incorporated
material is not limited to numerical parameters, data structure
layouts and accessors, or small macros, inline functions and templates
(ten or fewer lines in length), you do both of the following:
a) Give prominent notice with each copy of the object code that the
Library is used in it and that the Library and its use are
covered by this License.
b) Accompany the object code with a copy of the GNU GPL and this license
document.
4. Combined Works.
You may convey a Combined Work under terms of your choice that,
taken together, effectively do not restrict modification of the
portions of the Library contained in the Combined Work and reverse
engineering for debugging such modifications, if you also do each of
the following:
a) Give prominent notice with each copy of the Combined Work that
the Library is used in it and that the Library and its use are
covered by this License.
b) Accompany the Combined Work with a copy of the GNU GPL and this license
document.
c) For a Combined Work that displays copyright notices during
execution, include the copyright notice for the Library among
these notices, as well as a reference directing the user to the
copies of the GNU GPL and this license document.
d) Do one of the following:
0) Convey the Minimal Corresponding Source under the terms of this
License, and the Corresponding Application Code in a form
suitable for, and under terms that permit, the user to
recombine or relink the Application with a modified version of
the Linked Version to produce a modified Combined Work, in the
manner specified by section 6 of the GNU GPL for conveying
Corresponding Source.
1) Use a suitable shared library mechanism for linking with the
Library. A suitable mechanism is one that (a) uses at run time
a copy of the Library already present on the user's computer
system, and (b) will operate properly with a modified version
of the Library that is interface-compatible with the Linked
Version.
e) Provide Installation Information, but only if you would otherwise
be required to provide such information under section 6 of the
GNU GPL, and only to the extent that such information is
necessary to install and execute a modified version of the
Combined Work produced by recombining or relinking the
Application with a modified version of the Linked Version. (If
you use option 4d0, the Installation Information must accompany
the Minimal Corresponding Source and Corresponding Application
Code. If you use option 4d1, you must provide the Installation
Information in the manner specified by section 6 of the GNU GPL
for conveying Corresponding Source.)
5. Combined Libraries.
You may place library facilities that are a work based on the
Library side by side in a single library together with other library
facilities that are not Applications and are not covered by this
License, and convey such a combined library under terms of your
choice, if you do both of the following:
a) Accompany the combined library with a copy of the same work based
on the Library, uncombined with any other library facilities,
conveyed under the terms of this License.
b) Give prominent notice with the combined library that part of it
is a work based on the Library, and explaining where to find the
accompanying uncombined form of the same work.
6. Revised Versions of the GNU Lesser General Public License.
The Free Software Foundation may publish revised and/or new versions
of the GNU Lesser General Public License from time to time. Such new
versions will be similar in spirit to the present version, but may
differ in detail to address new problems or concerns.
Each version is given a distinguishing version number. If the
Library as you received it specifies that a certain numbered version
of the GNU Lesser General Public License "or any later version"
applies to it, you have the option of following the terms and
conditions either of that published version or of any later version
published by the Free Software Foundation. If the Library as you
received it does not specify a version number of the GNU Lesser
General Public License, you may choose any version of the GNU Lesser
General Public License ever published by the Free Software Foundation.
If the Library as you received it specifies that a proxy can decide
whether future versions of the GNU Lesser General Public License shall
apply, that proxy's public statement of acceptance of any version is
permanent authorization for you to choose that version for the
Library.

502
ground/openpilotgcs/src/libs/eigen/COPYING.LGPL Normal file
View File

@ -0,0 +1,502 @@
GNU LESSER GENERAL PUBLIC LICENSE
Version 2.1, February 1999
Copyright (C) 1991, 1999 Free Software Foundation, Inc.
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
Everyone is permitted to copy and distribute verbatim copies
of this license document, but changing it is not allowed.
[This is the first released version of the Lesser GPL. It also counts
as the successor of the GNU Library Public License, version 2, hence
the version number 2.1.]
Preamble
The licenses for most software are designed to take away your
freedom to share and change it. By contrast, the GNU General Public
Licenses are intended to guarantee your freedom to share and change
free software--to make sure the software is free for all its users.
This license, the Lesser General Public License, applies to some
specially designated software packages--typically libraries--of the
Free Software Foundation and other authors who decide to use it. You
can use it too, but we suggest you first think carefully about whether
this license or the ordinary General Public License is the better
strategy to use in any particular case, based on the explanations below.
When we speak of free software, we are referring to freedom of use,
not price. Our General Public Licenses are designed to make sure that
you have the freedom to distribute copies of free software (and charge
for this service if you wish); that you receive source code or can get
it if you want it; that you can change the software and use pieces of
it in new free programs; and that you are informed that you can do
these things.
To protect your rights, we need to make restrictions that forbid
distributors to deny you these rights or to ask you to surrender these
rights. These restrictions translate to certain responsibilities for
you if you distribute copies of the library or if you modify it.
For example, if you distribute copies of the library, whether gratis
or for a fee, you must give the recipients all the rights that we gave
you. You must make sure that they, too, receive or can get the source
code. If you link other code with the library, you must provide
complete object files to the recipients, so that they can relink them
with the library after making changes to the library and recompiling
it. And you must show them these terms so they know their rights.
We protect your rights with a two-step method: (1) we copyright the
library, and (2) we offer you this license, which gives you legal
permission to copy, distribute and/or modify the library.
To protect each distributor, we want to make it very clear that
there is no warranty for the free library. Also, if the library is
modified by someone else and passed on, the recipients should know
that what they have is not the original version, so that the original
author's reputation will not be affected by problems that might be
introduced by others.
Finally, software patents pose a constant threat to the existence of
any free program. We wish to make sure that a company cannot
effectively restrict the users of a free program by obtaining a
restrictive license from a patent holder. Therefore, we insist that
any patent license obtained for a version of the library must be
consistent with the full freedom of use specified in this license.
Most GNU software, including some libraries, is covered by the
ordinary GNU General Public License. This license, the GNU Lesser
General Public License, applies to certain designated libraries, and
is quite different from the ordinary General Public License. We use
this license for certain libraries in order to permit linking those
libraries into non-free programs.
When a program is linked with a library, whether statically or using
a shared library, the combination of the two is legally speaking a
combined work, a derivative of the original library. The ordinary
General Public License therefore permits such linking only if the
entire combination fits its criteria of freedom. The Lesser General
Public License permits more lax criteria for linking other code with
the library.
We call this license the "Lesser" General Public License because it
does Less to protect the user's freedom than the ordinary General
Public License. It also provides other free software developers Less
of an advantage over competing non-free programs. These disadvantages
are the reason we use the ordinary General Public License for many
libraries. However, the Lesser license provides advantages in certain
special circumstances.
For example, on rare occasions, there may be a special need to
encourage the widest possible use of a certain library, so that it becomes
a de-facto standard. To achieve this, non-free programs must be
allowed to use the library. A more frequent case is that a free
library does the same job as widely used non-free libraries. In this
case, there is little to gain by limiting the free library to free
software only, so we use the Lesser General Public License.
In other cases, permission to use a particular library in non-free
programs enables a greater number of people to use a large body of
free software. For example, permission to use the GNU C Library in
non-free programs enables many more people to use the whole GNU
operating system, as well as its variant, the GNU/Linux operating
system.
Although the Lesser General Public License is Less protective of the
users' freedom, it does ensure that the user of a program that is
linked with the Library has the freedom and the wherewithal to run
that program using a modified version of the Library.
The precise terms and conditions for copying, distribution and
modification follow. Pay close attention to the difference between a
"work based on the library" and a "work that uses the library". The
former contains code derived from the library, whereas the latter must
be combined with the library in order to run.
GNU LESSER GENERAL PUBLIC LICENSE
TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
0. This License Agreement applies to any software library or other
program which contains a notice placed by the copyright holder or
other authorized party saying it may be distributed under the terms of
this Lesser General Public License (also called "this License").
Each licensee is addressed as "you".
A "library" means a collection of software functions and/or data
prepared so as to be conveniently linked with application programs
(which use some of those functions and data) to form executables.
The "Library", below, refers to any such software library or work
which has been distributed under these terms. A "work based on the
Library" means either the Library or any derivative work under
copyright law: that is to say, a work containing the Library or a
portion of it, either verbatim or with modifications and/or translated
straightforwardly into another language. (Hereinafter, translation is
included without limitation in the term "modification".)
"Source code" for a work means the preferred form of the work for
making modifications to it. For a library, complete source code means
all the source code for all modules it contains, plus any associated
interface definition files, plus the scripts used to control compilation
and installation of the library.
Activities other than copying, distribution and modification are not
covered by this License; they are outside its scope. The act of
running a program using the Library is not restricted, and output from
such a program is covered only if its contents constitute a work based
on the Library (independent of the use of the Library in a tool for
writing it). Whether that is true depends on what the Library does
and what the program that uses the Library does.
1. You may copy and distribute verbatim copies of the Library's
complete source code as you receive it, in any medium, provided that
you conspicuously and appropriately publish on each copy an
appropriate copyright notice and disclaimer of warranty; keep intact
all the notices that refer to this License and to the absence of any
warranty; and distribute a copy of this License along with the
Library.
You may charge a fee for the physical act of transferring a copy,
and you may at your option offer warranty protection in exchange for a
fee.
2. You may modify your copy or copies of the Library or any portion
of it, thus forming a work based on the Library, and copy and
distribute such modifications or work under the terms of Section 1
above, provided that you also meet all of these conditions:
a) The modified work must itself be a software library.
b) You must cause the files modified to carry prominent notices
stating that you changed the files and the date of any change.
c) You must cause the whole of the work to be licensed at no
charge to all third parties under the terms of this License.
d) If a facility in the modified Library refers to a function or a
table of data to be supplied by an application program that uses
the facility, other than as an argument passed when the facility
is invoked, then you must make a good faith effort to ensure that,
in the event an application does not supply such function or
table, the facility still operates, and performs whatever part of
its purpose remains meaningful.
(For example, a function in a library to compute square roots has
a purpose that is entirely well-defined independent of the
application. Therefore, Subsection 2d requires that any
application-supplied function or table used by this function must
be optional: if the application does not supply it, the square
root function must still compute square roots.)
These requirements apply to the modified work as a whole. If
identifiable sections of that work are not derived from the Library,
and can be reasonably considered independent and separate works in
themselves, then this License, and its terms, do not apply to those
sections when you distribute them as separate works. But when you
distribute the same sections as part of a whole which is a work based
on the Library, the distribution of the whole must be on the terms of
this License, whose permissions for other licensees extend to the
entire whole, and thus to each and every part regardless of who wrote
it.
Thus, it is not the intent of this section to claim rights or contest
your rights to work written entirely by you; rather, the intent is to
exercise the right to control the distribution of derivative or
collective works based on the Library.
In addition, mere aggregation of another work not based on the Library
with the Library (or with a work based on the Library) on a volume of
a storage or distribution medium does not bring the other work under
the scope of this License.
3. You may opt to apply the terms of the ordinary GNU General Public
License instead of this License to a given copy of the Library. To do
this, you must alter all the notices that refer to this License, so
that they refer to the ordinary GNU General Public License, version 2,
instead of to this License. (If a newer version than version 2 of the
ordinary GNU General Public License has appeared, then you can specify
that version instead if you wish.) Do not make any other change in
these notices.
Once this change is made in a given copy, it is irreversible for
that copy, so the ordinary GNU General Public License applies to all
subsequent copies and derivative works made from that copy.
This option is useful when you wish to copy part of the code of
the Library into a program that is not a library.
4. You may copy and distribute the Library (or a portion or
derivative of it, under Section 2) in object code or executable form
under the terms of Sections 1 and 2 above provided that you accompany
it with the complete corresponding machine-readable source code, which
must be distributed under the terms of Sections 1 and 2 above on a
medium customarily used for software interchange.
If distribution of object code is made by offering access to copy
from a designated place, then offering equivalent access to copy the
source code from the same place satisfies the requirement to
distribute the source code, even though third parties are not
compelled to copy the source along with the object code.
5. A program that contains no derivative of any portion of the
Library, but is designed to work with the Library by being compiled or
linked with it, is called a "work that uses the Library". Such a
work, in isolation, is not a derivative work of the Library, and
therefore falls outside the scope of this License.
However, linking a "work that uses the Library" with the Library
creates an executable that is a derivative of the Library (because it
contains portions of the Library), rather than a "work that uses the
library". The executable is therefore covered by this License.
Section 6 states terms for distribution of such executables.
When a "work that uses the Library" uses material from a header file
that is part of the Library, the object code for the work may be a
derivative work of the Library even though the source code is not.
Whether this is true is especially significant if the work can be
linked without the Library, or if the work is itself a library. The
threshold for this to be true is not precisely defined by law.
If such an object file uses only numerical parameters, data
structure layouts and accessors, and small macros and small inline
functions (ten lines or less in length), then the use of the object
file is unrestricted, regardless of whether it is legally a derivative
work. (Executables containing this object code plus portions of the
Library will still fall under Section 6.)
Otherwise, if the work is a derivative of the Library, you may
distribute the object code for the work under the terms of Section 6.
Any executables containing that work also fall under Section 6,
whether or not they are linked directly with the Library itself.
6. As an exception to the Sections above, you may also combine or
link a "work that uses the Library" with the Library to produce a
work containing portions of the Library, and distribute that work
under terms of your choice, provided that the terms permit
modification of the work for the customer's own use and reverse
engineering for debugging such modifications.
You must give prominent notice with each copy of the work that the
Library is used in it and that the Library and its use are covered by
this License. You must supply a copy of this License. If the work
during execution displays copyright notices, you must include the
copyright notice for the Library among them, as well as a reference
directing the user to the copy of this License. Also, you must do one
of these things:
a) Accompany the work with the complete corresponding
machine-readable source code for the Library including whatever
changes were used in the work (which must be distributed under
Sections 1 and 2 above); and, if the work is an executable linked
with the Library, with the complete machine-readable "work that
uses the Library", as object code and/or source code, so that the
user can modify the Library and then relink to produce a modified
executable containing the modified Library. (It is understood
that the user who changes the contents of definitions files in the
Library will not necessarily be able to recompile the application
to use the modified definitions.)
b) Use a suitable shared library mechanism for linking with the
Library. A suitable mechanism is one that (1) uses at run time a
copy of the library already present on the user's computer system,
rather than copying library functions into the executable, and (2)
will operate properly with a modified version of the library, if
the user installs one, as long as the modified version is
interface-compatible with the version that the work was made with.
c) Accompany the work with a written offer, valid for at
least three years, to give the same user the materials
specified in Subsection 6a, above, for a charge no more
than the cost of performing this distribution.
d) If distribution of the work is made by offering access to copy
from a designated place, offer equivalent access to copy the above
specified materials from the same place.
e) Verify that the user has already received a copy of these
materials or that you have already sent this user a copy.
For an executable, the required form of the "work that uses the
Library" must include any data and utility programs needed for
reproducing the executable from it. However, as a special exception,
the materials to be distributed need not include anything that is
normally distributed (in either source or binary form) with the major
components (compiler, kernel, and so on) of the operating system on
which the executable runs, unless that component itself accompanies
the executable.
It may happen that this requirement contradicts the license
restrictions of other proprietary libraries that do not normally
accompany the operating system. Such a contradiction means you cannot
use both them and the Library together in an executable that you
distribute.
7. You may place library facilities that are a work based on the
Library side-by-side in a single library together with other library
facilities not covered by this License, and distribute such a combined
library, provided that the separate distribution of the work based on
the Library and of the other library facilities is otherwise
permitted, and provided that you do these two things:
a) Accompany the combined library with a copy of the same work
based on the Library, uncombined with any other library
facilities. This must be distributed under the terms of the
Sections above.
b) Give prominent notice with the combined library of the fact
that part of it is a work based on the Library, and explaining
where to find the accompanying uncombined form of the same work.
8. You may not copy, modify, sublicense, link with, or distribute
the Library except as expressly provided under this License. Any
attempt otherwise to copy, modify, sublicense, link with, or
distribute the Library is void, and will automatically terminate your
rights under this License. However, parties who have received copies,
or rights, from you under this License will not have their licenses
terminated so long as such parties remain in full compliance.
9. You are not required to accept this License, since you have not
signed it. However, nothing else grants you permission to modify or
distribute the Library or its derivative works. These actions are
prohibited by law if you do not accept this License. Therefore, by
modifying or distributing the Library (or any work based on the
Library), you indicate your acceptance of this License to do so, and
all its terms and conditions for copying, distributing or modifying
the Library or works based on it.
10. Each time you redistribute the Library (or any work based on the
Library), the recipient automatically receives a license from the
original licensor to copy, distribute, link with or modify the Library
subject to these terms and conditions. You may not impose any further
restrictions on the recipients' exercise of the rights granted herein.
You are not responsible for enforcing compliance by third parties with
this License.
11. If, as a consequence of a court judgment or allegation of patent
infringement or for any other reason (not limited to patent issues),
conditions are imposed on you (whether by court order, agreement or
otherwise) that contradict the conditions of this License, they do not
excuse you from the conditions of this License. If you cannot
distribute so as to satisfy simultaneously your obligations under this
License and any other pertinent obligations, then as a consequence you
may not distribute the Library at all. For example, if a patent
license would not permit royalty-free redistribution of the Library by
all those who receive copies directly or indirectly through you, then
the only way you could satisfy both it and this License would be to
refrain entirely from distribution of the Library.
If any portion of this section is held invalid or unenforceable under any
particular circumstance, the balance of the section is intended to apply,
and the section as a whole is intended to apply in other circumstances.
It is not the purpose of this section to induce you to infringe any
patents or other property right claims or to contest validity of any
such claims; this section has the sole purpose of protecting the
integrity of the free software distribution system which is
implemented by public license practices. Many people have made
generous contributions to the wide range of software distributed
through that system in reliance on consistent application of that
system; it is up to the author/donor to decide if he or she is willing
to distribute software through any other system and a licensee cannot
impose that choice.
This section is intended to make thoroughly clear what is believed to
be a consequence of the rest of this License.
12. If the distribution and/or use of the Library is restricted in
certain countries either by patents or by copyrighted interfaces, the
original copyright holder who places the Library under this License may add
an explicit geographical distribution limitation excluding those countries,
so that distribution is permitted only in or among countries not thus
excluded. In such case, this License incorporates the limitation as if
written in the body of this License.
13. The Free Software Foundation may publish revised and/or new
versions of the Lesser General Public License from time to time.
Such new versions will be similar in spirit to the present version,
but may differ in detail to address new problems or concerns.
Each version is given a distinguishing version number. If the Library
specifies a version number of this License which applies to it and
"any later version", you have the option of following the terms and
conditions either of that version or of any later version published by
the Free Software Foundation. If the Library does not specify a
license version number, you may choose any version ever published by
the Free Software Foundation.
14. If you wish to incorporate parts of the Library into other free
programs whose distribution conditions are incompatible with these,
write to the author to ask for permission. For software which is
copyrighted by the Free Software Foundation, write to the Free
Software Foundation; we sometimes make exceptions for this. Our
decision will be guided by the two goals of preserving the free status
of all derivatives of our free software and of promoting the sharing
and reuse of software generally.
NO WARRANTY
15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
LIBRARY IS WITH YOU. SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
DAMAGES.
END OF TERMS AND CONDITIONS
How to Apply These Terms to Your New Libraries
If you develop a new library, and you want it to be of the greatest
possible use to the public, we recommend making it free software that
everyone can redistribute and change. You can do so by permitting
redistribution under these terms (or, alternatively, under the terms of the
ordinary General Public License).
To apply these terms, attach the following notices to the library. It is
safest to attach them to the start of each source file to most effectively
convey the exclusion of warranty; and each file should have at least the
"copyright" line and a pointer to where the full notice is found.
<one line to give the library's name and a brief idea of what it does.>
Copyright (C) <year> <name of author>
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
Also add information on how to contact you by electronic and paper mail.
You should also get your employer (if you work as a programmer) or your
school, if any, to sign a "copyright disclaimer" for the library, if
necessary. Here is a sample; alter the names:
Yoyodyne, Inc., hereby disclaims all copyright interest in the
library `Frob' (a library for tweaking knobs) written by James Random Hacker.
<signature of Ty Coon>, 1 April 1990
Ty Coon, President of Vice
That's all there is to it!

52
ground/openpilotgcs/src/libs/eigen/COPYING.MINPACK Normal file
View File

@ -0,0 +1,52 @@
Minpack Copyright Notice (1999) University of Chicago. All rights reserved
Redistribution and use in source and binary forms, with or
without modification, are permitted provided that the
following conditions are met:
1. Redistributions of source code must retain the above
copyright notice, this list of conditions and the following
disclaimer.
2. Redistributions in binary form must reproduce the above
copyright notice, this list of conditions and the following
disclaimer in the documentation and/or other materials
provided with the distribution.
3. The end-user documentation included with the
redistribution, if any, must include the following
acknowledgment:
"This product includes software developed by the
University of Chicago, as Operator of Argonne National
Laboratory.
Alternately, this acknowledgment may appear in the software
itself, if and wherever such third-party acknowledgments
normally appear.
4. WARRANTY DISCLAIMER. THE SOFTWARE IS SUPPLIED "AS IS"
WITHOUT WARRANTY OF ANY KIND. THE COPYRIGHT HOLDER, THE
UNITED STATES, THE UNITED STATES DEPARTMENT OF ENERGY, AND
THEIR EMPLOYEES: (1) DISCLAIM ANY WARRANTIES, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO ANY IMPLIED WARRANTIES
OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, TITLE
OR NON-INFRINGEMENT, (2) DO NOT ASSUME ANY LEGAL LIABILITY
OR RESPONSIBILITY FOR THE ACCURACY, COMPLETENESS, OR
USEFULNESS OF THE SOFTWARE, (3) DO NOT REPRESENT THAT USE OF
THE SOFTWARE WOULD NOT INFRINGE PRIVATELY OWNED RIGHTS, (4)
DO NOT WARRANT THAT THE SOFTWARE WILL FUNCTION
UNINTERRUPTED, THAT IT IS ERROR-FREE OR THAT ANY ERRORS WILL
BE CORRECTED.
5. LIMITATION OF LIABILITY. IN NO EVENT WILL THE COPYRIGHT
HOLDER, THE UNITED STATES, THE UNITED STATES DEPARTMENT OF
ENERGY, OR THEIR EMPLOYEES: BE LIABLE FOR ANY INDIRECT,
INCIDENTAL, CONSEQUENTIAL, SPECIAL OR PUNITIVE DAMAGES OF
ANY KIND OR NATURE, INCLUDING BUT NOT LIMITED TO LOSS OF
PROFITS OR LOSS OF DATA, FOR ANY REASON WHATSOEVER, WHETHER
SUCH LIABILITY IS ASSERTED ON THE BASIS OF CONTRACT, TORT
(INCLUDING NEGLIGENCE OR STRICT LIABILITY), OR OTHERWISE,
EVEN IF ANY OF SAID PARTIES HAS BEEN WARNED OF THE
POSSIBILITY OF SUCH LOSS OR DAMAGES.

373
ground/openpilotgcs/src/libs/eigen/COPYING.MPL2 Normal file
View File

@ -0,0 +1,373 @@
Mozilla Public License Version 2.0
==================================
1. Definitions
--------------
1.1. "Contributor"
means each individual or legal entity that creates, contributes to
the creation of, or owns Covered Software.
1.2. "Contributor Version"
means the combination of the Contributions of others (if any) used
by a Contributor and that particular Contributor's Contribution.
1.3. "Contribution"
means Covered Software of a particular Contributor.
1.4. "Covered Software"
means Source Code Form to which the initial Contributor has attached
the notice in Exhibit A, the Executable Form of such Source Code
Form, and Modifications of such Source Code Form, in each case
including portions thereof.
1.5. "Incompatible With Secondary Licenses"
means
(a) that the initial Contributor has attached the notice described
in Exhibit B to the Covered Software; or
(b) that the Covered Software was made available under the terms of
version 1.1 or earlier of the License, but not also under the
terms of a Secondary License.
1.6. "Executable Form"
means any form of the work other than Source Code Form.
1.7. "Larger Work"
means a work that combines Covered Software with other material, in
a separate file or files, that is not Covered Software.
1.8. "License"
means this document.
1.9. "Licensable"
means having the right to grant, to the maximum extent possible,
whether at the time of the initial grant or subsequently, any and
all of the rights conveyed by this License.
1.10. "Modifications"
means any of the following:
(a) any file in Source Code Form that results from an addition to,
deletion from, or modification of the contents of Covered
Software; or
(b) any new file in Source Code Form that contains any Covered
Software.
1.11. "Patent Claims" of a Contributor
means any patent claim(s), including without limitation, method,
process, and apparatus claims, in any patent Licensable by such
Contributor that would be infringed, but for the grant of the
License, by the making, using, selling, offering for sale, having
made, import, or transfer of either its Contributions or its
Contributor Version.
1.12. "Secondary License"
means either the GNU General Public License, Version 2.0, the GNU
Lesser General Public License, Version 2.1, the GNU Affero General
Public License, Version 3.0, or any later versions of those
licenses.
1.13. "Source Code Form"
means the form of the work preferred for making modifications.
1.14. "You" (or "Your")
means an individual or a legal entity exercising rights under this
License. For legal entities, "You" includes any entity that
controls, is controlled by, or is under common control with You. For
purposes of this definition, "control" means (a) the power, direct
or indirect, to cause the direction or management of such entity,
whether by contract or otherwise, or (b) ownership of more than
fifty percent (50%) of the outstanding shares or beneficial
ownership of such entity.
2. License Grants and Conditions
--------------------------------
2.1. Grants
Each Contributor hereby grants You a world-wide, royalty-free,
non-exclusive license:
(a) under intellectual property rights (other than patent or trademark)
Licensable by such Contributor to use, reproduce, make available,
modify, display, perform, distribute, and otherwise exploit its
Contributions, either on an unmodified basis, with Modifications, or
as part of a Larger Work; and
(b) under Patent Claims of such Contributor to make, use, sell, offer
for sale, have made, import, and otherwise transfer either its
Contributions or its Contributor Version.
2.2. Effective Date
The licenses granted in Section 2.1 with respect to any Contribution
become effective for each Contribution on the date the Contributor first
distributes such Contribution.
2.3. Limitations on Grant Scope
The licenses granted in this Section 2 are the only rights granted under
this License. No additional rights or licenses will be implied from the
distribution or licensing of Covered Software under this License.
Notwithstanding Section 2.1(b) above, no patent license is granted by a
Contributor:
(a) for any code that a Contributor has removed from Covered Software;
or
(b) for infringements caused by: (i) Your and any other third party's
modifications of Covered Software, or (ii) the combination of its
Contributions with other software (except as part of its Contributor
Version); or
(c) under Patent Claims infringed by Covered Software in the absence of
its Contributions.
This License does not grant any rights in the trademarks, service marks,
or logos of any Contributor (except as may be necessary to comply with
the notice requirements in Section 3.4).
2.4. Subsequent Licenses
No Contributor makes additional grants as a result of Your choice to
distribute the Covered Software under a subsequent version of this
License (see Section 10.2) or under the terms of a Secondary License (if
permitted under the terms of Section 3.3).
2.5. Representation
Each Contributor represents that the Contributor believes its
Contributions are its original creation(s) or it has sufficient rights
to grant the rights to its Contributions conveyed by this License.
2.6. Fair Use
This License is not intended to limit any rights You have under
applicable copyright doctrines of fair use, fair dealing, or other
equivalents.
2.7. Conditions
Sections 3.1, 3.2, 3.3, and 3.4 are conditions of the licenses granted
in Section 2.1.
3. Responsibilities
-------------------
3.1. Distribution of Source Form
All distribution of Covered Software in Source Code Form, including any
Modifications that You create or to which You contribute, must be under
the terms of this License. You must inform recipients that the Source
Code Form of the Covered Software is governed by the terms of this
License, and how they can obtain a copy of this License. You may not
attempt to alter or restrict the recipients' rights in the Source Code
Form.
3.2. Distribution of Executable Form
If You distribute Covered Software in Executable Form then:
(a) such Covered Software must also be made available in Source Code
Form, as described in Section 3.1, and You must inform recipients of
the Executable Form how they can obtain a copy of such Source Code
Form by reasonable means in a timely manner, at a charge no more
than the cost of distribution to the recipient; and
(b) You may distribute such Executable Form under the terms of this
License, or sublicense it under different terms, provided that the
license for the Executable Form does not attempt to limit or alter
the recipients' rights in the Source Code Form under this License.
3.3. Distribution of a Larger Work
You may create and distribute a Larger Work under terms of Your choice,
provided that You also comply with the requirements of this License for
the Covered Software. If the Larger Work is a combination of Covered
Software with a work governed by one or more Secondary Licenses, and the
Covered Software is not Incompatible With Secondary Licenses, this
License permits You to additionally distribute such Covered Software
under the terms of such Secondary License(s), so that the recipient of
the Larger Work may, at their option, further distribute the Covered
Software under the terms of either this License or such Secondary
License(s).
3.4. Notices
You may not remove or alter the substance of any license notices
(including copyright notices, patent notices, disclaimers of warranty,
or limitations of liability) contained within the Source Code Form of
the Covered Software, except that You may alter any license notices to
the extent required to remedy known factual inaccuracies.
3.5. Application of Additional Terms
You may choose to offer, and to charge a fee for, warranty, support,
indemnity or liability obligations to one or more recipients of Covered
Software. However, You may do so only on Your own behalf, and not on
behalf of any Contributor. You must make it absolutely clear that any
such warranty, support, indemnity, or liability obligation is offered by
You alone, and You hereby agree to indemnify every Contributor for any
liability incurred by such Contributor as a result of warranty, support,
indemnity or liability terms You offer. You may include additional
disclaimers of warranty and limitations of liability specific to any
jurisdiction.
4. Inability to Comply Due to Statute or Regulation
---------------------------------------------------
If it is impossible for You to comply with any of the terms of this
License with respect to some or all of the Covered Software due to
statute, judicial order, or regulation then You must: (a) comply with
the terms of this License to the maximum extent possible; and (b)
describe the limitations and the code they affect. Such description must
be placed in a text file included with all distributions of the Covered
Software under this License. Except to the extent prohibited by statute
or regulation, such description must be sufficiently detailed for a
recipient of ordinary skill to be able to understand it.
5. Termination
--------------
5.1. The rights granted under this License will terminate automatically
if You fail to comply with any of its terms. However, if You become
compliant, then the rights granted under this License from a particular
Contributor are reinstated (a) provisionally, unless and until such
Contributor explicitly and finally terminates Your grants, and (b) on an
ongoing basis, if such Contributor fails to notify You of the
non-compliance by some reasonable means prior to 60 days after You have
come back into compliance. Moreover, Your grants from a particular
Contributor are reinstated on an ongoing basis if such Contributor
notifies You of the non-compliance by some reasonable means, this is the
first time You have received notice of non-compliance with this License
from such Contributor, and You become compliant prior to 30 days after
Your receipt of the notice.
5.2. If You initiate litigation against any entity by asserting a patent
infringement claim (excluding declaratory judgment actions,
counter-claims, and cross-claims) alleging that a Contributor Version
directly or indirectly infringes any patent, then the rights granted to
You by any and all Contributors for the Covered Software under Section
2.1 of this License shall terminate.
5.3. In the event of termination under Sections 5.1 or 5.2 above, all
end user license agreements (excluding distributors and resellers) which
have been validly granted by You or Your distributors under this License
prior to termination shall survive termination.
************************************************************************
* *
* 6. Disclaimer of Warranty *
* ------------------------- *
* *
* Covered Software is provided under this License on an "as is" *
* basis, without warranty of any kind, either expressed, implied, or *
* statutory, including, without limitation, warranties that the *
* Covered Software is free of defects, merchantable, fit for a *
* particular purpose or non-infringing. The entire risk as to the *
* quality and performance of the Covered Software is with You. *
* Should any Covered Software prove defective in any respect, You *
* (not any Contributor) assume the cost of any necessary servicing, *
* repair, or correction. This disclaimer of warranty constitutes an *
* essential part of this License. No use of any Covered Software is *
* authorized under this License except under this disclaimer. *
* *
************************************************************************
************************************************************************
* *
* 7. Limitation of Liability *
* -------------------------- *
* *
* Under no circumstances and under no legal theory, whether tort *
* (including negligence), contract, or otherwise, shall any *
* Contributor, or anyone who distributes Covered Software as *
* permitted above, be liable to You for any direct, indirect, *
* special, incidental, or consequential damages of any character *
* including, without limitation, damages for lost profits, loss of *
* goodwill, work stoppage, computer failure or malfunction, or any *
* and all other commercial damages or losses, even if such party *
* shall have been informed of the possibility of such damages. This *
* limitation of liability shall not apply to liability for death or *
* personal injury resulting from such party's negligence to the *
* extent applicable law prohibits such limitation. Some *
* jurisdictions do not allow the exclusion or limitation of *
* incidental or consequential damages, so this exclusion and *
* limitation may not apply to You. *
* *
************************************************************************
8. Litigation
-------------
Any litigation relating to this License may be brought only in the
courts of a jurisdiction where the defendant maintains its principal
place of business and such litigation shall be governed by laws of that
jurisdiction, without reference to its conflict-of-law provisions.
Nothing in this Section shall prevent a party's ability to bring
cross-claims or counter-claims.
9. Miscellaneous
----------------
This License represents the complete agreement concerning the subject
matter hereof. If any provision of this License is held to be
unenforceable, such provision shall be reformed only to the extent
necessary to make it enforceable. Any law or regulation which provides
that the language of a contract shall be construed against the drafter
shall not be used to construe this License against a Contributor.
10. Versions of the License
---------------------------
10.1. New Versions
Mozilla Foundation is the license steward. Except as provided in Section
10.3, no one other than the license steward has the right to modify or
publish new versions of this License. Each version will be given a
distinguishing version number.
10.2. Effect of New Versions
You may distribute the Covered Software under the terms of the version
of the License under which You originally received the Covered Software,
or under the terms of any subsequent version published by the license
steward.
10.3. Modified Versions
If you create software not governed by this License, and you want to
create a new license for such software, you may create and use a
modified version of this License if you rename the license and remove
any references to the name of the license steward (except to note that
such modified license differs from this License).
10.4. Distributing Source Code Form that is Incompatible With Secondary
Licenses
If You choose to distribute Source Code Form that is Incompatible With
Secondary Licenses under the terms of this version of the License, the
notice described in Exhibit B of this License must be attached.
Exhibit A - Source Code Form License Notice
-------------------------------------------
This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.
If it is not possible or desirable to put the notice in a particular
file, then You may include the notice in a location (such as a LICENSE
file in a relevant directory) where a recipient would be likely to look
for such a notice.
You may add additional accurate notices of copyright ownership.
Exhibit B - "Incompatible With Secondary Licenses" Notice
---------------------------------------------------------
This Source Code Form is "Incompatible With Secondary Licenses", as
defined by the Mozilla Public License, v. 2.0.

18
ground/openpilotgcs/src/libs/eigen/COPYING.README Normal file
View File

@ -0,0 +1,18 @@
Eigen is primarily MPL2 licensed. See COPYING.MPL2 and these links:
http://www.mozilla.org/MPL/2.0/
http://www.mozilla.org/MPL/2.0/FAQ.html
Some files contain third-party code under BSD or LGPL licenses, whence the other
COPYING.* files here.
All the LGPL code is either LGPL 2.1-only, or LGPL 2.1-or-later.
For this reason, the COPYING.LGPL file contains the LGPL 2.1 text.
If you want to guarantee that the Eigen code that you are #including is licensed
under the MPL2 and possibly more permissive licenses (like BSD), #define this
preprocessor symbol:
EIGEN_MPL2_ONLY
For example, with most compilers, you could add this to your project CXXFLAGS:
-DEIGEN_MPL2_ONLY
This will cause a compilation error to be generated if you #include any code that is
LGPL licensed.

17
ground/openpilotgcs/src/libs/eigen/CTestConfig.cmake Normal file
View File

@ -0,0 +1,17 @@
## This file should be placed in the root directory of your project.
## Then modify the CMakeLists.txt file in the root directory of your
## project to incorporate the testing dashboard.
## # The following are required to uses Dart and the Cdash dashboard
## ENABLE_TESTING()
## INCLUDE(CTest)
set(CTEST_PROJECT_NAME "Eigen")
set(CTEST_NIGHTLY_START_TIME "00:00:00 UTC")
set(CTEST_DROP_METHOD "http")
set(CTEST_DROP_SITE "manao.inria.fr")
set(CTEST_DROP_LOCATION "/CDash/submit.php?project=Eigen")
set(CTEST_DROP_SITE_CDASH TRUE)
set(CTEST_PROJECT_SUBPROJECTS
Official
Unsupported
)

3
ground/openpilotgcs/src/libs/eigen/CTestCustom.cmake.in Normal file
View File

@ -0,0 +1,3 @@
set(CTEST_CUSTOM_MAXIMUM_NUMBER_OF_WARNINGS "2000")
set(CTEST_CUSTOM_MAXIMUM_NUMBER_OF_ERRORS "2000")

36
ground/openpilotgcs/src/libs/eigen/Eigen/Array
View File

@ -1,39 +1,11 @@
#ifndef EIGEN_ARRAY_MODULE_H
#define EIGEN_ARRAY_MODULE_H
// include Core first to handle Eigen2 support macros
#include "Core"
#include "src/Core/util/DisableMSVCWarnings.h"
namespace Eigen {
/** \defgroup Array_Module Array module
* This module provides several handy features to manipulate matrices as simple array of values.
* In addition to listed classes, it defines various methods of the Cwise interface
* (accessible from MatrixBase::cwise()), including:
* - matrix-scalar sum,
* - coeff-wise comparison operators,
* - sin, cos, sqrt, pow, exp, log, square, cube, inverse (reciprocal).
*
* This module also provides various MatrixBase methods, including:
* - \ref MatrixBase::all() "all", \ref MatrixBase::any() "any",
* - \ref MatrixBase::Random() "random matrix initialization"
*
* \code
* #include <Eigen/Array>
* \endcode
*/
#include "src/Array/CwiseOperators.h"
#include "src/Array/Functors.h"
#include "src/Array/BooleanRedux.h"
#include "src/Array/Select.h"
#include "src/Array/PartialRedux.h"
#include "src/Array/Random.h"
#include "src/Array/Norms.h"
} // namespace Eigen
#include "src/Core/util/EnableMSVCWarnings.h"
#ifndef EIGEN2_SUPPORT
#error The Eigen/Array header does no longer exist in Eigen3. All that functionality has moved to Eigen/Core.
#endif
#endif // EIGEN_ARRAY_MODULE_H

34
ground/openpilotgcs/src/libs/eigen/Eigen/CMakeLists.txt
View File

@ -1,31 +1,19 @@
set(Eigen_HEADERS Core LU Cholesky QR Geometry
Sparse Array SVD LeastSquares
QtAlignedMalloc StdVector NewStdVector
Eigen Dense)
include(RegexUtils)
test_escape_string_as_regex()
if(EIGEN_BUILD_LIB)
set(Eigen_SRCS
src/Core/CoreInstantiations.cpp
src/Cholesky/CholeskyInstantiations.cpp
src/QR/QrInstantiations.cpp
)
file(GLOB Eigen_directory_files "*")
add_library(Eigen2 SHARED ${Eigen_SRCS})
escape_string_as_regex(ESCAPED_CMAKE_CURRENT_SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}")
install(TARGETS Eigen2
RUNTIME DESTINATION bin
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib)
endif(EIGEN_BUILD_LIB)
if(CMAKE_COMPILER_IS_GNUCXX)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g1 -O2")
set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "${CMAKE_CXX_FLAGS_RELWITHDEBINFO} -g1 -O2")
endif(CMAKE_COMPILER_IS_GNUCXX)
foreach(f ${Eigen_directory_files})
if(NOT f MATCHES "\\.txt" AND NOT f MATCHES "${ESCAPED_CMAKE_CURRENT_SOURCE_DIR}/[.].+" AND NOT f MATCHES "${ESCAPED_CMAKE_CURRENT_SOURCE_DIR}/src")
list(APPEND Eigen_directory_files_to_install ${f})
endif()
endforeach(f ${Eigen_directory_files})
install(FILES
${Eigen_HEADERS}
DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen
${Eigen_directory_files_to_install}
DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen COMPONENT Devel
)
add_subdirectory(src)

47
ground/openpilotgcs/src/libs/eigen/Eigen/Cholesky
View File

@ -3,22 +3,11 @@
#include "Core"
#include "src/Core/util/DisableMSVCWarnings.h"
// Note that EIGEN_HIDE_HEAVY_CODE has to be defined per module
#if (defined EIGEN_EXTERN_INSTANTIATIONS) && (EIGEN_EXTERN_INSTANTIATIONS>=2)
#ifndef EIGEN_HIDE_HEAVY_CODE
#define EIGEN_HIDE_HEAVY_CODE
#endif
#elif defined EIGEN_HIDE_HEAVY_CODE
#undef EIGEN_HIDE_HEAVY_CODE
#endif
namespace Eigen {
#include "src/Core/util/DisableStupidWarnings.h"
/** \defgroup Cholesky_Module Cholesky module
*
* \nonstableyet
*
*
* This module provides two variants of the Cholesky decomposition for selfadjoint (hermitian) matrices.
* Those decompositions are accessible via the following MatrixBase methods:
@ -30,36 +19,14 @@ namespace Eigen {
* \endcode
*/
#include "src/Array/CwiseOperators.h"
#include "src/Array/Functors.h"
#include "src/misc/Solve.h"
#include "src/Cholesky/LLT.h"
#include "src/Cholesky/LDLT.h"
} // namespace Eigen
#define EIGEN_CHOLESKY_MODULE_INSTANTIATE_TYPE(MATRIXTYPE,PREFIX) \
PREFIX template class LLT<MATRIXTYPE>; \
PREFIX template class LDLT<MATRIXTYPE>
#define EIGEN_CHOLESKY_MODULE_INSTANTIATE(PREFIX) \
EIGEN_CHOLESKY_MODULE_INSTANTIATE_TYPE(Matrix2f,PREFIX); \
EIGEN_CHOLESKY_MODULE_INSTANTIATE_TYPE(Matrix2d,PREFIX); \
EIGEN_CHOLESKY_MODULE_INSTANTIATE_TYPE(Matrix3f,PREFIX); \
EIGEN_CHOLESKY_MODULE_INSTANTIATE_TYPE(Matrix3d,PREFIX); \
EIGEN_CHOLESKY_MODULE_INSTANTIATE_TYPE(Matrix4f,PREFIX); \
EIGEN_CHOLESKY_MODULE_INSTANTIATE_TYPE(Matrix4d,PREFIX); \
EIGEN_CHOLESKY_MODULE_INSTANTIATE_TYPE(MatrixXf,PREFIX); \
EIGEN_CHOLESKY_MODULE_INSTANTIATE_TYPE(MatrixXd,PREFIX); \
EIGEN_CHOLESKY_MODULE_INSTANTIATE_TYPE(MatrixXcf,PREFIX); \
EIGEN_CHOLESKY_MODULE_INSTANTIATE_TYPE(MatrixXcd,PREFIX)
#ifdef EIGEN_EXTERN_INSTANTIATIONS
namespace Eigen {
EIGEN_CHOLESKY_MODULE_INSTANTIATE(extern);
} // namespace Eigen
#ifdef EIGEN_USE_LAPACKE
#include "src/Cholesky/LLT_MKL.h"
#endif
#include "src/Core/util/EnableMSVCWarnings.h"
#include "src/Core/util/ReenableStupidWarnings.h"
#endif // EIGEN_CHOLESKY_MODULE_H
/* vim: set filetype=cpp et sw=2 ts=2 ai: */

45
ground/openpilotgcs/src/libs/eigen/Eigen/CholmodSupport Normal file
View File

@ -0,0 +1,45 @@
#ifndef EIGEN_CHOLMODSUPPORT_MODULE_H
#define EIGEN_CHOLMODSUPPORT_MODULE_H
#include "SparseCore"
#include "src/Core/util/DisableStupidWarnings.h"
extern "C" {
#include <cholmod.h>
}
/** \ingroup Support_modules
* \defgroup CholmodSupport_Module CholmodSupport module
*
* This module provides an interface to the Cholmod library which is part of the <a href="http://www.cise.ufl.edu/research/sparse/SuiteSparse/">suitesparse</a> package.
* It provides the two following main factorization classes:
* - class CholmodSupernodalLLT: a supernodal LLT Cholesky factorization.
* - class CholmodDecomposiiton: a general L(D)LT Cholesky factorization with automatic or explicit runtime selection of the underlying factorization method (supernodal or simplicial).
*
* For the sake of completeness, this module also propose the two following classes:
* - class CholmodSimplicialLLT
* - class CholmodSimplicialLDLT
* Note that these classes does not bring any particular advantage compared to the built-in
* SimplicialLLT and SimplicialLDLT factorization classes.
*
* \code
* #include <Eigen/CholmodSupport>
* \endcode
*
* In order to use this module, the cholmod headers must be accessible from the include paths, and your binary must be linked to the cholmod library and its dependencies.
* The dependencies depend on how cholmod has been compiled.
* For a cmake based project, you can use our FindCholmod.cmake module to help you in this task.
*
*/
#include "src/misc/Solve.h"
#include "src/misc/SparseSolve.h"
#include "src/CholmodSupport/CholmodSupport.h"
#include "src/Core/util/ReenableStupidWarnings.h"
#endif // EIGEN_CHOLMODSUPPORT_MODULE_H

310
ground/openpilotgcs/src/libs/eigen/Eigen/Core
View File

@ -1,8 +1,43 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2007-2011 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
#ifndef EIGEN_CORE_H
#define EIGEN_CORE_H
// first thing Eigen does: prevent MSVC from committing suicide
#include "src/Core/util/DisableMSVCWarnings.h"
// first thing Eigen does: stop the compiler from committing suicide
#include "src/Core/util/DisableStupidWarnings.h"
// then include this file where all our macros are defined. It's really important to do it first because
// it's where we do all the alignment settings (platform detection and honoring the user's will if he
// defined e.g. EIGEN_DONT_ALIGN) so it needs to be done before we do anything with vectorization.
#include "src/Core/util/Macros.h"
// Disable the ipa-cp-clone optimization flag with MinGW 6.x or newer (enabled by default with -O3)
// See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=556 for details.
#if defined(__MINGW32__) && EIGEN_GNUC_AT_LEAST(4,6)
#pragma GCC optimize ("-fno-ipa-cp-clone")
#endif
#include <complex>
// this include file manages BLAS and MKL related macros
// and inclusion of their respective header files
#include "src/Core/util/MKL_support.h"
// if alignment is disabled, then disable vectorization. Note: EIGEN_ALIGN is the proper check, it takes into
// account both the user's will (EIGEN_DONT_ALIGN) and our own platform checks
#if !EIGEN_ALIGN
#ifndef EIGEN_DONT_VECTORIZE
#define EIGEN_DONT_VECTORIZE
#endif
#endif
#ifdef _MSC_VER
#include <malloc.h> // for _aligned_malloc -- need it regardless of whether vectorization is enabled
@ -11,35 +46,74 @@
// a user reported that in 64-bit mode, MSVC doesn't care to define _M_IX86_FP.
#if (defined(_M_IX86_FP) && (_M_IX86_FP >= 2)) || defined(_M_X64)
#define EIGEN_SSE2_ON_MSVC_2008_OR_LATER
#endif
#endif
#endif
#else
// Remember that usage of defined() in a #define is undefined by the standard
#if (defined __SSE2__) && ( (!defined __GNUC__) || (defined __INTEL_COMPILER) || EIGEN_GNUC_AT_LEAST(4,2) )
#define EIGEN_SSE2_ON_NON_MSVC_BUT_NOT_OLD_GCC
#endif
#endif
// FIXME: this check should not be against __QNXNTO__, which is also defined
// while compiling with GCC for QNX target. Better solution is welcome!
#if defined(__GNUC__) && !defined(__QNXNTO__)
#define EIGEN_GNUC_AT_LEAST(x,y) ((__GNUC__>=x && __GNUC_MINOR__>=y) || __GNUC__>x)
#else
#define EIGEN_GNUC_AT_LEAST(x,y) 0
#endif
#ifndef EIGEN_DONT_VECTORIZE
// Remember that usage of defined() in a #define is undefined by the standard
#if (defined __SSE2__) && ( (!defined __GNUC__) || EIGEN_GNUC_AT_LEAST(4,2) )
#define EIGEN_SSE2_BUT_NOT_OLD_GCC
#endif
#if defined (EIGEN_SSE2_ON_NON_MSVC_BUT_NOT_OLD_GCC) || defined(EIGEN_SSE2_ON_MSVC_2008_OR_LATER)
#if !defined(EIGEN_DONT_VECTORIZE) && !defined(EIGEN_DONT_ALIGN)
#if defined (EIGEN_SSE2_BUT_NOT_OLD_GCC) || defined(EIGEN_SSE2_ON_MSVC_2008_OR_LATER)
// Defines symbols for compile-time detection of which instructions are
// used.
// EIGEN_VECTORIZE_YY is defined if and only if the instruction set YY is used
#define EIGEN_VECTORIZE
#define EIGEN_VECTORIZE_SSE
#include <emmintrin.h>
#include <xmmintrin.h>
#define EIGEN_VECTORIZE_SSE2
// Detect sse3/ssse3/sse4:
// gcc and icc defines __SSE3__, ...
// there is no way to know about this on msvc. You can define EIGEN_VECTORIZE_SSE* if you
// want to force the use of those instructions with msvc.
#ifdef __SSE3__
#include <pmmintrin.h>
#define EIGEN_VECTORIZE_SSE3
#endif
#ifdef __SSSE3__
#include <tmmintrin.h>
#define EIGEN_VECTORIZE_SSSE3
#endif
#ifdef __SSE4_1__
#define EIGEN_VECTORIZE_SSE4_1
#endif
#ifdef __SSE4_2__
#define EIGEN_VECTORIZE_SSE4_2
#endif
// include files
// This extern "C" works around a MINGW-w64 compilation issue
// https://sourceforge.net/tracker/index.php?func=detail&aid=3018394&group_id=202880&atid=983354
// In essence, intrin.h is included by windows.h and also declares intrinsics (just as emmintrin.h etc. below do).
// However, intrin.h uses an extern "C" declaration, and g++ thus complains of duplicate declarations
// with conflicting linkage. The linkage for intrinsics doesn't matter, but at that stage the compiler doesn't know;
// so, to avoid compile errors when windows.h is included after Eigen/Core, ensure intrinsics are extern "C" here too.
// notice that since these are C headers, the extern "C" is theoretically needed anyways.
extern "C" {
// In theory we should only include immintrin.h and not the other *mmintrin.h header files directly.
// Doing so triggers some issues with ICC. However old gcc versions seems to not have this file, thus:
#ifdef __INTEL_COMPILER
#include <immintrin.h>
#else
#include <emmintrin.h>
#include <xmmintrin.h>
#ifdef EIGEN_VECTORIZE_SSE3
#include <pmmintrin.h>
#endif
#ifdef EIGEN_VECTORIZE_SSSE3
#include <tmmintrin.h>
#endif
#ifdef EIGEN_VECTORIZE_SSE4_1
#include <smmintrin.h>
#endif
#ifdef EIGEN_VECTORIZE_SSE4_2
#include <nmmintrin.h>
#endif
#endif
} // end extern "C"
#elif defined __ALTIVEC__
#define EIGEN_VECTORIZE
#define EIGEN_VECTORIZE_ALTIVEC
@ -49,20 +123,53 @@
#undef bool
#undef vector
#undef pixel
#elif defined __ARM_NEON__
#define EIGEN_VECTORIZE
#define EIGEN_VECTORIZE_NEON
#include <arm_neon.h>
#endif
#endif
#if (defined _OPENMP) && (!defined EIGEN_DONT_PARALLELIZE)
#define EIGEN_HAS_OPENMP
#endif
#ifdef EIGEN_HAS_OPENMP
#include <omp.h>
#endif
// MSVC for windows mobile does not have the errno.h file
#if !(defined(_MSC_VER) && defined(_WIN32_WCE)) && !defined(__ARMCC_VERSION)
#define EIGEN_HAS_ERRNO
#endif
#ifdef EIGEN_HAS_ERRNO
#include <cerrno>
#endif
#include <cstddef>
#include <cstdlib>
#include <cmath>
#include <complex>
#include <cassert>
#include <functional>
#include <iosfwd>
#include <cstring>
#include <string>
#include <limits>
#include <climits> // for CHAR_BIT
// for min/max:
#include <algorithm>
#if (defined(_CPPUNWIND) || defined(__EXCEPTIONS)) && !defined(EIGEN_NO_EXCEPTIONS)
// for outputting debug info
#ifdef EIGEN_DEBUG_ASSIGN
#include <iostream>
#endif
// required for __cpuid, needs to be included after cmath
#if defined(_MSC_VER) && (defined(_M_IX86)||defined(_M_X64))
#include <intrin.h>
#endif
#if defined(_CPPUNWIND) || defined(__EXCEPTIONS)
#define EIGEN_EXCEPTIONS
#endif
@ -70,18 +177,66 @@
#include <new>
#endif
// this needs to be done after all possible windows C header includes and before any Eigen source includes
// (system C++ includes are supposed to be able to deal with this already):
// windows.h defines min and max macros which would make Eigen fail to compile.
#if defined(min) || defined(max)
#error The preprocessor symbols 'min' or 'max' are defined. If you are compiling on Windows, do #define NOMINMAX to prevent windows.h from defining these symbols.
/** \brief Namespace containing all symbols from the %Eigen library. */
namespace Eigen {
inline static const char *SimdInstructionSetsInUse(void) {
#if defined(EIGEN_VECTORIZE_SSE4_2)
return "SSE, SSE2, SSE3, SSSE3, SSE4.1, SSE4.2";
#elif defined(EIGEN_VECTORIZE_SSE4_1)
return "SSE, SSE2, SSE3, SSSE3, SSE4.1";
#elif defined(EIGEN_VECTORIZE_SSSE3)
return "SSE, SSE2, SSE3, SSSE3";
#elif defined(EIGEN_VECTORIZE_SSE3)
return "SSE, SSE2, SSE3";
#elif defined(EIGEN_VECTORIZE_SSE2)
return "SSE, SSE2";
#elif defined(EIGEN_VECTORIZE_ALTIVEC)
return "AltiVec";
#elif defined(EIGEN_VECTORIZE_NEON)
return "ARM NEON";
#else
return "None";
#endif
}
} // end namespace Eigen
#define STAGE10_FULL_EIGEN2_API 10
#define STAGE20_RESOLVE_API_CONFLICTS 20
#define STAGE30_FULL_EIGEN3_API 30
#define STAGE40_FULL_EIGEN3_STRICTNESS 40
#define STAGE99_NO_EIGEN2_SUPPORT 99
#if defined EIGEN2_SUPPORT_STAGE40_FULL_EIGEN3_STRICTNESS
#define EIGEN2_SUPPORT
#define EIGEN2_SUPPORT_STAGE STAGE40_FULL_EIGEN3_STRICTNESS
#elif defined EIGEN2_SUPPORT_STAGE30_FULL_EIGEN3_API
#define EIGEN2_SUPPORT
#define EIGEN2_SUPPORT_STAGE STAGE30_FULL_EIGEN3_API
#elif defined EIGEN2_SUPPORT_STAGE20_RESOLVE_API_CONFLICTS
#define EIGEN2_SUPPORT
#define EIGEN2_SUPPORT_STAGE STAGE20_RESOLVE_API_CONFLICTS
#elif defined EIGEN2_SUPPORT_STAGE10_FULL_EIGEN2_API
#define EIGEN2_SUPPORT
#define EIGEN2_SUPPORT_STAGE STAGE10_FULL_EIGEN2_API
#elif defined EIGEN2_SUPPORT
// default to stage 3, that's what it's always meant
#define EIGEN2_SUPPORT_STAGE30_FULL_EIGEN3_API
#define EIGEN2_SUPPORT_STAGE STAGE30_FULL_EIGEN3_API
#else
#define EIGEN2_SUPPORT_STAGE STAGE99_NO_EIGEN2_SUPPORT
#endif
namespace Eigen {
#ifdef EIGEN2_SUPPORT
#undef minor
#endif
// we use size_t frequently and we'll never remember to prepend it with std:: everytime just to
// ensure QNX/QCC support
using std::size_t;
// gcc 4.6.0 wants std:: for ptrdiff_t
using std::ptrdiff_t;
/** \defgroup Core_Module Core module
* This is the main module of Eigen providing dense matrix and vector support
@ -93,12 +248,11 @@ using std::size_t;
* \endcode
*/
#include "src/Core/util/Macros.h"
#include "src/Core/util/Constants.h"
#include "src/Core/util/ForwardDeclarations.h"
#include "src/Core/util/Meta.h"
#include "src/Core/util/XprHelper.h"
#include "src/Core/util/StaticAssert.h"
#include "src/Core/util/XprHelper.h"
#include "src/Core/util/Memory.h"
#include "src/Core/NumTraits.h"
@ -107,54 +261,116 @@ using std::size_t;
#if defined EIGEN_VECTORIZE_SSE
#include "src/Core/arch/SSE/PacketMath.h"
#include "src/Core/arch/SSE/MathFunctions.h"
#include "src/Core/arch/SSE/Complex.h"
#elif defined EIGEN_VECTORIZE_ALTIVEC
#include "src/Core/arch/AltiVec/PacketMath.h"
#include "src/Core/arch/AltiVec/Complex.h"
#elif defined EIGEN_VECTORIZE_NEON
#include "src/Core/arch/NEON/PacketMath.h"
#include "src/Core/arch/NEON/Complex.h"
#endif
#ifndef EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD
#define EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 16
#endif
#include "src/Core/arch/Default/Settings.h"
#include "src/Core/Functors.h"
#include "src/Core/DenseCoeffsBase.h"
#include "src/Core/DenseBase.h"
#include "src/Core/MatrixBase.h"
#include "src/Core/Coeffs.h"
#include "src/Core/EigenBase.h"
#ifndef EIGEN_PARSED_BY_DOXYGEN // work around Doxygen bug triggered by Assign.h r814874
// at least confirmed with Doxygen 1.5.5 and 1.5.6
#include "src/Core/Assign.h"
#endif
#include "src/Core/MatrixStorage.h"
#include "src/Core/util/BlasUtil.h"
#include "src/Core/DenseStorage.h"
#include "src/Core/NestByValue.h"
#include "src/Core/Flagged.h"
#include "src/Core/ForceAlignedAccess.h"
#include "src/Core/ReturnByValue.h"
#include "src/Core/NoAlias.h"
#include "src/Core/PlainObjectBase.h"
#include "src/Core/Matrix.h"
#include "src/Core/Cwise.h"
#include "src/Core/Array.h"
#include "src/Core/CwiseBinaryOp.h"
#include "src/Core/CwiseUnaryOp.h"
#include "src/Core/CwiseNullaryOp.h"
#include "src/Core/CwiseUnaryView.h"
#include "src/Core/SelfCwiseBinaryOp.h"
#include "src/Core/Dot.h"
#include "src/Core/Product.h"
#include "src/Core/DiagonalProduct.h"
#include "src/Core/SolveTriangular.h"
#include "src/Core/StableNorm.h"
#include "src/Core/MapBase.h"
#include "src/Core/Stride.h"
#include "src/Core/Map.h"
#include "src/Core/Block.h"
#include "src/Core/Minor.h"
#include "src/Core/VectorBlock.h"
#include "src/Core/Ref.h"
#include "src/Core/Transpose.h"
#include "src/Core/DiagonalMatrix.h"
#include "src/Core/DiagonalCoeffs.h"
#include "src/Core/Sum.h"
#include "src/Core/Diagonal.h"
#include "src/Core/DiagonalProduct.h"
#include "src/Core/PermutationMatrix.h"
#include "src/Core/Transpositions.h"
#include "src/Core/Redux.h"
#include "src/Core/Visitor.h"
#include "src/Core/Fuzzy.h"
#include "src/Core/IO.h"
#include "src/Core/Swap.h"
#include "src/Core/CommaInitializer.h"
#include "src/Core/Part.h"
#include "src/Core/CacheFriendlyProduct.h"
#include "src/Core/Flagged.h"
#include "src/Core/ProductBase.h"
#include "src/Core/GeneralProduct.h"
#include "src/Core/TriangularMatrix.h"
#include "src/Core/SelfAdjointView.h"
#include "src/Core/products/GeneralBlockPanelKernel.h"
#include "src/Core/products/Parallelizer.h"
#include "src/Core/products/CoeffBasedProduct.h"
#include "src/Core/products/GeneralMatrixVector.h"
#include "src/Core/products/GeneralMatrixMatrix.h"
#include "src/Core/SolveTriangular.h"
#include "src/Core/products/GeneralMatrixMatrixTriangular.h"
#include "src/Core/products/SelfadjointMatrixVector.h"
#include "src/Core/products/SelfadjointMatrixMatrix.h"
#include "src/Core/products/SelfadjointProduct.h"
#include "src/Core/products/SelfadjointRank2Update.h"
#include "src/Core/products/TriangularMatrixVector.h"
#include "src/Core/products/TriangularMatrixMatrix.h"
#include "src/Core/products/TriangularSolverMatrix.h"
#include "src/Core/products/TriangularSolverVector.h"
#include "src/Core/BandMatrix.h"
#include "src/Core/CoreIterators.h"
} // namespace Eigen
#include "src/Core/BooleanRedux.h"
#include "src/Core/Select.h"
#include "src/Core/VectorwiseOp.h"
#include "src/Core/Random.h"
#include "src/Core/Replicate.h"
#include "src/Core/Reverse.h"
#include "src/Core/ArrayBase.h"
#include "src/Core/ArrayWrapper.h"
#include "src/Core/util/EnableMSVCWarnings.h"
#ifdef EIGEN_USE_BLAS
#include "src/Core/products/GeneralMatrixMatrix_MKL.h"
#include "src/Core/products/GeneralMatrixVector_MKL.h"
#include "src/Core/products/GeneralMatrixMatrixTriangular_MKL.h"
#include "src/Core/products/SelfadjointMatrixMatrix_MKL.h"
#include "src/Core/products/SelfadjointMatrixVector_MKL.h"
#include "src/Core/products/TriangularMatrixMatrix_MKL.h"
#include "src/Core/products/TriangularMatrixVector_MKL.h"
#include "src/Core/products/TriangularSolverMatrix_MKL.h"
#endif // EIGEN_USE_BLAS
#ifdef EIGEN_USE_MKL_VML
#include "src/Core/Assign_MKL.h"
#endif
#include "src/Core/GlobalFunctions.h"
#include "src/Core/util/ReenableStupidWarnings.h"
#ifdef EIGEN2_SUPPORT
#include "Eigen2Support"
#endif
#endif // EIGEN_CORE_H

3
ground/openpilotgcs/src/libs/eigen/Eigen/Dense
View File

@ -1,8 +1,7 @@
#include "Core"
#include "Array"
#include "LU"
#include "Cholesky"
#include "QR"
#include "SVD"
#include "Geometry"
#include "LeastSquares"
#include "Eigenvalues"

2
ground/openpilotgcs/src/libs/eigen/Eigen/Eigen
View File

@ -1,2 +1,2 @@
#include "Dense"
#include "Sparse"
//#include "Sparse"

82
ground/openpilotgcs/src/libs/eigen/Eigen/Eigen2Support Normal file
View File

@ -0,0 +1,82 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
#ifndef EIGEN2SUPPORT_H
#define EIGEN2SUPPORT_H
#if (!defined(EIGEN2_SUPPORT)) || (!defined(EIGEN_CORE_H))
#error Eigen2 support must be enabled by defining EIGEN2_SUPPORT before including any Eigen header
#endif
#include "src/Core/util/DisableStupidWarnings.h"
/** \ingroup Support_modules
* \defgroup Eigen2Support_Module Eigen2 support module
* This module provides a couple of deprecated functions improving the compatibility with Eigen2.
*
* To use it, define EIGEN2_SUPPORT before including any Eigen header
* \code
* #define EIGEN2_SUPPORT
* \endcode
*
*/
#include "src/Eigen2Support/Macros.h"
#include "src/Eigen2Support/Memory.h"
#include "src/Eigen2Support/Meta.h"
#include "src/Eigen2Support/Lazy.h"
#include "src/Eigen2Support/Cwise.h"
#include "src/Eigen2Support/CwiseOperators.h"
#include "src/Eigen2Support/TriangularSolver.h"
#include "src/Eigen2Support/Block.h"
#include "src/Eigen2Support/VectorBlock.h"
#include "src/Eigen2Support/Minor.h"
#include "src/Eigen2Support/MathFunctions.h"
#include "src/Core/util/ReenableStupidWarnings.h"
// Eigen2 used to include iostream
#include<iostream>
#define EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, SizeSuffix) \
using Eigen::Matrix##SizeSuffix##TypeSuffix; \
using Eigen::Vector##SizeSuffix##TypeSuffix; \
using Eigen::RowVector##SizeSuffix##TypeSuffix;
#define EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(TypeSuffix) \
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, 2) \
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, 3) \
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, 4) \
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, X) \
#define EIGEN_USING_MATRIX_TYPEDEFS \
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(i) \
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(f) \
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(d) \
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(cf) \
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(cd)
#define USING_PART_OF_NAMESPACE_EIGEN \
EIGEN_USING_MATRIX_TYPEDEFS \
using Eigen::Matrix; \
using Eigen::MatrixBase; \
using Eigen::ei_random; \
using Eigen::ei_real; \
using Eigen::ei_imag; \
using Eigen::ei_conj; \
using Eigen::ei_abs; \
using Eigen::ei_abs2; \
using Eigen::ei_sqrt; \
using Eigen::ei_exp; \
using Eigen::ei_log; \
using Eigen::ei_sin; \
using Eigen::ei_cos;
#endif // EIGEN2SUPPORT_H

48
ground/openpilotgcs/src/libs/eigen/Eigen/Eigenvalues Normal file
View File

@ -0,0 +1,48 @@
#ifndef EIGEN_EIGENVALUES_MODULE_H
#define EIGEN_EIGENVALUES_MODULE_H
#include "Core"
#include "src/Core/util/DisableStupidWarnings.h"
#include "Cholesky"
#include "Jacobi"
#include "Householder"
#include "LU"
#include "Geometry"
/** \defgroup Eigenvalues_Module Eigenvalues module
*
*
*
* This module mainly provides various eigenvalue solvers.
* This module also provides some MatrixBase methods, including:
* - MatrixBase::eigenvalues(),
* - MatrixBase::operatorNorm()
*
* \code
* #include <Eigen/Eigenvalues>
* \endcode
*/
#include "src/Eigenvalues/Tridiagonalization.h"
#include "src/Eigenvalues/RealSchur.h"
#include "src/Eigenvalues/EigenSolver.h"
#include "src/Eigenvalues/SelfAdjointEigenSolver.h"
#include "src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h"
#include "src/Eigenvalues/HessenbergDecomposition.h"
#include "src/Eigenvalues/ComplexSchur.h"
#include "src/Eigenvalues/ComplexEigenSolver.h"
#include "src/Eigenvalues/RealQZ.h"
#include "src/Eigenvalues/GeneralizedEigenSolver.h"
#include "src/Eigenvalues/MatrixBaseEigenvalues.h"
#ifdef EIGEN_USE_LAPACKE
#include "src/Eigenvalues/RealSchur_MKL.h"
#include "src/Eigenvalues/ComplexSchur_MKL.h"
#include "src/Eigenvalues/SelfAdjointEigenSolver_MKL.h"
#endif
#include "src/Core/util/ReenableStupidWarnings.h"
#endif // EIGEN_EIGENVALUES_MODULE_H
/* vim: set filetype=cpp et sw=2 ts=2 ai: */

46
ground/openpilotgcs/src/libs/eigen/Eigen/Geometry
View File

@ -3,20 +3,19 @@
#include "Core"
#include "src/Core/util/DisableMSVCWarnings.h"
#include "src/Core/util/DisableStupidWarnings.h"
#include "Array"
#include "SVD"
#include "LU"
#include <limits>
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
namespace Eigen {
/** \defgroup Geometry_Module Geometry module
*
* \nonstableyet
*
*
* This module provides support for:
* - fixed-size homogeneous transformations
@ -32,20 +31,33 @@ namespace Eigen {
*/
#include "src/Geometry/OrthoMethods.h"
#include "src/Geometry/RotationBase.h"
#include "src/Geometry/Rotation2D.h"
#include "src/Geometry/Quaternion.h"
#include "src/Geometry/AngleAxis.h"
#include "src/Geometry/EulerAngles.h"
#include "src/Geometry/Transform.h"
#include "src/Geometry/Translation.h"
#include "src/Geometry/Scaling.h"
#include "src/Geometry/Hyperplane.h"
#include "src/Geometry/ParametrizedLine.h"
#include "src/Geometry/AlignedBox.h"
} // namespace Eigen
#if EIGEN2_SUPPORT_STAGE > STAGE20_RESOLVE_API_CONFLICTS
#include "src/Geometry/Homogeneous.h"
#include "src/Geometry/RotationBase.h"
#include "src/Geometry/Rotation2D.h"
#include "src/Geometry/Quaternion.h"
#include "src/Geometry/AngleAxis.h"
#include "src/Geometry/Transform.h"
#include "src/Geometry/Translation.h"
#include "src/Geometry/Scaling.h"
#include "src/Geometry/Hyperplane.h"
#include "src/Geometry/ParametrizedLine.h"
#include "src/Geometry/AlignedBox.h"
#include "src/Geometry/Umeyama.h"
#include "src/Core/util/EnableMSVCWarnings.h"
#if defined EIGEN_VECTORIZE_SSE
#include "src/Geometry/arch/Geometry_SSE.h"
#endif
#endif
#ifdef EIGEN2_SUPPORT
#include "src/Eigen2Support/Geometry/All.h"
#endif
#include "src/Core/util/ReenableStupidWarnings.h"
#endif // EIGEN_GEOMETRY_MODULE_H
/* vim: set filetype=cpp et sw=2 ts=2 ai: */

23
ground/openpilotgcs/src/libs/eigen/Eigen/Householder Normal file
View File

@ -0,0 +1,23 @@
#ifndef EIGEN_HOUSEHOLDER_MODULE_H
#define EIGEN_HOUSEHOLDER_MODULE_H
#include "Core"
#include "src/Core/util/DisableStupidWarnings.h"
/** \defgroup Householder_Module Householder module
* This module provides Householder transformations.
*
* \code
* #include <Eigen/Householder>
* \endcode
*/
#include "src/Householder/Householder.h"
#include "src/Householder/HouseholderSequence.h"
#include "src/Householder/BlockHouseholder.h"
#include "src/Core/util/ReenableStupidWarnings.h"
#endif // EIGEN_HOUSEHOLDER_MODULE_H
/* vim: set filetype=cpp et sw=2 ts=2 ai: */

40
ground/openpilotgcs/src/libs/eigen/Eigen/IterativeLinearSolvers Normal file
View File

@ -0,0 +1,40 @@
#ifndef EIGEN_ITERATIVELINEARSOLVERS_MODULE_H
#define EIGEN_ITERATIVELINEARSOLVERS_MODULE_H
#include "SparseCore"
#include "OrderingMethods"
#include "src/Core/util/DisableStupidWarnings.h"
/**
* \defgroup IterativeLinearSolvers_Module IterativeLinearSolvers module
*
* This module currently provides iterative methods to solve problems of the form \c A \c x = \c b, where \c A is a squared matrix, usually very large and sparse.
* Those solvers are accessible via the following classes:
* - ConjugateGradient for selfadjoint (hermitian) matrices,
* - BiCGSTAB for general square matrices.
*
* These iterative solvers are associated with some preconditioners:
* - IdentityPreconditioner - not really useful
* - DiagonalPreconditioner - also called JAcobi preconditioner, work very well on diagonal dominant matrices.
* - IncompleteILUT - incomplete LU factorization with dual thresholding
*
* Such problems can also be solved using the direct sparse decomposition modules: SparseCholesky, CholmodSupport, UmfPackSupport, SuperLUSupport.
*
* \code
* #include <Eigen/IterativeLinearSolvers>
* \endcode
*/
#include "src/misc/Solve.h"
#include "src/misc/SparseSolve.h"
#include "src/IterativeLinearSolvers/IterativeSolverBase.h"
#include "src/IterativeLinearSolvers/BasicPreconditioners.h"
#include "src/IterativeLinearSolvers/ConjugateGradient.h"
#include "src/IterativeLinearSolvers/BiCGSTAB.h"
#include "src/IterativeLinearSolvers/IncompleteLUT.h"
#include "src/Core/util/ReenableStupidWarnings.h"
#endif // EIGEN_ITERATIVELINEARSOLVERS_MODULE_H

26
ground/openpilotgcs/src/libs/eigen/Eigen/Jacobi Normal file
View File

@ -0,0 +1,26 @@
#ifndef EIGEN_JACOBI_MODULE_H
#define EIGEN_JACOBI_MODULE_H
#include "Core"
#include "src/Core/util/DisableStupidWarnings.h"
/** \defgroup Jacobi_Module Jacobi module
* This module provides Jacobi and Givens rotations.
*
* \code
* #include <Eigen/Jacobi>
* \endcode
*
* In addition to listed classes, it defines the two following MatrixBase methods to apply a Jacobi or Givens rotation:
* - MatrixBase::applyOnTheLeft()
* - MatrixBase::applyOnTheRight().
*/
#include "src/Jacobi/Jacobi.h"
#include "src/Core/util/ReenableStupidWarnings.h"
#endif // EIGEN_JACOBI_MODULE_H
/* vim: set filetype=cpp et sw=2 ts=2 ai: */

24
ground/openpilotgcs/src/libs/eigen/Eigen/LU
View File

@ -3,9 +3,7 @@
#include "Core"
#include "src/Core/util/DisableMSVCWarnings.h"
namespace Eigen {
#include "src/Core/util/DisableStupidWarnings.h"
/** \defgroup LU_Module LU module
* This module includes %LU decomposition and related notions such as matrix inversion and determinant.
@ -18,12 +16,26 @@ namespace Eigen {
* \endcode
*/
#include "src/LU/LU.h"
#include "src/misc/Solve.h"
#include "src/misc/Kernel.h"
#include "src/misc/Image.h"
#include "src/LU/FullPivLU.h"
#include "src/LU/PartialPivLU.h"
#ifdef EIGEN_USE_LAPACKE
#include "src/LU/PartialPivLU_MKL.h"
#endif
#include "src/LU/Determinant.h"
#include "src/LU/Inverse.h"
} // namespace Eigen
#if defined EIGEN_VECTORIZE_SSE
#include "src/LU/arch/Inverse_SSE.h"
#endif
#include "src/Core/util/EnableMSVCWarnings.h"
#ifdef EIGEN2_SUPPORT
#include "src/Eigen2Support/LU.h"
#endif
#include "src/Core/util/ReenableStupidWarnings.h"
#endif // EIGEN_LU_MODULE_H
/* vim: set filetype=cpp et sw=2 ts=2 ai: */

19
ground/openpilotgcs/src/libs/eigen/Eigen/LeastSquares
View File

@ -1,15 +1,20 @@
#ifndef EIGEN_REGRESSION_MODULE_H
#define EIGEN_REGRESSION_MODULE_H
#ifndef EIGEN2_SUPPORT
#error LeastSquares is only available in Eigen2 support mode (define EIGEN2_SUPPORT)
#endif
// exclude from normal eigen3-only documentation
#ifdef EIGEN2_SUPPORT
#include "Core"
#include "src/Core/util/DisableMSVCWarnings.h"
#include "src/Core/util/DisableStupidWarnings.h"
#include "QR"
#include "Eigenvalues"
#include "Geometry"
namespace Eigen {
/** \defgroup LeastSquares_Module LeastSquares module
* This module provides linear regression and related features.
*
@ -18,10 +23,10 @@ namespace Eigen {
* \endcode
*/
#include "src/LeastSquares/LeastSquares.h"
#include "src/Eigen2Support/LeastSquares.h"
} // namespace Eigen
#include "src/Core/util/ReenableStupidWarnings.h"
#include "src/Core/util/EnableMSVCWarnings.h"
#endif // EIGEN2_SUPPORT
#endif // EIGEN_REGRESSION_MODULE_H

28
ground/openpilotgcs/src/libs/eigen/Eigen/MetisSupport Normal file
View File

@ -0,0 +1,28 @@
#ifndef EIGEN_METISSUPPORT_MODULE_H
#define EIGEN_METISSUPPORT_MODULE_H
#include "SparseCore"
#include "src/Core/util/DisableStupidWarnings.h"
extern "C" {
#include <metis.h>
}
/** \ingroup Support_modules
* \defgroup MetisSupport_Module MetisSupport module
*
* \code
* #include <Eigen/MetisSupport>
* \endcode
* This module defines an interface to the METIS reordering package (http://glaros.dtc.umn.edu/gkhome/views/metis).
* It can be used just as any other built-in method as explained in \link OrderingMethods_Module here. \endlink
*/
#include "src/MetisSupport/MetisSupport.h"
#include "src/Core/util/ReenableStupidWarnings.h"
#endif // EIGEN_METISSUPPORT_MODULE_H

168
ground/openpilotgcs/src/libs/eigen/Eigen/NewStdVector
View File

@ -1,168 +0,0 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Gael Guennebaud <g.gael@free.fr>
// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
//
// Eigen is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// Alternatively, you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of
// the License, or (at your option) any later version.
//
// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_STDVECTOR_MODULE_H
#define EIGEN_STDVECTOR_MODULE_H
#include "Core"
#include <vector>
namespace Eigen {
// This one is needed to prevent reimplementing the whole std::vector.
template <class T>
class aligned_allocator_indirection : public aligned_allocator<T>
{
public:
typedef std::size_t size_type;
typedef std::ptrdiff_t difference_type;
typedef T* pointer;
typedef const T* const_pointer;
typedef T& reference;
typedef const T& const_reference;
typedef T value_type;
template<class U>
struct rebind
{
typedef aligned_allocator_indirection<U> other;
};
aligned_allocator_indirection() throw() {}
aligned_allocator_indirection(const aligned_allocator_indirection& ) throw() : aligned_allocator<T>() {}
aligned_allocator_indirection(const aligned_allocator<T>& ) throw() {}
template<class U>
aligned_allocator_indirection(const aligned_allocator_indirection<U>& ) throw() {}
template<class U>
aligned_allocator_indirection(const aligned_allocator<U>& ) throw() {}
~aligned_allocator_indirection() throw() {}
};
#ifdef _MSC_VER
// sometimes, MSVC detects, at compile time, that the argument x
// in std::vector::resize(size_t s,T x) won't be aligned and generate an error
// even if this function is never called. Whence this little wrapper.
#define EIGEN_WORKAROUND_MSVC_STD_VECTOR(T) Eigen::ei_workaround_msvc_std_vector<T>
template<typename T> struct ei_workaround_msvc_std_vector : public T
{
inline ei_workaround_msvc_std_vector() : T() {}
inline ei_workaround_msvc_std_vector(const T& other) : T(other) {}
inline operator T& () { return *static_cast<T*>(this); }
inline operator const T& () const { return *static_cast<const T*>(this); }
template<typename OtherT>
inline T& operator=(const OtherT& other)
{ T::operator=(other); return *this; }
inline ei_workaround_msvc_std_vector& operator=(const ei_workaround_msvc_std_vector& other)
{ T::operator=(other); return *this; }
};
#else
#define EIGEN_WORKAROUND_MSVC_STD_VECTOR(T) T
#endif
}
namespace std {
#define EIGEN_STD_VECTOR_SPECIALIZATION_BODY \
public: \
typedef T value_type; \
typedef typename vector_base::allocator_type allocator_type; \
typedef typename vector_base::size_type size_type; \
typedef typename vector_base::iterator iterator; \
typedef typename vector_base::const_iterator const_iterator; \
explicit vector(const allocator_type& a = allocator_type()) : vector_base(a) {} \
template<typename InputIterator> \
vector(InputIterator first, InputIterator last, const allocator_type& a = allocator_type()) \
: vector_base(first, last, a) {} \
vector(const vector& c) : vector_base(c) {} \
explicit vector(size_type num, const value_type& val = value_type()) : vector_base(num, val) {} \
vector(iterator start, iterator end) : vector_base(start, end) {} \
vector& operator=(const vector& x) { \
vector_base::operator=(x); \
return *this; \
}
template<typename T>
class vector<T,Eigen::aligned_allocator<T> >
: public vector<EIGEN_WORKAROUND_MSVC_STD_VECTOR(T),
Eigen::aligned_allocator_indirection<EIGEN_WORKAROUND_MSVC_STD_VECTOR(T)> >
{
typedef vector<EIGEN_WORKAROUND_MSVC_STD_VECTOR(T),
Eigen::aligned_allocator_indirection<EIGEN_WORKAROUND_MSVC_STD_VECTOR(T)> > vector_base;
EIGEN_STD_VECTOR_SPECIALIZATION_BODY
void resize(size_type new_size)
{ resize(new_size, T()); }
#if defined(_VECTOR_)
// workaround MSVC std::vector implementation
void resize(size_type new_size, const value_type& x)
{
if (vector_base::size() < new_size)
vector_base::_Insert_n(vector_base::end(), new_size - vector_base::size(), x);
else if (new_size < vector_base::size())
vector_base::erase(vector_base::begin() + new_size, vector_base::end());
}
void push_back(const value_type& x)
{ vector_base::push_back(x); }
using vector_base::insert;
iterator insert(const_iterator position, const value_type& x)
{ return vector_base::insert(position,x); }
void insert(const_iterator position, size_type new_size, const value_type& x)
{ vector_base::insert(position, new_size, x); }
#elif defined(_GLIBCXX_VECTOR) && EIGEN_GNUC_AT_LEAST(4,2)
// workaround GCC std::vector implementation
void resize(size_type new_size, const value_type& x)
{
if (new_size < vector_base::size())
vector_base::_M_erase_at_end(this->_M_impl._M_start + new_size);
else
vector_base::insert(vector_base::end(), new_size - vector_base::size(), x);
}
#elif defined(_GLIBCXX_VECTOR) && (!EIGEN_GNUC_AT_LEAST(4,1))
// Note that before gcc-4.1 we already have: std::vector::resize(size_type,const T&),
// no no need to workaround !
using vector_base::resize;
#else
// either GCC 4.1 or non-GCC
// default implementation which should always work.
void resize(size_type new_size, const value_type& x)
{
if (new_size < vector_base::size())
vector_base::erase(vector_base::begin() + new_size, vector_base::end());
else if (new_size > vector_base::size())
vector_base::insert(vector_base::end(), new_size - vector_base::size(), x);
}
#endif
};
}
#endif // EIGEN_STDVECTOR_MODULE_H

66
ground/openpilotgcs/src/libs/eigen/Eigen/OrderingMethods Normal file
View File

@ -0,0 +1,66 @@
#ifndef EIGEN_ORDERINGMETHODS_MODULE_H
#define EIGEN_ORDERINGMETHODS_MODULE_H
#include "SparseCore"
#include "src/Core/util/DisableStupidWarnings.h"
/**
* \defgroup OrderingMethods_Module OrderingMethods module
*
* This module is currently for internal use only
*
* It defines various built-in and external ordering methods for sparse matrices.
* They are typically used to reduce the number of elements during
* the sparse matrix decomposition (LLT, LU, QR).
* Precisely, in a preprocessing step, a permutation matrix P is computed using
* those ordering methods and applied to the columns of the matrix.
* Using for instance the sparse Cholesky decomposition, it is expected that
* the nonzeros elements in LLT(A*P) will be much smaller than that in LLT(A).
*
*
* Usage :
* \code
* #include <Eigen/OrderingMethods>
* \endcode
*
* A simple usage is as a template parameter in the sparse decomposition classes :
*
* \code
* SparseLU<MatrixType, COLAMDOrdering<int> > solver;
* \endcode
*
* \code
* SparseQR<MatrixType, COLAMDOrdering<int> > solver;
* \endcode
*
* It is possible as well to call directly a particular ordering method for your own purpose,
* \code
* AMDOrdering<int> ordering;
* PermutationMatrix<Dynamic, Dynamic, int> perm;
* SparseMatrix<double> A;
* //Fill the matrix ...
*
* ordering(A, perm); // Call AMD
* \endcode
*
* \note Some of these methods (like AMD or METIS), need the sparsity pattern
* of the input matrix to be symmetric. When the matrix is structurally unsymmetric,
* Eigen computes internally the pattern of \f$A^T*A\f$ before calling the method.
* If your matrix is already symmetric (at leat in structure), you can avoid that
* by calling the method with a SelfAdjointView type.
*
* \code
* // Call the ordering on the pattern of the lower triangular matrix A
* ordering(A.selfadjointView<Lower>(), perm);
* \endcode
*/
#ifndef EIGEN_MPL2_ONLY
#include "src/OrderingMethods/Amd.h"
#endif
#include "src/OrderingMethods/Ordering.h"
#include "src/Core/util/ReenableStupidWarnings.h"
#endif // EIGEN_ORDERINGMETHODS_MODULE_H

46
ground/openpilotgcs/src/libs/eigen/Eigen/PaStiXSupport Normal file
View File

@ -0,0 +1,46 @@
#ifndef EIGEN_PASTIXSUPPORT_MODULE_H
#define EIGEN_PASTIXSUPPORT_MODULE_H
#include "SparseCore"
#include "src/Core/util/DisableStupidWarnings.h"
#include <complex.h>
extern "C" {
#include <pastix_nompi.h>
#include <pastix.h>
}
#ifdef complex
#undef complex
#endif
/** \ingroup Support_modules
* \defgroup PaStiXSupport_Module PaStiXSupport module
*
* This module provides an interface to the <a href="http://pastix.gforge.inria.fr/">PaSTiX</a> library.
* PaSTiX is a general \b supernodal, \b parallel and \b opensource sparse solver.
* It provides the two following main factorization classes:
* - class PastixLLT : a supernodal, parallel LLt Cholesky factorization.
* - class PastixLDLT: a supernodal, parallel LDLt Cholesky factorization.
* - class PastixLU : a supernodal, parallel LU factorization (optimized for a symmetric pattern).
*
* \code
* #include <Eigen/PaStiXSupport>
* \endcode
*
* In order to use this module, the PaSTiX headers must be accessible from the include paths, and your binary must be linked to the PaSTiX library and its dependencies.
* The dependencies depend on how PaSTiX has been compiled.
* For a cmake based project, you can use our FindPaSTiX.cmake module to help you in this task.
*
*/
#include "src/misc/Solve.h"
#include "src/misc/SparseSolve.h"
#include "src/PaStiXSupport/PaStiXSupport.h"
#include "src/Core/util/ReenableStupidWarnings.h"
#endif // EIGEN_PASTIXSUPPORT_MODULE_H

30
ground/openpilotgcs/src/libs/eigen/Eigen/PardisoSupport Normal file
View File

@ -0,0 +1,30 @@
#ifndef EIGEN_PARDISOSUPPORT_MODULE_H
#define EIGEN_PARDISOSUPPORT_MODULE_H
#include "SparseCore"
#include "src/Core/util/DisableStupidWarnings.h"
#include <mkl_pardiso.h>
#include <unsupported/Eigen/SparseExtra>
/** \ingroup Support_modules
* \defgroup PardisoSupport_Module PardisoSupport module
*
* This module brings support for the Intel(R) MKL PARDISO direct sparse solvers.
*
* \code
* #include <Eigen/PardisoSupport>
* \endcode
*
* In order to use this module, the MKL headers must be accessible from the include paths, and your binary must be linked to the MKL library and its dependencies.
* See this \ref TopicUsingIntelMKL "page" for more information on MKL-Eigen integration.
*
*/
#include "src/PardisoSupport/PardisoSupport.h"
#include "src/Core/util/ReenableStupidWarnings.h"
#endif // EIGEN_PARDISOSUPPORT_MODULE_H

Some files were not shown because too many files have changed in this diff Show More