diff --git a/flight/libraries/insgps13state.c b/flight/libraries/insgps13state.c index 7eca68a51..8bbfed86e 100644 --- a/flight/libraries/insgps13state.c +++ b/flight/libraries/insgps13state.c @@ -49,7 +49,7 @@ void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW], float Q[NUMW], float dT, float P[NUMX][NUMX]); void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV], - float Y[NUMV], float P[NUMX][NUMX], float X[NUMX], + float Y[NUMV], float P[NUMX][NUMX], float X[NUMX], float K[NUMX][NUMV], uint16_t SensorsUsed); void RungeKutta(float X[NUMX], float U[NUMU], float dT); void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]); @@ -59,12 +59,22 @@ void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]); void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]); // Private variables -float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices - // global to init to zero and maintain zero elements -float Be[3]; // local magnetic unit vector in NED frame -float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector -float Q[NUMW], R[NUMV]; // input noise and measurement noise variances -float K[NUMX][NUMV]; // feedback gain matrix +struct EKFData { + // linearized system matrices + float F[NUMX][NUMX]; + float G[NUMX][NUMW]; + float H[NUMV][NUMX]; + // local magnetic unit vector in NED frame + float Be[3]; + // covariance matrix and state vector + float P[NUMX][NUMX]; + float X[NUMX]; + // input noise and measurement noise variances + float Q[NUMW]; + float R[NUMV]; + float K[NUMX][NUMV]; // feedback gain matrix +} ekf; + // Global variables struct NavStruct Nav; @@ -79,52 +89,52 @@ uint16_t ins_get_num_states() void INSGPSInit() //pretty much just a place holder for now { - Be[0] = 1.0f; - Be[1] = 0.0f; - Be[2] = 0.0f; // local magnetic unit vector + ekf.Be[0] = 1.0f; + ekf.Be[1] = 0.0f; + ekf.Be[2] = 0.0f; // local magnetic unit vector for (int i = 0; i < NUMX; i++) { for (int j = 0; j < NUMX; j++) { - P[i][j] = 0.0f; // zero all terms - F[i][j] = 0.0f; + ekf.P[i][j] = 0.0f; // zero all terms + ekf.F[i][j] = 0.0f; } for (int j = 0; j < NUMW; j++) - G[i][j] = 0.0f; + ekf.G[i][j] = 0.0f; for (int j = 0; j < NUMV; j++) { - H[j][i] = 0.0f; - K[i][j] = 0.0f; + ekf.H[j][i] = 0.0f; + ekf.K[i][j] = 0.0f; } - X[i] = 0.0f; + ekf.X[i] = 0.0f; } for (int i = 0; i < NUMW; i++) - Q[i] = 0.0f; + ekf.Q[i] = 0.0f; for (int i = 0; i < NUMV; i++) - R[i] = 0.0f; + ekf.R[i] = 0.0f; - P[0][0] = P[1][1] = P[2][2] = 25.0f; // initial position variance (m^2) - P[3][3] = P[4][4] = P[5][5] = 5.0f; // initial velocity variance (m/s)^2 - P[6][6] = P[7][7] = P[8][8] = P[9][9] = 1e-5f; // initial quaternion variance - P[10][10] = P[11][11] = P[12][12] = 1e-9f; // initial gyro bias variance (rad/s)^2 + ekf.P[0][0] = ekf.P[1][1] = ekf.P[2][2] = 25.0f; // initial position variance (m^2) + ekf.P[3][3] = ekf.P[4][4] = ekf.P[5][5] = 5.0f; // initial velocity variance (m/s)^2 + ekf.P[6][6] = ekf.P[7][7] = ekf.P[8][8] = ekf.P[9][9] = 1e-5f; // initial quaternion variance + ekf.P[10][10] = ekf.P[11][11] = ekf.P[12][12] = 1e-9f; // initial gyro bias variance (rad/s)^2 - X[0] = X[1] = X[2] = X[3] = X[4] = X[5] = 0.0f; // initial pos and vel (m) - X[6] = 1.0f; - X[7] = X[8] = X[9] = 0.0f; // initial quaternion (level and North) (m/s) - X[10] = X[11] = X[12] = 0.0f; // initial gyro bias (rad/s) + ekf.X[0] = ekf.X[1] = ekf.X[2] = ekf.X[3] = ekf.X[4] = ekf.X[5] = 0.0f; // initial pos and vel (m) + ekf.X[6] = 1.0f; + ekf.X[7] = ekf.X[8] = ekf.X[9] = 0.0f; // initial quaternion (level and North) (m/s) + ekf.X[10] = ekf.X[11] = ekf.X[12] = 0.0f; // initial gyro bias (rad/s) - Q[0] = Q[1] = Q[2] = 50e-4f; // gyro noise variance (rad/s)^2 - Q[3] = Q[4] = Q[5] = 0.00001f; // accelerometer noise variance (m/s^2)^2 - Q[6] = Q[7] = Q[8] = 2e-8f; // gyro bias random walk variance (rad/s^2)^2 + ekf.Q[0] = ekf.Q[1] = ekf.Q[2] = 50e-4f; // gyro noise variance (rad/s)^2 + ekf.Q[3] = ekf.Q[4] = ekf.Q[5] = 0.00001f; // accelerometer noise variance (m/s^2)^2 + ekf.Q[6] = ekf.Q[7] = ekf.Q[8] = 2e-8f; // gyro bias random walk variance (rad/s^2)^2 - R[0] = R[1] = 0.004f; // High freq GPS horizontal position noise variance (m^2) - R[2] = 0.036f; // High freq GPS vertical position noise variance (m^2) - R[3] = R[4] = 0.004f; // High freq GPS horizontal velocity noise variance (m/s)^2 - R[5] = 100.0f; // High freq GPS vertical velocity noise variance (m/s)^2 - R[6] = R[7] = R[8] = 0.005f; // magnetometer unit vector noise variance - R[9] = .25f; // High freq altimeter noise variance (m^2) + ekf.R[0] = ekf.R[1] = 0.004f; // High freq GPS horizontal position noise variance (m^2) + ekf.R[2] = 0.036f; // High freq GPS vertical position noise variance (m^2) + ekf.R[3] = ekf.R[4] = 0.004f; // High freq GPS horizontal velocity noise variance (m/s)^2 + ekf.R[5] = 100.0f; // High freq GPS vertical velocity noise variance (m/s)^2 + ekf.R[6] = ekf.R[7] = ekf.R[8] = 0.005f; // magnetometer unit vector noise variance + ekf.R[9] = .25f; // High freq altimeter noise variance (m^2) } void INSResetP(float PDiag[NUMX]) @@ -135,8 +145,8 @@ void INSResetP(float PDiag[NUMX]) for (i=0;iRoll * stabSettings.RollMax; - altitudeHoldDesired.Pitch = cmd->Pitch * stabSettings.PitchMax; - altitudeHoldDesired.Yaw = cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; + altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax; + altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax; + altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; float currentDown; PositionActualDownGet(¤tDown); if(changed) { // After not being in this mode for a while init at current height - altitudeHoldDesired.Altitude = 0; + altitudeHoldDesiredData.Altitude = 0; zeroed = false; } else if (cmd->Throttle > DEADBAND_HIGH && zeroed) { // being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1 // then apply an "exp" f(x,k) = (k∙x∙x∙x + x∙(256−k)) / 256 - altitudeHoldDesired.Altitude += (throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (256 - throttleExp)) / 256 * throttleRate * dT; + altitudeHoldDesiredData.Altitude += (throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (256 - throttleExp)) / 256 * throttleRate * dT; } else if (cmd->Throttle < DEADBAND_LOW && zeroed) { - altitudeHoldDesired.Altitude -= (throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (256 - throttleExp)) / 256 * throttleRate * dT; + altitudeHoldDesiredData.Altitude -= (throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (256 - throttleExp)) / 256 * throttleRate * dT; } else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH) { // Require the stick to enter the dead band before they can move height zeroed = true; } - AltitudeHoldDesiredSet(&altitudeHoldDesired); + AltitudeHoldDesiredSet(&altitudeHoldDesiredData); } #else // TODO: These functions should never be accessible on CC. Any configuration that -// could allow them to be called sholud already throw an error to prevent this happening +// could allow them to be called should already throw an error to prevent this happening // in flight static void updatePathDesired(__attribute__((unused)) ManualControlCommandData * cmd, __attribute__((unused)) bool changed, diff --git a/flight/modules/Osd/osdgen/osdgen.c b/flight/modules/Osd/osdgen/osdgen.c index e67580bcd..9e9d94d8a 100644 --- a/flight/modules/Osd/osdgen/osdgen.c +++ b/flight/modules/Osd/osdgen/osdgen.c @@ -1063,7 +1063,7 @@ int fetch_font_info(uint8_t ch, int font, struct FontEntry *font_info, char *loo void write_char16(char ch, unsigned int x, unsigned int y, int font) { unsigned int yy, addr_temp, row, row_temp, xshift; - uint16_t and_mask, or_mask, level_bits; + uint16_t and_mask, or_mask, levels; struct FontEntry font_info; //char lookup = 0; fetch_font_info(0, font, &font_info, NULL); @@ -1103,17 +1103,17 @@ void write_char16(char ch, unsigned int x, unsigned int y, int font) addr = addr_temp; for (yy = y; yy < y + font_info.height; yy++) { if (font == 3) { - level_bits = font_frame12x18[row]; + levels = font_frame12x18[row]; //if(!(flags & FONT_INVERT)) // data is normally inverted - level_bits = ~level_bits; + levels = ~levels; or_mask = font_mask12x18[row] << xshift; - and_mask = (font_mask12x18[row] & level_bits) << xshift; + and_mask = (font_mask12x18[row] & levels) << xshift; } else { - level_bits = font_frame8x10[row]; + levels = font_frame8x10[row]; //if(!(flags & FONT_INVERT)) // data is normally inverted - level_bits = ~level_bits; + levels = ~levels; or_mask = font_mask8x10[row] << xshift; - and_mask = (font_mask8x10[row] & level_bits) << xshift; + and_mask = (font_mask8x10[row] & levels) << xshift; } write_word_misaligned_OR(draw_buffer_level, or_mask, addr, wbit); // If we're not bold write the AND mask. @@ -1139,7 +1139,7 @@ void write_char16(char ch, unsigned int x, unsigned int y, int font) void write_char(char ch, unsigned int x, unsigned int y, int flags, int font) { unsigned int yy, addr_temp, row, row_temp, xshift; - uint16_t and_mask, or_mask, level_bits; + uint16_t and_mask, or_mask, levels; struct FontEntry font_info; char lookup = 0; fetch_font_info(ch, font, &font_info, &lookup); @@ -1178,13 +1178,13 @@ void write_char(char ch, unsigned int x, unsigned int y, int flags, int font) row = row_temp; addr = addr_temp; for (yy = y; yy < y + font_info.height; yy++) { - level_bits = font_info.data[row + font_info.height]; + levels = font_info.data[row + font_info.height]; if (!(flags & FONT_INVERT)) { // data is normally inverted - level_bits = ~level_bits; + levels = ~levels; } or_mask = font_info.data[row] << xshift; - and_mask = (font_info.data[row] & level_bits) << xshift; + and_mask = (font_info.data[row] & levels) << xshift; write_word_misaligned_OR(draw_buffer_level, or_mask, addr, wbit); // If we're not bold write the AND mask. //if(!(flags & FONT_BOLD)) diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index 530cc6ca8..d9bd44724 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -215,29 +215,29 @@ void updatePathDesired(__attribute__((unused)) UAVObjEvent * ev) { // use local variables, dont use stack since this is huge and a callback, // dont use the globals because we cant use mutexes here - static WaypointActiveData waypointActive; - static PathActionData pathAction; - static WaypointData waypoint; + static WaypointActiveData waypointActiveData; + static PathActionData pathActionData; + static WaypointData waypointData; static PathDesiredData pathDesired; // find out current waypoint - WaypointActiveGet(&waypointActive); + WaypointActiveGet(&waypointActiveData); - WaypointInstGet(waypointActive.Index,&waypoint); - PathActionInstGet(waypoint.Action, &pathAction); + WaypointInstGet(waypointActiveData.Index,&waypointData); + PathActionInstGet(waypointData.Action, &pathActionData); - pathDesired.End[PATHDESIRED_END_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH]; - pathDesired.End[PATHDESIRED_END_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST]; - pathDesired.End[PATHDESIRED_END_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN]; - pathDesired.EndingVelocity = waypoint.Velocity; - pathDesired.Mode = pathAction.Mode; - pathDesired.ModeParameters[0] = pathAction.ModeParameters[0]; - pathDesired.ModeParameters[1] = pathAction.ModeParameters[1]; - pathDesired.ModeParameters[2] = pathAction.ModeParameters[2]; - pathDesired.ModeParameters[3] = pathAction.ModeParameters[3]; - pathDesired.UID = waypointActive.Index; + pathDesired.End[PATHDESIRED_END_NORTH] = waypointData.Position[WAYPOINT_POSITION_NORTH]; + pathDesired.End[PATHDESIRED_END_EAST] = waypointData.Position[WAYPOINT_POSITION_EAST]; + pathDesired.End[PATHDESIRED_END_DOWN] = waypointData.Position[WAYPOINT_POSITION_DOWN]; + pathDesired.EndingVelocity = waypointData.Velocity; + pathDesired.Mode = pathActionData.Mode; + pathDesired.ModeParameters[0] = pathActionData.ModeParameters[0]; + pathDesired.ModeParameters[1] = pathActionData.ModeParameters[1]; + pathDesired.ModeParameters[2] = pathActionData.ModeParameters[2]; + pathDesired.ModeParameters[3] = pathActionData.ModeParameters[3]; + pathDesired.UID = waypointActiveData.Index; - if(waypointActive.Index == 0) { + if(waypointActiveData.Index == 0) { PositionActualData positionActual; PositionActualGet(&positionActual); // First waypoint has itself as start point (used to be home position but that proved dangerous when looping) @@ -260,7 +260,6 @@ void updatePathDesired(__attribute__((unused)) UAVObjEvent * ev) { pathDesired.StartingVelocity = waypointPrev.Velocity; } PathDesiredSet(&pathDesired); - } // helper function to go to a specific waypoint diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index 86829956e..feefbe1e2 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -487,7 +487,7 @@ static void magOffsetEstimation(MagnetometerData *mag) const float Rz = homeLocation.Be[2]; const float rate = cal.MagBiasNullingRate; - float R[3][3]; + float Rot[3][3]; float B_e[3]; float xy[2]; float delta[3]; @@ -496,9 +496,9 @@ static void magOffsetEstimation(MagnetometerData *mag) Quaternion2R(&attitude.q1, R); // Rotate the mag into the NED frame - B_e[0] = R[0][0] * mag->x + R[1][0] * mag->y + R[2][0] * mag->z; - B_e[1] = R[0][1] * mag->x + R[1][1] * mag->y + R[2][1] * mag->z; - B_e[2] = R[0][2] * mag->x + R[1][2] * mag->y + R[2][2] * mag->z; + B_e[0] = Rot[0][0] * mag->x + Rot[1][0] * mag->y + Rot[2][0] * mag->z; + B_e[1] = Rot[0][1] * mag->x + Rot[1][1] * mag->y + Rot[2][1] * mag->z; + B_e[2] = Rot[0][2] * mag->x + Rot[1][2] * mag->y + Rot[2][2] * mag->z; float cy = cosf(DEG2RAD(attitude.Yaw)); float sy = sinf(DEG2RAD(attitude.Yaw)); diff --git a/flight/modules/VtolPathFollower/vtolpathfollower.c b/flight/modules/VtolPathFollower/vtolpathfollower.c index 8d1d21484..f18be2605 100644 --- a/flight/modules/VtolPathFollower/vtolpathfollower.c +++ b/flight/modules/VtolPathFollower/vtolpathfollower.c @@ -566,7 +566,6 @@ static void updateVtolDesiredAttitude(bool yaw_attitude) StabilizationDesiredData stabDesired; AttitudeActualData attitudeActual; NedAccelData nedAccel; - VtolPathFollowerSettingsData vtolpathfollowerSettings; StabilizationSettingsData stabSettings; SystemSettingsData systemSettings; @@ -580,8 +579,6 @@ static void updateVtolDesiredAttitude(bool yaw_attitude) float downCommand; SystemSettingsGet(&systemSettings); - VtolPathFollowerSettingsGet(&vtolpathfollowerSettings); - VelocityActualGet(&velocityActual); VelocityDesiredGet(&velocityDesired); StabilizationDesiredGet(&stabDesired); diff --git a/flight/pios/common/pios_adxl345.c b/flight/pios/common/pios_adxl345.c index f8e23e8d3..0b2b08a97 100644 --- a/flight/pios/common/pios_adxl345.c +++ b/flight/pios/common/pios_adxl345.c @@ -70,13 +70,13 @@ static struct adxl345_dev * PIOS_ADXL345_alloc(void) * @brief Validate the handle to the spi device * @returns 0 for valid device or -1 otherwise */ -static int32_t PIOS_ADXL345_Validate(struct adxl345_dev * dev) +static int32_t PIOS_ADXL345_Validate(struct adxl345_dev *vdev) { - if (dev == NULL) + if (vdev == NULL) return -1; - if (dev->magic != PIOS_ADXL345_DEV_MAGIC) + if (vdev->magic != PIOS_ADXL345_DEV_MAGIC) return -2; - if (dev->spi_id == 0) + if (vdev->spi_id == 0) return -3; return 0; } diff --git a/flight/pios/common/pios_bma180.c b/flight/pios/common/pios_bma180.c index 35fd09d2f..c6a7210b5 100644 --- a/flight/pios/common/pios_bma180.c +++ b/flight/pios/common/pios_bma180.c @@ -86,13 +86,13 @@ static struct bma180_dev * PIOS_BMA180_alloc(void) * @brief Validate the handle to the spi device * @returns 0 for valid device or -1 otherwise */ -static int32_t PIOS_BMA180_Validate(struct bma180_dev * dev) +static int32_t PIOS_BMA180_Validate(struct bma180_dev *vdev) { - if (dev == NULL) + if (vdev == NULL) return -1; - if (dev->magic != PIOS_BMA180_DEV_MAGIC) + if (vdev->magic != PIOS_BMA180_DEV_MAGIC) return -2; - if (dev->spi_id == 0) + if (vdev->spi_id == 0) return -3; return 0; } @@ -128,10 +128,11 @@ int32_t PIOS_BMA180_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_ */ int32_t PIOS_BMA180_ClaimBus() { - if(PIOS_BMA180_Validate(dev) != 0) + if (PIOS_BMA180_Validate(dev) != 0) { return -1; + } - if(PIOS_SPI_ClaimBus(dev->spi_id) != 0) { + if (PIOS_SPI_ClaimBus(dev->spi_id) != 0) { return -1; } @@ -141,19 +142,22 @@ int32_t PIOS_BMA180_ClaimBus() } /** - * @brief Claim the SPI bus for the accel communications and select this chip + * @brief Claim the SPI bus for the accel communications and select this chip. Call from an ISR. + * @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority + * task has is now eligible to run, else unchanged * @return 0 if successful, -1 if unable to claim bus */ -int32_t PIOS_BMA180_ClaimBusISR() +int32_t PIOS_BMA180_ClaimBusISR(bool *woken) { - if(PIOS_BMA180_Validate(dev) != 0) - return -1; - - if(PIOS_SPI_ClaimBusISR(dev->spi_id) != 0) { + if (PIOS_BMA180_Validate(dev) != 0) { return -1; } - PIOS_SPI_RC_PinSet(dev->spi_id,dev->slave_num,0); + if (PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) { + return -1; + } + + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0); return 0; } @@ -162,13 +166,29 @@ int32_t PIOS_BMA180_ClaimBusISR() * @return 0 if successful */ int32_t PIOS_BMA180_ReleaseBus() +{ + if (PIOS_BMA180_Validate(dev) != 0) { + return -1; + } + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); + + return PIOS_SPI_ReleaseBus(dev->spi_id); +} + +/** + * @brief Release the SPI bus for the accel communications and end the transaction. Call from an ISR + * @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority + * task has is now eligible to run, else unchanged + * @return 0 if successful + */ +int32_t PIOS_BMA180_ReleaseBusISR(bool *woken) { if(PIOS_BMA180_Validate(dev) != 0) return -1; - PIOS_SPI_RC_PinSet(dev->spi_id,dev->slave_num,1); + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); - return PIOS_SPI_ReleaseBus(dev->spi_id); + return PIOS_SPI_ReleaseBusISR(dev->spi_id, woken); } /** @@ -431,19 +451,21 @@ int32_t PIOS_BMA180_Test() } /** - * @brief IRQ Handler. Read data from the BMA180 FIFO and push onto a local fifo. + * @brief EXTI IRQ Handler. Read data from the BMA180 FIFO and push onto a local fifo. + * @return a boolean to the EXTI IRQ Handler wrapper indicating if a + * higher priority task is now eligible to run */ int32_t bma180_irqs = 0; bool PIOS_BMA180_IRQHandler(void) { - bma180_irqs++; - + bool woken = false; static const uint8_t pios_bma180_req_buf[7] = {BMA_X_LSB_ADDR | 0x80,0,0,0,0,0}; uint8_t pios_bma180_dmabuf[8]; - + bma180_irqs++; + // If we can't get the bus then just move on for efficiency - if(PIOS_BMA180_ClaimBusISR() != 0) { - return false; // Something else is using bus, miss this data + if (PIOS_BMA180_ClaimBusISR(&woken) != 0) { + return woken; // Something else is using bus, miss this data } PIOS_SPI_TransferBlock(dev->spi_id,pios_bma180_req_buf,(uint8_t *) pios_bma180_dmabuf, @@ -453,11 +475,12 @@ bool PIOS_BMA180_IRQHandler(void) struct pios_bma180_data data; // Don't release bus till data has copied - PIOS_BMA180_ReleaseBus(); + PIOS_BMA180_ReleaseBusISR(&woken); // Must not return before releasing bus - if(fifoBuf_getFree(&dev->fifo) < sizeof(data)) - return false; + if (fifoBuf_getFree(&dev->fifo) < sizeof(data)) { + return woken; + } // Bottom two bits indicate new data and are constant zeros. Don't right // shift because it drops sign bit @@ -471,7 +494,7 @@ bool PIOS_BMA180_IRQHandler(void) fifoBuf_putData(&dev->fifo, (uint8_t *) &data, sizeof(data)); - return false; + return woken; } #endif /* PIOS_INCLUDE_BMA180 */ diff --git a/flight/pios/common/pios_l3gd20.c b/flight/pios/common/pios_l3gd20.c index 6ca37a2d1..fb621fffd 100644 --- a/flight/pios/common/pios_l3gd20.c +++ b/flight/pios/common/pios_l3gd20.c @@ -60,9 +60,11 @@ static int32_t PIOS_L3GD20_Validate(struct l3gd20_dev * dev); static void PIOS_L3GD20_Config(struct pios_l3gd20_cfg const * cfg); static int32_t PIOS_L3GD20_SetReg(uint8_t address, uint8_t buffer); static int32_t PIOS_L3GD20_GetReg(uint8_t address); +static int32_t PIOS_L3GD20_GetRegISR(uint8_t address, bool *woken); static int32_t PIOS_L3GD20_ClaimBus(); -static int32_t PIOS_L3GD20_ClaimBusIsr(); +static int32_t PIOS_L3GD20_ClaimBusISR(bool *woken); static int32_t PIOS_L3GD20_ReleaseBus(); +static int32_t PIOS_L3GD20_ReleaseBusISR(bool *woken); volatile bool l3gd20_configured = false; @@ -93,13 +95,13 @@ static struct l3gd20_dev * PIOS_L3GD20_alloc(void) * @brief Validate the handle to the spi device * @returns 0 for valid device or -1 otherwise */ -static int32_t PIOS_L3GD20_Validate(struct l3gd20_dev * dev) +static int32_t PIOS_L3GD20_Validate(struct l3gd20_dev *vdev) { - if (dev == NULL) + if (vdev == NULL) return -1; - if (dev->magic != PIOS_L3GD20_DEV_MAGIC) + if (vdev->magic != PIOS_L3GD20_DEV_MAGIC) return -2; - if (dev->spi_id == 0) + if (vdev->spi_id == 0) return -3; return 0; } @@ -191,17 +193,19 @@ static int32_t PIOS_L3GD20_ClaimBus() /** * @brief Claim the SPI bus for the accel communications and select this chip + * @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority + * task has is now eligible to run, else unchanged * @return 0 if successful, -1 for invalid device, -2 if unable to claim bus */ -static int32_t PIOS_L3GD20_ClaimBusIsr() +static int32_t PIOS_L3GD20_ClaimBusISR(bool *woken) { - if(PIOS_L3GD20_Validate(dev) != 0) + if(PIOS_L3GD20_Validate(dev) != 0) { return -1; - - if(PIOS_SPI_ClaimBusISR(dev->spi_id) != 0) + } + if(PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) { return -2; - - PIOS_SPI_RC_PinSet(dev->spi_id,dev->slave_num,0); + } + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0); return 0; } @@ -211,14 +215,28 @@ static int32_t PIOS_L3GD20_ClaimBusIsr() */ int32_t PIOS_L3GD20_ReleaseBus() { - if(PIOS_L3GD20_Validate(dev) != 0) + if(PIOS_L3GD20_Validate(dev) != 0) { return -1; - - PIOS_SPI_RC_PinSet(dev->spi_id,dev->slave_num,1); - + } + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); return PIOS_SPI_ReleaseBus(dev->spi_id); } +/** + * @brief Release the SPI bus for the accel communications and end the transaction + * @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority + * task has is now eligible to run, else unchanged + * @return 0 if successful, -1 for invalid device + */ +int32_t PIOS_L3GD20_ReleaseBusISR(bool *woken) +{ + if(PIOS_L3GD20_Validate(dev) != 0) { + return -1; + } + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); + return PIOS_SPI_ReleaseBusISR(dev->spi_id, woken); +} + /** * @brief Read a register from L3GD20 * @returns The register value or -1 if failure to get bus @@ -238,6 +256,26 @@ static int32_t PIOS_L3GD20_GetReg(uint8_t reg) return data; } +/** + * @brief Read a register from L3GD20 in an ISR context + * @param reg[in] Register address to be read + * @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority + * task has is now eligible to run, else unchanged + * @return The register value or -1 if failure to get bus + */ +static int32_t PIOS_L3GD20_GetRegISR(uint8_t reg, bool *woken) +{ + uint8_t data; + + if(PIOS_L3GD20_ClaimBusISR(woken) != 0) { + return -1; + } + PIOS_SPI_TransferByte(dev->spi_id,(0x80 | reg) ); // request byte + data = PIOS_SPI_TransferByte(dev->spi_id,0 ); // receive response + PIOS_L3GD20_ReleaseBusISR(woken); + return data; +} + /** * @brief Writes one byte to the L3GD20 * \param[in] reg Register address @@ -349,34 +387,38 @@ uint8_t PIOS_L3GD20_Test(void) } /** -* @brief IRQ Handler. Read all the data from onboard buffer +* @brief EXTI IRQ Handler. Read all the data from onboard buffer + * @return a boolean to the EXTI IRQ Handler wrapper indicating if a + * higher priority task is now eligible to run */ bool PIOS_L3GD20_IRQHandler(void) { - l3gd20_irq++; - + bool woken = false; struct pios_l3gd20_data data; uint8_t buf[7] = {PIOS_L3GD20_GYRO_X_OUT_LSB | 0x80 | 0x40, 0, 0, 0, 0, 0, 0}; uint8_t rec[7]; + l3gd20_irq++; + /* This code duplicates ReadGyros above but uses ClaimBusIsr */ - if(PIOS_L3GD20_ClaimBusIsr() != 0) - return false; - - if(PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) { - PIOS_L3GD20_ReleaseBus(); - return false; + if (PIOS_L3GD20_ClaimBusISR(&woken) != 0) { + return woken; } - PIOS_L3GD20_ReleaseBus(); + if(PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) { + PIOS_L3GD20_ReleaseBusISR(&woken); + return woken; + } + + PIOS_L3GD20_ReleaseBusISR(&woken); memcpy((uint8_t *) &(data.gyro_x), &rec[1], 6); - data.temperature = PIOS_L3GD20_GetReg(PIOS_L3GD20_OUT_TEMP); + data.temperature = PIOS_L3GD20_GetRegISR(PIOS_L3GD20_OUT_TEMP, &woken); - portBASE_TYPE xHigherPriorityTaskWoken; - xQueueSendToBackFromISR(dev->queue, (void *) &data, &xHigherPriorityTaskWoken); + signed portBASE_TYPE higherPriorityTaskWoken; + xQueueSendToBackFromISR(dev->queue, (void *) &data, &higherPriorityTaskWoken); - return xHigherPriorityTaskWoken == pdTRUE; + return (woken || higherPriorityTaskWoken == pdTRUE); } #endif /* PIOS_INCLUDE_L3GD20 */ diff --git a/flight/pios/common/pios_mpu6000.c b/flight/pios/common/pios_mpu6000.c index 2b2d06b0b..acbc9d4c5 100644 --- a/flight/pios/common/pios_mpu6000.c +++ b/flight/pios/common/pios_mpu6000.c @@ -91,13 +91,13 @@ static struct mpu6000_dev * PIOS_MPU6000_alloc(void) * @brief Validate the handle to the spi device * @returns 0 for valid device or -1 otherwise */ -static int32_t PIOS_MPU6000_Validate(struct mpu6000_dev * dev) +static int32_t PIOS_MPU6000_Validate(struct mpu6000_dev *vdev) { - if (dev == NULL) + if (vdev == NULL) return -1; - if (dev->magic != PIOS_MPU6000_DEV_MAGIC) + if (vdev->magic != PIOS_MPU6000_DEV_MAGIC) return -2; - if (dev->spi_id == 0) + if (vdev->spi_id == 0) return -3; return 0; } @@ -197,7 +197,12 @@ int32_t PIOS_MPU6000_ConfigureRanges( { if(dev == NULL) return -1; + + // TODO: check that changing the SPI clock speed is safe + // to do here given that interrupts are enabled and the bus has + // not been claimed/is not claimed during this call PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_256); + // update filter settings while (PIOS_MPU6000_SetReg(PIOS_MPU6000_DLPF_CFG_REG, filterSetting) != 0); @@ -225,39 +230,63 @@ int32_t PIOS_MPU6000_ConfigureRanges( /** * @brief Claim the SPI bus for the accel communications and select this chip * @return 0 if successful, -1 for invalid device, -2 if unable to claim bus - * @param fromIsr[in] Tells if the function is being called from a ISR or not */ -static int32_t PIOS_MPU6000_ClaimBus(bool fromIsr) +static int32_t PIOS_MPU6000_ClaimBus() { - if(PIOS_MPU6000_Validate(dev) != 0) + if(PIOS_MPU6000_Validate(dev) != 0) { return -1; - if(fromIsr){ - if(PIOS_SPI_ClaimBusISR(dev->spi_id) != 0) - return -2; - } else { - if(PIOS_SPI_ClaimBus(dev->spi_id) != 0) - 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; } /** * @brief Release the SPI bus for the accel communications and end the transaction * @return 0 if successful - * @param fromIsr[in] Tells if the function is being called from a ISR or not */ -int32_t PIOS_MPU6000_ReleaseBus(bool fromIsr) +static int32_t PIOS_MPU6000_ReleaseBus() { - if(PIOS_MPU6000_Validate(dev) != 0) + if(PIOS_MPU6000_Validate(dev) != 0) { return -1; - - PIOS_SPI_RC_PinSet(dev->spi_id,dev->slave_num,1); - if(fromIsr){ - return PIOS_SPI_ReleaseBusISR(dev->spi_id); - } else { - return PIOS_SPI_ReleaseBus(dev->spi_id); } + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); + return PIOS_SPI_ReleaseBus(dev->spi_id); +} + +/** + * @brief Release the SPI bus for the accel communications and end the transaction + * @return 0 if successful + * @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority + * task has is now eligible to run, else unchanged + */ +static int32_t PIOS_MPU6000_ReleaseBusISR(bool *woken) +{ + if(PIOS_MPU6000_Validate(dev) != 0) { + return -1; + } + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); + return PIOS_SPI_ReleaseBusISR(dev->spi_id, woken); } /** @@ -269,13 +298,14 @@ static int32_t PIOS_MPU6000_GetReg(uint8_t reg) { uint8_t data; - if(PIOS_MPU6000_ClaimBus(false) != 0) + if(PIOS_MPU6000_ClaimBus() != 0) { return -1; + } PIOS_SPI_TransferByte(dev->spi_id,(0x80 | reg) ); // request byte data = PIOS_SPI_TransferByte(dev->spi_id,0 ); // receive response - PIOS_MPU6000_ReleaseBus(false); + PIOS_MPU6000_ReleaseBus(); return data; } @@ -289,20 +319,21 @@ static int32_t PIOS_MPU6000_GetReg(uint8_t reg) */ static int32_t PIOS_MPU6000_SetReg(uint8_t reg, uint8_t data) { - if(PIOS_MPU6000_ClaimBus(false) != 0) + if (PIOS_MPU6000_ClaimBus() != 0) { return -1; + } - if(PIOS_SPI_TransferByte(dev->spi_id, 0x7f & reg) != 0) { - PIOS_MPU6000_ReleaseBus(false); + if (PIOS_SPI_TransferByte(dev->spi_id, 0x7f & reg) != 0) { + PIOS_MPU6000_ReleaseBus(); return -2; } - if(PIOS_SPI_TransferByte(dev->spi_id, data) != 0) { - PIOS_MPU6000_ReleaseBus(false); + if (PIOS_SPI_TransferByte(dev->spi_id, data) != 0) { + PIOS_MPU6000_ReleaseBus(); return -3; } - - PIOS_MPU6000_ReleaseBus(false); + + PIOS_MPU6000_ReleaseBus(); return 0; } @@ -318,13 +349,15 @@ int32_t PIOS_MPU6000_ReadGyros(struct pios_mpu6000_data * data) uint8_t buf[7] = {PIOS_MPU6000_GYRO_X_OUT_MSB | 0x80, 0, 0, 0, 0, 0, 0}; uint8_t rec[7]; - if(PIOS_MPU6000_ClaimBus(false) != 0) + if (PIOS_MPU6000_ClaimBus() != 0) { return -1; + } - if(PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) + if (PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) { return -2; - - PIOS_MPU6000_ReleaseBus(false); + } + + PIOS_MPU6000_ReleaseBus(); data->gyro_x = rec[1] << 8 | rec[2]; data->gyro_y = rec[3] << 8 | rec[4]; @@ -407,31 +440,34 @@ int32_t PIOS_MPU6000_Test(void) } /** - * @brief Run self-test operation. - * \return 0 if test succeeded - * \return non-zero value if test succeeded - * @param fromIsr[in] Tells if the function is being called from a ISR or not + * @brief Obtains the number of bytes in the FIFO. Call from ISR only. + * @return the number of bytes in the FIFO + * @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority + * task has is now eligible to run, else unchanged */ -static int32_t PIOS_MPU6000_FifoDepth(bool fromIsr) +static int32_t PIOS_MPU6000_FifoDepthISR(bool *woken) { uint8_t mpu6000_send_buf[3] = {PIOS_MPU6000_FIFO_CNT_MSB | 0x80, 0, 0}; uint8_t mpu6000_rec_buf[3]; - if(PIOS_MPU6000_ClaimBus(fromIsr) != 0) - return -1; - - if(PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) { - PIOS_MPU6000_ReleaseBus(fromIsr); + if(PIOS_MPU6000_ClaimBusISR(woken) != 0) { return -1; } - PIOS_MPU6000_ReleaseBus(fromIsr); + if(PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) { + PIOS_MPU6000_ReleaseBusISR(woken); + return -1; + } + + PIOS_MPU6000_ReleaseBusISR(woken); return (mpu6000_rec_buf[1] << 8) | mpu6000_rec_buf[2]; } /** -* @brief IRQ Handler. Read all the data from onboard buffer +* @brief EXTI IRQ Handler. Read all the data from onboard buffer +* @return a boolean to the EXTI IRQ Handler wrapper indicating if a +* higher priority task is now eligible to run */ uint32_t mpu6000_irq = 0; int32_t mpu6000_count; @@ -446,46 +482,50 @@ uint32_t mpu6000_transfer_size; bool PIOS_MPU6000_IRQHandler(void) { + bool woken = false; static uint32_t timeval; mpu6000_interval_us = PIOS_DELAY_DiffuS(timeval); timeval = PIOS_DELAY_GetRaw(); - if (!mpu6000_configured) + if (!mpu6000_configured) { return false; + } - mpu6000_count = PIOS_MPU6000_FifoDepth(true); - if (mpu6000_count < (int32_t)sizeof(struct pios_mpu6000_data)) - return false; + mpu6000_count = PIOS_MPU6000_FifoDepthISR(&woken); + if (mpu6000_count < (int32_t)sizeof(struct pios_mpu6000_data)) { + return woken; + } - if (PIOS_MPU6000_ClaimBus(true) != 0) - return false; + if (PIOS_MPU6000_ClaimBusISR(&woken) != 0) { + return woken; + } static uint8_t mpu6000_send_buf[1 + sizeof(struct pios_mpu6000_data) ] = {PIOS_MPU6000_FIFO_REG | 0x80, 0, 0, 0, 0, 0, 0, 0, 0}; static uint8_t mpu6000_rec_buf[1 + sizeof(struct pios_mpu6000_data) ]; if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) { - PIOS_MPU6000_ReleaseBus(true); + PIOS_MPU6000_ReleaseBusISR(&woken); mpu6000_fails++; - return false; + return woken; } - PIOS_MPU6000_ReleaseBus(true); + PIOS_MPU6000_ReleaseBusISR(&woken); static struct pios_mpu6000_data data; // In the case where extras samples backed up grabbed an extra if (mpu6000_count >= (int32_t)(sizeof(data) * 2)) { mpu6000_fifo_backup++; - if (PIOS_MPU6000_ClaimBus(true) != 0) - return false; + if (PIOS_MPU6000_ClaimBusISR(&woken) != 0) + return woken; if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) { - PIOS_MPU6000_ReleaseBus(true); + PIOS_MPU6000_ReleaseBusISR(&woken); mpu6000_fails++; - return false; + return woken; } - PIOS_MPU6000_ReleaseBus(true); + PIOS_MPU6000_ReleaseBusISR(&woken); } // Rotate the sensor to OP convention. The datasheet defines X as towards the right @@ -548,14 +588,14 @@ bool PIOS_MPU6000_IRQHandler(void) data.temperature = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]; #endif - portBASE_TYPE xHigherPriorityTaskWoken; - xQueueSendToBackFromISR(dev->queue, (void *) &data, &xHigherPriorityTaskWoken); + signed portBASE_TYPE higherPriorityTaskWoken; + xQueueSendToBackFromISR(dev->queue, (void *) &data, &higherPriorityTaskWoken); mpu6000_irq++; mpu6000_time_us = PIOS_DELAY_DiffuS(timeval); - return xHigherPriorityTaskWoken == pdTRUE; + return (woken || higherPriorityTaskWoken == pdTRUE); } #endif /* PIOS_INCLUDE_MPU6000 */ diff --git a/flight/pios/inc/pios_spi.h b/flight/pios/inc/pios_spi.h index 951459b94..5740e1e67 100644 --- a/flight/pios/inc/pios_spi.h +++ b/flight/pios/inc/pios_spi.h @@ -49,9 +49,9 @@ extern int32_t PIOS_SPI_TransferByte(uint32_t spi_id, uint8_t b); extern int32_t PIOS_SPI_TransferBlock(uint32_t spi_id, const uint8_t *send_buffer, uint8_t *receive_buffer, uint16_t len, void *callback); extern int32_t PIOS_SPI_Busy(uint32_t spi_id); extern int32_t PIOS_SPI_ClaimBus(uint32_t spi_id); -extern int32_t PIOS_SPI_ClaimBusISR(uint32_t spi_id); +extern int32_t PIOS_SPI_ClaimBusISR(uint32_t spi_id, bool *woken); extern int32_t PIOS_SPI_ReleaseBus(uint32_t spi_id); -extern int32_t PIOS_SPI_ReleaseBusISR(uint32_t spi_id); +extern int32_t PIOS_SPI_ReleaseBusISR(uint32_t spi_id, bool *woken); extern void PIOS_SPI_IRQ_Handler(uint32_t spi_id); extern void PIOS_SPI_SetPrescalar(uint32_t spi_id, uint32_t prescalar); diff --git a/flight/pios/stm32f10x/pios_ppm.c b/flight/pios/stm32f10x/pios_ppm.c index 2088e426e..8933d9a46 100644 --- a/flight/pios/stm32f10x/pios_ppm.c +++ b/flight/pios/stm32f10x/pios_ppm.c @@ -50,7 +50,7 @@ const struct pios_rcvr_driver pios_ppm_rcvr_driver = { #define PIOS_PPM_IN_MAX_CHANNEL_PULSE_US 2250 // microseconds /* Local Variables */ -static TIM_ICInitTypeDef TIM_ICInitStructure; +//static TIM_ICInitTypeDef TIM_ICInitStructure; static void PIOS_PPM_Supervisor(uint32_t ppm_id); @@ -156,12 +156,13 @@ extern int32_t PIOS_PPM_Init(uint32_t * ppm_id, const struct pios_ppm_cfg * cfg) return -1; } + TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init; + /* Configure the channels to be in capture/compare mode */ for (uint8_t i = 0; i < cfg->num_channels; i++) { const struct pios_tim_channel * chan = &cfg->channels[i]; /* Configure timer for input capture */ - TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init; TIM_ICInitStructure.TIM_Channel = chan->timer_chan; TIM_ICInit(chan->timer, &TIM_ICInitStructure); @@ -182,12 +183,6 @@ extern int32_t PIOS_PPM_Init(uint32_t * ppm_id, const struct pios_ppm_cfg * cfg) } } - /* Setup local variable which stays in this scope */ - /* Doing this here and using a local variable saves doing it in the ISR */ - TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; - TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1; - TIM_ICInitStructure.TIM_ICFilter = 0x0; - ppm_dev->supv_timer = 0; if (!PIOS_RTC_RegisterTickCallback(PIOS_PPM_Supervisor, (uint32_t)ppm_dev)) { PIOS_DEBUG_Assert(0); diff --git a/flight/pios/stm32f10x/pios_spi.c b/flight/pios/stm32f10x/pios_spi.c index 4e001b775..6b8d2c88e 100644 --- a/flight/pios/stm32f10x/pios_spi.c +++ b/flight/pios/stm32f10x/pios_spi.c @@ -265,22 +265,33 @@ int32_t PIOS_SPI_ClaimBus(uint32_t spi_id) /** * Claim the SPI bus semaphore from an ISR. Has no timeout. * \param[in] spi SPI number (0 or 1) + * \param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority + * task has is now eligible to run, else unchanged * \return 0 if no error * \return -1 if timeout before claiming semaphore */ -int32_t PIOS_SPI_ClaimBusISR(uint32_t spi_id) +int32_t PIOS_SPI_ClaimBusISR(uint32_t spi_id, bool *woken) { #if defined(PIOS_INCLUDE_FREERTOS) - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; - + struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + signed portBASE_TYPE higherPriorityTaskWoken = pdFALSE; + bool valid = PIOS_SPI_validate(spi_dev); PIOS_Assert(valid) - if (xSemaphoreTakeFromISR(spi_dev->busy, NULL) != pdTRUE){ + if (xSemaphoreTakeFromISR(spi_dev->busy, &higherPriorityTaskWoken) != pdTRUE){ return -1; } -#endif + if (woken) { + *woken = *woken || (higherPriorityTaskWoken == pdTRUE); + } return 0; +#else + if (woken) { + *woken = false; + } + return PIOS_SPI_ClaimBus(spi_id); +#endif } /** @@ -302,7 +313,6 @@ int32_t PIOS_SPI_ReleaseBus(uint32_t spi_id) PIOS_IRQ_Disable(); spi_dev->busy = 0; PIOS_IRQ_Enable(); - #endif return 0; } @@ -310,25 +320,30 @@ int32_t PIOS_SPI_ReleaseBus(uint32_t spi_id) /** * Release the SPI bus semaphore from ISR. Calling the SPI functions does not require this * \param[in] spi SPI number (0 or 1) + * \param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority + * task has is now eligible to run, else unchanged * \return 0 if no error */ -int32_t PIOS_SPI_ReleaseBusISR(uint32_t spi_id) +int32_t PIOS_SPI_ReleaseBusISR(uint32_t spi_id, bool *woken) { #if defined(PIOS_INCLUDE_FREERTOS) struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + signed portBASE_TYPE higherPriorityTaskWoken = pdFALSE; bool valid = PIOS_SPI_validate(spi_dev); PIOS_Assert(valid) - xSemaphoreGiveFromISR(spi_dev->busy, NULL); + xSemaphoreGiveFromISR(spi_dev->busy, &higherPriorityTaskWoken); + if (woken) { + *woken = *woken || (higherPriorityTaskWoken == pdTRUE); + } + return 0; #else - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; - PIOS_IRQ_Disable(); - spi_dev->busy = 0; - PIOS_IRQ_Enable(); - + if (woken) { + *woken = false; + } + return PIOS_SPI_ReleaseBus(spi_id); #endif - return 0; } diff --git a/flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/src/usb_core.c b/flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/src/usb_core.c index 1b6cb84b4..3ba8b0486 100644 --- a/flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/src/usb_core.c +++ b/flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/src/usb_core.c @@ -1355,7 +1355,6 @@ USB_OTG_STS USB_OTG_CoreInitDev (USB_OTG_CORE_HANDLE *pdev) } for (i = 0; i < pdev->cfg.dev_endpoints; i++) { - USB_OTG_DEPCTL_TypeDef depctl; depctl.d32 = USB_OTG_READ_REG32(&pdev->regs.OUTEP_REGS[i]->DOEPCTL); if (depctl.b.epena) { diff --git a/flight/pios/stm32f4xx/pios_ppm.c b/flight/pios/stm32f4xx/pios_ppm.c index fad9a22e3..9d976320b 100644 --- a/flight/pios/stm32f4xx/pios_ppm.c +++ b/flight/pios/stm32f4xx/pios_ppm.c @@ -48,9 +48,6 @@ const struct pios_rcvr_driver pios_ppm_rcvr_driver = { #define PIOS_PPM_IN_MIN_CHANNEL_PULSE_US 750 // microseconds #define PIOS_PPM_IN_MAX_CHANNEL_PULSE_US 2250 // microseconds -/* Local Variables */ -static TIM_ICInitTypeDef TIM_ICInitStructure; - static void PIOS_PPM_Supervisor(uint32_t ppm_id); enum pios_ppm_dev_magic { @@ -155,12 +152,13 @@ extern int32_t PIOS_PPM_Init(uint32_t * ppm_id, const struct pios_ppm_cfg * cfg) return -1; } + TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init; + /* Configure the channels to be in capture/compare mode */ for (uint8_t i = 0; i < cfg->num_channels; i++) { const struct pios_tim_channel * chan = &cfg->channels[i]; /* Configure timer for input capture */ - TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init; TIM_ICInitStructure.TIM_Channel = chan->timer_chan; TIM_ICInit(chan->timer, &TIM_ICInitStructure); @@ -181,12 +179,6 @@ extern int32_t PIOS_PPM_Init(uint32_t * ppm_id, const struct pios_ppm_cfg * cfg) } } - /* Setup local variable which stays in this scope */ - /* Doing this here and using a local variable saves doing it in the ISR */ - TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; - TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1; - TIM_ICInitStructure.TIM_ICFilter = 0x0; - if (!PIOS_RTC_RegisterTickCallback(PIOS_PPM_Supervisor, (uint32_t)ppm_dev)) { PIOS_DEBUG_Assert(0); } diff --git a/flight/pios/stm32f4xx/pios_spi.c b/flight/pios/stm32f4xx/pios_spi.c index 56134c165..bc8304596 100644 --- a/flight/pios/stm32f4xx/pios_spi.c +++ b/flight/pios/stm32f4xx/pios_spi.c @@ -257,6 +257,18 @@ int32_t PIOS_SPI_ClaimBus(uint32_t spi_id) if (xSemaphoreTake(spi_dev->busy, 0xffff) != pdTRUE) return -1; +#else + struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + uint32_t timeout = 0xffff; + while((PIOS_SPI_Busy(spi_id) || spi_dev->busy) && --timeout); + if(timeout == 0) //timed out + return -1; + + PIOS_IRQ_Disable(); + if(spi_dev->busy) + return -1; + spi_dev->busy = 1; + PIOS_IRQ_Enable(); #endif return 0; } @@ -264,25 +276,35 @@ int32_t PIOS_SPI_ClaimBus(uint32_t spi_id) /** * Claim the SPI bus semaphore from an ISR. Has no timeout. * \param[in] spi SPI number (0 or 1) + * \param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority + * task has is now eligible to run, else unchanged * \return 0 if no error * \return -1 if timeout before claiming semaphore */ -int32_t PIOS_SPI_ClaimBusISR(uint32_t spi_id) +int32_t PIOS_SPI_ClaimBusISR(uint32_t spi_id, bool *woken) { #if defined(PIOS_INCLUDE_FREERTOS) - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; - + struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + signed portBASE_TYPE higherPriorityTaskWoken = pdFALSE; + bool valid = PIOS_SPI_validate(spi_dev); PIOS_Assert(valid) - if (xSemaphoreTakeFromISR(spi_dev->busy, NULL) != pdTRUE){ + if (xSemaphoreTakeFromISR(spi_dev->busy, &higherPriorityTaskWoken) != pdTRUE){ return -1; } -#endif + if (woken) { + *woken = *woken || (higherPriorityTaskWoken == pdTRUE); + } return 0; +#else + if (woken) { + *woken = false; + } + return PIOS_SPI_ClaimBus(spi_id); +#endif } - /** * Release the SPI bus semaphore. Calling the SPI functions does not require this * \param[in] spi SPI number (0 or 1) @@ -297,6 +319,11 @@ int32_t PIOS_SPI_ReleaseBus(uint32_t spi_id) PIOS_Assert(valid) xSemaphoreGive(spi_dev->busy); +#else + struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + PIOS_IRQ_Disable(); + spi_dev->busy = 0; + PIOS_IRQ_Enable(); #endif return 0; } @@ -304,21 +331,33 @@ int32_t PIOS_SPI_ReleaseBus(uint32_t spi_id) /** * Release the SPI bus semaphore from ISR. Calling the SPI functions does not require this * \param[in] spi SPI number (0 or 1) + * \param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority + * task has is now eligible to run, else unchanged * \return 0 if no error */ -int32_t PIOS_SPI_ReleaseBusISR(uint32_t spi_id) +int32_t PIOS_SPI_ReleaseBusISR(uint32_t spi_id, bool *woken) { #if defined(PIOS_INCLUDE_FREERTOS) struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + signed portBASE_TYPE higherPriorityTaskWoken = pdFALSE; bool valid = PIOS_SPI_validate(spi_dev); PIOS_Assert(valid) - xSemaphoreGiveFromISR(spi_dev->busy, NULL); + 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 - return 0; } + /** * Controls the RC (Register Clock alias Chip Select) pin of a SPI port * \param[in] spi SPI number (0 or 1) diff --git a/flight/targets/boards/oplinkmini/board_hw_defs.c b/flight/targets/boards/oplinkmini/board_hw_defs.c index ad8b4eacc..3487f33c1 100644 --- a/flight/targets/boards/oplinkmini/board_hw_defs.c +++ b/flight/targets/boards/oplinkmini/board_hw_defs.c @@ -582,8 +582,6 @@ const struct pios_ppm_cfg pios_ppm_main_cfg = { #if defined(PIOS_INCLUDE_PPM_OUT) #include -uint32_t pios_ppm_id; - static const struct pios_tim_channel pios_tim_ppmout[] = { { .timer = TIM2, diff --git a/flight/targets/boards/oplinkmini/firmware/pios_board.c b/flight/targets/boards/oplinkmini/firmware/pios_board.c index c3bd75b71..92569c0b9 100644 --- a/flight/targets/boards/oplinkmini/firmware/pios_board.c +++ b/flight/targets/boards/oplinkmini/firmware/pios_board.c @@ -218,10 +218,9 @@ void PIOS_Board_Init(void) { /* Initalize the RFM22B radio COM device. */ #if defined(PIOS_INCLUDE_RFM22B) { - extern const struct pios_rfm22b_cfg * PIOS_BOARD_HW_DEFS_GetRfm22Cfg (uint32_t board_revision); const struct pios_board_info * bdinfo = &pios_board_info_blob; - const struct pios_rfm22b_cfg *pios_rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); - if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, pios_rfm22b_cfg->slave_num, pios_rfm22b_cfg)) { + const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); + if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, rfm22b_cfg->slave_num, rfm22b_cfg)) { PIOS_Assert(0); } diff --git a/flight/targets/boards/osd/firmware/pios_board.c b/flight/targets/boards/osd/firmware/pios_board.c index 5aba31dc1..a241c27e9 100644 --- a/flight/targets/boards/osd/firmware/pios_board.c +++ b/flight/targets/boards/osd/firmware/pios_board.c @@ -343,10 +343,10 @@ PIOS_FLASHFS_Logfs_Init(&fs_id, &flashfs_internal_cfg, &pios_internal_flash_driv PIOS_Assert(0); } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); - PIOS_Assert(rx_buffer); + uint8_t *gps_rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); + PIOS_Assert(gps_rx_buffer); if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id, - rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, + gps_rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, NULL, 0)) { PIOS_Assert(0); } @@ -361,14 +361,14 @@ PIOS_FLASHFS_Logfs_Init(&fs_id, &flashfs_internal_cfg, &pios_internal_flash_driv PIOS_DEBUG_Assert(0); } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_AUX_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_AUX_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); + uint8_t *aux_rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_AUX_RX_BUF_LEN); + uint8_t *aux_tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_AUX_TX_BUF_LEN); + PIOS_Assert(aux_rx_buffer); + PIOS_Assert(aux_tx_buffer); if (PIOS_COM_Init(&pios_com_aux_id, &pios_usart_com_driver, pios_usart_aux_id, - rx_buffer, PIOS_COM_AUX_RX_BUF_LEN, - tx_buffer, PIOS_COM_AUX_TX_BUF_LEN)) { + aux_rx_buffer, PIOS_COM_AUX_RX_BUF_LEN, + aux_tx_buffer, PIOS_COM_AUX_TX_BUF_LEN)) { PIOS_DEBUG_Assert(0); } } @@ -383,13 +383,13 @@ PIOS_FLASHFS_Logfs_Init(&fs_id, &flashfs_internal_cfg, &pios_internal_flash_driv PIOS_Assert(0); } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); + uint8_t *telem_rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); + uint8_t *telem_tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); + PIOS_Assert(telem_rx_buffer); + PIOS_Assert(telem_tx_buffer); if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id, - rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { + telem_rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, + telem_tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { PIOS_Assert(0); } } diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index d7611a634..febdf32d2 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -269,7 +269,7 @@ static void PIOS_Board_configure_com(const struct pios_usart_cfg *usart_port_cfg } static void PIOS_Board_configure_dsm(const struct pios_usart_cfg *pios_usart_dsm_cfg, const struct pios_dsm_cfg *pios_dsm_cfg, - const struct pios_com_driver *pios_usart_com_driver,enum pios_dsm_proto *proto, + const struct pios_com_driver *usart_com_driver,enum pios_dsm_proto *proto, ManualControlSettingsChannelGroupsOptions channelgroup,uint8_t *bind) { uint32_t pios_usart_dsm_id; @@ -278,7 +278,7 @@ static void PIOS_Board_configure_dsm(const struct pios_usart_cfg *pios_usart_dsm } uint32_t pios_dsm_id; - if (PIOS_DSM_Init(&pios_dsm_id, pios_dsm_cfg, pios_usart_com_driver, + if (PIOS_DSM_Init(&pios_dsm_id, pios_dsm_cfg, usart_com_driver, pios_usart_dsm_id, *proto, *bind)) { PIOS_Assert(0); } @@ -290,11 +290,11 @@ static void PIOS_Board_configure_dsm(const struct pios_usart_cfg *pios_usart_dsm pios_rcvr_group_map[channelgroup] = pios_dsm_rcvr_id; } -static void PIOS_Board_configure_pwm(const struct pios_pwm_cfg *pios_pwm_cfg) +static void PIOS_Board_configure_pwm(const struct pios_pwm_cfg *pwm_cfg) { /* Set up the receiver port. Later this should be optional */ uint32_t pios_pwm_id; - PIOS_PWM_Init(&pios_pwm_id, pios_pwm_cfg); + PIOS_PWM_Init(&pios_pwm_id, pwm_cfg); uint32_t pios_pwm_rcvr_id; if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { @@ -303,10 +303,10 @@ static void PIOS_Board_configure_pwm(const struct pios_pwm_cfg *pios_pwm_cfg) pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; } -static void PIOS_Board_configure_ppm(const struct pios_ppm_cfg *pios_ppm_cfg) +static void PIOS_Board_configure_ppm(const struct pios_ppm_cfg *ppm_cfg) { uint32_t pios_ppm_id; - PIOS_PPM_Init(&pios_ppm_id, pios_ppm_cfg); + PIOS_PPM_Init(&pios_ppm_id, ppm_cfg); uint32_t pios_ppm_rcvr_id; if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { @@ -667,10 +667,8 @@ void PIOS_Board_Init(void) { break; case HWSETTINGS_RADIOPORT_TELEMETRY: { - extern const struct pios_rfm22b_cfg * PIOS_BOARD_HW_DEFS_GetRfm22Cfg (uint32_t board_revision); - const struct pios_board_info * bdinfo = &pios_board_info_blob; - const struct pios_rfm22b_cfg *pios_rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); - if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, pios_rfm22b_cfg->slave_num, pios_rfm22b_cfg)) { + const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); + if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, rfm22b_cfg->slave_num, rfm22b_cfg)) { PIOS_Assert(0); } diff --git a/flight/targets/boards/revoproto/firmware/pios_board.c b/flight/targets/boards/revoproto/firmware/pios_board.c index 03f532b65..5c5a5e3b6 100644 --- a/flight/targets/boards/revoproto/firmware/pios_board.c +++ b/flight/targets/boards/revoproto/firmware/pios_board.c @@ -339,7 +339,7 @@ static void PIOS_Board_configure_com(const struct pios_usart_cfg *usart_port_cfg } static void PIOS_Board_configure_dsm(const struct pios_usart_cfg *pios_usart_dsm_cfg, const struct pios_dsm_cfg *pios_dsm_cfg, - const struct pios_com_driver *pios_usart_com_driver,enum pios_dsm_proto *proto, + const struct pios_com_driver *usart_com_driver,enum pios_dsm_proto *proto, ManualControlSettingsChannelGroupsOptions channelgroup,uint8_t *bind) { uint32_t pios_usart_dsm_id; @@ -348,7 +348,7 @@ static void PIOS_Board_configure_dsm(const struct pios_usart_cfg *pios_usart_dsm } uint32_t pios_dsm_id; - if (PIOS_DSM_Init(&pios_dsm_id, pios_dsm_cfg, pios_usart_com_driver, + if (PIOS_DSM_Init(&pios_dsm_id, pios_dsm_cfg, usart_com_driver, pios_usart_dsm_id, *proto, *bind)) { PIOS_Assert(0); } diff --git a/flight/uavobjects/eventdispatcher.c b/flight/uavobjects/eventdispatcher.c index e56e916b3..962298980 100644 --- a/flight/uavobjects/eventdispatcher.c +++ b/flight/uavobjects/eventdispatcher.c @@ -68,11 +68,11 @@ struct PeriodicObjectListStruct { typedef struct PeriodicObjectListStruct PeriodicObjectList; // Private variables -static PeriodicObjectList* objList; -static xQueueHandle queue; -static xTaskHandle eventTaskHandle; -static xSemaphoreHandle mutex; -static EventStats stats; +static PeriodicObjectList* mObjList; +static xQueueHandle mQueue; +static xTaskHandle mEventTaskHandle; +static xSemaphoreHandle mMutex; +static EventStats mStats; // Private functions static int32_t processPeriodicUpdates(); @@ -89,19 +89,19 @@ static uint16_t randomizePeriod(uint16_t periodMs); int32_t EventDispatcherInitialize() { // Initialize variables - objList = NULL; - memset(&stats, 0, sizeof(EventStats)); + mObjList = NULL; + memset(&mStats, 0, sizeof(EventStats)); - // Create mutex - mutex = xSemaphoreCreateRecursiveMutex(); - if (mutex == NULL) + // Create mMutex + mMutex = xSemaphoreCreateRecursiveMutex(); + if (mMutex == NULL) return -1; // Create event queue - queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(EventCallbackInfo)); + mQueue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(EventCallbackInfo)); // Create task - xTaskCreate( eventTask, (signed char*)"Event", STACK_SIZE, NULL, TASK_PRIORITY, &eventTaskHandle ); + xTaskCreate(eventTask, (signed char*)"Event", STACK_SIZE, NULL, TASK_PRIORITY, &mEventTaskHandle); // Done return 0; @@ -113,9 +113,9 @@ int32_t EventDispatcherInitialize() */ void EventGetStats(EventStats* statsOut) { - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - memcpy(statsOut, &stats, sizeof(EventStats)); - xSemaphoreGiveRecursive(mutex); + xSemaphoreTakeRecursive(mMutex, portMAX_DELAY); + memcpy(statsOut, &mStats, sizeof(EventStats)); + xSemaphoreGiveRecursive(mMutex); } /** @@ -123,9 +123,9 @@ void EventGetStats(EventStats* statsOut) */ void EventClearStats() { - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - memset(&stats, 0, sizeof(EventStats)); - xSemaphoreGiveRecursive(mutex); + xSemaphoreTakeRecursive(mMutex, portMAX_DELAY); + memset(&mStats, 0, sizeof(EventStats)); + xSemaphoreGiveRecursive(mMutex); } /** @@ -143,7 +143,7 @@ int32_t EventCallbackDispatch(UAVObjEvent* ev, UAVObjEventCallback cb) evInfo.cb = cb; evInfo.queue = 0; // Push to queue - return xQueueSend(queue, &evInfo, 0); // will not block if queue is full + return xQueueSend(mQueue, &evInfo, 0); // will not block if queue is full } /** @@ -204,26 +204,26 @@ int32_t EventPeriodicQueueUpdate(UAVObjEvent* ev, xQueueHandle queue, uint16_t p */ static int32_t eventPeriodicCreate(UAVObjEvent* ev, UAVObjEventCallback cb, xQueueHandle queue, uint16_t periodMs) { - PeriodicObjectList* objEntry; + PeriodicObjectList *objEntry; // Get lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + xSemaphoreTakeRecursive(mMutex, portMAX_DELAY); // Check that the object is not already connected - LL_FOREACH(objList, objEntry) - { + LL_FOREACH(mObjList, objEntry) { if (objEntry->evInfo.cb == cb && objEntry->evInfo.queue == queue && objEntry->evInfo.ev.obj == ev->obj && objEntry->evInfo.ev.instId == ev->instId && - objEntry->evInfo.ev.event == ev->event) - { + objEntry->evInfo.ev.event == ev->event) { // Already registered, do nothing - xSemaphoreGiveRecursive(mutex); + xSemaphoreGiveRecursive(mMutex); return -1; } } // Create handle objEntry = (PeriodicObjectList*)pvPortMalloc(sizeof(PeriodicObjectList)); - if (objEntry == NULL) return -1; + if (objEntry == NULL) { + return -1; + } objEntry->evInfo.ev.obj = ev->obj; objEntry->evInfo.ev.instId = ev->instId; objEntry->evInfo.ev.event = ev->event; @@ -232,9 +232,9 @@ static int32_t eventPeriodicCreate(UAVObjEvent* ev, UAVObjEventCallback cb, xQue objEntry->updatePeriodMs = periodMs; objEntry->timeToNextUpdateMs = randomizePeriod(periodMs); // avoid bunching of updates // Add to list - LL_APPEND(objList, objEntry); + LL_APPEND(mObjList, objEntry); // Release lock - xSemaphoreGiveRecursive(mutex); + xSemaphoreGiveRecursive(mMutex); return 0; } @@ -250,26 +250,24 @@ static int32_t eventPeriodicUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, xQue { PeriodicObjectList* objEntry; // Get lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + xSemaphoreTakeRecursive(mMutex, portMAX_DELAY); // Find object - LL_FOREACH(objList, objEntry) - { + LL_FOREACH(mObjList, objEntry) { if (objEntry->evInfo.cb == cb && objEntry->evInfo.queue == queue && objEntry->evInfo.ev.obj == ev->obj && objEntry->evInfo.ev.instId == ev->instId && - objEntry->evInfo.ev.event == ev->event) - { + objEntry->evInfo.ev.event == ev->event) { // Object found, update period objEntry->updatePeriodMs = periodMs; objEntry->timeToNextUpdateMs = randomizePeriod(periodMs); // avoid bunching of updates // Release lock - xSemaphoreGiveRecursive(mutex); + xSemaphoreGiveRecursive(mMutex); return 0; } } // If this point is reached the object was not found - xSemaphoreGiveRecursive(mutex); + xSemaphoreGiveRecursive(mMutex); return -1; } @@ -283,7 +281,7 @@ static void eventTask() EventCallbackInfo evInfo; /* Must do this in task context to ensure that TaskMonitor has already finished its init */ - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_EVENTDISPATCHER, eventTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_EVENTDISPATCHER, mEventTaskHandle); // Initialize time timeToNextUpdateMs = xTaskGetTickCount()*portTICK_RATE_MS; @@ -295,7 +293,7 @@ static void eventTask() delayMs = timeToNextUpdateMs - (xTaskGetTickCount() * portTICK_RATE_MS); // Wait for queue message - if ( xQueueReceive(queue, &evInfo, delayMs/portTICK_RATE_MS) == pdTRUE ) + if ( xQueueReceive(mQueue, &evInfo, delayMs/portTICK_RATE_MS) == pdTRUE ) { // Invoke callback, if one if ( evInfo.cb != 0) @@ -324,49 +322,43 @@ static int32_t processPeriodicUpdates() int32_t offset; // Get lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + xSemaphoreTakeRecursive(mMutex, portMAX_DELAY); // Iterate through each object and update its timer, if zero then transmit object. // Also calculate smallest delay to next update. timeToNextUpdate = xTaskGetTickCount()*portTICK_RATE_MS + MAX_UPDATE_PERIOD_MS; - LL_FOREACH(objList, objEntry) - { + LL_FOREACH(mObjList, objEntry) { // If object is configured for periodic updates - if (objEntry->updatePeriodMs > 0) - { + if (objEntry->updatePeriodMs > 0) { // Check if time for the next update timeNow = xTaskGetTickCount()*portTICK_RATE_MS; - if (objEntry->timeToNextUpdateMs <= timeNow) - { + if (objEntry->timeToNextUpdateMs <= timeNow) { // Reset timer - offset = ( timeNow - objEntry->timeToNextUpdateMs ) % objEntry->updatePeriodMs; + offset = (timeNow - objEntry->timeToNextUpdateMs) % objEntry->updatePeriodMs; objEntry->timeToNextUpdateMs = timeNow + objEntry->updatePeriodMs - offset; // Invoke callback, if one - if ( objEntry->evInfo.cb != 0) - { + if (objEntry->evInfo.cb != 0) { objEntry->evInfo.cb(&objEntry->evInfo.ev); // the function is expected to copy the event information } // Push event to queue, if one - if ( objEntry->evInfo.queue != 0) - { - if ( xQueueSend(objEntry->evInfo.queue, &objEntry->evInfo.ev, 0) != pdTRUE ) // do not block if queue is full - { - if (objEntry->evInfo.ev.obj != NULL) - stats.lastErrorID = UAVObjGetID(objEntry->evInfo.ev.obj); - ++stats.eventErrors; + if (objEntry->evInfo.queue != 0) { + if (xQueueSend(objEntry->evInfo.queue, &objEntry->evInfo.ev, 0) != pdTRUE ) { // do not block if queue is full + if (objEntry->evInfo.ev.obj != NULL) { + mStats.lastErrorID = UAVObjGetID(objEntry->evInfo.ev.obj); + } + ++mStats.eventErrors; } } } // Update minimum delay - if (objEntry->timeToNextUpdateMs < timeToNextUpdate) - { + if (objEntry->timeToNextUpdateMs < timeToNextUpdate) { timeToNextUpdate = objEntry->timeToNextUpdateMs; } } } // Done - xSemaphoreGiveRecursive(mutex); + xSemaphoreGiveRecursive(mMutex); return timeToNextUpdate; } diff --git a/make/common-defs.mk b/make/common-defs.mk index 76cb43746..a06625f2d 100644 --- a/make/common-defs.mk +++ b/make/common-defs.mk @@ -120,6 +120,7 @@ CFLAGS += -mapcs-frame CFLAGS += -fomit-frame-pointer CFLAGS += -Wall -Wextra CFLAGS += -Wfloat-equal -Wunsuffixed-float-constants -Wdouble-promotion +CFLAGS += -Wshadow CFLAGS += -Werror CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I. CFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))