rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-18 08:54:15 +01:00
Code Issues Releases Activity

Merge branch 'next' into brian/support_inverted_USB_vsense

Browse Source
This commit is contained in:
Richard Flay (Hyper) 2013-05-17 05:54:42 +09:30
commit a6aedee9fa
31 changed files with 598 additions and 457 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], void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
float Q[NUMW], float dT, float P[NUMX][NUMX]); float Q[NUMW], float dT, float P[NUMX][NUMX]);
void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV], 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); uint16_t SensorsUsed);
void RungeKutta(float X[NUMX], float U[NUMU], float dT); void RungeKutta(float X[NUMX], float U[NUMU], float dT);
void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]); 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]); void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]);
// Private variables // Private variables
float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices struct EKFData {
// global to init to zero and maintain zero elements // linearized system matrices
float Be[3]; // local magnetic unit vector in NED frame float F[NUMX][NUMX];
float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector float G[NUMX][NUMW];
float Q[NUMW], R[NUMV]; // input noise and measurement noise variances float H[NUMV][NUMX];
float K[NUMX][NUMV]; // feedback gain matrix // 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 // Global variables
struct NavStruct Nav; struct NavStruct Nav;
@ -79,52 +89,52 @@ uint16_t ins_get_num_states()
void INSGPSInit() //pretty much just a place holder for now void INSGPSInit() //pretty much just a place holder for now
{ {
Be[0] = 1.0f; ekf.Be[0] = 1.0f;
Be[1] = 0.0f; ekf.Be[1] = 0.0f;
Be[2] = 0.0f; // local magnetic unit vector ekf.Be[2] = 0.0f; // local magnetic unit vector
for (int i = 0; i < NUMX; i++) { for (int i = 0; i < NUMX; i++) {
for (int j = 0; j < NUMX; j++) { for (int j = 0; j < NUMX; j++) {
P[i][j] = 0.0f; // zero all terms ekf.P[i][j] = 0.0f; // zero all terms
F[i][j] = 0.0f; ekf.F[i][j] = 0.0f;
} }
for (int j = 0; j < NUMW; j++) 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++) { for (int j = 0; j < NUMV; j++) {
H[j][i] = 0.0f; ekf.H[j][i] = 0.0f;
K[i][j] = 0.0f; ekf.K[i][j] = 0.0f;
} }
X[i] = 0.0f; ekf.X[i] = 0.0f;
} }
for (int i = 0; i < NUMW; i++) for (int i = 0; i < NUMW; i++)
Q[i] = 0.0f; ekf.Q[i] = 0.0f;
for (int i = 0; i < NUMV; i++) 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) ekf.P[0][0] = ekf.P[1][1] = ekf.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 ekf.P[3][3] = ekf.P[4][4] = ekf.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 ekf.P[6][6] = ekf.P[7][7] = ekf.P[8][8] = ekf.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[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) 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)
X[6] = 1.0f; ekf.X[6] = 1.0f;
X[7] = X[8] = X[9] = 0.0f; // initial quaternion (level and North) (m/s) ekf.X[7] = ekf.X[8] = ekf.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[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 ekf.Q[0] = ekf.Q[1] = ekf.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 ekf.Q[3] = ekf.Q[4] = ekf.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[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) ekf.R[0] = ekf.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) ekf.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 ekf.R[3] = ekf.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 ekf.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 ekf.R[6] = ekf.R[7] = ekf.R[8] = 0.005f; // magnetometer unit vector noise variance
R[9] = .25f; // High freq altimeter noise variance (m^2) ekf.R[9] = .25f; // High freq altimeter noise variance (m^2)
} }
void INSResetP(float PDiag[NUMX]) void INSResetP(float PDiag[NUMX])
@ -135,8 +145,8 @@ void INSResetP(float PDiag[NUMX])
for (i=0;i<NUMX;i++){ for (i=0;i<NUMX;i++){
if (PDiag != 0){ if (PDiag != 0){
for (j=0;j<NUMX;j++) for (j=0;j<NUMX;j++)
P[i][j]=P[j][i]=0.0f; ekf.P[i][j]=ekf.P[j][i]=0.0f;
P[i][i]=PDiag[i]; ekf.P[i][i]=PDiag[i];
} }
} }
} }
@ -148,7 +158,7 @@ void INSGetP(float PDiag[NUMX])
// retrieve diagonal elements (aka state variance) // retrieve diagonal elements (aka state variance)
for (i=0;i<NUMX;i++){ for (i=0;i<NUMX;i++){
if (PDiag != 0){ 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]) 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 */ /* Note: accel_bias not used in 13 state INS */
X[0] = pos[0]; ekf.X[0] = pos[0];
X[1] = pos[1]; ekf.X[1] = pos[1];
X[2] = pos[2]; ekf.X[2] = pos[2];
X[3] = vel[0]; ekf.X[3] = vel[0];
X[4] = vel[1]; ekf.X[4] = vel[1];
X[5] = vel[2]; ekf.X[5] = vel[2];
X[6] = q[0]; ekf.X[6] = q[0];
X[7] = q[1]; ekf.X[7] = q[1];
X[8] = q[2]; ekf.X[8] = q[2];
X[9] = q[3]; ekf.X[9] = q[3];
X[10] = gyro_bias[0]; ekf.X[10] = gyro_bias[0];
X[11] = gyro_bias[1]; ekf.X[11] = gyro_bias[1];
X[12] = gyro_bias[2]; ekf.X[12] = gyro_bias[2];
} }
void INSPosVelReset(float pos[3], float vel[3]) void INSPosVelReset(float pos[3], float vel[3])
{ {
for (int i = 0; i < 6; i++) { for (int i = 0; i < 6; i++) {
for(int j = i; j < NUMX; j++) { for(int j = i; j < NUMX; j++) {
P[i][j] = 0; // zero the first 6 rows and columns ekf.P[i][j] = 0; // zero the first 6 rows and columns
P[j][i] = 0; ekf.P[j][i] = 0;
} }
} }
P[0][0] = P[1][1] = P[2][2] = 25; // initial position variance (m^2) ekf.P[0][0] = ekf.P[1][1] = ekf.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[3][3] = ekf.P[4][4] = ekf.P[5][5] = 5; // initial velocity variance (m/s)^2
X[0] = pos[0]; ekf.X[0] = pos[0];
X[1] = pos[1]; ekf.X[1] = pos[1];
X[2] = pos[2]; ekf.X[2] = pos[2];
X[3] = vel[0]; ekf.X[3] = vel[0];
X[4] = vel[1]; ekf.X[4] = vel[1];
X[5] = vel[2]; ekf.X[5] = vel[2];
} }
void INSSetPosVelVar(float PosVar[3], float VelVar[3]) void INSSetPosVelVar(float PosVar[3], float VelVar[3])
{ {
R[0] = PosVar[0]; ekf.R[0] = PosVar[0];
R[1] = PosVar[1]; ekf.R[1] = PosVar[1];
R[2] = PosVar[2]; ekf.R[2] = PosVar[2];
R[3] = VelVar[0]; ekf.R[3] = VelVar[0];
R[4] = VelVar[1]; ekf.R[4] = VelVar[1];
R[5] = VelVar[2]; ekf.R[5] = VelVar[2];
} }
void INSSetGyroBias(float gyro_bias[3]) void INSSetGyroBias(float gyro_bias[3])
{ {
X[10] = gyro_bias[0]; ekf.X[10] = gyro_bias[0];
X[11] = gyro_bias[1]; ekf.X[11] = gyro_bias[1];
X[12] = gyro_bias[2]; ekf.X[12] = gyro_bias[2];
} }
void INSSetAccelVar(float accel_var[3]) void INSSetAccelVar(float accel_var[3])
{ {
Q[3] = accel_var[0]; ekf.Q[3] = accel_var[0];
Q[4] = accel_var[1]; ekf.Q[4] = accel_var[1];
Q[5] = accel_var[2]; ekf.Q[5] = accel_var[2];
} }
void INSSetGyroVar(float gyro_var[3]) void INSSetGyroVar(float gyro_var[3])
{ {
Q[0] = gyro_var[0]; ekf.Q[0] = gyro_var[0];
Q[1] = gyro_var[1]; ekf.Q[1] = gyro_var[1];
Q[2] = gyro_var[2]; ekf.Q[2] = gyro_var[2];
} }
void INSSetGyroBiasVar(float gyro_bias_var[3]) void INSSetGyroBiasVar(float gyro_bias_var[3])
{ {
Q[6] = gyro_bias_var[0]; ekf.Q[6] = gyro_bias_var[0];
Q[7] = gyro_bias_var[1]; ekf.Q[7] = gyro_bias_var[1];
Q[8] = gyro_bias_var[2]; ekf.Q[8] = gyro_bias_var[2];
} }
void INSSetMagVar(float scaled_mag_var[3]) void INSSetMagVar(float scaled_mag_var[3])
{ {
R[6] = scaled_mag_var[0]; ekf.R[6] = scaled_mag_var[0];
R[7] = scaled_mag_var[1]; ekf.R[7] = scaled_mag_var[1];
R[8] = scaled_mag_var[2]; ekf.R[8] = scaled_mag_var[2];
} }
void INSSetBaroVar(float baro_var) void INSSetBaroVar(float baro_var)
{ {
R[9] = baro_var; ekf.R[9] = baro_var;
} }
void INSSetMagNorth(float B[3]) void INSSetMagNorth(float B[3])
{ {
float mag = sqrtf(B[0] * B[0] + B[1] * B[1] + B[2] * B[2]); float mag = sqrtf(B[0] * B[0] + B[1] * B[1] + B[2] * B[2]);
Be[0] = B[0] / mag; ekf.Be[0] = B[0] / mag;
Be[1] = B[1] / mag; ekf.Be[1] = B[1] / mag;
Be[2] = B[2] / mag; ekf.Be[2] = B[2] / mag;
} }
void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT) 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]; U[5] = accel_data[2];
// EKF prediction step // EKF prediction step
LinearizeFG(X, U, F, G); LinearizeFG(ekf.X, U, ekf.F, ekf.G);
RungeKutta(X, U, dT); RungeKutta(ekf.X, U, dT);
qmag = sqrtf(X[6] * X[6] + X[7] * X[7] + X[8] * X[8] + X[9] * X[9]); 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]);
X[6] /= qmag; ekf.X[6] /= qmag;
X[7] /= qmag; ekf.X[7] /= qmag;
X[8] /= qmag; ekf.X[8] /= qmag;
X[9] /= qmag; ekf.X[9] /= qmag;
//CovariancePrediction(F,G,Q,dT,P); //CovariancePrediction(ekf.F,ekf.G,ekf.Q,dT,ekf.P);
// Update Nav solution structure // Update Nav solution structure
Nav.Pos[0] = X[0]; Nav.Pos[0] = ekf.X[0];
Nav.Pos[1] = X[1]; Nav.Pos[1] = ekf.X[1];
Nav.Pos[2] = X[2]; Nav.Pos[2] = ekf.X[2];
Nav.Vel[0] = X[3]; Nav.Vel[0] = ekf.X[3];
Nav.Vel[1] = X[4]; Nav.Vel[1] = ekf.X[4];
Nav.Vel[2] = X[5]; Nav.Vel[2] = ekf.X[5];
Nav.q[0] = X[6]; Nav.q[0] = ekf.X[6];
Nav.q[1] = X[7]; Nav.q[1] = ekf.X[7];
Nav.q[2] = X[8]; Nav.q[2] = ekf.X[8];
Nav.q[3] = X[9]; Nav.q[3] = ekf.X[9];
Nav.gyro_bias[0] = X[10]; Nav.gyro_bias[0] = ekf.X[10];
Nav.gyro_bias[1] = X[11]; Nav.gyro_bias[1] = ekf.X[11];
Nav.gyro_bias[2] = X[12]; Nav.gyro_bias[2] = ekf.X[12];
} }
void INSCovariancePrediction(float dT) 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 }; 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; Z[9] = BaroAlt;
// EKF correction step // EKF correction step
LinearizeH(X, Be, H); LinearizeH(ekf.X, ekf.Be, ekf.H);
MeasurementEq(X, Be, Y); MeasurementEq(ekf.X, ekf.Be, Y);
SerialUpdate(H, R, Z, Y, P, X, SensorsUsed); SerialUpdate(ekf.H, ekf.R, Z, Y, ekf.P, ekf.X, ekf.K, SensorsUsed);
qmag = sqrtf(X[6] * X[6] + X[7] * X[7] + X[8] * X[8] + X[9] * X[9]); 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]);
X[6] /= qmag; ekf.X[6] /= qmag;
X[7] /= qmag; ekf.X[7] /= qmag;
X[8] /= qmag; ekf.X[8] /= qmag;
X[9] /= qmag; ekf.X[9] /= qmag;
// Update Nav solution structure // Update Nav solution structure
Nav.Pos[0] = X[0]; Nav.Pos[0] = ekf.X[0];
Nav.Pos[1] = X[1]; Nav.Pos[1] = ekf.X[1];
Nav.Pos[2] = X[2]; Nav.Pos[2] = ekf.X[2];
Nav.Vel[0] = X[3]; Nav.Vel[0] = ekf.X[3];
Nav.Vel[1] = X[4]; Nav.Vel[1] = ekf.X[4];
Nav.Vel[2] = X[5]; Nav.Vel[2] = ekf.X[5];
Nav.q[0] = X[6]; Nav.q[0] = ekf.X[6];
Nav.q[1] = X[7]; Nav.q[1] = ekf.X[7];
Nav.q[2] = X[8]; Nav.q[2] = ekf.X[8];
Nav.q[3] = X[9]; Nav.q[3] = ekf.X[9];
Nav.gyro_bias[0] = X[10]; Nav.gyro_bias[0] = ekf.X[10];
Nav.gyro_bias[1] = X[11]; Nav.gyro_bias[1] = ekf.X[11];
Nav.gyro_bias[2] = X[12]; Nav.gyro_bias[2] = ekf.X[12];
} }
// ************* CovariancePrediction ************* // ************* 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], 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) uint16_t SensorsUsed)
{ {
float HP[NUMX], HPHR, Error; float HP[NUMX], HPHR, Error;

4
flight/libraries/op_dfu.c
View File

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

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

@ -103,7 +103,9 @@ int32_t ActuatorStart()
// Start main task // Start main task
xTaskCreate(actuatorTask, (signed char*)"Actuator", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); xTaskCreate(actuatorTask, (signed char*)"Actuator", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ACTUATOR, taskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ACTUATOR, taskHandle);
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_ACTUATOR); PIOS_WDG_RegisterFlag(PIOS_WDG_ACTUATOR);
#endif
return 0; return 0;
} }
@ -185,7 +187,9 @@ static void actuatorTask(__attribute__((unused)) void* parameters)
lastSysTime = xTaskGetTickCount(); lastSysTime = xTaskGetTickCount();
while (1) while (1)
{ {
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_ACTUATOR); PIOS_WDG_UpdateFlag(PIOS_WDG_ACTUATOR);
#endif
// Wait until the ActuatorDesired object is updated // Wait until the ActuatorDesired object is updated
uint8_t rc = xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS); uint8_t rc = xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS);

