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

Merge branch 'next' into corvuscorax/OP-942_task-diagnostics-for-callbacks

Browse Source 
Conflicts:
	flight/targets/boards/simposix/firmware/Makefile
This commit is contained in:
Corvus Corax 2014-02-07 00:31:32 +01:00
commit 9a6072d58c
13 changed files with 177 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -55,7 +55,7 @@
#define STACK_SIZE_BYTES 1312
#endif
#define TASK_PRIORITY (tskIDLE_PRIORITY + 4)
#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) // device driver
#define FAILSAFE_TIMEOUT_MS 100
#define MAX_MIX_ACTUATORS ACTUATORCOMMAND_CHANNEL_NUMELEM

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

@ -72,10 +72,16 @@
#define UPDATE_RATE 25.0f
#define GYRO_NEUTRAL 1665
#define UPDATE_EXPECTED (1.0f / 666.0f)
#define UPDATE_MIN 1.0e-6f
#define UPDATE_MAX 1.0f
#define UPDATE_ALPHA 1.0e-2f
// Private types
// Private variables
static xTaskHandle taskHandle;
static PiOSDeltatimeConfig dtconfig;
// Private functions
static void AttitudeTask(void *parameters);
@ -214,6 +220,8 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
// Force settings update to make sure rotation loaded
settingsUpdatedCb(AttitudeSettingsHandle());
PIOS_DELTATIME_Init(&dtconfig, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
// Main task loop
while (1) {
FlightStatusData flightStatus;
@ -473,12 +481,7 @@ static inline void apply_accel_filter(const float *raw, float *filtered)
static void updateAttitude(AccelStateData *accelStateData, GyroStateData *gyrosData)
{
float dT;
portTickType thisSysTime = xTaskGetTickCount();
static portTickType lastSysTime = 0;
dT = (thisSysTime == lastSysTime) ? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f;
lastSysTime = thisSysTime;
float dT = PIOS_DELTATIME_GetAverageSeconds(&dtconfig);
// Bad practice to assume structure order, but saves memory
float *gyros = &gyrosData->x;

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

@ -64,7 +64,7 @@
#define STACK_SIZE_BYTES 1152
#endif
#define TASK_PRIORITY (tskIDLE_PRIORITY + 4)
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) // 3 = flight control
#define UPDATE_PERIOD_MS 20
#define THROTTLE_FAILSAFE -0.1f
#define ARMED_THRESHOLD 0.50f

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

@ -65,6 +65,11 @@
#include "relay_tuning.h"
// Private constants
#define UPDATE_EXPECTED (1.0f / 666.0f)
#define UPDATE_MIN 1.0e-6f
#define UPDATE_MAX 1.0f
#define UPDATE_ALPHA 1.0e-2f
#define MAX_QUEUE_SIZE 1
#if defined(PIOS_STABILIZATION_STACK_SIZE)
@ -73,7 +78,7 @@
#define STACK_SIZE_BYTES 840
#endif
#define TASK_PRIORITY (tskIDLE_PRIORITY + 4)
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) // FLIGHT CONTROL priority
#define FAILSAFE_TIMEOUT_MS 30
// number of flight mode switch positions
@ -194,7 +199,9 @@ MODULE_INITCALL(StabilizationInitialize, StabilizationStart);
static void stabilizationTask(__attribute__((unused)) void *parameters)
{
UAVObjEvent ev;
uint32_t timeval = PIOS_DELAY_GetRaw();
PiOSDeltatimeConfig timeval;
PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
ActuatorDesiredData actuatorDesired;
StabilizationDesiredData stabDesired;
@ -226,9 +233,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
continue;
}
dT = PIOS_DELAY_DiffuS(timeval) * 1.0e-6f;
timeval = PIOS_DELAY_GetRaw();
dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
FlightStatusGet(&flightStatus);
StabilizationDesiredGet(&stabDesired);
AttitudeStateGet(&attitudeState);

63
flight/modules/StateEstimation/filteraltitude.c
View File

@ -40,20 +40,22 @@
#define STACK_REQUIRED 128
#define DT_ALPHA 1e-3f
#define DT_ALPHA 1e-2f
#define DT_MIN 1e-6f
#define DT_MAX 1.0f
#define DT_AVERAGE 1e-3f
// Private types
struct data {
float state[4]; // state = altitude,velocity,accel_offset,accel
float pos[3]; // position updates from other filters
float vel[3]; // position updates from other filters
float dTA;
float dTA2;
int32_t lastTime;
float accelLast;
float baroLast;
int32_t baroLastTime;
bool first_run;
float state[4]; // state = altitude,velocity,accel_offset,accel
float pos[3]; // position updates from other filters
float vel[3]; // position updates from other filters
PiOSDeltatimeConfig dt1config;
PiOSDeltatimeConfig dt2config;
float accelLast;
float baroLast;
bool first_run;
AltitudeFilterSettingsData settings;
};
@ -89,8 +91,8 @@ static int32_t init(stateFilter *self)
this->vel[0] = 0.0f;
this->vel[1] = 0.0f;
this->vel[2] = 0.0f;
this->dTA = -1.0f;
this->dTA2 = -1.0f;
PIOS_DELTATIME_Init(&this->dt1config, DT_AVERAGE, DT_MIN, DT_MAX, DT_ALPHA);
PIOS_DELTATIME_Init(&this->dt2config, DT_AVERAGE, DT_MIN, DT_MAX, DT_ALPHA);
this->baroLast = 0.0f;
this->accelLast = 0.0f;
this->first_run = 1;
@ -104,12 +106,8 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
if (this->first_run) {
// Initialize to current altitude reading at initial location
if (IS_SET(state->updated, SENSORUPDATES_accel)) {
this->lastTime = PIOS_DELAY_GetRaw();
}
if (IS_SET(state->updated, SENSORUPDATES_baro)) {
this->first_run = 0;
this->baroLastTime = PIOS_DELAY_GetRaw();
this->first_run = 0;
}
} else {
// save existing position and velocity updates so GPS will still work
@ -141,22 +139,14 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
// correct velocity and position state (integration)
// low pass for average dT, compensate timing jitter from scheduler
float dT = PIOS_DELAY_DiffuS(this->lastTime) / 1.0e6f;
this->lastTime = PIOS_DELAY_GetRaw();
if (dT < 0.001f) {
dT = 0.001f;
}
if (this->dTA < 0) {
this->dTA = dT;
} else {
this->dTA = this->dTA * (1.0f - DT_ALPHA) + dT * DT_ALPHA;
}
//
float dT = PIOS_DELTATIME_GetAverageSeconds(&this->dt1config);
float speedLast = this->state[1];
this->state[1] += 0.5f * (this->accelLast + (this->state[3] - this->state[2])) * this->dTA;
this->state[1] += 0.5f * (this->accelLast + (this->state[3] - this->state[2])) * dT;
this->accelLast = this->state[3] - this->state[2];
this->state[0] += 0.5f * (speedLast + this->state[1]) * this->dTA;
this->state[0] += 0.5f * (speedLast + this->state[1]) * dT;
state->pos[0] = this->pos[0];
@ -175,17 +165,8 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
// correct the velocity state (low pass differentiation)
// low pass for average dT, compensate timing jitter from scheduler
float dT = PIOS_DELAY_DiffuS(this->baroLastTime) / 1.0e6f;
this->baroLastTime = PIOS_DELAY_GetRaw();
if (dT < 0.001f) {
dT = 0.001f;
}
if (this->dTA2 < 0) {
this->dTA2 = dT;
} else {
this->dTA2 = this->dTA2 * (1.0f - DT_ALPHA) + dT * DT_ALPHA;
}
this->state[1] = (1.0f - (this->settings.BaroKp * this->settings.BaroKp)) * this->state[1] + (this->settings.BaroKp * this->settings.BaroKp) * (state->baro[0] - this->baroLast) / this->dTA2;
float dT = PIOS_DELTATIME_GetAverageSeconds(&this->dt2config);
this->state[1] = (1.0f - (this->settings.BaroKp * this->settings.BaroKp)) * this->state[1] + (this->settings.BaroKp * this->settings.BaroKp) * (state->baro[0] - this->baroLast) / dT;
this->baroLast = state->baro[0];
state->pos[0] = this->pos[0];

34
flight/modules/StateEstimation/filterekf.c
View File

@ -45,6 +45,8 @@
#define STACK_REQUIRED 2048
#define DT_ALPHA 1e-3f
#define DT_MIN 1e-6f
#define DT_MAX 1.0f
#define DT_INIT (1.0f / 666.0f) // initialize with 666 Hz (default sensor update rate on revo)
#define IMPORT_SENSOR_IF_UPDATED(shortname, num) \
@ -66,10 +68,9 @@ struct data {
stateEstimation work;
uint32_t ins_last_time;
bool inited;
bool inited;
float dTa;
PiOSDeltatimeConfig dtconfig;
};
// Private variables
@ -154,11 +155,10 @@ static int32_t maininit(stateFilter *self)
{
struct data *this = (struct data *)self->localdata;
this->inited = false;
this->init_stage = 0;
this->work.updated = 0;
this->ins_last_time = PIOS_DELAY_GetRaw();
this->dTa = DT_INIT;
this->inited = false;
this->init_stage = 0;
this->work.updated = 0;
PIOS_DELTATIME_Init(&this->dtconfig, DT_INIT, DT_MIN, DT_MAX, DT_ALPHA);
EKFConfigurationGet(&this->ekfConfiguration);
int t;
@ -224,17 +224,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
return 0;
}
dT = PIOS_DELAY_DiffuS(this->ins_last_time) / 1.0e6f;
this->ins_last_time = PIOS_DELAY_GetRaw();
// This should only happen at start up or at mode switches
if (dT > 0.01f) {
dT = 0.01f;
} else if (dT <= 0.001f) {
dT = 0.001f;
}
this->dTa = this->dTa * (1.0f - DT_ALPHA) + dT * DT_ALPHA; // low pass for average dT, compensate timing jitter from scheduler
dT = PIOS_DELTATIME_GetAverageSeconds(&this->dtconfig);
if (!this->inited && IS_SET(this->work.updated, SENSORUPDATES_mag) && IS_SET(this->work.updated, SENSORUPDATES_baro) && IS_SET(this->work.updated, SENSORUPDATES_pos)) {
// Don't initialize until all sensors are read
@ -300,7 +290,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
// Run prediction a bit before any corrections
float gyros[3] = { DEG2RAD(this->work.gyro[0]), DEG2RAD(this->work.gyro[1]), DEG2RAD(this->work.gyro[2]) };
INSStatePrediction(gyros, this->work.accel, this->dTa);
INSStatePrediction(gyros, this->work.accel, dT);
// Copy the attitude into the state
// NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true
@ -335,7 +325,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
float gyros[3] = { DEG2RAD(this->work.gyro[0]), DEG2RAD(this->work.gyro[1]), DEG2RAD(this->work.gyro[2]) };
// Advance the state estimate
INSStatePrediction(gyros, this->work.accel, this->dTa);
INSStatePrediction(gyros, this->work.accel, dT);
// Copy the attitude into the state
// NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true
@ -355,7 +345,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
state->updated |= SENSORUPDATES_attitude | SENSORUPDATES_pos | SENSORUPDATES_vel;
// Advance the covariance estimate
INSCovariancePrediction(this->dTa);
INSCovariancePrediction(dT);
if (IS_SET(this->work.updated, SENSORUPDATES_mag)) {
sensors |= MAG_SENSORS;

64
flight/pios/common/pios_deltatime.c Normal file
View File

@ -0,0 +1,64 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_DELTATIME time measurement Functions
* @brief PiOS Delay functionality
* @{
*
* @file pios_deltatime.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Settings functions header
* @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>
void PIOS_DELTATIME_Init(PiOSDeltatimeConfig *config, float average, float min, float max, float alpha)
{
PIOS_Assert(config);
config->average = average;
config->min = min;
config->max = max;
config->alpha = alpha;
config->last = PIOS_DELAY_GetRaw();
};
float PIOS_DELTATIME_GetAverageSeconds(PiOSDeltatimeConfig *config)
{
PIOS_Assert(config);
float dT = PIOS_DELAY_DiffuS(config->last) * 1.0e-6f;
config->last = PIOS_DELAY_GetRaw();
if (dT < config->min) {
dT = config->min;
}
if (dT > config->max) {
dT = config->max;
}
config->average = config->average * (1.0f - config->alpha) + dT * config->alpha;
return config->average;
}
/**
* @}
* @}
*/

2
flight/pios/common/pios_rfm22b.c
View File

@ -64,7 +64,7 @@
/* Local Defines */
#define STACK_SIZE_BYTES 200
#define TASK_PRIORITY (tskIDLE_PRIORITY + 2)
#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) // flight control relevant device driver (ppm link)
#define ISR_TIMEOUT 1 // ms
#define EVENT_QUEUE_SIZE 5
#define RFM22B_DEFAULT_RX_DATARATE RFM22_datarate_9600

53
flight/pios/inc/pios_deltatime.h Normal file
View File

@ -0,0 +1,53 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_DELTATIME time measurement Functions
* @brief PiOS Delay functionality
* @{
*
* @file pios_deltatime.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Settings functions header
* @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_DELTATIME_H
#define PIOS_DELTATIME_H
struct PiOSDeltatimeConfigStruct {
uint32_t last;
float average;
float min;
float max;
float alpha;
};
typedef struct PiOSDeltatimeConfigStruct PiOSDeltatimeConfig;
/* Public Functions */
void PIOS_DELTATIME_Init(PiOSDeltatimeConfig *config, float average, float min, float max, float alpha);
float PIOS_DELTATIME_GetAverageSeconds(PiOSDeltatimeConfig *config);
#endif /* PIOS_DELTATIME_H */
/**
* @}
* @}
*/

1
flight/pios/pios.h
View File

@ -110,6 +110,7 @@
/* PIOS system functions */
#ifdef PIOS_INCLUDE_DELAY
#include <pios_delay.h>
#include <pios_deltatime.h>
#endif
#ifdef PIOS_INCLUDE_INITCALL

1
flight/pios/pios_sim_posix.h
View File

@ -87,6 +87,7 @@ extern void PIOS_LED_Init(void);
#include <pios_wdg.h>
#include <pios_debug.h>
#include <pios_debuglog.h>
#include <pios_deltatime.h>
#include <pios_crc.h>
#include <pios_rcvr.h>
#include <pios_flash.h>

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

@ -101,6 +101,7 @@ SRC += $(PIOSCORECOMMON)/pios_task_monitor.c
SRC += $(PIOSCORECOMMON)/pios_dosfs_logfs.c
SRC += $(PIOSCORECOMMON)/pios_debuglog.c
SRC += $(PIOSCORECOMMON)/pios_callbackscheduler.c
SRC += $(PIOSCORECOMMON)/pios_deltatime.c
## PIOS Hardware
include $(PIOS)/posix/library.mk

1
make/apps-defs.mk
View File

@ -82,6 +82,7 @@ SRC += $(PIOSCOMMON)/pios_crc.c
SRC += $(PIOSCOMMON)/pios_flashfs_logfs.c
SRC += $(PIOSCOMMON)/pios_flash_jedec.c
SRC += $(PIOSCOMMON)/pios_debuglog.c
SRC += $(PIOSCOMMON)/pios_deltatime.c
SRC += $(PIOSCOMMON)/pios_rcvr.c
SRC += $(PIOSCOMMON)/pios_rfm22b.c
SRC += $(PIOSCOMMON)/pios_rfm22b_com.c