jakob.kampichler/libcreate
jakob.kampichler/libcreate
1
0
Fork 0
forked from compAIR/libcreate
Code Pull requests Activity

Use average dt values for velocity calculation (#60)

Browse source
This commit is contained in:
tim-fan 2021-04-19 18:24:43 +12:00 • committed by GitHub
parent fd1d0a220f
commit 20ed0b16ae
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
2 changed files with 35 additions and 5 deletions
Download patch file Download diff file Expand all files Collapse all files

29
src/create.cpp
View file

@ -41,6 +41,7 @@ namespace create {
poseCovar = Matrix(3, 3, 0.0);
requestedLeftVel = 0;
requestedRightVel = 0;
dtHistoryLength = 100;
data = std::shared_ptr<Data>(new Data(model.getVersion()));
if (model.getVersion() == V_1) {
serial = std::make_shared<SerialQuery>(data);
@ -167,8 +168,22 @@ namespace create {
deltaYaw = wheelDistDiff / model.getAxleLength();
}
measuredLeftVel = leftWheelDist / dt;
measuredRightVel = rightWheelDist / dt;
// determine average dt over window
dtHistory.push_front(dt);
if (dtHistory.size() > dtHistoryLength){
dtHistory.pop_back();
}
float dtHistorySum = 0;
for (auto it = dtHistory.cbegin(); it != dtHistory.cend(); ++it)
{
dtHistorySum += *it;
}
auto dtAverage = dtHistorySum / dtHistory.size();
measuredLeftVel = leftWheelDist / dtAverage;
measuredRightVel = rightWheelDist / dtAverage;
// Moving straight
if (fabs(wheelDistDiff) < util::EPS) {
@ -183,10 +198,10 @@ namespace create {
totalLeftDist += leftWheelDist;
totalRightDist += rightWheelDist;
if (fabs(dt) > util::EPS) {
vel.x = deltaDist / dt;
if (fabs(dtAverage) > util::EPS) {
vel.x = deltaDist / dtAverage;
vel.y = 0.0;
vel.yaw = deltaYaw / dt;
vel.yaw = deltaYaw / dtAverage;
} else {
vel.x = 0.0;
vel.y = 0.0;
@ -593,6 +608,10 @@ namespace create {
return serial->send(cmd, 2);
}
void Create::setDtHistoryLength(const uint8_t& dtHistoryLength) {
this->dtHistoryLength = dtHistoryLength;
}
bool Create::isWheeldrop() const {
if (data->isValidPacketID(ID_BUMP_WHEELDROP)) {
return (GET_DATA(ID_BUMP_WHEELDROP) & 0x0C) != 0;