2
flight/modules/Fault/Fault.c
View File

@ -112,7 +112,7 @@ static int32_t fault_start(void)
} }
MODULE_INITCALL(fault_initialize, fault_start) MODULE_INITCALL(fault_initialize, fault_start)
static void fault_task(void *parameters) static void fault_task(__attribute__((unused))void *parameters)
{ {
switch (active_fault) { switch (active_fault) {
case FAULTSETTINGS_ACTIVATEFAULT_RUNAWAYTASK: case FAULTSETTINGS_ACTIVATEFAULT_RUNAWAYTASK:

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 */ /* Note: Cant just wait timeout value, because first time is randomized */
reset_count = 0; reset_count = 0;
lastResetSysTime = xTaskGetTickCount(); lastResetSysTime = xTaskGetTickCount();
UAVObjEvent * ev = pvPortMalloc(sizeof(UAVObjEvent)); UAVObjEvent *event = pvPortMalloc(sizeof(UAVObjEvent));
memset(ev,0,sizeof(UAVObjEvent)); memset(event, 0, sizeof(UAVObjEvent));
EventPeriodicCallbackCreate(ev, resetTask, 100); EventPeriodicCallbackCreate(event, resetTask, 100);
iap_state = IAP_STATE_RESETTING; iap_state = IAP_STATE_RESETTING;
} else { } else {
iap_state = IAP_STATE_READY; iap_state = IAP_STATE_READY;

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

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

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

@ -138,7 +138,9 @@ int32_t ManualControlStart()
// Start main task // Start main task
xTaskCreate(manualControlTask, (signed char *) "ManualControl", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); xTaskCreate(manualControlTask, (signed char *) "ManualControl", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_MANUALCONTROL, taskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_MANUALCONTROL, taskHandle);
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL); PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL);
#endif
return 0; return 0;
} }
@ -203,12 +205,16 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
// Main task loop // Main task loop
lastSysTime = xTaskGetTickCount(); lastSysTime = xTaskGetTickCount();
float scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM] = {0};
while (1) { while (1) {
float scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM];
// Wait until next update // Wait until next update
vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS); vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS);
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL); PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL);
#endif
// Read settings // Read settings
ManualControlSettingsGet(&settings); ManualControlSettingsGet(&settings);
@ -806,13 +812,13 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
uint8_t throttleRate; uint8_t throttleRate;
uint8_t throttleExp; uint8_t throttleExp;
static portTickType lastSysTime; static portTickType lastSysTimeAH;
static bool zeroed = false; static bool zeroed = false;
portTickType thisSysTime; portTickType thisSysTime;
float dT; float dT;
AltitudeHoldDesiredData altitudeHoldDesired; AltitudeHoldDesiredData altitudeHoldDesiredData;
AltitudeHoldDesiredGet(&altitudeHoldDesired); AltitudeHoldDesiredGet(&altitudeHoldDesiredData);
AltitudeHoldSettingsThrottleExpGet(&throttleExp); AltitudeHoldSettingsThrottleExpGet(&throttleExp);
AltitudeHoldSettingsThrottleRateGet(&throttleRate); AltitudeHoldSettingsThrottleRateGet(&throttleRate);
@ -821,36 +827,36 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
StabilizationSettingsGet(&stabSettings); StabilizationSettingsGet(&stabSettings);
thisSysTime = xTaskGetTickCount(); thisSysTime = xTaskGetTickCount();
dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f); dT = ((thisSysTime == lastSysTimeAH)? 0.001f : (thisSysTime - lastSysTimeAH) * portTICK_RATE_MS * 0.001f);
lastSysTime = thisSysTime; lastSysTimeAH = thisSysTime;
altitudeHoldDesired.Roll = cmd->Roll * stabSettings.RollMax; altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax;
altitudeHoldDesired.Pitch = cmd->Pitch * stabSettings.PitchMax; altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax;
altitudeHoldDesired.Yaw = cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW];
float currentDown; float currentDown;
PositionActualDownGet(&currentDown); PositionActualDownGet(&currentDown);
if(changed) { if(changed) {
// After not being in this mode for a while init at current height // After not being in this mode for a while init at current height
altitudeHoldDesired.Altitude = 0; altitudeHoldDesiredData.Altitude = 0;
zeroed = false; zeroed = false;
} else if (cmd->Throttle > DEADBAND_HIGH && zeroed) { } 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 // 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 // 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) { } 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) { } else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH) {
// Require the stick to enter the dead band before they can move height // Require the stick to enter the dead band before they can move height
zeroed = true; zeroed = true;
} }
AltitudeHoldDesiredSet(&altitudeHoldDesired); AltitudeHoldDesiredSet(&altitudeHoldDesiredData);
} }
#else #else
// TODO: These functions should never be accessible on CC. Any configuration that // 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 // in flight
static void updatePathDesired(__attribute__((unused)) ManualControlCommandData * cmd, static void updatePathDesired(__attribute__((unused)) ManualControlCommandData * cmd,
__attribute__((unused)) bool changed, __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) void write_char16(char ch, unsigned int x, unsigned int y, int font)
{ {
unsigned int yy, addr_temp, row, row_temp, xshift; 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; struct FontEntry font_info;
//char lookup = 0; //char lookup = 0;
fetch_font_info(0, font, &font_info, NULL); 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; addr = addr_temp;
for (yy = y; yy < y + font_info.height; yy++) { for (yy = y; yy < y + font_info.height; yy++) {
if (font == 3) { if (font == 3) {
level_bits = font_frame12x18[row]; levels = font_frame12x18[row];
//if(!(flags & FONT_INVERT)) // data is normally inverted //if(!(flags & FONT_INVERT)) // data is normally inverted
level_bits = ~level_bits; levels = ~levels;
or_mask = font_mask12x18[row] << xshift; or_mask = font_mask12x18[row] << xshift;
and_mask = (font_mask12x18[row] & level_bits) << xshift; and_mask = (font_mask12x18[row] & levels) << xshift;
} else { } else {
level_bits = font_frame8x10[row]; levels = font_frame8x10[row];
//if(!(flags & FONT_INVERT)) // data is normally inverted //if(!(flags & FONT_INVERT)) // data is normally inverted
level_bits = ~level_bits; levels = ~levels;
or_mask = font_mask8x10[row] << xshift; 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); write_word_misaligned_OR(draw_buffer_level, or_mask, addr, wbit);
// If we're not bold write the AND mask. // 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) void write_char(char ch, unsigned int x, unsigned int y, int flags, int font)
{ {
unsigned int yy, addr_temp, row, row_temp, xshift; 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; struct FontEntry font_info;
char lookup = 0; char lookup = 0;
fetch_font_info(ch, font, &font_info, &lookup); 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; row = row_temp;
addr = addr_temp; addr = addr_temp;
for (yy = y; yy < y + font_info.height; yy++) { 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)) { if (!(flags & FONT_INVERT)) {
// data is normally inverted // data is normally inverted
level_bits = ~level_bits; levels = ~levels;
} }
or_mask = font_info.data[row] << xshift; 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); write_word_misaligned_OR(draw_buffer_level, or_mask, addr, wbit);
// If we're not bold write the AND mask. // If we're not bold write the AND mask.
//if(!(flags & FONT_BOLD)) //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, // use local variables, dont use stack since this is huge and a callback,
// dont use the globals because we cant use mutexes here // dont use the globals because we cant use mutexes here
static WaypointActiveData waypointActive; static WaypointActiveData waypointActiveData;
static PathActionData pathAction; static PathActionData pathActionData;
static WaypointData waypoint; static WaypointData waypointData;
static PathDesiredData pathDesired; static PathDesiredData pathDesired;
// find out current waypoint // find out current waypoint
WaypointActiveGet(&waypointActive); WaypointActiveGet(&waypointActiveData);
WaypointInstGet(waypointActive.Index,&waypoint); WaypointInstGet(waypointActiveData.Index,&waypointData);
PathActionInstGet(waypoint.Action, &pathAction); PathActionInstGet(waypointData.Action, &pathActionData);
pathDesired.End[PATHDESIRED_END_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH]; pathDesired.End[PATHDESIRED_END_NORTH] = waypointData.Position[WAYPOINT_POSITION_NORTH];
pathDesired.End[PATHDESIRED_END_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST]; pathDesired.End[PATHDESIRED_END_EAST] = waypointData.Position[WAYPOINT_POSITION_EAST];
pathDesired.End[PATHDESIRED_END_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN]; pathDesired.End[PATHDESIRED_END_DOWN] = waypointData.Position[WAYPOINT_POSITION_DOWN];
pathDesired.EndingVelocity = waypoint.Velocity; pathDesired.EndingVelocity = waypointData.Velocity;
pathDesired.Mode = pathAction.Mode; pathDesired.Mode = pathActionData.Mode;
pathDesired.ModeParameters[0] = pathAction.ModeParameters[0]; pathDesired.ModeParameters[0] = pathActionData.ModeParameters[0];
pathDesired.ModeParameters[1] = pathAction.ModeParameters[1]; pathDesired.ModeParameters[1] = pathActionData.ModeParameters[1];
pathDesired.ModeParameters[2] = pathAction.ModeParameters[2]; pathDesired.ModeParameters[2] = pathActionData.ModeParameters[2];
pathDesired.ModeParameters[3] = pathAction.ModeParameters[3]; pathDesired.ModeParameters[3] = pathActionData.ModeParameters[3];
pathDesired.UID = waypointActive.Index; pathDesired.UID = waypointActiveData.Index;
if(waypointActive.Index == 0) { if(waypointActiveData.Index == 0) {
PositionActualData positionActual; PositionActualData positionActual;
PositionActualGet(&positionActual); PositionActualGet(&positionActual);
// First waypoint has itself as start point (used to be home position but that proved dangerous when looping) // 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; pathDesired.StartingVelocity = waypointPrev.Velocity;
} }
PathDesiredSet(&pathDesired); PathDesiredSet(&pathDesired);
} }
// helper function to go to a specific waypoint // 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 Rz = homeLocation.Be[2];
const float rate = cal.MagBiasNullingRate; const float rate = cal.MagBiasNullingRate;
float R[3][3]; float Rot[3][3];
float B_e[3]; float B_e[3];
float xy[2]; float xy[2];
float delta[3]; float delta[3];
@ -496,9 +496,9 @@ static void magOffsetEstimation(MagnetometerData *mag)
Quaternion2R(&attitude.q1, R); Quaternion2R(&attitude.q1, R);
// Rotate the mag into the NED frame // 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[0] = Rot[0][0] * mag->x + Rot[1][0] * mag->y + Rot[2][0] * mag->z;
B_e[1] = R[0][1] * mag->x + R[1][1] * mag->y + R[2][1] * mag->z; B_e[1] = Rot[0][1] * mag->x + Rot[1][1] * mag->y + Rot[2][1] * mag->z;
B_e[2] = R[0][2] * mag->x + R[1][2] * mag->y + R[2][2] * 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 cy = cosf(DEG2RAD(attitude.Yaw));
float sy = sinf(DEG2RAD(attitude.Yaw)); float sy = sinf(DEG2RAD(attitude.Yaw));

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

@ -113,7 +113,9 @@ int32_t StabilizationStart()
// Start main task // Start main task
xTaskCreate(stabilizationTask, (signed char*)"Stabilization", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); xTaskCreate(stabilizationTask, (signed char*)"Stabilization", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_STABILIZATION, taskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_STABILIZATION, taskHandle);
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_STABILIZATION); PIOS_WDG_RegisterFlag(PIOS_WDG_STABILIZATION);
#endif
return 0; return 0;
} }
@ -161,7 +163,9 @@ static void stabilizationTask(__attribute__((unused)) void* parameters)
while(1) { while(1) {
float dT; float dT;
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION); PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);
#endif
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE ) if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE )

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

