Skip to content

Opto filter #534

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 2 commits into
base: development
Choose a base branch
from
Open

Opto filter #534

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
3b3678c
moving average filter for optoncdt's
Aug 26, 2017
ee84053
fix syntax
Aug 26, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 6 additions & 4 deletions FIRMWARE/PROJECT_CODE/LCCM655__RLOOP__FCU_CORE/LASER_OPTO/fcu__laser_opto.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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,
Expand Down Expand Up @@ -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;
}


Expand Down Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion FIRMWARE/PROJECT_CODE/LCCM655__RLOOP__FCU_CORE/LASER_OPTO/fcu__laser_opto__ethernet.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
83 changes: 37 additions & 46 deletions FIRMWARE/PROJECT_CODE/LCCM655__RLOOP__FCU_CORE/LASER_OPTO/fcu__laser_opto__filtering.c
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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.
*/
/**
Expand All @@ -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
Expand All @@ -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
Expand Down
23 changes: 11 additions & 12 deletions FIRMWARE/PROJECT_CODE/LCCM655__RLOOP__FCU_CORE/fcu_core.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand All @@ -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];

Expand Down Expand Up @@ -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);
Expand Down
2 changes: 2 additions & 0 deletions FIRMWARE/PROJECT_CODE/LCCM655__RLOOP__FCU_CORE/fcu_core__defines.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand Down