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

Merge branch 'next' into LP-194_Hidde_Attitude_Yaw

Browse Source
This commit is contained in:
Laurent Lalanne 2016-01-10 13:08:29 +01:00
commit f8e651fc3f
15 changed files with 97 additions and 101 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
flight/libraries/insgps13state.c
View File

@ -280,11 +280,11 @@ void INSSetBaroVar(float baro_var)
void INSSetMagNorth(float B[3])
{
float mag = sqrtf(B[0] * B[0] + B[1] * B[1] + B[2] * B[2]);
float invmag = invsqrtf(B[0] * B[0] + B[1] * B[1] + B[2] * B[2]);
ekf.Be[0] = B[0] / mag;
ekf.Be[1] = B[1] / mag;
ekf.Be[2] = B[2] / mag;
ekf.Be[0] = B[0] * invmag;
ekf.Be[1] = B[1] * invmag;
ekf.Be[2] = B[2] * invmag;
}
void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
@ -305,7 +305,7 @@ void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
// EKF prediction step
LinearizeFG(ekf.X, U, ekf.F, ekf.G);
RungeKutta(ekf.X, U, dT);
invqmag = fast_invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]);
invqmag = invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]);
ekf.X[6] *= invqmag;
ekf.X[7] *= invqmag;
ekf.X[8] *= invqmag;
@ -390,7 +390,7 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
if (SensorsUsed & MAG_SENSORS) {
float invBmag = fast_invsqrtf(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] + mag_data[2] * mag_data[2]);
float invBmag = invsqrtf(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] + mag_data[2] * mag_data[2]);
Z[6] = mag_data[0] * invBmag;
Z[7] = mag_data[1] * invBmag;
Z[8] = mag_data[2] * invBmag;
@ -404,7 +404,7 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
MeasurementEq(ekf.X, ekf.Be, Y);
SerialUpdate(ekf.H, ekf.R, Z, Y, ekf.P, ekf.X, SensorsUsed);
float invqmag = fast_invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]);
float invqmag = invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]);
ekf.X[6] *= invqmag;
ekf.X[7] *= invqmag;
ekf.X[8] *= invqmag;

22
flight/libraries/math/mathmisc.h
View File

@ -119,28 +119,12 @@ static inline float y_on_curve(float x, const pointf points[], int num_points)
// Find the y value on the selected line.
return y_on_line(x, &points[end_point - 1], &points[end_point]);
}
// Fast inverse square root implementation from "quake3-1.32b/code/game/q_math.c"
// http://en.wikipedia.org/wiki/Fast_inverse_square_root
static inline float fast_invsqrtf(float number)
static inline float invsqrtf(float number)
{
float x2, y;
const float threehalfs = 1.5F;
union {
float f;
uint32_t u;
} i;
x2 = number * 0.5F;
y = number;
i.f = y; // evil floating point bit level hacking
i.u = 0x5f3759df - (i.u >> 1); // what the fxck?
y = i.f;
y = y * (threehalfs - (x2 * y * y)); // 1st iteration
// y = y * ( threehalfs - ( x2 * y * y ) ); // 2nd iteration, this can be removed
float y;
y = 1.0f / sqrtf(number);
return y;
}

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

@ -628,7 +628,7 @@ __attribute__((optimize("O3"))) static void updateAttitude(AccelStateData *accel
CrossProduct((const float *)accels_filtered, (const float *)grot_filtered, accel_err);
// Account for accel magnitude
float inv_accel_mag = fast_invsqrtf(accels_filtered[0] * accels_filtered[0] + accels_filtered[1] * accels_filtered[1] + accels_filtered[2] * accels_filtered[2]);
float inv_accel_mag = invsqrtf(accels_filtered[0] * accels_filtered[0] + accels_filtered[1] * accels_filtered[1] + accels_filtered[2] * accels_filtered[2]);
if (inv_accel_mag > 1e3f) {
return;
}
@ -637,7 +637,7 @@ __attribute__((optimize("O3"))) static void updateAttitude(AccelStateData *accel
float inv_grot_mag;
if (accel_filter_enabled) {
inv_grot_mag = fast_invsqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]);
inv_grot_mag = invsqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]);
} else {
inv_grot_mag = 1.0f;
}
@ -685,8 +685,8 @@ __attribute__((optimize("O3"))) static void updateAttitude(AccelStateData *accel
}
}
// Renomalize
float inv_qmag = fast_invsqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
// Renormalize
float inv_qmag = invsqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
// If quaternion has become inappropriately short or is nan reinit.
// THIS SHOULD NEVER ACTUALLY HAPPEN

