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 brian/code_cleanup_130514

Browse Source
This commit is contained in:
Richard Flay (Hyper) 2013-05-17 05:49:38 +09:30
commit 031ea3e069
26 changed files with 583 additions and 445 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

270
flight/libraries/insgps13state.c
View File

@ -49,7 +49,7 @@
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
float Q[NUMW], float dT, float P[NUMX][NUMX]);
void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
float Y[NUMV], float P[NUMX][NUMX], float X[NUMX], float K[NUMX][NUMV],
uint16_t SensorsUsed);
void RungeKutta(float X[NUMX], float U[NUMU], float dT);
void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]);
@ -59,12 +59,22 @@ void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]);
void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]);
// Private variables
float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices
// global to init to zero and maintain zero elements
float Be[3]; // local magnetic unit vector in NED frame
float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector
float Q[NUMW], R[NUMV]; // input noise and measurement noise variances
float K[NUMX][NUMV]; // feedback gain matrix
struct EKFData {
// linearized system matrices
float F[NUMX][NUMX];
float G[NUMX][NUMW];
float H[NUMV][NUMX];
// local magnetic unit vector in NED frame
float Be[3];
// covariance matrix and state vector
float P[NUMX][NUMX];
float X[NUMX];
// input noise and measurement noise variances
float Q[NUMW];
float R[NUMV];
float K[NUMX][NUMV]; // feedback gain matrix
} ekf;
// Global variables
struct NavStruct Nav;
@ -79,52 +89,52 @@ uint16_t ins_get_num_states()
void INSGPSInit() //pretty much just a place holder for now
{
Be[0] = 1.0f;
Be[1] = 0.0f;
Be[2] = 0.0f; // local magnetic unit vector
ekf.Be[0] = 1.0f;
ekf.Be[1] = 0.0f;
ekf.Be[2] = 0.0f; // local magnetic unit vector
for (int i = 0; i < NUMX; i++) {
for (int j = 0; j < NUMX; j++) {
P[i][j] = 0.0f; // zero all terms
F[i][j] = 0.0f;
ekf.P[i][j] = 0.0f; // zero all terms
ekf.F[i][j] = 0.0f;
}
for (int j = 0; j < NUMW; j++)
G[i][j] = 0.0f;
ekf.G[i][j] = 0.0f;
for (int j = 0; j < NUMV; j++) {
H[j][i] = 0.0f;
K[i][j] = 0.0f;
ekf.H[j][i] = 0.0f;
ekf.K[i][j] = 0.0f;
}
X[i] = 0.0f;
ekf.X[i] = 0.0f;
}
for (int i = 0; i < NUMW; i++)
Q[i] = 0.0f;
ekf.Q[i] = 0.0f;
for (int i = 0; i < NUMV; i++)
R[i] = 0.0f;
ekf.R[i] = 0.0f;
P[0][0] = P[1][1] = P[2][2] = 25.0f; // initial position variance (m^2)
P[3][3] = P[4][4] = P[5][5] = 5.0f; // initial velocity variance (m/s)^2
P[6][6] = P[7][7] = P[8][8] = P[9][9] = 1e-5f; // initial quaternion variance
P[10][10] = P[11][11] = P[12][12] = 1e-9f; // initial gyro bias variance (rad/s)^2
ekf.P[0][0] = ekf.P[1][1] = ekf.P[2][2] = 25.0f; // initial position variance (m^2)
ekf.P[3][3] = ekf.P[4][4] = ekf.P[5][5] = 5.0f; // initial velocity variance (m/s)^2
ekf.P[6][6] = ekf.P[7][7] = ekf.P[8][8] = ekf.P[9][9] = 1e-5f; // initial quaternion variance
ekf.P[10][10] = ekf.P[11][11] = ekf.P[12][12] = 1e-9f; // initial gyro bias variance (rad/s)^2
X[0] = X[1] = X[2] = X[3] = X[4] = X[5] = 0.0f; // initial pos and vel (m)
X[6] = 1.0f;
X[7] = X[8] = X[9] = 0.0f; // initial quaternion (level and North) (m/s)
X[10] = X[11] = X[12] = 0.0f; // initial gyro bias (rad/s)
ekf.X[0] = ekf.X[1] = ekf.X[2] = ekf.X[3] = ekf.X[4] = ekf.X[5] = 0.0f; // initial pos and vel (m)
ekf.X[6] = 1.0f;
ekf.X[7] = ekf.X[8] = ekf.X[9] = 0.0f; // initial quaternion (level and North) (m/s)
ekf.X[10] = ekf.X[11] = ekf.X[12] = 0.0f; // initial gyro bias (rad/s)
Q[0] = Q[1] = Q[2] = 50e-4f; // gyro noise variance (rad/s)^2
Q[3] = Q[4] = Q[5] = 0.00001f; // accelerometer noise variance (m/s^2)^2
Q[6] = Q[7] = Q[8] = 2e-8f; // gyro bias random walk variance (rad/s^2)^2
ekf.Q[0] = ekf.Q[1] = ekf.Q[2] = 50e-4f; // gyro noise variance (rad/s)^2
ekf.Q[3] = ekf.Q[4] = ekf.Q[5] = 0.00001f; // accelerometer noise variance (m/s^2)^2
ekf.Q[6] = ekf.Q[7] = ekf.Q[8] = 2e-8f; // gyro bias random walk variance (rad/s^2)^2
R[0] = R[1] = 0.004f; // High freq GPS horizontal position noise variance (m^2)
R[2] = 0.036f; // High freq GPS vertical position noise variance (m^2)
R[3] = R[4] = 0.004f; // High freq GPS horizontal velocity noise variance (m/s)^2
R[5] = 100.0f; // High freq GPS vertical velocity noise variance (m/s)^2
R[6] = R[7] = R[8] = 0.005f; // magnetometer unit vector noise variance
R[9] = .25f; // High freq altimeter noise variance (m^2)
ekf.R[0] = ekf.R[1] = 0.004f; // High freq GPS horizontal position noise variance (m^2)
ekf.R[2] = 0.036f; // High freq GPS vertical position noise variance (m^2)
ekf.R[3] = ekf.R[4] = 0.004f; // High freq GPS horizontal velocity noise variance (m/s)^2
ekf.R[5] = 100.0f; // High freq GPS vertical velocity noise variance (m/s)^2
ekf.R[6] = ekf.R[7] = ekf.R[8] = 0.005f; // magnetometer unit vector noise variance
ekf.R[9] = .25f; // High freq altimeter noise variance (m^2)
}
void INSResetP(float PDiag[NUMX])
@ -135,8 +145,8 @@ void INSResetP(float PDiag[NUMX])
for (i=0;i<NUMX;i++){
if (PDiag != 0){
for (j=0;j<NUMX;j++)
P[i][j]=P[j][i]=0.0f;
P[i][i]=PDiag[i];
ekf.P[i][j]=ekf.P[j][i]=0.0f;
ekf.P[i][i]=PDiag[i];
}
}
}
@ -148,7 +158,7 @@ void INSGetP(float PDiag[NUMX])
// retrieve diagonal elements (aka state variance)
for (i=0;i<NUMX;i++){
if (PDiag != 0){
PDiag[i] = P[i][i];
PDiag[i] = ekf.P[i][i];
}
}
}
@ -156,97 +166,97 @@ void INSGetP(float PDiag[NUMX])
void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], __attribute__((unused)) float accel_bias[3])
{
/* Note: accel_bias not used in 13 state INS */
X[0] = pos[0];
X[1] = pos[1];
X[2] = pos[2];
X[3] = vel[0];
X[4] = vel[1];
X[5] = vel[2];
X[6] = q[0];
X[7] = q[1];
X[8] = q[2];
X[9] = q[3];
X[10] = gyro_bias[0];
X[11] = gyro_bias[1];
X[12] = gyro_bias[2];
ekf.X[0] = pos[0];
ekf.X[1] = pos[1];
ekf.X[2] = pos[2];
ekf.X[3] = vel[0];
ekf.X[4] = vel[1];
ekf.X[5] = vel[2];
ekf.X[6] = q[0];
ekf.X[7] = q[1];
ekf.X[8] = q[2];
ekf.X[9] = q[3];
ekf.X[10] = gyro_bias[0];
ekf.X[11] = gyro_bias[1];
ekf.X[12] = gyro_bias[2];
}
void INSPosVelReset(float pos[3], float vel[3])
{
for (int i = 0; i < 6; i++) {
for(int j = i; j < NUMX; j++) {
P[i][j] = 0; // zero the first 6 rows and columns
P[j][i] = 0;
ekf.P[i][j] = 0; // zero the first 6 rows and columns
ekf.P[j][i] = 0;
}
}
P[0][0] = P[1][1] = P[2][2] = 25; // initial position variance (m^2)
P[3][3] = P[4][4] = P[5][5] = 5; // initial velocity variance (m/s)^2
ekf.P[0][0] = ekf.P[1][1] = ekf.P[2][2] = 25; // initial position variance (m^2)
ekf.P[3][3] = ekf.P[4][4] = ekf.P[5][5] = 5; // initial velocity variance (m/s)^2
X[0] = pos[0];
X[1] = pos[1];
X[2] = pos[2];
X[3] = vel[0];
X[4] = vel[1];
X[5] = vel[2];
ekf.X[0] = pos[0];
ekf.X[1] = pos[1];
ekf.X[2] = pos[2];
ekf.X[3] = vel[0];
ekf.X[4] = vel[1];
ekf.X[5] = vel[2];
}
void INSSetPosVelVar(float PosVar[3], float VelVar[3])
{
R[0] = PosVar[0];
R[1] = PosVar[1];
R[2] = PosVar[2];
R[3] = VelVar[0];
R[4] = VelVar[1];
R[5] = VelVar[2];
ekf.R[0] = PosVar[0];
ekf.R[1] = PosVar[1];
ekf.R[2] = PosVar[2];
ekf.R[3] = VelVar[0];
ekf.R[4] = VelVar[1];
ekf.R[5] = VelVar[2];
}
void INSSetGyroBias(float gyro_bias[3])
{
X[10] = gyro_bias[0];
X[11] = gyro_bias[1];
X[12] = gyro_bias[2];
ekf.X[10] = gyro_bias[0];
ekf.X[11] = gyro_bias[1];
ekf.X[12] = gyro_bias[2];
}
void INSSetAccelVar(float accel_var[3])
{
Q[3] = accel_var[0];
Q[4] = accel_var[1];
Q[5] = accel_var[2];
ekf.Q[3] = accel_var[0];
ekf.Q[4] = accel_var[1];
ekf.Q[5] = accel_var[2];
}
void INSSetGyroVar(float gyro_var[3])
{
Q[0] = gyro_var[0];
Q[1] = gyro_var[1];
Q[2] = gyro_var[2];
ekf.Q[0] = gyro_var[0];
ekf.Q[1] = gyro_var[1];
ekf.Q[2] = gyro_var[2];
}
void INSSetGyroBiasVar(float gyro_bias_var[3])
{
Q[6] = gyro_bias_var[0];
Q[7] = gyro_bias_var[1];
Q[8] = gyro_bias_var[2];
ekf.Q[6] = gyro_bias_var[0];
ekf.Q[7] = gyro_bias_var[1];
ekf.Q[8] = gyro_bias_var[2];
}
void INSSetMagVar(float scaled_mag_var[3])
{
R[6] = scaled_mag_var[0];
R[7] = scaled_mag_var[1];
R[8] = scaled_mag_var[2];
ekf.R[6] = scaled_mag_var[0];
ekf.R[7] = scaled_mag_var[1];
ekf.R[8] = scaled_mag_var[2];
}
void INSSetBaroVar(float baro_var)
{
R[9] = baro_var;
ekf.R[9] = baro_var;
}
void INSSetMagNorth(float B[3])
{
float mag = sqrtf(B[0] * B[0] + B[1] * B[1] + B[2] * B[2]);
Be[0] = B[0] / mag;
Be[1] = B[1] / mag;
Be[2] = B[2] / mag;
ekf.Be[0] = B[0] / mag;
ekf.Be[1] = B[1] / mag;
ekf.Be[2] = B[2] / mag;
}
void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
@ -265,34 +275,34 @@ void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
U[5] = accel_data[2];
// EKF prediction step
LinearizeFG(X, U, F, G);
RungeKutta(X, U, dT);
qmag = sqrtf(X[6] * X[6] + X[7] * X[7] + X[8] * X[8] + X[9] * X[9]);
X[6] /= qmag;
X[7] /= qmag;
X[8] /= qmag;
X[9] /= qmag;
//CovariancePrediction(F,G,Q,dT,P);
LinearizeFG(ekf.X, U, ekf.F, ekf.G);
RungeKutta(ekf.X, U, dT);
qmag = sqrtf(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] /= qmag;
ekf.X[7] /= qmag;
ekf.X[8] /= qmag;
ekf.X[9] /= qmag;
//CovariancePrediction(ekf.F,ekf.G,ekf.Q,dT,ekf.P);
// Update Nav solution structure
Nav.Pos[0] = X[0];
Nav.Pos[1] = X[1];
Nav.Pos[2] = X[2];
Nav.Vel[0] = X[3];
Nav.Vel[1] = X[4];
Nav.Vel[2] = X[5];
Nav.q[0] = X[6];
Nav.q[1] = X[7];
Nav.q[2] = X[8];
Nav.q[3] = X[9];
Nav.gyro_bias[0] = X[10];
Nav.gyro_bias[1] = X[11];
Nav.gyro_bias[2] = X[12];
Nav.Pos[0] = ekf.X[0];
Nav.Pos[1] = ekf.X[1];
Nav.Pos[2] = ekf.X[2];
Nav.Vel[0] = ekf.X[3];
Nav.Vel[1] = ekf.X[4];
Nav.Vel[2] = ekf.X[5];
Nav.q[0] = ekf.X[6];
Nav.q[1] = ekf.X[7];
Nav.q[2] = ekf.X[8];
Nav.q[3] = ekf.X[9];
Nav.gyro_bias[0] = ekf.X[10];
Nav.gyro_bias[1] = ekf.X[11];
Nav.gyro_bias[2] = ekf.X[12];
}
void INSCovariancePrediction(float dT)
{
CovariancePrediction(F, G, Q, dT, P);
CovariancePrediction(ekf.F, ekf.G, ekf.Q, dT, ekf.P);
}
float zeros[3] = { 0, 0, 0 };
@ -361,29 +371,29 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
Z[9] = BaroAlt;
// EKF correction step
LinearizeH(X, Be, H);
MeasurementEq(X, Be, Y);
SerialUpdate(H, R, Z, Y, P, X, SensorsUsed);
qmag = sqrtf(X[6] * X[6] + X[7] * X[7] + X[8] * X[8] + X[9] * X[9]);
X[6] /= qmag;
X[7] /= qmag;
X[8] /= qmag;
X[9] /= qmag;
LinearizeH(ekf.X, ekf.Be, ekf.H);
MeasurementEq(ekf.X, ekf.Be, Y);
SerialUpdate(ekf.H, ekf.R, Z, Y, ekf.P, ekf.X, ekf.K, SensorsUsed);
qmag = sqrtf(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] /= qmag;
ekf.X[7] /= qmag;
ekf.X[8] /= qmag;
ekf.X[9] /= qmag;
// Update Nav solution structure
Nav.Pos[0] = X[0];
Nav.Pos[1] = X[1];
Nav.Pos[2] = X[2];
Nav.Vel[0] = X[3];
Nav.Vel[1] = X[4];
Nav.Vel[2] = X[5];
Nav.q[0] = X[6];
Nav.q[1] = X[7];
Nav.q[2] = X[8];
Nav.q[3] = X[9];
Nav.gyro_bias[0] = X[10];
Nav.gyro_bias[1] = X[11];
Nav.gyro_bias[2] = X[12];
Nav.Pos[0] = ekf.X[0];
Nav.Pos[1] = ekf.X[1];
Nav.Pos[2] = ekf.X[2];
Nav.Vel[0] = ekf.X[3];
Nav.Vel[1] = ekf.X[4];
Nav.Vel[2] = ekf.X[5];
Nav.q[0] = ekf.X[6];
Nav.q[1] = ekf.X[7];
Nav.q[2] = ekf.X[8];
Nav.q[3] = ekf.X[9];
Nav.gyro_bias[0] = ekf.X[10];
Nav.gyro_bias[1] = ekf.X[11];
Nav.gyro_bias[2] = ekf.X[12];
}
// ************* CovariancePrediction *************
@ -1381,7 +1391,7 @@ void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
// ************************************************
void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
float Y[NUMV], float P[NUMX][NUMX], float X[NUMX], float K[NUMX][NUMV],
uint16_t SensorsUsed)
{
float HP[NUMX], HPHR, Error;

4
flight/libraries/op_dfu.c
View File

@ -63,8 +63,6 @@ uint8_t Data2;
uint8_t Data3;
uint32_t Opt[3];
uint8_t offset = 0;
uint32_t aux;
//Download vars
uint32_t downSizeOfLastPacket = 0;
uint32_t downPacketTotal = 0;
@ -227,6 +225,8 @@ void processComand(uint8_t *xReceive_Buffer) {
numberOfWords = SizeOfLastPacket;
}
uint8_t result = 0;
uint32_t offset;
uint32_t aux;;
switch (currentProgrammingDestination) {
case Self_flash:
for (uint8_t x = 0; x < numberOfWords; ++x) {

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

@ -191,9 +191,9 @@ static void FirmwareIAPCallback(UAVObjEvent* ev)
/* Note: Cant just wait timeout value, because first time is randomized */
reset_count = 0;
lastResetSysTime = xTaskGetTickCount();
UAVObjEvent * ev = pvPortMalloc(sizeof(UAVObjEvent));
memset(ev,0,sizeof(UAVObjEvent));
EventPeriodicCallbackCreate(ev, resetTask, 100);
UAVObjEvent *event = pvPortMalloc(sizeof(UAVObjEvent));
memset(event, 0, sizeof(UAVObjEvent));
EventPeriodicCallbackCreate(event, resetTask, 100);
iap_state = IAP_STATE_RESETTING;
} else {
iap_state = IAP_STATE_READY;

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

@ -377,7 +377,6 @@ static uint8_t updateFixedDesiredAttitude()
StabilizationDesiredData stabDesired;
AttitudeActualData attitudeActual;
AccelsData accels;
FixedWingPathFollowerSettingsData fixedwingpathfollowerSettings;
StabilizationSettingsData stabSettings;
FixedWingPathFollowerStatusData fixedwingpathfollowerStatus;
AirspeedActualData airspeedActual;
@ -397,8 +396,6 @@ static uint8_t updateFixedDesiredAttitude()
float bearingError;
float bearingCommand;
FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings);
FixedWingPathFollowerStatusGet(&fixedwingpathfollowerStatus);
VelocityActualGet(&velocityActual);

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

@ -205,8 +205,10 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
// Main task loop
lastSysTime = xTaskGetTickCount();
float scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM] = {0};
while (1) {
float scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM];
// Wait until next update
vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS);
@ -810,13 +812,13 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
uint8_t throttleRate;
uint8_t throttleExp;
static portTickType lastSysTime;
static portTickType lastSysTimeAH;
static bool zeroed = false;
portTickType thisSysTime;
float dT;
AltitudeHoldDesiredData altitudeHoldDesired;
AltitudeHoldDesiredGet(&altitudeHoldDesired);
AltitudeHoldDesiredData altitudeHoldDesiredData;
AltitudeHoldDesiredGet(&altitudeHoldDesiredData);
AltitudeHoldSettingsThrottleExpGet(&throttleExp);
AltitudeHoldSettingsThrottleRateGet(&throttleRate);
@ -825,36 +827,36 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
StabilizationSettingsGet(&stabSettings);
thisSysTime = xTaskGetTickCount();
dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f);
lastSysTime = thisSysTime;
dT = ((thisSysTime == lastSysTimeAH)? 0.001f : (thisSysTime - lastSysTimeAH) * portTICK_RATE_MS * 0.001f);
lastSysTimeAH = thisSysTime;
altitudeHoldDesired.Roll = cmd->Roll * stabSettings.RollMax;
altitudeHoldDesired.Pitch = cmd->Pitch * stabSettings.PitchMax;
altitudeHoldDesired.Yaw = cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW];
altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax;
altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax;
altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW];
float currentDown;
PositionActualDownGet(&currentDown);
if(changed) {
// After not being in this mode for a while init at current height
altitudeHoldDesired.Altitude = 0;
altitudeHoldDesiredData.Altitude = 0;
zeroed = false;
} else if (cmd->Throttle > DEADBAND_HIGH && zeroed) {
// 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 + x∙(256−k)) / 256
altitudeHoldDesired.Altitude += (throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (256 - throttleExp)) / 256 * throttleRate * dT;
altitudeHoldDesiredData.Altitude += (throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (256 - throttleExp)) / 256 * throttleRate * dT;
} else if (cmd->Throttle < DEADBAND_LOW && zeroed) {
altitudeHoldDesired.Altitude -= (throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (256 - throttleExp)) / 256 * throttleRate * dT;
altitudeHoldDesiredData.Altitude -= (throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (256 - throttleExp)) / 256 * throttleRate * dT;
} else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH) {
// Require the stick to enter the dead band before they can move height
zeroed = true;
}
AltitudeHoldDesiredSet(&altitudeHoldDesired);
AltitudeHoldDesiredSet(&altitudeHoldDesiredData);
}
#else
// TODO: These functions should never be accessible on CC. Any configuration that
// could allow them to be called sholud already throw an error to prevent this happening
// 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,

