diff --git a/FIRMWARE/PROJECT_CODE/LCCM655__RLOOP__FCU_CORE/LASER_OPTO/fcu__laser_opto.c b/FIRMWARE/PROJECT_CODE/LCCM655__RLOOP__FCU_CORE/LASER_OPTO/fcu__laser_opto.c index 18f72361..a12a7697 100644 --- a/FIRMWARE/PROJECT_CODE/LCCM655__RLOOP__FCU_CORE/LASER_OPTO/fcu__laser_opto.c +++ b/FIRMWARE/PROJECT_CODE/LCCM655__RLOOP__FCU_CORE/LASER_OPTO/fcu__laser_opto.c @@ -71,8 +71,7 @@ void vFCU_LASEROPTO__Init(void) sFCU.sLaserOpto.sOptoLaser[u8Counter].u8NewDistanceAvail = 0U; //just set to some obscene distance sFCU.sLaserOpto.sOptoLaser[u8Counter].f32DistanceRAW = 0.0F; - sFCU.sLaserOpto.sOptoLaser[u8Counter].sFiltered.f32FilteredValue = 0.0F; - sFCU.sLaserOpto.sOptoLaser[u8Counter].sFiltered.f32PreviousValue = 0.0F; + sFCU.sLaserOpto.sOptoLaser[u8Counter].f32FilteredValue_mm = 0.0F; sFCU.sLaserOpto.sOptoLaser[u8Counter].u8Error = 0U; sFCU.sLaserOpto.sOptoLaser[u8Counter].sCounters.u32ErrorCode = 0U; @@ -86,6 +85,9 @@ void vFCU_LASEROPTO__Init(void) }//for(u8Counter = 0U; u8Counter < C_FCU__NUM_LASERS_OPTONCDT; u8Counter++) + //Init the filtering stuff + vFCU_LASEROPTO_FILT__Init(); + //check the CRC u8Test = u8SIL3_EEPARAM_CRC__Is_CRC_OK( C_LOCALDEF__LCCM655__FCTL_OPTONCDT___FL_ZERO, C_LOCALDEF__LCCM655__FCTL_OPTONCDT___RR_ZERO, @@ -463,7 +465,7 @@ void vFCU_LASEROPTO__Inject_Value(Luint8 u8LaserIndex, Lfloat32 f32Value) */ Lfloat32 f32FCU_LASEROPTO__Get_Distance(E_FCU__LASER_OPTO__INDEX_T eLaser) { - return sFCU.sLaserOpto.sOptoLaser[(Luint8)eLaser].sFiltered.f32FilteredValue; + return sFCU.sLaserOpto.sOptoLaser[(Luint8)eLaser].f32FilteredValue_mm; } @@ -535,7 +537,7 @@ void vFCU_LASEROPTO__Process_Packet(E_FCU__LASER_OPTO__INDEX_T eLaser) f32Temp /= 100.0F; //save off the distance. - sFCU.sLaserOpto.sOptoLaser[(Luint8)eLaser].f32DistanceRAW = f32Temp; + sFCU.sLaserOpto.sOptoLaser[(Luint8)eLaser].f32DistanceRAW = f32Temp; //save off. sFCU.sLaserOpto.sOptoLaser[(Luint8)eLaser].u8NewDistanceAvail = 1U; //todo: currently never cleared diff --git a/FIRMWARE/PROJECT_CODE/LCCM655__RLOOP__FCU_CORE/LASER_OPTO/fcu__laser_opto__ethernet.c b/FIRMWARE/PROJECT_CODE/LCCM655__RLOOP__FCU_CORE/LASER_OPTO/fcu__laser_opto__ethernet.c index 489ec9bc..1230ae05 100644 --- a/FIRMWARE/PROJECT_CODE/LCCM655__RLOOP__FCU_CORE/LASER_OPTO/fcu__laser_opto__ethernet.c +++ b/FIRMWARE/PROJECT_CODE/LCCM655__RLOOP__FCU_CORE/LASER_OPTO/fcu__laser_opto__ethernet.c @@ -95,7 +95,7 @@ void vFCU_LASEROPTO_ETH__Transmit(E_NET__PACKET_T ePacketType) pu8Buffer += 4U; //Filtered laser packet from filtering system - vSIL3_NUM_CONVERT__Array_F32(pu8Buffer, sFCU.sLaserOpto.sOptoLaser[u8Device].sFiltered.f32FilteredValue); + vSIL3_NUM_CONVERT__Array_F32(pu8Buffer, sFCU.sLaserOpto.sOptoLaser[u8Device].f32FilteredValue_mm); pu8Buffer += 4U; //Spare diff --git a/FIRMWARE/PROJECT_CODE/LCCM655__RLOOP__FCU_CORE/LASER_OPTO/fcu__laser_opto__filtering.c b/FIRMWARE/PROJECT_CODE/LCCM655__RLOOP__FCU_CORE/LASER_OPTO/fcu__laser_opto__filtering.c index 9fbf5b11..6299ec00 100644 --- a/FIRMWARE/PROJECT_CODE/LCCM655__RLOOP__FCU_CORE/LASER_OPTO/fcu__laser_opto__filtering.c +++ b/FIRMWARE/PROJECT_CODE/LCCM655__RLOOP__FCU_CORE/LASER_OPTO/fcu__laser_opto__filtering.c @@ -1,7 +1,7 @@ /** * @file FCU__LASER_OPTO__FILTERING.C - * @brief OptoNCDT laser filtering system - * @author Lachlan Grogan, Marek Gutt-Mostowy + * @brief OptoNCDT laser moving average filtering system + * @author Lachlan Grogan, Dean Skoien * @copyright rLoop Inc. */ /** @@ -24,6 +24,18 @@ //the structure extern struct _strFCU sFCU; + +void vFCU_LASEROPTO_FILT__Init(void){ + Luint8 u8Counter; + + for(u8Counter = 0U; u8Counter < C_FCU__NUM_LASERS_OPTONCDT; u8Counter++){ + sFCU.sLaserOpto.sOptoLaser[u8Counter].sCounters.u8AverageCounter = 0U; + sFCU.sLaserOpto.sOptoLaser[u8Counter].sCounters.u8AverageCounter_wait = 0U; + } + +} + + /***************************************************************************//** * @brief * Apply the laser opto filter @@ -34,56 +46,35 @@ extern struct _strFCU sFCU; */ void vFCU_LASEROPTO_FILT__FilterPacket(E_FCU__LASER_OPTO__INDEX_T eLaser) { - Lfloat32 f32Temp; - Lfloat32 f32Temp2; - Lfloat32 f32FilterAlpha; - Lfloat32 f32NewSampleInfluence; - Lfloat32 f32OldSampleInfluence; - - //initial parameters - f32FilterAlpha = 0.01F; - f32NewSampleInfluence = 0.0F; - f32OldSampleInfluence = 0.0F; - - //LG HACK - f32Temp = sFCU.sLaserOpto.sOptoLaser[(Luint8)eLaser].f32DistanceRAW; - //subtract the zero - f32Temp += sFCU.sLaserOpto.sCalibration[(Luint8)eLaser].f32Offset; - sFCU.sLaserOpto.sOptoLaser[(Luint8)eLaser].sFiltered.f32FilteredValue = f32Temp; + //update + sFCU.sLaserOpto.sOptoLaser[eLaser].f32PrevDistances_mm[sFCU.sLaserOpto.sOptoLaser[eLaser].sCounters.u8AverageCounter] = sFCU.sLaserOpto.sOptoLaser[eLaser].f32DistanceRAW; - //LG Hack - return; + if(sFCU.sLaserOpto.sOptoLaser[eLaser].sCounters.u8AverageCounter_wait == 9U){ + //enough measurements taken, calculate moving average of distance + Luint8 u8Counter; + Lfloat32 f32AvgTemp; + f32AvgTemp = 0U; + for(u8Counter = 0U; u8Counter < C_FCU__OPTO_FILTER_WINDOW; u8Counter++){ + f32AvgTemp += sFCU.sLaserOpto.sOptoLaser[eLaser].f32PrevDistances_mm[u8Counter]; + f32AvgTemp /= C_FCU__OPTO_FILTER_WINDOW; + } - //exponential moving average filter - f32Temp = sFCU.sLaserOpto.sOptoLaser[(Luint8)eLaser].f32DistanceRAW; - f32Temp -= sFCU.sLaserOpto.sOptoLaser[(Luint8)eLaser].sFiltered.f32PreviousValue; + //done calculating moving average, store it + sFCU.sLaserOpto.sOptoLaser[eLaser].f32FilteredValue_mm = f32AvgTemp; - f32Temp2 = f32SIL3_NUM_ABS__F32(f32Temp); - - //reject current value which differ by more than 5 mm from the previous value - if(f32Temp2 < 5.0F) - { - // @see http://dsp.stackexchange.com/questions/20333/how-to-implement-a-moving-average-in-c-without-a-buffer - - //Calculate the old sample influence - f32NewSampleInfluence = f32FilterAlpha * sFCU.sLaserOpto.sOptoLaser[(Luint8)eLaser].f32DistanceRAW; - - //Calculatre the new sample influence - f32OldSampleInfluence = (1.0F - f32FilterAlpha) * sFCU.sLaserOpto.sOptoLaser[(Luint8)eLaser].sFiltered.f32PreviousValue; - - //Update the current value - sFCU.sLaserOpto.sOptoLaser[(Luint8)eLaser].sFiltered.f32FilteredValue = f32OldSampleInfluence + f32NewSampleInfluence; - - // Remember the current value for next iteration - sFCU.sLaserOpto.sOptoLaser[(Luint8)eLaser].sFiltered.f32PreviousValue = sFCU.sLaserOpto.sOptoLaser[(Luint8)eLaser].sFiltered.f32FilteredValue; - } - else - { - // set current filtered value to the previous filtered value in case the differential is higher than 5 - sFCU.sLaserOpto.sOptoLaser[(Luint8)eLaser].sFiltered.f32FilteredValue = sFCU.sLaserOpto.sOptoLaser[(Luint8)eLaser].sFiltered.f32PreviousValue; + else{ + // need at least ten measurements to compute the moving average, increment waiting counter + sFCU.sLaserOpto.sOptoLaser[eLaser].sCounters.u8AverageCounter_wait++; + } + + //increment counter for next data point + sFCU.sLaserOpto.sOptoLaser[eLaser].sCounters.u8AverageCounter++; + if(sFCU.sLaserOpto.sOptoLaser[eLaser].sCounters.u8AverageCounter == 10U){ + sFCU.sLaserOpto.sOptoLaser[eLaser].sCounters.u8AverageCounter = 0U; } + } #endif //C_LOCALDEF__LCCM655__ENABLE_LASER_OPTONCDT diff --git a/FIRMWARE/PROJECT_CODE/LCCM655__RLOOP__FCU_CORE/fcu_core.h b/FIRMWARE/PROJECT_CODE/LCCM655__RLOOP__FCU_CORE/fcu_core.h index 2c49b71f..ee2e9160 100644 --- a/FIRMWARE/PROJECT_CODE/LCCM655__RLOOP__FCU_CORE/fcu_core.h +++ b/FIRMWARE/PROJECT_CODE/LCCM655__RLOOP__FCU_CORE/fcu_core.h @@ -606,7 +606,12 @@ /** is f32Distance equal to error value? */ Luint8 u8Error; + /** The filtered height */ + Lfloat32 f32FilteredValue_mm; + /** The previous value */ + Lfloat32 f32PrevDistances_mm[10]; + /** Diagnostic Counters */ struct { @@ -623,19 +628,13 @@ /** Keep track of how long its takes between bytes being seen */ Luint32 u32ByteSeenTimeOut; - }sCounters; - - /** Filtered data structure */ - struct - { + /** Wait until there are at least ten measurements for a laser */ + Luint8 u8AverageCounter_wait; - /** The filtered height */ - Lfloat32 f32FilteredValue; + /** index counter to keep track oldest data point */ + Luint8 u8AverageCounter; - /** The previous value */ - Lfloat32 f32PreviousValue; - - }sFiltered; + }sCounters; }sOptoLaser[C_FCU__NUM_LASERS_OPTONCDT]; @@ -1521,9 +1520,9 @@ void vFCU_LASEROPTO_ETH__Transmit(E_NET__PACKET_T ePacketType); //filtering + void vFCU_LASEROPTO_FILT__Init(void); void vFCU_LASEROPTO_FILT__FilterPacket(E_FCU__LASER_OPTO__INDEX_T eLaser); - //brakes void vFCU_BRAKES__Init(void); diff --git a/FIRMWARE/PROJECT_CODE/LCCM655__RLOOP__FCU_CORE/fcu_core__defines.h b/FIRMWARE/PROJECT_CODE/LCCM655__RLOOP__FCU_CORE/fcu_core__defines.h index 503cf4ff..e2096c62 100644 --- a/FIRMWARE/PROJECT_CODE/LCCM655__RLOOP__FCU_CORE/fcu_core__defines.h +++ b/FIRMWARE/PROJECT_CODE/LCCM655__RLOOP__FCU_CORE/fcu_core__defines.h @@ -40,6 +40,8 @@ /** Total number of Laser OptoNCDT's*/ #define C_FCU__NUM_LASERS_OPTONCDT (C_FCU__NUM_LASERS_GROUND + C_FCU__NUM_LASERS_IBEAM) + #define C_FCU__OPTO_FILTER_WINDOW (10U) + /** number of lasers for the constrast detection system */ #define C_FCU__NUM_LASERS_CONTRAST (3U)