2
flight/modules/Receiver/receiver.c
View File

@ -99,7 +99,7 @@ static uint8_t isAssistedFlightMode(uint8_t position);
static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsResponseTimeData *responseTime, uint8_t deadband, float dT);
#endif
#define RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP 18 // Sbus max channel
#define RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP 18 // Sbus max channel
#define RCVR_ACTIVITY_MONITOR_MIN_RANGE 15
struct rcvr_activity_fsm {
ManualControlSettingsChannelGroupsOptions group;

14
flight/modules/StateEstimation/filtercf.c
View File

@ -438,16 +438,16 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float
attitude[3] = -attitude[3];
}
// Renomalize
float qmag = sqrtf(attitude[0] * attitude[0] + attitude[1] * attitude[1] + attitude[2] * attitude[2] + attitude[3] * attitude[3]);
attitude[0] = attitude[0] / qmag;
attitude[1] = attitude[1] / qmag;
attitude[2] = attitude[2] / qmag;
attitude[3] = attitude[3] / qmag;
// Renormalize
float inv_qmag = invsqrtf(attitude[0] * attitude[0] + attitude[1] * attitude[1] + attitude[2] * attitude[2] + attitude[3] * attitude[3]);
attitude[0] = attitude[0] * inv_qmag;
attitude[1] = attitude[1] * inv_qmag;
attitude[2] = attitude[2] * inv_qmag;
attitude[3] = attitude[3] * inv_qmag;
// If quaternion has become inappropriately short or is nan reinit.
// THIS SHOULD NEVER ACTUALLY HAPPEN
if ((fabsf(qmag) < 1.0e-3f) || isnan(qmag)) {
if ((fabsf(inv_qmag) > 1e3f) || isnan(inv_qmag)) {
this->first_run = 1;
return FILTERRESULT_WARNING;
}

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

@ -114,8 +114,8 @@ typedef struct {
#endif /* PIOS_TELEM_PRIORITY_QUEUE */
// Transmit/receive task handles
xTaskHandle txTaskHandle;
xTaskHandle rxTaskHandle;
xTaskHandle txTaskHandle;
xTaskHandle rxTaskHandle;
// Telemetry stream
UAVTalkConnection uavTalkCon;
} channelContext;
@ -170,7 +170,6 @@ static void gcsTelemetryStatsUpdated();
*/
int32_t TelemetryStart(void)
{
#ifdef HAS_RADIO
// Only start the local telemetry tasks if needed
if (localPort()) {
@ -623,7 +622,6 @@ static void telemetryTxTask(void *parameters)
processObjEvent(channel, &ev);
}
#endif /* PIOS_TELEM_PRIORITY_QUEUE */
}
}

5
flight/pios/stm32f10x/pios_ppm.c
View File

@ -341,6 +341,11 @@ static void PIOS_PPM_tim_edge_cb(__attribute__((unused)) uint32_t tim_id,
}
}
#endif /* USE_FREERTOS */
} else {
for (uint32_t i = 0;
i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) {
ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT;
}
}
ppm_dev->Fresh = TRUE;

8
flight/pios/stm32f4xx/pios_bl_helper.c
View File

