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

OP-951: Adds -Wshadow to flight CFLAGS and fixes resulting compilation breakage.

Browse Source 
+review OPReview
This commit is contained in:
Richard Flay (Hyper) 2013-05-14 07:01:45 +09:30
parent b76df471ee
commit ed68fbe68d
23 changed files with 289 additions and 312 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);

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

@ -800,45 +800,45 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
const float DEADBAND_HIGH = 0.55f;
const float DEADBAND_LOW = 0.45f;
static portTickType lastSysTime;
static portTickType lastSysTimeAH;
static bool zeroed = false;
portTickType thisSysTime;
float dT;
AltitudeHoldDesiredData altitudeHoldDesired;
AltitudeHoldDesiredGet(&altitudeHoldDesired);
AltitudeHoldDesiredData altitudeHoldDesiredData;
AltitudeHoldDesiredGet(&altitudeHoldDesiredData);
StabilizationSettingsData stabSettings;
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) {
altitudeHoldDesired.Altitude += (cmd->Throttle - DEADBAND_HIGH) * dT;
altitudeHoldDesiredData.Altitude += (cmd->Throttle - DEADBAND_HIGH) * dT;
} else if (cmd->Throttle < DEADBAND_LOW && zeroed) {
altitudeHoldDesired.Altitude += (cmd->Throttle - DEADBAND_LOW) * dT;
altitudeHoldDesiredData.Altitude += (cmd->Throttle - DEADBAND_LOW) * 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;
}

8
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;
}

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

@ -93,13 +93,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;
}

8
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;
}

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);

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);
}

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 $<))))