@ -566,7 +566,6 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
StabilizationDesiredData stabDesired; StabilizationDesiredData stabDesired;
AttitudeActualData attitudeActual; AttitudeActualData attitudeActual;
NedAccelData nedAccel; NedAccelData nedAccel;
VtolPathFollowerSettingsData vtolpathfollowerSettings;
StabilizationSettingsData stabSettings; StabilizationSettingsData stabSettings;
SystemSettingsData systemSettings; SystemSettingsData systemSettings;
@ -580,8 +579,6 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
float downCommand; float downCommand;
SystemSettingsGet(&systemSettings); SystemSettingsGet(&systemSettings);
VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
VelocityActualGet(&velocityActual); VelocityActualGet(&velocityActual);
VelocityDesiredGet(&velocityDesired); VelocityDesiredGet(&velocityDesired);
StabilizationDesiredGet(&stabDesired); 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 * @brief Validate the handle to the spi device
* @returns 0 for valid device or -1 otherwise * @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; return -1;
if (dev->magic != PIOS_ADXL345_DEV_MAGIC) if (vdev->magic != PIOS_ADXL345_DEV_MAGIC)
return -2; return -2;
if (dev->spi_id == 0) if (vdev->spi_id == 0)
return -3; return -3;
return 0; return 0;
} }