@ -238,13 +238,13 @@ static bool erase_flash(uint32_t startAddress, uint32_t endAddress)
PIOS_Assert(0);
}
for (int retry = 0; retry < MAX_DEL_RETRYS; ++retry) {
//if erasing area contain whole bank2 area, using bank erase
//bank2: sector 12 to sector 23
// if erasing area contain whole bank2 area, using bank erase
// bank2: sector 12 to sector 23
if (sector_start == flash_sectors[12].start &&
endAddress >= (flash_sectors[23].start + flash_sectors[23].size)){
endAddress >= (flash_sectors[23].start + flash_sectors[23].size)) {
if (FLASH_EraseAllBank2Sectors(VoltageRange_3) == FLASH_COMPLETE) {
fail = false;
//using bank2 total size substitute sector_size
// using bank2 total size substitute sector_size
sector_size = flash_sectors[23].start - flash_sectors[12].start + flash_sectors[23].size;
break;
} else {

5
flight/pios/stm32f4xx/pios_ppm.c
View File

@ -330,6 +330,11 @@ static void PIOS_PPM_tim_edge_cb(__attribute__((unused)) uint32_t tim_id, uint32
}
}
#endif /* USE_FREERTOS */
} else {
for (uint32_t i = 0;
i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) {
ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT;
}
}
ppm_dev->Fresh = true;

6
flight/pios/stm32f4xx/pios_sys.c
View File

@ -74,7 +74,7 @@ void PIOS_SYS_Init(void)
RCC_AHB1Periph_GPIOC |
RCC_AHB1Periph_GPIOD |
RCC_AHB1Periph_GPIOE |
#if defined (STM32F40_41xxx) || defined (STM32F427_437xx) || defined (STM32F429_439xx)
#if defined(STM32F40_41xxx) || defined(STM32F427_437xx) || defined(STM32F429_439xx)
RCC_AHB1Periph_GPIOF |
RCC_AHB1Periph_GPIOG |
RCC_AHB1Periph_GPIOH |
@ -85,7 +85,7 @@ void PIOS_SYS_Init(void)
RCC_AHB1Periph_SRAM1 |
RCC_AHB1Periph_SRAM2 |
RCC_AHB1Periph_BKPSRAM |
#if defined (STM32F427_437xx) || defined (STM32F429_439xx)
#if defined(STM32F427_437xx) || defined(STM32F429_439xx)
RCC_AHB1Periph_SRAM3 |
#endif
RCC_AHB1Periph_DMA1 |
@ -174,7 +174,7 @@ void PIOS_SYS_Init(void)
GPIO_Init(GPIOD, &GPIO_InitStructure);
GPIO_Init(GPIOE, &GPIO_InitStructure);
#if defined (STM32F40_41xxx) || defined (STM32F427_437xx) || defined (STM32F429_439xx)
#if defined(STM32F40_41xxx) || defined(STM32F427_437xx) || defined(STM32F429_439xx)
GPIO_Init(GPIOF, &GPIO_InitStructure);
GPIO_Init(GPIOG, &GPIO_InitStructure);
GPIO_Init(GPIOH, &GPIO_InitStructure);

2
ground/gcs/src/libs/osgearth/copydata.pro
View File

@ -36,7 +36,7 @@ equals(copyosg, 1) {
win32 {
# set debug suffix if needed
CONFIG(debug, debug|release):DS = "d"
#CONFIG(debug, debug|release):DS = "d"
# copy osg libraries
OSG_LIBS = \

16
ground/gcs/src/libs/osgearth/osgearth.pro
View File

@ -91,18 +91,18 @@ macx {
win32 {
LIBS += -L$$OSG_SDK_DIR/lib
CONFIG(release, debug|release) {
#CONFIG(release, debug|release) {
LIBS += -lOpenThreads
LIBS += -losg -losgUtil -losgDB -losgGA -losgViewer -losgText
LIBS += -losgEarth -losgEarthUtil -losgEarthFeatures -losgEarthSymbology -losgEarthAnnotation
LIBS += -losgQt -losgEarthQt
}
CONFIG(debug, debug|release) {
LIBS += -lOpenThreadsd
LIBS += -losgd -losgUtild -losgDBd -losgGAd -losgViewerd -losgTextd
LIBS += -losgEarthd -losgEarthUtild -losgEarthFeaturesd -losgEarthSymbologyd -losgEarthAnnotationd
LIBS += -losgQtd -losgEarthQtd
}
#}
#CONFIG(debug, debug|release) {
# LIBS += -lOpenThreadsd
# LIBS += -losgd -losgUtild -losgDBd -losgGAd -losgViewerd -losgTextd
# LIBS += -losgEarthd -losgEarthUtild -losgEarthFeaturesd -losgEarthSymbologyd -losgEarthAnnotationd
# LIBS += -losgQtd -losgEarthQtd
#}
}
include(copydata.pro)

6
ground/gcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp
View File

@ -230,9 +230,9 @@ void ConfigMultiRotorWidget::setupUI(QString frameType)
setComboCurrentIndex(m_aircraft->multirotorFrameType,
m_aircraft->multirotorFrameType->findText("Hexacopter Y6"));
m_aircraft->mrRollMixLevel->setValue(100);
m_aircraft->mrPitchMixLevel->setValue(50);
setYawMixLevel(66);
m_aircraft->mrRollMixLevel->setValue(86);
m_aircraft->mrPitchMixLevel->setValue(100);
setYawMixLevel(100);
} else if (frameType == "Octo" || frameType == "Octocopter") {
setComboCurrentIndex(m_aircraft->multirotorFrameType, m_aircraft->multirotorFrameType->findText("Octocopter"));

54
ground/gcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp
View File

@ -770,30 +770,30 @@ void VehicleConfigurationHelper::applyMixerConfiguration(mixerChannelSettings ch
case VehicleConfigurationSource::MULTI_ROTOR_HEXA:
case VehicleConfigurationSource::MULTI_ROTOR_HEXA_H:
case VehicleConfigurationSource::MULTI_ROTOR_HEXA_X:
mSettings->setMixerValueRoll((qint8) 100);
mSettings->setMixerValuePitch((qint8) 100);
mSettings->setMixerValueYaw((qint8) 100);
mSettings->setMixerValueRoll((qint8)100);
mSettings->setMixerValuePitch((qint8)100);
mSettings->setMixerValueYaw((qint8)100);
break;
case VehicleConfigurationSource::MULTI_ROTOR_QUAD_X:
mSettings->setMixerValueRoll((qint8) 50);
mSettings->setMixerValuePitch((qint8) 50);
mSettings->setMixerValueYaw((qint8) 50);
mSettings->setMixerValueRoll((qint8)50);
mSettings->setMixerValuePitch((qint8)50);
mSettings->setMixerValueYaw((qint8)50);
break;
case VehicleConfigurationSource::MULTI_ROTOR_QUAD_PLUS:
mSettings->setMixerValueRoll((qint8) 100);
mSettings->setMixerValuePitch((qint8) 100);
mSettings->setMixerValueYaw((qint8) 50);
mSettings->setMixerValueRoll((qint8)100);
mSettings->setMixerValuePitch((qint8)100);
mSettings->setMixerValueYaw((qint8)50);
break;
case VehicleConfigurationSource::MULTI_ROTOR_HEXA_COAX_Y:
mSettings->setMixerValueRoll((qint8) 100);
mSettings->setMixerValuePitch((qint8) 50);
mSettings->setMixerValueYaw((qint8) 66);
mSettings->setMixerValueRoll((qint8)86);
mSettings->setMixerValuePitch((qint8)100);
mSettings->setMixerValueYaw((qint8)100);
break;
case VehicleConfigurationSource::MULTI_ROTOR_OCTO:
case VehicleConfigurationSource::MULTI_ROTOR_OCTO_X:
mSettings->setMixerValueRoll((qint8) 100);
mSettings->setMixerValuePitch((qint8) 100);
mSettings->setMixerValueYaw((qint8) 100);
mSettings->setMixerValueRoll((qint8)100);
mSettings->setMixerValuePitch((qint8)100);
mSettings->setMixerValueYaw((qint8)100);
break;
case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_X:
case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_PLUS:
@ -805,9 +805,9 @@ void VehicleConfigurationHelper::applyMixerConfiguration(mixerChannelSettings ch
break;
}
case VehicleConfigurationSource::VEHICLE_FIXEDWING:
mSettings->setMixerValueRoll((qint8) 100);
mSettings->setMixerValuePitch((qint8) 100);
mSettings->setMixerValueYaw((qint8) 100);
mSettings->setMixerValueRoll((qint8)100);
mSettings->setMixerValuePitch((qint8)100);
mSettings->setMixerValueYaw((qint8)100);
maxThrottle = 1;
break;
case VehicleConfigurationSource::VEHICLE_HELI:
@ -816,22 +816,22 @@ void VehicleConfigurationHelper::applyMixerConfiguration(mixerChannelSettings ch
{
switch (m_configSource->getVehicleSubType()) {
case VehicleConfigurationSource::GROUNDVEHICLE_MOTORCYCLE:
mSettings->setMixerValueRoll((qint8) 100);
mSettings->setMixerValuePitch((qint8) 100);
mSettings->setMixerValueYaw((qint8) 100);
mSettings->setMixerValueRoll((qint8)100);
mSettings->setMixerValuePitch((qint8)100);
mSettings->setMixerValueYaw((qint8)100);
maxThrottle = 1;
break;
case VehicleConfigurationSource::GROUNDVEHICLE_CAR:
mSettings->setMixerValueRoll((qint8) 100);
mSettings->setMixerValuePitch((qint8) 100);
mSettings->setMixerValueYaw((qint8) 100);
mSettings->setMixerValueRoll((qint8)100);
mSettings->setMixerValuePitch((qint8)100);
mSettings->setMixerValueYaw((qint8)100);
maxThrottle = 1;
minThrottle = 0;
break;
case VehicleConfigurationSource::GROUNDVEHICLE_DIFFERENTIAL:
mSettings->setMixerValueRoll((qint8) 100);
mSettings->setMixerValuePitch((qint8) 100);
mSettings->setMixerValueYaw((qint8) 100);
mSettings->setMixerValueRoll((qint8)100);
mSettings->setMixerValuePitch((qint8)100);
mSettings->setMixerValueYaw((qint8)100);
maxThrottle = 0.8;
minThrottle = 0;
break;

30
ground/uavobjgenerator/generators/gcs/uavobjectgeneratorgcs.cpp
View File

@ -496,12 +496,18 @@ void generateIndexedProperty(Context &ctxt, FieldContext &fieldCtxt)
for (int elementIndex = 0; elementIndex < fieldCtxt.field->numElements; elementIndex++) {
QString elementName = fieldCtxt.field->elementNames[elementIndex];
QString sep;
if (fieldCtxt.propName.right(1)[0].isDigit() && elementName[0].isDigit()) {
info(ctxt.object, "Property \"" + fieldCtxt.propName + "\" and element \"" + elementName + "\" have digit conflict, consider fixing it.");
sep = "_";
}
FieldContext elementCtxt;
elementCtxt.field = fieldCtxt.field;
elementCtxt.fieldName = fieldCtxt.fieldName + "_" + elementName;
elementCtxt.fieldType = fieldCtxt.fieldType;
elementCtxt.propName = fieldCtxt.propName + elementName;
elementCtxt.ucPropName = fieldCtxt.ucPropName + elementName;
elementCtxt.propName = fieldCtxt.propName + sep + elementName;
elementCtxt.ucPropName = fieldCtxt.ucPropName + sep + elementName;
elementCtxt.propType = fieldCtxt.propType;
elementCtxt.propRefType = fieldCtxt.propRefType;
// deprecation
@ -621,24 +627,22 @@ bool UAVObjectGeneratorGCS::process_object(ObjectInfo *object)
// field context
FieldContext fieldCtxt;
fieldCtxt.field = field;
fieldCtxt.field = field;
// field properties
fieldCtxt.fieldName = field->name;
fieldCtxt.fieldType = fieldTypeStrCPP(field->type);
fieldCtxt.fieldName = field->name;
fieldCtxt.fieldType = fieldTypeStrCPP(field->type);
fieldCtxt.ucPropName = toPropertyName(field->name);
fieldCtxt.propName = toLowerCamelCase(fieldCtxt.ucPropName);
fieldCtxt.propType = fieldCtxt.fieldType;
fieldCtxt.ucPropName = toPropertyName(field->name);
fieldCtxt.propName = toLowerCamelCase(fieldCtxt.ucPropName);
fieldCtxt.propType = fieldCtxt.fieldType;
if (field->type == FIELDTYPE_INT8) {
fieldCtxt.propType = fieldTypeStrCPP(FIELDTYPE_INT16);
}
else if (field->type == FIELDTYPE_UINT8) {
} else if (field->type == FIELDTYPE_UINT8) {
fieldCtxt.propType = fieldTypeStrCPP(FIELDTYPE_UINT16);
}
else if (field->type == FIELDTYPE_ENUM) {
} else if (field->type == FIELDTYPE_ENUM) {
QString enumClassName = object->name + "_" + fieldCtxt.ucPropName;
fieldCtxt.propType = enumClassName + "::Enum";
fieldCtxt.propType = enumClassName + "::Enum";
}
// reference type
fieldCtxt.propRefType = fieldCtxt.propType;