diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavitem.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavitem.cpp index 6937382a1..3c0c13037 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavitem.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavitem.cpp @@ -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; } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavitem.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavitem.h index 155f61581..263a99930 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavitem.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavitem.h @@ -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 * diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp index 1bd3a18eb..9c796d651 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp @@ -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;