73
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 * @brief Validate the handle to the spi device
* @returns 0 for valid device or -1 otherwise * @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; return -1;
if (dev->magic != PIOS_BMA180_DEV_MAGIC) if (vdev->magic != PIOS_BMA180_DEV_MAGIC)
return -2; return -2;
if (dev->spi_id == 0) if (vdev->spi_id == 0)
return -3; return -3;
return 0; 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() int32_t PIOS_BMA180_ClaimBus()
{ {
if(PIOS_BMA180_Validate(dev) != 0) if (PIOS_BMA180_Validate(dev) != 0) {
return -1; return -1;
}
if(PIOS_SPI_ClaimBus(dev->spi_id) != 0) { if (PIOS_SPI_ClaimBus(dev->spi_id) != 0) {
return -1; 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 * @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) if (PIOS_BMA180_Validate(dev) != 0) {
return -1;
if(PIOS_SPI_ClaimBusISR(dev->spi_id) != 0) {
return -1; 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; return 0;
} }
@ -162,13 +166,29 @@ int32_t PIOS_BMA180_ClaimBusISR()
* @return 0 if successful * @return 0 if successful
*/ */
int32_t PIOS_BMA180_ReleaseBus() 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) if(PIOS_BMA180_Validate(dev) != 0)
return -1; 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; int32_t bma180_irqs = 0;
bool PIOS_BMA180_IRQHandler(void) 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}; 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]; uint8_t pios_bma180_dmabuf[8];
bma180_irqs++;
// If we can't get the bus then just move on for efficiency // If we can't get the bus then just move on for efficiency
if(PIOS_BMA180_ClaimBusISR() != 0) { if (PIOS_BMA180_ClaimBusISR(&woken) != 0) {
return false; // Something else is using bus, miss this data 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, 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; struct pios_bma180_data data;
// Don't release bus till data has copied // Don't release bus till data has copied
PIOS_BMA180_ReleaseBus(); PIOS_BMA180_ReleaseBusISR(&woken);
// Must not return before releasing bus // Must not return before releasing bus
if(fifoBuf_getFree(&dev->fifo) < sizeof(data)) if (fifoBuf_getFree(&dev->fifo) < sizeof(data)) {
return false; return woken;
}
// Bottom two bits indicate new data and are constant zeros. Don't right // Bottom two bits indicate new data and are constant zeros. Don't right
// shift because it drops sign bit // shift because it drops sign bit
@ -471,7 +494,7 @@ bool PIOS_BMA180_IRQHandler(void)
fifoBuf_putData(&dev->fifo, (uint8_t *) &data, sizeof(data)); fifoBuf_putData(&dev->fifo, (uint8_t *) &data, sizeof(data));
return false; return woken;
} }
#endif /* PIOS_INCLUDE_BMA180 */ #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 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_SetReg(uint8_t address, uint8_t buffer);
static int32_t PIOS_L3GD20_GetReg(uint8_t address); 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_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_ReleaseBus();
static int32_t PIOS_L3GD20_ReleaseBusISR(bool *woken);
volatile bool l3gd20_configured = false; 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 * @brief Validate the handle to the spi device
* @returns 0 for valid device or -1 otherwise * @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; return -1;
if (dev->magic != PIOS_L3GD20_DEV_MAGIC) if (vdev->magic != PIOS_L3GD20_DEV_MAGIC)
return -2; return -2;
if (dev->spi_id == 0) if (vdev->spi_id == 0)
return -3; return -3;
return 0; 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 * @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 * @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; return -1;
}
if(PIOS_SPI_ClaimBusISR(dev->spi_id) != 0) if(PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) {
return -2; 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; return 0;
} }
@ -211,14 +215,28 @@ static int32_t PIOS_L3GD20_ClaimBusIsr()
*/ */
int32_t PIOS_L3GD20_ReleaseBus() int32_t PIOS_L3GD20_ReleaseBus()
{ {
if(PIOS_L3GD20_Validate(dev) != 0) if(PIOS_L3GD20_Validate(dev) != 0) {
return -1; 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_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 * @brief Read a register from L3GD20
* @returns The register value or -1 if failure to get bus * @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; 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 * @brief Writes one byte to the L3GD20
* \param[in] reg Register address * \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) bool PIOS_L3GD20_IRQHandler(void)
{ {
l3gd20_irq++; bool woken = false;
struct pios_l3gd20_data data; 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 buf[7] = {PIOS_L3GD20_GYRO_X_OUT_LSB | 0x80 | 0x40, 0, 0, 0, 0, 0, 0};
uint8_t rec[7]; uint8_t rec[7];
l3gd20_irq++;
/* This code duplicates ReadGyros above but uses ClaimBusIsr */ /* This code duplicates ReadGyros above but uses ClaimBusIsr */
if(PIOS_L3GD20_ClaimBusIsr() != 0) if (PIOS_L3GD20_ClaimBusISR(&woken) != 0) {
return false; return woken;
if(PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) {
PIOS_L3GD20_ReleaseBus();
return false;
} }
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); 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; signed portBASE_TYPE higherPriorityTaskWoken;
xQueueSendToBackFromISR(dev->queue, (void *) &data, &xHigherPriorityTaskWoken); xQueueSendToBackFromISR(dev->queue, (void *) &data, &higherPriorityTaskWoken);
return xHigherPriorityTaskWoken == pdTRUE; return (woken || higherPriorityTaskWoken == pdTRUE);
} }
#endif /* PIOS_INCLUDE_L3GD20 */ #endif /* PIOS_INCLUDE_L3GD20 */

170
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 * @brief Validate the handle to the spi device
* @returns 0 for valid device or -1 otherwise * @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; return -1;
if (dev->magic != PIOS_MPU6000_DEV_MAGIC) if (vdev->magic != PIOS_MPU6000_DEV_MAGIC)
return -2; return -2;
if (dev->spi_id == 0) if (vdev->spi_id == 0)
return -3; return -3;
return 0; return 0;
} }
@ -197,7 +197,12 @@ int32_t PIOS_MPU6000_ConfigureRanges(
{ {
if(dev == NULL) if(dev == NULL)
return -1; 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); PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_256);
// update filter settings // update filter settings
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_DLPF_CFG_REG, filterSetting) != 0); 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 * @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 * @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; return -1;
if(fromIsr){
if(PIOS_SPI_ClaimBusISR(dev->spi_id) != 0)
return -2;
} else {
if(PIOS_SPI_ClaimBus(dev->spi_id) != 0)
return -2;
} }
PIOS_SPI_RC_PinSet(dev->spi_id,dev->slave_num,0); if (PIOS_SPI_ClaimBus(dev->spi_id) != 0) {
return -2;
}
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; return 0;
} }
/** /**
* @brief Release the SPI bus for the accel communications and end the transaction * @brief Release the SPI bus for the accel communications and end the transaction
* @return 0 if successful * @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; 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; uint8_t data;
if(PIOS_MPU6000_ClaimBus(false) != 0) if(PIOS_MPU6000_ClaimBus() != 0) {
return -1; return -1;
}
PIOS_SPI_TransferByte(dev->spi_id,(0x80 | reg) ); // request byte PIOS_SPI_TransferByte(dev->spi_id,(0x80 | reg) ); // request byte
data = PIOS_SPI_TransferByte(dev->spi_id,0 ); // receive response data = PIOS_SPI_TransferByte(dev->spi_id,0 ); // receive response
PIOS_MPU6000_ReleaseBus(false); PIOS_MPU6000_ReleaseBus();
return data; 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) 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; return -1;
}
if(PIOS_SPI_TransferByte(dev->spi_id, 0x7f & reg) != 0) { if (PIOS_SPI_TransferByte(dev->spi_id, 0x7f & reg) != 0) {
PIOS_MPU6000_ReleaseBus(false); PIOS_MPU6000_ReleaseBus();
return -2; return -2;
} }
if(PIOS_SPI_TransferByte(dev->spi_id, data) != 0) { if (PIOS_SPI_TransferByte(dev->spi_id, data) != 0) {
PIOS_MPU6000_ReleaseBus(false); PIOS_MPU6000_ReleaseBus();
return -3; return -3;
} }
PIOS_MPU6000_ReleaseBus(false); PIOS_MPU6000_ReleaseBus();
return 0; 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 buf[7] = {PIOS_MPU6000_GYRO_X_OUT_MSB | 0x80, 0, 0, 0, 0, 0, 0};
uint8_t rec[7]; uint8_t rec[7];
if(PIOS_MPU6000_ClaimBus(false) != 0) if (PIOS_MPU6000_ClaimBus() != 0) {
return -1; 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; return -2;
}
PIOS_MPU6000_ReleaseBus(false);
PIOS_MPU6000_ReleaseBus();
data->gyro_x = rec[1] << 8 | rec[2]; data->gyro_x = rec[1] << 8 | rec[2];
data->gyro_y = rec[3] << 8 | rec[4]; data->gyro_y = rec[3] << 8 | rec[4];
@ -407,31 +440,34 @@ int32_t PIOS_MPU6000_Test(void)
} }
/** /**
* @brief Run self-test operation. * @brief Obtains the number of bytes in the FIFO. Call from ISR only.
* \return 0 if test succeeded * @return the number of bytes in the FIFO
* \return non-zero value if test succeeded * @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
* @param fromIsr[in] Tells if the function is being called from a ISR or not * 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_send_buf[3] = {PIOS_MPU6000_FIFO_CNT_MSB | 0x80, 0, 0};
uint8_t mpu6000_rec_buf[3]; uint8_t mpu6000_rec_buf[3];
if(PIOS_MPU6000_ClaimBus(fromIsr) != 0) if(PIOS_MPU6000_ClaimBusISR(woken) != 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);
return -1; 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]; 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; uint32_t mpu6000_irq = 0;
int32_t mpu6000_count; int32_t mpu6000_count;
@ -446,46 +482,50 @@ uint32_t mpu6000_transfer_size;
bool PIOS_MPU6000_IRQHandler(void) bool PIOS_MPU6000_IRQHandler(void)
{ {
bool woken = false;
static uint32_t timeval; static uint32_t timeval;
mpu6000_interval_us = PIOS_DELAY_DiffuS(timeval); mpu6000_interval_us = PIOS_DELAY_DiffuS(timeval);
timeval = PIOS_DELAY_GetRaw(); timeval = PIOS_DELAY_GetRaw();
if (!mpu6000_configured) if (!mpu6000_configured) {
return false; return false;
}
mpu6000_count = PIOS_MPU6000_FifoDepth(true); mpu6000_count = PIOS_MPU6000_FifoDepthISR(&woken);
if (mpu6000_count < (int32_t)sizeof(struct pios_mpu6000_data)) if (mpu6000_count < (int32_t)sizeof(struct pios_mpu6000_data)) {
return false; return woken;
}
if (PIOS_MPU6000_ClaimBus(true) != 0) if (PIOS_MPU6000_ClaimBusISR(&woken) != 0) {
return false; 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_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) ]; 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) { 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++; mpu6000_fails++;
return false; return woken;
} }
PIOS_MPU6000_ReleaseBus(true); PIOS_MPU6000_ReleaseBusISR(&woken);
static struct pios_mpu6000_data data; static struct pios_mpu6000_data data;
// In the case where extras samples backed up grabbed an extra // In the case where extras samples backed up grabbed an extra
if (mpu6000_count >= (int32_t)(sizeof(data) * 2)) { if (mpu6000_count >= (int32_t)(sizeof(data) * 2)) {
mpu6000_fifo_backup++; mpu6000_fifo_backup++;
if (PIOS_MPU6000_ClaimBus(true) != 0) if (PIOS_MPU6000_ClaimBusISR(&woken) != 0)
return false; return woken;
if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) { 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++; 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 // 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]; data.temperature = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2];
#endif #endif
portBASE_TYPE xHigherPriorityTaskWoken; signed portBASE_TYPE higherPriorityTaskWoken;
xQueueSendToBackFromISR(dev->queue, (void *) &data, &xHigherPriorityTaskWoken); xQueueSendToBackFromISR(dev->queue, (void *) &data, &higherPriorityTaskWoken);
mpu6000_irq++; mpu6000_irq++;
mpu6000_time_us = PIOS_DELAY_DiffuS(timeval); mpu6000_time_us = PIOS_DELAY_DiffuS(timeval);
return xHigherPriorityTaskWoken == pdTRUE; return (woken || higherPriorityTaskWoken == pdTRUE);
} }
#endif /* PIOS_INCLUDE_MPU6000 */ #endif /* PIOS_INCLUDE_MPU6000 */