22
flight/modules/Osd/osdgen/osdgen.c
View File

@ -1063,7 +1063,7 @@ int fetch_font_info(uint8_t ch, int font, struct FontEntry *font_info, char *loo
void write_char16(char ch, unsigned int x, unsigned int y, int font)
{
unsigned int yy, addr_temp, row, row_temp, xshift;
uint16_t and_mask, or_mask, level_bits;
uint16_t and_mask, or_mask, levels;
struct FontEntry font_info;
//char lookup = 0;
fetch_font_info(0, font, &font_info, NULL);
@ -1103,17 +1103,17 @@ void write_char16(char ch, unsigned int x, unsigned int y, int font)
addr = addr_temp;
for (yy = y; yy < y + font_info.height; yy++) {
if (font == 3) {
level_bits = font_frame12x18[row];
levels = font_frame12x18[row];
//if(!(flags & FONT_INVERT)) // data is normally inverted
level_bits = ~level_bits;
levels = ~levels;
or_mask = font_mask12x18[row] << xshift;
and_mask = (font_mask12x18[row] & level_bits) << xshift;
and_mask = (font_mask12x18[row] & levels) << xshift;
} else {
level_bits = font_frame8x10[row];
levels = font_frame8x10[row];
//if(!(flags & FONT_INVERT)) // data is normally inverted
level_bits = ~level_bits;
levels = ~levels;
or_mask = font_mask8x10[row] << xshift;
and_mask = (font_mask8x10[row] & level_bits) << xshift;
and_mask = (font_mask8x10[row] & levels) << xshift;
}
write_word_misaligned_OR(draw_buffer_level, or_mask, addr, wbit);
// If we're not bold write the AND mask.
@ -1139,7 +1139,7 @@ void write_char16(char ch, unsigned int x, unsigned int y, int font)
void write_char(char ch, unsigned int x, unsigned int y, int flags, int font)
{
unsigned int yy, addr_temp, row, row_temp, xshift;
uint16_t and_mask, or_mask, level_bits;
uint16_t and_mask, or_mask, levels;
struct FontEntry font_info;
char lookup = 0;
fetch_font_info(ch, font, &font_info, &lookup);
@ -1178,13 +1178,13 @@ void write_char(char ch, unsigned int x, unsigned int y, int flags, int font)
row = row_temp;
addr = addr_temp;
for (yy = y; yy < y + font_info.height; yy++) {
level_bits = font_info.data[row + font_info.height];
levels = font_info.data[row + font_info.height];
if (!(flags & FONT_INVERT)) {
// data is normally inverted
level_bits = ~level_bits;
levels = ~levels;
}
or_mask = font_info.data[row] << xshift;
and_mask = (font_info.data[row] & level_bits) << xshift;
and_mask = (font_info.data[row] & levels) << xshift;
write_word_misaligned_OR(draw_buffer_level, or_mask, addr, wbit);
// If we're not bold write the AND mask.
//if(!(flags & FONT_BOLD))

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

@ -215,29 +215,29 @@ void updatePathDesired(__attribute__((unused)) UAVObjEvent * ev) {
// use local variables, dont use stack since this is huge and a callback,
// dont use the globals because we cant use mutexes here
static WaypointActiveData waypointActive;
static PathActionData pathAction;
static WaypointData waypoint;
static WaypointActiveData waypointActiveData;
static PathActionData pathActionData;
static WaypointData waypointData;
static PathDesiredData pathDesired;
// find out current waypoint
WaypointActiveGet(&waypointActive);
WaypointActiveGet(&waypointActiveData);
WaypointInstGet(waypointActive.Index,&waypoint);
PathActionInstGet(waypoint.Action, &pathAction);
WaypointInstGet(waypointActiveData.Index,&waypointData);
PathActionInstGet(waypointData.Action, &pathActionData);
pathDesired.End[PATHDESIRED_END_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH];
pathDesired.End[PATHDESIRED_END_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST];
pathDesired.End[PATHDESIRED_END_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN];
pathDesired.EndingVelocity = waypoint.Velocity;
pathDesired.Mode = pathAction.Mode;
pathDesired.ModeParameters[0] = pathAction.ModeParameters[0];
pathDesired.ModeParameters[1] = pathAction.ModeParameters[1];
pathDesired.ModeParameters[2] = pathAction.ModeParameters[2];
pathDesired.ModeParameters[3] = pathAction.ModeParameters[3];
pathDesired.UID = waypointActive.Index;
pathDesired.End[PATHDESIRED_END_NORTH] = waypointData.Position[WAYPOINT_POSITION_NORTH];
pathDesired.End[PATHDESIRED_END_EAST] = waypointData.Position[WAYPOINT_POSITION_EAST];
pathDesired.End[PATHDESIRED_END_DOWN] = waypointData.Position[WAYPOINT_POSITION_DOWN];
pathDesired.EndingVelocity = waypointData.Velocity;
pathDesired.Mode = pathActionData.Mode;
pathDesired.ModeParameters[0] = pathActionData.ModeParameters[0];
pathDesired.ModeParameters[1] = pathActionData.ModeParameters[1];
pathDesired.ModeParameters[2] = pathActionData.ModeParameters[2];
pathDesired.ModeParameters[3] = pathActionData.ModeParameters[3];
pathDesired.UID = waypointActiveData.Index;
if(waypointActive.Index == 0) {
if(waypointActiveData.Index == 0) {
PositionActualData positionActual;
PositionActualGet(&positionActual);
// First waypoint has itself as start point (used to be home position but that proved dangerous when looping)
@ -260,7 +260,6 @@ void updatePathDesired(__attribute__((unused)) UAVObjEvent * ev) {
pathDesired.StartingVelocity = waypointPrev.Velocity;
}
PathDesiredSet(&pathDesired);
}
// helper function to go to a specific waypoint

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

@ -487,7 +487,7 @@ static void magOffsetEstimation(MagnetometerData *mag)
const float Rz = homeLocation.Be[2];
const float rate = cal.MagBiasNullingRate;
float R[3][3];
float Rot[3][3];
float B_e[3];
float xy[2];
float delta[3];
@ -496,9 +496,9 @@ static void magOffsetEstimation(MagnetometerData *mag)
Quaternion2R(&attitude.q1, R);
// Rotate the mag into the NED frame
B_e[0] = R[0][0] * mag->x + R[1][0] * mag->y + R[2][0] * mag->z;
B_e[1] = R[0][1] * mag->x + R[1][1] * mag->y + R[2][1] * mag->z;
B_e[2] = R[0][2] * mag->x + R[1][2] * mag->y + R[2][2] * mag->z;
B_e[0] = Rot[0][0] * mag->x + Rot[1][0] * mag->y + Rot[2][0] * mag->z;
B_e[1] = Rot[0][1] * mag->x + Rot[1][1] * mag->y + Rot[2][1] * mag->z;
B_e[2] = Rot[0][2] * mag->x + Rot[1][2] * mag->y + Rot[2][2] * mag->z;
float cy = cosf(DEG2RAD(attitude.Yaw));
float sy = sinf(DEG2RAD(attitude.Yaw));

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

@ -566,7 +566,6 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
StabilizationDesiredData stabDesired;
AttitudeActualData attitudeActual;
NedAccelData nedAccel;
VtolPathFollowerSettingsData vtolpathfollowerSettings;
StabilizationSettingsData stabSettings;
SystemSettingsData systemSettings;
@ -580,8 +579,6 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
float downCommand;
SystemSettingsGet(&systemSettings);
VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
VelocityActualGet(&velocityActual);
VelocityDesiredGet(&velocityDesired);
StabilizationDesiredGet(&stabDesired);

8
flight/pios/common/pios_adxl345.c
View File

@ -70,13 +70,13 @@ static struct adxl345_dev * PIOS_ADXL345_alloc(void)
* @brief Validate the handle to the spi device
* @returns 0 for valid device or -1 otherwise
*/
static int32_t PIOS_ADXL345_Validate(struct adxl345_dev * dev)
static int32_t PIOS_ADXL345_Validate(struct adxl345_dev *vdev)
{
if (dev == NULL)
if (vdev == NULL)
return -1;
if (dev->magic != PIOS_ADXL345_DEV_MAGIC)
if (vdev->magic != PIOS_ADXL345_DEV_MAGIC)
return -2;
if (dev->spi_id == 0)
if (vdev->spi_id == 0)
return -3;
return 0;
}

71
flight/pios/common/pios_bma180.c
View File

@ -86,13 +86,13 @@ static struct bma180_dev * PIOS_BMA180_alloc(void)
* @brief Validate the handle to the spi device
* @returns 0 for valid device or -1 otherwise
*/
static int32_t PIOS_BMA180_Validate(struct bma180_dev * dev)
static int32_t PIOS_BMA180_Validate(struct bma180_dev *vdev)
{
if (dev == NULL)
if (vdev == NULL)
return -1;
if (dev->magic != PIOS_BMA180_DEV_MAGIC)
if (vdev->magic != PIOS_BMA180_DEV_MAGIC)
return -2;
if (dev->spi_id == 0)
if (vdev->spi_id == 0)
return -3;
return 0;
}
@ -128,10 +128,11 @@ int32_t PIOS_BMA180_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_
*/
int32_t PIOS_BMA180_ClaimBus()
{
if(PIOS_BMA180_Validate(dev) != 0)
if (PIOS_BMA180_Validate(dev) != 0) {
return -1;
}
if(PIOS_SPI_ClaimBus(dev->spi_id) != 0) {
if (PIOS_SPI_ClaimBus(dev->spi_id) != 0) {
return -1;
}
@ -141,19 +142,22 @@ int32_t PIOS_BMA180_ClaimBus()
}
/**
* @brief Claim the SPI bus for the accel communications and select this chip
* @brief Claim the SPI bus for the accel communications and select this chip. Call from an ISR.
* @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
* task has is now eligible to run, else unchanged
* @return 0 if successful, -1 if unable to claim bus
*/
int32_t PIOS_BMA180_ClaimBusISR()
int32_t PIOS_BMA180_ClaimBusISR(bool *woken)
{
if(PIOS_BMA180_Validate(dev) != 0)
return -1;
if(PIOS_SPI_ClaimBusISR(dev->spi_id) != 0) {
if (PIOS_BMA180_Validate(dev) != 0) {
return -1;
}
PIOS_SPI_RC_PinSet(dev->spi_id,dev->slave_num,0);
if (PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) {
return -1;
}
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
return 0;
}
@ -162,13 +166,29 @@ int32_t PIOS_BMA180_ClaimBusISR()
* @return 0 if successful
*/
int32_t PIOS_BMA180_ReleaseBus()
{
if (PIOS_BMA180_Validate(dev) != 0) {
return -1;
}
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1);
return PIOS_SPI_ReleaseBus(dev->spi_id);
}
/**
* @brief Release the SPI bus for the accel communications and end the transaction. Call from an ISR
* @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
* task has is now eligible to run, else unchanged
* @return 0 if successful
*/
int32_t PIOS_BMA180_ReleaseBusISR(bool *woken)
{
if(PIOS_BMA180_Validate(dev) != 0)
return -1;
PIOS_SPI_RC_PinSet(dev->spi_id,dev->slave_num,1);
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1);
return PIOS_SPI_ReleaseBus(dev->spi_id);
return PIOS_SPI_ReleaseBusISR(dev->spi_id, woken);
}
/**
@ -431,19 +451,21 @@ int32_t PIOS_BMA180_Test()
}
/**
* @brief IRQ Handler. Read data from the BMA180 FIFO and push onto a local fifo.
* @brief EXTI IRQ Handler. Read data from the BMA180 FIFO and push onto a local fifo.
* @return a boolean to the EXTI IRQ Handler wrapper indicating if a
* higher priority task is now eligible to run
*/
int32_t bma180_irqs = 0;
bool PIOS_BMA180_IRQHandler(void)
{
bma180_irqs++;
bool woken = false;
static const uint8_t pios_bma180_req_buf[7] = {BMA_X_LSB_ADDR | 0x80,0,0,0,0,0};
uint8_t pios_bma180_dmabuf[8];
bma180_irqs++;
// If we can't get the bus then just move on for efficiency
if(PIOS_BMA180_ClaimBusISR() != 0) {
return false; // Something else is using bus, miss this data
if (PIOS_BMA180_ClaimBusISR(&woken) != 0) {
return woken; // Something else is using bus, miss this data
}
PIOS_SPI_TransferBlock(dev->spi_id,pios_bma180_req_buf,(uint8_t *) pios_bma180_dmabuf,
@ -453,11 +475,12 @@ bool PIOS_BMA180_IRQHandler(void)
struct pios_bma180_data data;
// Don't release bus till data has copied
PIOS_BMA180_ReleaseBus();
PIOS_BMA180_ReleaseBusISR(&woken);
// Must not return before releasing bus
if(fifoBuf_getFree(&dev->fifo) < sizeof(data))
return false;
if (fifoBuf_getFree(&dev->fifo) < sizeof(data)) {
return woken;
}
// Bottom two bits indicate new data and are constant zeros. Don't right
// shift because it drops sign bit
@ -471,7 +494,7 @@ bool PIOS_BMA180_IRQHandler(void)
fifoBuf_putData(&dev->fifo, (uint8_t *) &data, sizeof(data));
return false;
return woken;
}
#endif /* PIOS_INCLUDE_BMA180 */

100
flight/pios/common/pios_l3gd20.c
View File

@ -60,9 +60,11 @@ static int32_t PIOS_L3GD20_Validate(struct l3gd20_dev * dev);
static void PIOS_L3GD20_Config(struct pios_l3gd20_cfg const * cfg);
static int32_t PIOS_L3GD20_SetReg(uint8_t address, uint8_t buffer);
static int32_t PIOS_L3GD20_GetReg(uint8_t address);
static int32_t PIOS_L3GD20_GetRegISR(uint8_t address, bool *woken);
static int32_t PIOS_L3GD20_ClaimBus();
static int32_t PIOS_L3GD20_ClaimBusIsr();
static int32_t PIOS_L3GD20_ClaimBusISR(bool *woken);
static int32_t PIOS_L3GD20_ReleaseBus();
static int32_t PIOS_L3GD20_ReleaseBusISR(bool *woken);
volatile bool l3gd20_configured = false;
@ -93,13 +95,13 @@ static struct l3gd20_dev * PIOS_L3GD20_alloc(void)
* @brief Validate the handle to the spi device
* @returns 0 for valid device or -1 otherwise
*/
static int32_t PIOS_L3GD20_Validate(struct l3gd20_dev * dev)
static int32_t PIOS_L3GD20_Validate(struct l3gd20_dev *vdev)
{
if (dev == NULL)
if (vdev == NULL)
return -1;
if (dev->magic != PIOS_L3GD20_DEV_MAGIC)
if (vdev->magic != PIOS_L3GD20_DEV_MAGIC)
return -2;
if (dev->spi_id == 0)
if (vdev->spi_id == 0)
return -3;
return 0;
}
@ -191,17 +193,19 @@ static int32_t PIOS_L3GD20_ClaimBus()
/**
* @brief Claim the SPI bus for the accel communications and select this chip
* @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
* task has is now eligible to run, else unchanged
* @return 0 if successful, -1 for invalid device, -2 if unable to claim bus
*/
static int32_t PIOS_L3GD20_ClaimBusIsr()
static int32_t PIOS_L3GD20_ClaimBusISR(bool *woken)
{
if(PIOS_L3GD20_Validate(dev) != 0)
if(PIOS_L3GD20_Validate(dev) != 0) {
return -1;
if(PIOS_SPI_ClaimBusISR(dev->spi_id) != 0)
}
if(PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) {
return -2;
PIOS_SPI_RC_PinSet(dev->spi_id,dev->slave_num,0);
}
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
return 0;
}
@ -211,14 +215,28 @@ static int32_t PIOS_L3GD20_ClaimBusIsr()
*/
int32_t PIOS_L3GD20_ReleaseBus()
{
if(PIOS_L3GD20_Validate(dev) != 0)
if(PIOS_L3GD20_Validate(dev) != 0) {
return -1;
PIOS_SPI_RC_PinSet(dev->spi_id,dev->slave_num,1);
}
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1);
return PIOS_SPI_ReleaseBus(dev->spi_id);
}
/**
* @brief Release the SPI bus for the accel communications and end the transaction
* @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
* task has is now eligible to run, else unchanged
* @return 0 if successful, -1 for invalid device
*/
int32_t PIOS_L3GD20_ReleaseBusISR(bool *woken)
{
if(PIOS_L3GD20_Validate(dev) != 0) {
return -1;
}
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1);
return PIOS_SPI_ReleaseBusISR(dev->spi_id, woken);
}
/**
* @brief Read a register from L3GD20
* @returns The register value or -1 if failure to get bus
@ -238,6 +256,26 @@ static int32_t PIOS_L3GD20_GetReg(uint8_t reg)
return data;
}
/**
* @brief Read a register from L3GD20 in an ISR context
* @param reg[in] Register address to be read
* @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
* task has is now eligible to run, else unchanged
* @return The register value or -1 if failure to get bus
*/
static int32_t PIOS_L3GD20_GetRegISR(uint8_t reg, bool *woken)
{
uint8_t data;
if(PIOS_L3GD20_ClaimBusISR(woken) != 0) {
return -1;
}
PIOS_SPI_TransferByte(dev->spi_id,(0x80 | reg) ); // request byte
data = PIOS_SPI_TransferByte(dev->spi_id,0 ); // receive response
PIOS_L3GD20_ReleaseBusISR(woken);
return data;
}
/**
* @brief Writes one byte to the L3GD20
* \param[in] reg Register address
@ -349,34 +387,38 @@ uint8_t PIOS_L3GD20_Test(void)
}
/**
* @brief IRQ Handler. Read all the data from onboard buffer
* @brief EXTI IRQ Handler. Read all the data from onboard buffer
* @return a boolean to the EXTI IRQ Handler wrapper indicating if a
* higher priority task is now eligible to run
*/
bool PIOS_L3GD20_IRQHandler(void)
{
l3gd20_irq++;
bool woken = false;
struct pios_l3gd20_data data;
uint8_t buf[7] = {PIOS_L3GD20_GYRO_X_OUT_LSB | 0x80 | 0x40, 0, 0, 0, 0, 0, 0};
uint8_t rec[7];
/* This code duplicates ReadGyros above but uses ClaimBusIsr */
if(PIOS_L3GD20_ClaimBusIsr() != 0)
return false;
l3gd20_irq++;
if(PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) {
PIOS_L3GD20_ReleaseBus();
return false;
/* This code duplicates ReadGyros above but uses ClaimBusIsr */
if (PIOS_L3GD20_ClaimBusISR(&woken) != 0) {
return woken;
}
PIOS_L3GD20_ReleaseBus();
if(PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) {
PIOS_L3GD20_ReleaseBusISR(&woken);
return woken;
}
PIOS_L3GD20_ReleaseBusISR(&woken);
memcpy((uint8_t *) &(data.gyro_x), &rec[1], 6);
data.temperature = PIOS_L3GD20_GetReg(PIOS_L3GD20_OUT_TEMP);
data.temperature = PIOS_L3GD20_GetRegISR(PIOS_L3GD20_OUT_TEMP, &woken);
portBASE_TYPE xHigherPriorityTaskWoken;
xQueueSendToBackFromISR(dev->queue, (void *) &data, &xHigherPriorityTaskWoken);
signed portBASE_TYPE higherPriorityTaskWoken;
xQueueSendToBackFromISR(dev->queue, (void *) &data, &higherPriorityTaskWoken);
return xHigherPriorityTaskWoken == pdTRUE;
return (woken || higherPriorityTaskWoken == pdTRUE);
}
#endif /* PIOS_INCLUDE_L3GD20 */

164
flight/pios/common/pios_mpu6000.c
View File

@ -91,13 +91,13 @@ static struct mpu6000_dev * PIOS_MPU6000_alloc(void)
* @brief Validate the handle to the spi device
* @returns 0 for valid device or -1 otherwise
*/
static int32_t PIOS_MPU6000_Validate(struct mpu6000_dev * dev)
static int32_t PIOS_MPU6000_Validate(struct mpu6000_dev *vdev)
{
if (dev == NULL)
if (vdev == NULL)
return -1;
if (dev->magic != PIOS_MPU6000_DEV_MAGIC)
if (vdev->magic != PIOS_MPU6000_DEV_MAGIC)
return -2;
if (dev->spi_id == 0)
if (vdev->spi_id == 0)
return -3;
return 0;
}
@ -197,7 +197,12 @@ int32_t PIOS_MPU6000_ConfigureRanges(
{
if(dev == NULL)
return -1;
// TODO: check that changing the SPI clock speed is safe
// to do here given that interrupts are enabled and the bus has
// not been claimed/is not claimed during this call
PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_256);
// update filter settings
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_DLPF_CFG_REG, filterSetting) != 0);
@ -225,39 +230,63 @@ int32_t PIOS_MPU6000_ConfigureRanges(
/**
* @brief Claim the SPI bus for the accel communications and select this chip
* @return 0 if successful, -1 for invalid device, -2 if unable to claim bus
* @param fromIsr[in] Tells if the function is being called from a ISR or not
*/
static int32_t PIOS_MPU6000_ClaimBus(bool fromIsr)
static int32_t PIOS_MPU6000_ClaimBus()
{
if(PIOS_MPU6000_Validate(dev) != 0)
if(PIOS_MPU6000_Validate(dev) != 0) {
return -1;
if(fromIsr){
if(PIOS_SPI_ClaimBusISR(dev->spi_id) != 0)
return -2;
} else {
if(PIOS_SPI_ClaimBus(dev->spi_id) != 0)
}
if (PIOS_SPI_ClaimBus(dev->spi_id) != 0) {
return -2;
}
PIOS_SPI_RC_PinSet(dev->spi_id,dev->slave_num,0);
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
return 0;
}
/**
* @brief Claim the SPI bus for the accel communications and select this chip
* @return 0 if successful, -1 for invalid device, -2 if unable to claim bus
* @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
* task has is now eligible to run, else unchanged
*/
static int32_t PIOS_MPU6000_ClaimBusISR(bool *woken)
{
if (PIOS_MPU6000_Validate(dev) != 0) {
return -1;
}
if (PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) {
return -2;
}
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
return 0;
}
/**
* @brief Release the SPI bus for the accel communications and end the transaction
* @return 0 if successful
* @param fromIsr[in] Tells if the function is being called from a ISR or not
*/
int32_t PIOS_MPU6000_ReleaseBus(bool fromIsr)
static int32_t PIOS_MPU6000_ReleaseBus()
{
if(PIOS_MPU6000_Validate(dev) != 0)
if(PIOS_MPU6000_Validate(dev) != 0) {
return -1;
PIOS_SPI_RC_PinSet(dev->spi_id,dev->slave_num,1);
if(fromIsr){
return PIOS_SPI_ReleaseBusISR(dev->spi_id);
} else {
return PIOS_SPI_ReleaseBus(dev->spi_id);
}
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1);
return PIOS_SPI_ReleaseBus(dev->spi_id);
}
/**
* @brief Release the SPI bus for the accel communications and end the transaction
* @return 0 if successful
* @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
* task has is now eligible to run, else unchanged
*/
static int32_t PIOS_MPU6000_ReleaseBusISR(bool *woken)
{
if(PIOS_MPU6000_Validate(dev) != 0) {
return -1;
}
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1);
return PIOS_SPI_ReleaseBusISR(dev->spi_id, woken);
}
/**
@ -269,13 +298,14 @@ static int32_t PIOS_MPU6000_GetReg(uint8_t reg)
{
uint8_t data;
if(PIOS_MPU6000_ClaimBus(false) != 0)
if(PIOS_MPU6000_ClaimBus() != 0) {
return -1;
}
PIOS_SPI_TransferByte(dev->spi_id,(0x80 | reg) ); // request byte
data = PIOS_SPI_TransferByte(dev->spi_id,0 ); // receive response
PIOS_MPU6000_ReleaseBus(false);
PIOS_MPU6000_ReleaseBus();
return data;
}
@ -289,20 +319,21 @@ static int32_t PIOS_MPU6000_GetReg(uint8_t reg)
*/
static int32_t PIOS_MPU6000_SetReg(uint8_t reg, uint8_t data)
{
if(PIOS_MPU6000_ClaimBus(false) != 0)
if (PIOS_MPU6000_ClaimBus() != 0) {
return -1;
}
if(PIOS_SPI_TransferByte(dev->spi_id, 0x7f & reg) != 0) {
PIOS_MPU6000_ReleaseBus(false);
if (PIOS_SPI_TransferByte(dev->spi_id, 0x7f & reg) != 0) {
PIOS_MPU6000_ReleaseBus();
return -2;
}
if(PIOS_SPI_TransferByte(dev->spi_id, data) != 0) {
PIOS_MPU6000_ReleaseBus(false);
if (PIOS_SPI_TransferByte(dev->spi_id, data) != 0) {
PIOS_MPU6000_ReleaseBus();
return -3;
}
PIOS_MPU6000_ReleaseBus(false);
PIOS_MPU6000_ReleaseBus();
return 0;
}
@ -318,13 +349,15 @@ int32_t PIOS_MPU6000_ReadGyros(struct pios_mpu6000_data * data)
uint8_t buf[7] = {PIOS_MPU6000_GYRO_X_OUT_MSB | 0x80, 0, 0, 0, 0, 0, 0};
uint8_t rec[7];
if(PIOS_MPU6000_ClaimBus(false) != 0)
if (PIOS_MPU6000_ClaimBus() != 0) {
return -1;
}
if(PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0)
if (PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) {
return -2;
}
PIOS_MPU6000_ReleaseBus(false);
PIOS_MPU6000_ReleaseBus();
data->gyro_x = rec[1] << 8 | rec[2];
data->gyro_y = rec[3] << 8 | rec[4];
@ -407,31 +440,34 @@ int32_t PIOS_MPU6000_Test(void)
}
/**
* @brief Run self-test operation.
* \return 0 if test succeeded
* \return non-zero value if test succeeded
* @param fromIsr[in] Tells if the function is being called from a ISR or not
* @brief Obtains the number of bytes in the FIFO. Call from ISR only.
* @return the number of bytes in the FIFO
* @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
* task has is now eligible to run, else unchanged
*/
static int32_t PIOS_MPU6000_FifoDepth(bool fromIsr)
static int32_t PIOS_MPU6000_FifoDepthISR(bool *woken)
{
uint8_t mpu6000_send_buf[3] = {PIOS_MPU6000_FIFO_CNT_MSB | 0x80, 0, 0};
uint8_t mpu6000_rec_buf[3];
if(PIOS_MPU6000_ClaimBus(fromIsr) != 0)
return -1;
if(PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) {
PIOS_MPU6000_ReleaseBus(fromIsr);
if(PIOS_MPU6000_ClaimBusISR(woken) != 0) {
return -1;
}
PIOS_MPU6000_ReleaseBus(fromIsr);
if(PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) {
PIOS_MPU6000_ReleaseBusISR(woken);
return -1;
}
PIOS_MPU6000_ReleaseBusISR(woken);
return (mpu6000_rec_buf[1] << 8) | mpu6000_rec_buf[2];
}
/**
* @brief IRQ Handler. Read all the data from onboard buffer
* @brief EXTI IRQ Handler. Read all the data from onboard buffer
* @return a boolean to the EXTI IRQ Handler wrapper indicating if a
* higher priority task is now eligible to run
*/
uint32_t mpu6000_irq = 0;
int32_t mpu6000_count;
@ -446,46 +482,50 @@ uint32_t mpu6000_transfer_size;
bool PIOS_MPU6000_IRQHandler(void)
{
bool woken = false;
static uint32_t timeval;
mpu6000_interval_us = PIOS_DELAY_DiffuS(timeval);
timeval = PIOS_DELAY_GetRaw();
if (!mpu6000_configured)
if (!mpu6000_configured) {
return false;
}
mpu6000_count = PIOS_MPU6000_FifoDepth(true);
if (mpu6000_count < (int32_t)sizeof(struct pios_mpu6000_data))
return false;
mpu6000_count = PIOS_MPU6000_FifoDepthISR(&woken);
if (mpu6000_count < (int32_t)sizeof(struct pios_mpu6000_data)) {
return woken;
}
if (PIOS_MPU6000_ClaimBus(true) != 0)
return false;
if (PIOS_MPU6000_ClaimBusISR(&woken) != 0) {
return woken;
}
static uint8_t mpu6000_send_buf[1 + sizeof(struct pios_mpu6000_data) ] = {PIOS_MPU6000_FIFO_REG | 0x80, 0, 0, 0, 0, 0, 0, 0, 0};
static uint8_t mpu6000_rec_buf[1 + sizeof(struct pios_mpu6000_data) ];
if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) {
PIOS_MPU6000_ReleaseBus(true);
PIOS_MPU6000_ReleaseBusISR(&woken);
mpu6000_fails++;
return false;
return woken;
}
PIOS_MPU6000_ReleaseBus(true);
PIOS_MPU6000_ReleaseBusISR(&woken);
static struct pios_mpu6000_data data;
// In the case where extras samples backed up grabbed an extra
if (mpu6000_count >= (int32_t)(sizeof(data) * 2)) {
mpu6000_fifo_backup++;
if (PIOS_MPU6000_ClaimBus(true) != 0)
return false;
if (PIOS_MPU6000_ClaimBusISR(&woken) != 0)
return woken;
if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) {
PIOS_MPU6000_ReleaseBus(true);
PIOS_MPU6000_ReleaseBusISR(&woken);
mpu6000_fails++;
return false;
return woken;
}
PIOS_MPU6000_ReleaseBus(true);
PIOS_MPU6000_ReleaseBusISR(&woken);
}
// Rotate the sensor to OP convention. The datasheet defines X as towards the right
@ -548,14 +588,14 @@ bool PIOS_MPU6000_IRQHandler(void)
data.temperature = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2];
#endif
portBASE_TYPE xHigherPriorityTaskWoken;
xQueueSendToBackFromISR(dev->queue, (void *) &data, &xHigherPriorityTaskWoken);
signed portBASE_TYPE higherPriorityTaskWoken;
xQueueSendToBackFromISR(dev->queue, (void *) &data, &higherPriorityTaskWoken);
mpu6000_irq++;
mpu6000_time_us = PIOS_DELAY_DiffuS(timeval);
return xHigherPriorityTaskWoken == pdTRUE;
return (woken || higherPriorityTaskWoken == pdTRUE);
}
#endif /* PIOS_INCLUDE_MPU6000 */

4
flight/pios/inc/pios_spi.h
View File

@ -49,9 +49,9 @@ extern int32_t PIOS_SPI_TransferByte(uint32_t spi_id, uint8_t b);
extern int32_t PIOS_SPI_TransferBlock(uint32_t spi_id, const uint8_t *send_buffer, uint8_t *receive_buffer, uint16_t len, void *callback);
extern int32_t PIOS_SPI_Busy(uint32_t spi_id);
extern int32_t PIOS_SPI_ClaimBus(uint32_t spi_id);
extern int32_t PIOS_SPI_ClaimBusISR(uint32_t spi_id);
extern int32_t PIOS_SPI_ClaimBusISR(uint32_t spi_id, bool *woken);
extern int32_t PIOS_SPI_ReleaseBus(uint32_t spi_id);
extern int32_t PIOS_SPI_ReleaseBusISR(uint32_t spi_id);
extern int32_t PIOS_SPI_ReleaseBusISR(uint32_t spi_id, bool *woken);
extern void PIOS_SPI_IRQ_Handler(uint32_t spi_id);
extern void PIOS_SPI_SetPrescalar(uint32_t spi_id, uint32_t prescalar);

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

@ -50,7 +50,7 @@ const struct pios_rcvr_driver pios_ppm_rcvr_driver = {
#define PIOS_PPM_IN_MAX_CHANNEL_PULSE_US 2250 // microseconds
/* Local Variables */
static TIM_ICInitTypeDef TIM_ICInitStructure;
//static TIM_ICInitTypeDef TIM_ICInitStructure;
static void PIOS_PPM_Supervisor(uint32_t ppm_id);
@ -156,12 +156,13 @@ extern int32_t PIOS_PPM_Init(uint32_t * ppm_id, const struct pios_ppm_cfg * cfg)
return -1;
}
TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init;
/* Configure the channels to be in capture/compare mode */
for (uint8_t i = 0; i < cfg->num_channels; i++) {
const struct pios_tim_channel * chan = &cfg->channels[i];
/* Configure timer for input capture */
TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init;
TIM_ICInitStructure.TIM_Channel = chan->timer_chan;
TIM_ICInit(chan->timer, &TIM_ICInitStructure);
@ -182,12 +183,6 @@ extern int32_t PIOS_PPM_Init(uint32_t * ppm_id, const struct pios_ppm_cfg * cfg)
}
}
/* Setup local variable which stays in this scope */
/* Doing this here and using a local variable saves doing it in the ISR */
TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
TIM_ICInitStructure.TIM_ICFilter = 0x0;
ppm_dev->supv_timer = 0;
if (!PIOS_RTC_RegisterTickCallback(PIOS_PPM_Supervisor, (uint32_t)ppm_dev)) {
PIOS_DEBUG_Assert(0);

41
flight/pios/stm32f10x/pios_spi.c
View File

@ -265,22 +265,33 @@ int32_t PIOS_SPI_ClaimBus(uint32_t spi_id)
/**
* Claim the SPI bus semaphore from an ISR. Has no timeout.
* \param[in] spi SPI number (0 or 1)
* \param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
* task has is now eligible to run, else unchanged
* \return 0 if no error
* \return -1 if timeout before claiming semaphore
*/
int32_t PIOS_SPI_ClaimBusISR(uint32_t spi_id)
int32_t PIOS_SPI_ClaimBusISR(uint32_t spi_id, bool *woken)
{
#if defined(PIOS_INCLUDE_FREERTOS)
struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id;
signed portBASE_TYPE higherPriorityTaskWoken = pdFALSE;
bool valid = PIOS_SPI_validate(spi_dev);
PIOS_Assert(valid)
if (xSemaphoreTakeFromISR(spi_dev->busy, NULL) != pdTRUE){
if (xSemaphoreTakeFromISR(spi_dev->busy, &higherPriorityTaskWoken) != pdTRUE){
return -1;
}
#endif
if (woken) {
*woken = *woken || (higherPriorityTaskWoken == pdTRUE);
}
return 0;
#else
if (woken) {
*woken = false;
}
return PIOS_SPI_ClaimBus(spi_id);
#endif
}
/**
@ -302,7 +313,6 @@ int32_t PIOS_SPI_ReleaseBus(uint32_t spi_id)
PIOS_IRQ_Disable();
spi_dev->busy = 0;
PIOS_IRQ_Enable();
#endif
return 0;
}
@ -310,25 +320,30 @@ int32_t PIOS_SPI_ReleaseBus(uint32_t spi_id)
/**
* Release the SPI bus semaphore from ISR. Calling the SPI functions does not require this
* \param[in] spi SPI number (0 or 1)
* \param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
* task has is now eligible to run, else unchanged
* \return 0 if no error
*/
int32_t PIOS_SPI_ReleaseBusISR(uint32_t spi_id)
int32_t PIOS_SPI_ReleaseBusISR(uint32_t spi_id, bool *woken)
{
#if defined(PIOS_INCLUDE_FREERTOS)
struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id;
signed portBASE_TYPE higherPriorityTaskWoken = pdFALSE;
bool valid = PIOS_SPI_validate(spi_dev);
PIOS_Assert(valid)
xSemaphoreGiveFromISR(spi_dev->busy, NULL);
#else
struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id;
PIOS_IRQ_Disable();
spi_dev->busy = 0;
PIOS_IRQ_Enable();
#endif
xSemaphoreGiveFromISR(spi_dev->busy, &higherPriorityTaskWoken);
if (woken) {
*woken = *woken || (higherPriorityTaskWoken == pdTRUE);
}
return 0;
#else
if (woken) {
*woken = false;
}
return PIOS_SPI_ReleaseBus(spi_id);
#endif
}

1
flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/src/usb_core.c
View File

@ -1355,7 +1355,6 @@ USB_OTG_STS USB_OTG_CoreInitDev (USB_OTG_CORE_HANDLE *pdev)
}
for (i = 0; i < pdev->cfg.dev_endpoints; i++)
{
USB_OTG_DEPCTL_TypeDef depctl;
depctl.d32 = USB_OTG_READ_REG32(&pdev->regs.OUTEP_REGS[i]->DOEPCTL);
if (depctl.b.epena)
{

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

@ -48,9 +48,6 @@ const struct pios_rcvr_driver pios_ppm_rcvr_driver = {
#define PIOS_PPM_IN_MIN_CHANNEL_PULSE_US 750 // microseconds
#define PIOS_PPM_IN_MAX_CHANNEL_PULSE_US 2250 // microseconds
/* Local Variables */
static TIM_ICInitTypeDef TIM_ICInitStructure;
static void PIOS_PPM_Supervisor(uint32_t ppm_id);
enum pios_ppm_dev_magic {
@ -155,12 +152,13 @@ extern int32_t PIOS_PPM_Init(uint32_t * ppm_id, const struct pios_ppm_cfg * cfg)
return -1;
}
TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init;
/* Configure the channels to be in capture/compare mode */
for (uint8_t i = 0; i < cfg->num_channels; i++) {
const struct pios_tim_channel * chan = &cfg->channels[i];
/* Configure timer for input capture */
TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init;
TIM_ICInitStructure.TIM_Channel = chan->timer_chan;
TIM_ICInit(chan->timer, &TIM_ICInitStructure);
@ -181,12 +179,6 @@ extern int32_t PIOS_PPM_Init(uint32_t * ppm_id, const struct pios_ppm_cfg * cfg)
}
}
/* Setup local variable which stays in this scope */
/* Doing this here and using a local variable saves doing it in the ISR */
TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
TIM_ICInitStructure.TIM_ICFilter = 0x0;
if (!PIOS_RTC_RegisterTickCallback(PIOS_PPM_Supervisor, (uint32_t)ppm_dev)) {
PIOS_DEBUG_Assert(0);
}

53
flight/pios/stm32f4xx/pios_spi.c
View File

@ -257,6 +257,18 @@ int32_t PIOS_SPI_ClaimBus(uint32_t spi_id)
if (xSemaphoreTake(spi_dev->busy, 0xffff) != pdTRUE)
return -1;
#else
struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id;
uint32_t timeout = 0xffff;
while((PIOS_SPI_Busy(spi_id) || spi_dev->busy) && --timeout);
if(timeout == 0) //timed out
return -1;
PIOS_IRQ_Disable();
if(spi_dev->busy)
return -1;
spi_dev->busy = 1;
PIOS_IRQ_Enable();
#endif
return 0;
}
@ -264,25 +276,35 @@ int32_t PIOS_SPI_ClaimBus(uint32_t spi_id)
/**
* Claim the SPI bus semaphore from an ISR. Has no timeout.
* \param[in] spi SPI number (0 or 1)
* \param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
* task has is now eligible to run, else unchanged
* \return 0 if no error
* \return -1 if timeout before claiming semaphore
*/
int32_t PIOS_SPI_ClaimBusISR(uint32_t spi_id)
int32_t PIOS_SPI_ClaimBusISR(uint32_t spi_id, bool *woken)
{
#if defined(PIOS_INCLUDE_FREERTOS)
struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id;
signed portBASE_TYPE higherPriorityTaskWoken = pdFALSE;
bool valid = PIOS_SPI_validate(spi_dev);
PIOS_Assert(valid)
if (xSemaphoreTakeFromISR(spi_dev->busy, NULL) != pdTRUE){
if (xSemaphoreTakeFromISR(spi_dev->busy, &higherPriorityTaskWoken) != pdTRUE){
return -1;
}
#endif
if (woken) {
*woken = *woken || (higherPriorityTaskWoken == pdTRUE);
}
return 0;
#else
if (woken) {
*woken = false;
}
return PIOS_SPI_ClaimBus(spi_id);
#endif
}
/**
* Release the SPI bus semaphore. Calling the SPI functions does not require this
* \param[in] spi SPI number (0 or 1)
@ -297,6 +319,11 @@ int32_t PIOS_SPI_ReleaseBus(uint32_t spi_id)
PIOS_Assert(valid)
xSemaphoreGive(spi_dev->busy);
#else
struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id;
PIOS_IRQ_Disable();
spi_dev->busy = 0;
PIOS_IRQ_Enable();
#endif
return 0;
}
@ -304,21 +331,33 @@ int32_t PIOS_SPI_ReleaseBus(uint32_t spi_id)
/**
* Release the SPI bus semaphore from ISR. Calling the SPI functions does not require this
* \param[in] spi SPI number (0 or 1)
* \param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
* task has is now eligible to run, else unchanged
* \return 0 if no error
*/
int32_t PIOS_SPI_ReleaseBusISR(uint32_t spi_id)
int32_t PIOS_SPI_ReleaseBusISR(uint32_t spi_id, bool *woken)
{
#if defined(PIOS_INCLUDE_FREERTOS)
struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id;
signed portBASE_TYPE higherPriorityTaskWoken = pdFALSE;
bool valid = PIOS_SPI_validate(spi_dev);
PIOS_Assert(valid)
xSemaphoreGiveFromISR(spi_dev->busy, NULL);
#endif
xSemaphoreGiveFromISR(spi_dev->busy, &higherPriorityTaskWoken);
if (woken) {
*woken = *woken || (higherPriorityTaskWoken == pdTRUE);
}
return 0;
#else
if (woken) {
*woken = false;
}
return PIOS_SPI_ReleaseBus(spi_id);
#endif
}
/**
* Controls the RC (Register Clock alias Chip Select) pin of a SPI port
* \param[in] spi SPI number (0 or 1)

2
flight/targets/boards/oplinkmini/board_hw_defs.c
View File

@ -582,8 +582,6 @@ const struct pios_ppm_cfg pios_ppm_main_cfg = {
#if defined(PIOS_INCLUDE_PPM_OUT)
#include <pios_ppm_out_priv.h>
uint32_t pios_ppm_id;
static const struct pios_tim_channel pios_tim_ppmout[] = {
{
.timer = TIM2,

5
flight/targets/boards/oplinkmini/firmware/pios_board.c
View File

@ -218,10 +218,9 @@ void PIOS_Board_Init(void) {
/* Initalize the RFM22B radio COM device. */
#if defined(PIOS_INCLUDE_RFM22B)
{
extern const struct pios_rfm22b_cfg * PIOS_BOARD_HW_DEFS_GetRfm22Cfg (uint32_t board_revision);
const struct pios_board_info * bdinfo = &pios_board_info_blob;
const struct pios_rfm22b_cfg *pios_rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev);
if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, pios_rfm22b_cfg->slave_num, pios_rfm22b_cfg)) {
const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev);
if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, rfm22b_cfg->slave_num, rfm22b_cfg)) {
PIOS_Assert(0);
}

30
flight/targets/boards/osd/firmware/pios_board.c
View File

@ -343,10 +343,10 @@ PIOS_FLASHFS_Logfs_Init(&fs_id, &flashfs_internal_cfg, &pios_internal_flash_driv
PIOS_Assert(0);
}
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN);
PIOS_Assert(rx_buffer);
uint8_t *gps_rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN);
PIOS_Assert(gps_rx_buffer);
if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id,
rx_buffer, PIOS_COM_GPS_RX_BUF_LEN,
gps_rx_buffer, PIOS_COM_GPS_RX_BUF_LEN,
NULL, 0)) {
PIOS_Assert(0);
}
@ -361,14 +361,14 @@ PIOS_FLASHFS_Logfs_Init(&fs_id, &flashfs_internal_cfg, &pios_internal_flash_driv
PIOS_DEBUG_Assert(0);
}
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_AUX_RX_BUF_LEN);
uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_AUX_TX_BUF_LEN);
PIOS_Assert(rx_buffer);
PIOS_Assert(tx_buffer);
uint8_t *aux_rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_AUX_RX_BUF_LEN);
uint8_t *aux_tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_AUX_TX_BUF_LEN);
PIOS_Assert(aux_rx_buffer);
PIOS_Assert(aux_tx_buffer);
if (PIOS_COM_Init(&pios_com_aux_id, &pios_usart_com_driver, pios_usart_aux_id,
rx_buffer, PIOS_COM_AUX_RX_BUF_LEN,
tx_buffer, PIOS_COM_AUX_TX_BUF_LEN)) {
aux_rx_buffer, PIOS_COM_AUX_RX_BUF_LEN,
aux_tx_buffer, PIOS_COM_AUX_TX_BUF_LEN)) {
PIOS_DEBUG_Assert(0);
}
}
@ -383,13 +383,13 @@ PIOS_FLASHFS_Logfs_Init(&fs_id, &flashfs_internal_cfg, &pios_internal_flash_driv
PIOS_Assert(0);
}
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN);
uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN);
PIOS_Assert(rx_buffer);
PIOS_Assert(tx_buffer);
uint8_t *telem_rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN);
uint8_t *telem_tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN);
PIOS_Assert(telem_rx_buffer);
PIOS_Assert(telem_tx_buffer);
if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id,
rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN,
tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) {
telem_rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN,
telem_tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) {
PIOS_Assert(0);
}
}

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

