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

Improved filtered groundspeed calculation. Now the filtering rise time no longer on the refresh rate.

Browse Source
This commit is contained in:
Laura Sebesta 2012-08-18 09:19:59 +02:00
parent 28f382d472
commit d3daa06713
3 changed files with 19 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavitem.cpp
View File

@ -146,9 +146,7 @@ namespace mapcontrol
//*********** Create time rings
double ringTime=10*pow(2,17-map->ZoomTotal()); //Basic ring is 10 seconds wide at zoom level 17
double alpha= .05;
groundspeed_mps_filt= (1-alpha)*groundspeed_mps_filt + alpha*groundspeed_mps;
if(groundspeed_mps > 0){ //Don't clutter the display with rings that are only one pixel wide
if(groundspeed_mps_filt > 0){ //Don't clutter the display with rings that are only one pixel wide
myPen.setWidth(2);
myPen.setColor(QColor(0, 0, 0, 100));
@ -274,11 +272,26 @@ namespace mapcontrol
this->CAS_mps=CAS_mps;
}
void UAVItem::SetGroundspeed(double vNED[3]){
void UAVItem::SetGroundspeed(double vNED[3], int m_maxUpdateRate_ms){
this->vNED[0] = vNED[0];
this->vNED[1] = vNED[1];
this->vNED[2] = vNED[2];
groundspeed_kph=sqrt(vNED[0]*vNED[0] + vNED[1]*vNED[1] + vNED[2]*vNED[2])*3.6;
//On the first pass, set the filtered speed to the reported speed.
static bool firstGroundspeed=true;
if (firstGroundspeed){
groundspeed_mps_filt=groundspeed_kph/3.6;
firstGroundspeed=false;
}
else{
int riseTime_ms=1000;
double alpha= m_maxUpdateRate_ms/(double)(m_maxUpdateRate_ms+riseTime_ms);
groundspeed_mps_filt= alpha*groundspeed_mps_filt + (1-alpha)*(groundspeed_kph/3.6);
}
refreshPaint_flag=true;
}

2
ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavitem.h
View File

@ -69,7 +69,7 @@ namespace mapcontrol
*
* @param NED
*/
void SetGroundspeed(double vNED[3]);
void SetGroundspeed(double vNED[3], int m_maxUpdateRate);
/**
* @brief Sets the UAV Calibrated Airspeed
*

2
ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp
View File

@ -604,7 +604,7 @@ void OPMapGadgetWidget::updatePosition()
//Set the position and heading estimates in the painter module
m_map->UAV->SetNED(NED);
m_map->UAV->SetCAS(-1); //THIS NEEDS TO BECOME AIRSPEED, ONCE WE SETTLE ON A UAVO
m_map->UAV->SetGroundspeed(vNED);
m_map->UAV->SetGroundspeed(vNED, m_maxUpdateRate);
//Convert angular velocities into a rotationg rate around the world-frame yaw axis. This is found by simply taking the dot product of the angular Euler-rate matrix with the angular rates.
float psiRate_dps=0*gyrosData.z + sin(attitudeActualData.Roll*deg_to_rad)/cos(attitudeActualData.Pitch*deg_to_rad)*gyrosData.y + cos(attitudeActualData.Roll*deg_to_rad)/cos(attitudeActualData.Pitch*deg_to_rad)*gyrosData.z;