2
flight/pios/inc/pios_rcvr_priv.h
View File

@ -33,8 +33,6 @@
#include <pios.h> #include <pios.h>
extern uint32_t pios_rcvr_max_channel;
extern int32_t PIOS_RCVR_Init(uint32_t * rcvr_id, const struct pios_rcvr_driver * driver, const uint32_t lower_id); extern int32_t PIOS_RCVR_Init(uint32_t * rcvr_id, const struct pios_rcvr_driver * driver, const uint32_t lower_id);
extern void PIOS_RCVR_IRQ_Handler(uint32_t rcvr_id); extern void PIOS_RCVR_IRQ_Handler(uint32_t rcvr_id);

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_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_Busy(uint32_t spi_id);
extern int32_t PIOS_SPI_ClaimBus(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_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_IRQ_Handler(uint32_t spi_id);
extern void PIOS_SPI_SetPrescalar(uint32_t spi_id, uint32_t prescalar); extern void PIOS_SPI_SetPrescalar(uint32_t spi_id, uint32_t prescalar);

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

@ -126,7 +126,7 @@ static void i2c_adapter_reset_bus(struct pios_i2c_adapter *i2c_adapter);
static void i2c_adapter_log_fault(enum pios_i2c_error_type type); static void i2c_adapter_log_fault(enum pios_i2c_error_type type);
const static struct i2c_adapter_transition i2c_adapter_transitions[I2C_STATE_NUM_STATES] = { static const struct i2c_adapter_transition i2c_adapter_transitions[I2C_STATE_NUM_STATES] = {
[I2C_STATE_FSM_FAULT] = { [I2C_STATE_FSM_FAULT] = {
.entry_fn = go_fsm_fault, .entry_fn = go_fsm_fault,
.next_state = { .next_state = {
@ -607,9 +607,6 @@ static void i2c_adapter_inject_event(struct pios_i2c_adapter *i2c_adapter, enum
* guarantee that the entry function never depends on the previous * guarantee that the entry function never depends on the previous
* state. This way, it cannot ever know what the previous state was. * state. This way, it cannot ever know what the previous state was.
*/ */
enum i2c_adapter_state prev_state = i2c_adapter->curr_state;
if (prev_state) ;
i2c_adapter->curr_state = i2c_adapter_transitions[i2c_adapter->curr_state].next_state[event]; i2c_adapter->curr_state = i2c_adapter_transitions[i2c_adapter->curr_state].next_state[event];
/* Call the entry function (if any) for the next state. */ /* Call the entry function (if any) for the next state. */
@ -626,10 +623,6 @@ static void i2c_adapter_inject_event(struct pios_i2c_adapter *i2c_adapter, enum
static void i2c_adapter_process_auto(struct pios_i2c_adapter *i2c_adapter) static void i2c_adapter_process_auto(struct pios_i2c_adapter *i2c_adapter)
{ {
PIOS_IRQ_Disable(); PIOS_IRQ_Disable();
enum i2c_adapter_state prev_state = i2c_adapter->curr_state;
if (prev_state) ;
while (i2c_adapter_transitions[i2c_adapter->curr_state].next_state[I2C_EVENT_AUTO]) { while (i2c_adapter_transitions[i2c_adapter->curr_state].next_state[I2C_EVENT_AUTO]) {
i2c_adapter->curr_state = i2c_adapter_transitions[i2c_adapter->curr_state].next_state[I2C_EVENT_AUTO]; i2c_adapter->curr_state = i2c_adapter_transitions[i2c_adapter->curr_state].next_state[I2C_EVENT_AUTO];
@ -659,7 +652,7 @@ static bool i2c_adapter_wait_for_stopped(struct pios_i2c_adapter *i2c_adapter)
* in spinning on this bit in the ISR forever. * in spinning on this bit in the ISR forever.
*/ */
#define I2C_CR1_STOP_REQUESTED 0x0200 #define I2C_CR1_STOP_REQUESTED 0x0200
for (guard = 1e6; /* FIXME: should use the configured bus timeout */ for (guard = 1000000; /* FIXME: should use the configured bus timeout */
guard && (i2c_adapter->cfg->regs->CR1 & I2C_CR1_STOP_REQUESTED); guard--) guard && (i2c_adapter->cfg->regs->CR1 & I2C_CR1_STOP_REQUESTED); guard--)
continue; continue;
if (!guard) { if (!guard) {

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 #define PIOS_PPM_IN_MAX_CHANNEL_PULSE_US 2250 // microseconds
/* Local Variables */ /* Local Variables */
static TIM_ICInitTypeDef TIM_ICInitStructure; //static TIM_ICInitTypeDef TIM_ICInitStructure;
static void PIOS_PPM_Supervisor(uint32_t ppm_id); 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; return -1;
} }
TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init;
/* Configure the channels to be in capture/compare mode */ /* Configure the channels to be in capture/compare mode */
for (uint8_t i = 0; i < cfg->num_channels; i++) { for (uint8_t i = 0; i < cfg->num_channels; i++) {
const struct pios_tim_channel * chan = &cfg->channels[i]; const struct pios_tim_channel * chan = &cfg->channels[i];
/* Configure timer for input capture */ /* Configure timer for input capture */
TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init;
TIM_ICInitStructure.TIM_Channel = chan->timer_chan; TIM_ICInitStructure.TIM_Channel = chan->timer_chan;
TIM_ICInit(chan->timer, &TIM_ICInitStructure); 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; ppm_dev->supv_timer = 0;
if (!PIOS_RTC_RegisterTickCallback(PIOS_PPM_Supervisor, (uint32_t)ppm_dev)) { if (!PIOS_RTC_RegisterTickCallback(PIOS_PPM_Supervisor, (uint32_t)ppm_dev)) {
PIOS_DEBUG_Assert(0); PIOS_DEBUG_Assert(0);

43
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. * Claim the SPI bus semaphore from an ISR. Has no timeout.
* \param[in] spi SPI number (0 or 1) * \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 0 if no error
* \return -1 if timeout before claiming semaphore * \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) #if defined(PIOS_INCLUDE_FREERTOS)
struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id;
signed portBASE_TYPE higherPriorityTaskWoken = pdFALSE;
bool valid = PIOS_SPI_validate(spi_dev); bool valid = PIOS_SPI_validate(spi_dev);
PIOS_Assert(valid) PIOS_Assert(valid)
if (xSemaphoreTakeFromISR(spi_dev->busy, NULL) != pdTRUE){ if (xSemaphoreTakeFromISR(spi_dev->busy, &higherPriorityTaskWoken) != pdTRUE){
return -1; return -1;
} }
#endif if (woken) {
*woken = *woken || (higherPriorityTaskWoken == pdTRUE);
}
return 0; 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(); PIOS_IRQ_Disable();
spi_dev->busy = 0; spi_dev->busy = 0;
PIOS_IRQ_Enable(); PIOS_IRQ_Enable();
#endif #endif
return 0; 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 * Release the SPI bus semaphore from ISR. Calling the SPI functions does not require this
* \param[in] spi SPI number (0 or 1) * \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 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) #if defined(PIOS_INCLUDE_FREERTOS)
struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id;
signed portBASE_TYPE higherPriorityTaskWoken = pdFALSE;
bool valid = PIOS_SPI_validate(spi_dev); bool valid = PIOS_SPI_validate(spi_dev);
PIOS_Assert(valid) PIOS_Assert(valid)
xSemaphoreGiveFromISR(spi_dev->busy, NULL); xSemaphoreGiveFromISR(spi_dev->busy, &higherPriorityTaskWoken);
if (woken) {
*woken = *woken || (higherPriorityTaskWoken == pdTRUE);
}
return 0;
#else #else
struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; if (woken) {
PIOS_IRQ_Disable(); *woken = false;
spi_dev->busy = 0; }
PIOS_IRQ_Enable(); return PIOS_SPI_ReleaseBus(spi_id);
#endif #endif
return 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++) 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); depctl.d32 = USB_OTG_READ_REG32(&pdev->regs.OUTEP_REGS[i]->DOEPCTL);
if (depctl.b.epena) 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_MIN_CHANNEL_PULSE_US 750 // microseconds
#define PIOS_PPM_IN_MAX_CHANNEL_PULSE_US 2250 // 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); static void PIOS_PPM_Supervisor(uint32_t ppm_id);
enum pios_ppm_dev_magic { 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; return -1;
} }
TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init;
/* Configure the channels to be in capture/compare mode */ /* Configure the channels to be in capture/compare mode */
for (uint8_t i = 0; i < cfg->num_channels; i++) { for (uint8_t i = 0; i < cfg->num_channels; i++) {
const struct pios_tim_channel * chan = &cfg->channels[i]; const struct pios_tim_channel * chan = &cfg->channels[i];
/* Configure timer for input capture */ /* Configure timer for input capture */
TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init;
TIM_ICInitStructure.TIM_Channel = chan->timer_chan; TIM_ICInitStructure.TIM_Channel = chan->timer_chan;
TIM_ICInit(chan->timer, &TIM_ICInitStructure); 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)) { if (!PIOS_RTC_RegisterTickCallback(PIOS_PPM_Supervisor, (uint32_t)ppm_dev)) {
PIOS_DEBUG_Assert(0); PIOS_DEBUG_Assert(0);
} }

57
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) if (xSemaphoreTake(spi_dev->busy, 0xffff) != pdTRUE)
return -1; 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 #endif
return 0; 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. * Claim the SPI bus semaphore from an ISR. Has no timeout.
* \param[in] spi SPI number (0 or 1) * \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 0 if no error
* \return -1 if timeout before claiming semaphore * \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) #if defined(PIOS_INCLUDE_FREERTOS)
struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id;
signed portBASE_TYPE higherPriorityTaskWoken = pdFALSE;
bool valid = PIOS_SPI_validate(spi_dev); bool valid = PIOS_SPI_validate(spi_dev);
PIOS_Assert(valid) PIOS_Assert(valid)
if (xSemaphoreTakeFromISR(spi_dev->busy, NULL) != pdTRUE){ if (xSemaphoreTakeFromISR(spi_dev->busy, &higherPriorityTaskWoken) != pdTRUE){
return -1; return -1;
} }
#endif if (woken) {
*woken = *woken || (higherPriorityTaskWoken == pdTRUE);
}
return 0; 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 * Release the SPI bus semaphore. Calling the SPI functions does not require this
* \param[in] spi SPI number (0 or 1) * \param[in] spi SPI number (0 or 1)
@ -297,6 +319,11 @@ int32_t PIOS_SPI_ReleaseBus(uint32_t spi_id)
PIOS_Assert(valid) PIOS_Assert(valid)
xSemaphoreGive(spi_dev->busy); 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 #endif
return 0; 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 * Release the SPI bus semaphore from ISR. Calling the SPI functions does not require this
* \param[in] spi SPI number (0 or 1) * \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 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) #if defined(PIOS_INCLUDE_FREERTOS)
struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id;
signed portBASE_TYPE higherPriorityTaskWoken = pdFALSE;
bool valid = PIOS_SPI_validate(spi_dev); bool valid = PIOS_SPI_validate(spi_dev);
PIOS_Assert(valid) PIOS_Assert(valid)
xSemaphoreGiveFromISR(spi_dev->busy, NULL); 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 #endif
return 0;
} }
/** /**
* Controls the RC (Register Clock alias Chip Select) pin of a SPI port * Controls the RC (Register Clock alias Chip Select) pin of a SPI port
* \param[in] spi SPI number (0 or 1) * \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) #if defined(PIOS_INCLUDE_PPM_OUT)
#include <pios_ppm_out_priv.h> #include <pios_ppm_out_priv.h>
uint32_t pios_ppm_id;
static const struct pios_tim_channel pios_tim_ppmout[] = { static const struct pios_tim_channel pios_tim_ppmout[] = {
{ {
.timer = TIM2, .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. */ /* Initalize the RFM22B radio COM device. */
#if defined(PIOS_INCLUDE_RFM22B) #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_board_info * bdinfo = &pios_board_info_blob;
const struct pios_rfm22b_cfg *pios_rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); 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, pios_rfm22b_cfg->slave_num, pios_rfm22b_cfg)) { if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, rfm22b_cfg->slave_num, rfm22b_cfg)) {
PIOS_Assert(0); 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); PIOS_Assert(0);
} }
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); uint8_t *gps_rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN);
PIOS_Assert(rx_buffer); PIOS_Assert(gps_rx_buffer);
if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id, 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)) { NULL, 0)) {
PIOS_Assert(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); PIOS_DEBUG_Assert(0);
} }
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_AUX_RX_BUF_LEN); uint8_t *aux_rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_AUX_RX_BUF_LEN);
uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_AUX_TX_BUF_LEN); uint8_t *aux_tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_AUX_TX_BUF_LEN);
PIOS_Assert(rx_buffer); PIOS_Assert(aux_rx_buffer);
PIOS_Assert(tx_buffer); PIOS_Assert(aux_tx_buffer);
if (PIOS_COM_Init(&pios_com_aux_id, &pios_usart_com_driver, pios_usart_aux_id, if (PIOS_COM_Init(&pios_com_aux_id, &pios_usart_com_driver, pios_usart_aux_id,
rx_buffer, PIOS_COM_AUX_RX_BUF_LEN, aux_rx_buffer, PIOS_COM_AUX_RX_BUF_LEN,
tx_buffer, PIOS_COM_AUX_TX_BUF_LEN)) { aux_tx_buffer, PIOS_COM_AUX_TX_BUF_LEN)) {
PIOS_DEBUG_Assert(0); PIOS_DEBUG_Assert(0);
} }
} }
@ -383,13 +383,13 @@ PIOS_FLASHFS_Logfs_Init(&fs_id, &flashfs_internal_cfg, &pios_internal_flash_driv
PIOS_Assert(0); PIOS_Assert(0);
} }
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); uint8_t *telem_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); uint8_t *telem_tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN);
PIOS_Assert(rx_buffer); PIOS_Assert(telem_rx_buffer);
PIOS_Assert(tx_buffer); PIOS_Assert(telem_tx_buffer);
if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id, 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, telem_rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN,
tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { telem_tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) {
PIOS_Assert(0); 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, 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) ManualControlSettingsChannelGroupsOptions channelgroup,uint8_t *bind)
{ {
uint32_t pios_usart_dsm_id; 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; 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_usart_dsm_id, *proto, *bind)) {
PIOS_Assert(0); 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; 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 */ /* Set up the receiver port. Later this should be optional */
uint32_t pios_pwm_id; 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; uint32_t pios_pwm_rcvr_id;
if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_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; 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; 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; uint32_t pios_ppm_rcvr_id;
if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_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; break;
case HWSETTINGS_RADIOPORT_TELEMETRY: case HWSETTINGS_RADIOPORT_TELEMETRY:
{ {
extern const struct pios_rfm22b_cfg * PIOS_BOARD_HW_DEFS_GetRfm22Cfg (uint32_t board_revision); const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev);
const struct pios_board_info * bdinfo = &pios_board_info_blob; if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, rfm22b_cfg->slave_num, rfm22b_cfg)) {
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)) {
PIOS_Assert(0); 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, 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) ManualControlSettingsChannelGroupsOptions channelgroup,uint8_t *bind)
{ {
uint32_t pios_usart_dsm_id; 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; 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_usart_dsm_id, *proto, *bind)) {
PIOS_Assert(0); PIOS_Assert(0);
} }

108
flight/uavobjects/eventdispatcher.c
View File

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

1
make/common-defs.mk
View File

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