@ -269,7 +269,7 @@ static void PIOS_Board_configure_com(const struct pios_usart_cfg *usart_port_cfg
}
static void PIOS_Board_configure_dsm(const struct pios_usart_cfg *pios_usart_dsm_cfg, const struct pios_dsm_cfg *pios_dsm_cfg,
const struct pios_com_driver *pios_usart_com_driver,enum pios_dsm_proto *proto,
const struct pios_com_driver *usart_com_driver,enum pios_dsm_proto *proto,
ManualControlSettingsChannelGroupsOptions channelgroup,uint8_t *bind)
{
uint32_t pios_usart_dsm_id;
@ -278,7 +278,7 @@ static void PIOS_Board_configure_dsm(const struct pios_usart_cfg *pios_usart_dsm
}
uint32_t pios_dsm_id;
if (PIOS_DSM_Init(&pios_dsm_id, pios_dsm_cfg, pios_usart_com_driver,
if (PIOS_DSM_Init(&pios_dsm_id, pios_dsm_cfg, usart_com_driver,
pios_usart_dsm_id, *proto, *bind)) {
PIOS_Assert(0);
}
@ -290,11 +290,11 @@ static void PIOS_Board_configure_dsm(const struct pios_usart_cfg *pios_usart_dsm
pios_rcvr_group_map[channelgroup] = pios_dsm_rcvr_id;
}
static void PIOS_Board_configure_pwm(const struct pios_pwm_cfg *pios_pwm_cfg)
static void PIOS_Board_configure_pwm(const struct pios_pwm_cfg *pwm_cfg)
{
/* Set up the receiver port. Later this should be optional */
uint32_t pios_pwm_id;
PIOS_PWM_Init(&pios_pwm_id, pios_pwm_cfg);
PIOS_PWM_Init(&pios_pwm_id, pwm_cfg);
uint32_t pios_pwm_rcvr_id;
if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) {
@ -303,10 +303,10 @@ static void PIOS_Board_configure_pwm(const struct pios_pwm_cfg *pios_pwm_cfg)
pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id;
}
static void PIOS_Board_configure_ppm(const struct pios_ppm_cfg *pios_ppm_cfg)
static void PIOS_Board_configure_ppm(const struct pios_ppm_cfg *ppm_cfg)
{
uint32_t pios_ppm_id;
PIOS_PPM_Init(&pios_ppm_id, pios_ppm_cfg);
PIOS_PPM_Init(&pios_ppm_id, ppm_cfg);
uint32_t pios_ppm_rcvr_id;
if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) {
@ -667,10 +667,8 @@ void PIOS_Board_Init(void) {
break;
case HWSETTINGS_RADIOPORT_TELEMETRY:
{
extern const struct pios_rfm22b_cfg * PIOS_BOARD_HW_DEFS_GetRfm22Cfg (uint32_t board_revision);
const struct pios_board_info * bdinfo = &pios_board_info_blob;
const struct pios_rfm22b_cfg *pios_rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev);
if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, pios_rfm22b_cfg->slave_num, pios_rfm22b_cfg)) {
const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev);
if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, rfm22b_cfg->slave_num, rfm22b_cfg)) {
PIOS_Assert(0);
}

4
flight/targets/boards/revoproto/firmware/pios_board.c
View File

@ -339,7 +339,7 @@ static void PIOS_Board_configure_com(const struct pios_usart_cfg *usart_port_cfg
}
static void PIOS_Board_configure_dsm(const struct pios_usart_cfg *pios_usart_dsm_cfg, const struct pios_dsm_cfg *pios_dsm_cfg,
const struct pios_com_driver *pios_usart_com_driver,enum pios_dsm_proto *proto,
const struct pios_com_driver *usart_com_driver,enum pios_dsm_proto *proto,
ManualControlSettingsChannelGroupsOptions channelgroup,uint8_t *bind)
{
uint32_t pios_usart_dsm_id;
@ -348,7 +348,7 @@ static void PIOS_Board_configure_dsm(const struct pios_usart_cfg *pios_usart_dsm
}
uint32_t pios_dsm_id;
if (PIOS_DSM_Init(&pios_dsm_id, pios_dsm_cfg, pios_usart_com_driver,
if (PIOS_DSM_Init(&pios_dsm_id, pios_dsm_cfg, usart_com_driver,
pios_usart_dsm_id, *proto, *bind)) {
PIOS_Assert(0);
}

108
flight/uavobjects/eventdispatcher.c
View File

@ -68,11 +68,11 @@ struct PeriodicObjectListStruct {
typedef struct PeriodicObjectListStruct PeriodicObjectList;
// Private variables
static PeriodicObjectList* objList;
static xQueueHandle queue;
static xTaskHandle eventTaskHandle;
static xSemaphoreHandle mutex;
static EventStats stats;
static PeriodicObjectList* mObjList;
static xQueueHandle mQueue;
static xTaskHandle mEventTaskHandle;
static xSemaphoreHandle mMutex;
static EventStats mStats;
// Private functions
static int32_t processPeriodicUpdates();
@ -89,19 +89,19 @@ static uint16_t randomizePeriod(uint16_t periodMs);
int32_t EventDispatcherInitialize()
{
// Initialize variables
objList = NULL;
memset(&stats, 0, sizeof(EventStats));
mObjList = NULL;
memset(&mStats, 0, sizeof(EventStats));
// Create mutex
mutex = xSemaphoreCreateRecursiveMutex();
if (mutex == NULL)
// Create mMutex
mMutex = xSemaphoreCreateRecursiveMutex();
if (mMutex == NULL)
return -1;
// Create event queue
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(EventCallbackInfo));
mQueue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(EventCallbackInfo));
// Create task
xTaskCreate( eventTask, (signed char*)"Event", STACK_SIZE, NULL, TASK_PRIORITY, &eventTaskHandle );
xTaskCreate(eventTask, (signed char*)"Event", STACK_SIZE, NULL, TASK_PRIORITY, &mEventTaskHandle);
// Done
return 0;
@ -113,9 +113,9 @@ int32_t EventDispatcherInitialize()
*/
void EventGetStats(EventStats* statsOut)
{
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
memcpy(statsOut, &stats, sizeof(EventStats));
xSemaphoreGiveRecursive(mutex);
xSemaphoreTakeRecursive(mMutex, portMAX_DELAY);
memcpy(statsOut, &mStats, sizeof(EventStats));
xSemaphoreGiveRecursive(mMutex);
}
/**
@ -123,9 +123,9 @@ void EventGetStats(EventStats* statsOut)
*/
void EventClearStats()
{
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
memset(&stats, 0, sizeof(EventStats));
xSemaphoreGiveRecursive(mutex);
xSemaphoreTakeRecursive(mMutex, portMAX_DELAY);
memset(&mStats, 0, sizeof(EventStats));
xSemaphoreGiveRecursive(mMutex);
}
/**
@ -143,7 +143,7 @@ int32_t EventCallbackDispatch(UAVObjEvent* ev, UAVObjEventCallback cb)
evInfo.cb = cb;
evInfo.queue = 0;
// Push to queue
return xQueueSend(queue, &evInfo, 0); // will not block if queue is full
return xQueueSend(mQueue, &evInfo, 0); // will not block if queue is full
}
/**
@ -204,26 +204,26 @@ int32_t EventPeriodicQueueUpdate(UAVObjEvent* ev, xQueueHandle queue, uint16_t p
*/
static int32_t eventPeriodicCreate(UAVObjEvent* ev, UAVObjEventCallback cb, xQueueHandle queue, uint16_t periodMs)
{
PeriodicObjectList* objEntry;
PeriodicObjectList *objEntry;
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
xSemaphoreTakeRecursive(mMutex, portMAX_DELAY);
// Check that the object is not already connected
LL_FOREACH(objList, objEntry)
{
LL_FOREACH(mObjList, objEntry) {
if (objEntry->evInfo.cb == cb &&
objEntry->evInfo.queue == queue &&
objEntry->evInfo.ev.obj == ev->obj &&
objEntry->evInfo.ev.instId == ev->instId &&
objEntry->evInfo.ev.event == ev->event)
{
objEntry->evInfo.ev.event == ev->event) {
// Already registered, do nothing
xSemaphoreGiveRecursive(mutex);
xSemaphoreGiveRecursive(mMutex);
return -1;
}
}
// Create handle
objEntry = (PeriodicObjectList*)pvPortMalloc(sizeof(PeriodicObjectList));
if (objEntry == NULL) return -1;
if (objEntry == NULL) {
return -1;
}
objEntry->evInfo.ev.obj = ev->obj;
objEntry->evInfo.ev.instId = ev->instId;
objEntry->evInfo.ev.event = ev->event;
@ -232,9 +232,9 @@ static int32_t eventPeriodicCreate(UAVObjEvent* ev, UAVObjEventCallback cb, xQue
objEntry->updatePeriodMs = periodMs;
objEntry->timeToNextUpdateMs = randomizePeriod(periodMs); // avoid bunching of updates
// Add to list
LL_APPEND(objList, objEntry);
LL_APPEND(mObjList, objEntry);
// Release lock
xSemaphoreGiveRecursive(mutex);
xSemaphoreGiveRecursive(mMutex);
return 0;
}
@ -250,26 +250,24 @@ static int32_t eventPeriodicUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, xQue
{
PeriodicObjectList* objEntry;
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
xSemaphoreTakeRecursive(mMutex, portMAX_DELAY);
// Find object
LL_FOREACH(objList, objEntry)
{
LL_FOREACH(mObjList, objEntry) {
if (objEntry->evInfo.cb == cb &&
objEntry->evInfo.queue == queue &&
objEntry->evInfo.ev.obj == ev->obj &&
objEntry->evInfo.ev.instId == ev->instId &&
objEntry->evInfo.ev.event == ev->event)
{
objEntry->evInfo.ev.event == ev->event) {
// Object found, update period
objEntry->updatePeriodMs = periodMs;
objEntry->timeToNextUpdateMs = randomizePeriod(periodMs); // avoid bunching of updates
// Release lock
xSemaphoreGiveRecursive(mutex);
xSemaphoreGiveRecursive(mMutex);
return 0;
}
}
// If this point is reached the object was not found
xSemaphoreGiveRecursive(mutex);
xSemaphoreGiveRecursive(mMutex);
return -1;
}
@ -283,7 +281,7 @@ static void eventTask()
EventCallbackInfo evInfo;
/* Must do this in task context to ensure that TaskMonitor has already finished its init */
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_EVENTDISPATCHER, eventTaskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_EVENTDISPATCHER, mEventTaskHandle);
// Initialize time
timeToNextUpdateMs = xTaskGetTickCount()*portTICK_RATE_MS;
@ -295,7 +293,7 @@ static void eventTask()
delayMs = timeToNextUpdateMs - (xTaskGetTickCount() * portTICK_RATE_MS);
// Wait for queue message
if ( xQueueReceive(queue, &evInfo, delayMs/portTICK_RATE_MS) == pdTRUE )
if ( xQueueReceive(mQueue, &evInfo, delayMs/portTICK_RATE_MS) == pdTRUE )
{
// Invoke callback, if one
if ( evInfo.cb != 0)
@ -324,49 +322,43 @@ static int32_t processPeriodicUpdates()
int32_t offset;
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
xSemaphoreTakeRecursive(mMutex, portMAX_DELAY);
// Iterate through each object and update its timer, if zero then transmit object.
// Also calculate smallest delay to next update.
timeToNextUpdate = xTaskGetTickCount()*portTICK_RATE_MS + MAX_UPDATE_PERIOD_MS;
LL_FOREACH(objList, objEntry)
{
LL_FOREACH(mObjList, objEntry) {
// If object is configured for periodic updates
if (objEntry->updatePeriodMs > 0)
{
if (objEntry->updatePeriodMs > 0) {
// Check if time for the next update
timeNow = xTaskGetTickCount()*portTICK_RATE_MS;
if (objEntry->timeToNextUpdateMs <= timeNow)
{
if (objEntry->timeToNextUpdateMs <= timeNow) {
// Reset timer
offset = ( timeNow - objEntry->timeToNextUpdateMs ) % objEntry->updatePeriodMs;
offset = (timeNow - objEntry->timeToNextUpdateMs) % objEntry->updatePeriodMs;
objEntry->timeToNextUpdateMs = timeNow + objEntry->updatePeriodMs - offset;
// Invoke callback, if one
if ( objEntry->evInfo.cb != 0)
{
if (objEntry->evInfo.cb != 0) {
objEntry->evInfo.cb(&objEntry->evInfo.ev); // the function is expected to copy the event information
}
// 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 (objEntry->evInfo.ev.obj != NULL)
stats.lastErrorID = UAVObjGetID(objEntry->evInfo.ev.obj);
++stats.eventErrors;
if (objEntry->evInfo.queue != 0) {
if (xQueueSend(objEntry->evInfo.queue, &objEntry->evInfo.ev, 0) != pdTRUE ) { // do not block if queue is full
if (objEntry->evInfo.ev.obj != NULL) {
mStats.lastErrorID = UAVObjGetID(objEntry->evInfo.ev.obj);
}
++mStats.eventErrors;
}
}
}
// Update minimum delay
if (objEntry->timeToNextUpdateMs < timeToNextUpdate)
{
if (objEntry->timeToNextUpdateMs < timeToNextUpdate) {
timeToNextUpdate = objEntry->timeToNextUpdateMs;
}
}
}
// Done
xSemaphoreGiveRecursive(mutex);
xSemaphoreGiveRecursive(mMutex);
return timeToNextUpdate;
}

1
make/common-defs.mk
View File

@ -120,6 +120,7 @@ CFLAGS += -mapcs-frame
CFLAGS += -fomit-frame-pointer
CFLAGS += -Wall -Wextra
CFLAGS += -Wfloat-equal -Wunsuffixed-float-constants -Wdouble-promotion
CFLAGS += -Wshadow
CFLAGS += -Werror
CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
CFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))