From f6c7ac88d02f53d1df63cd8ebffd5b3d56ce4ae7 Mon Sep 17 00:00:00 2001 From: Ebin Date: Thu, 26 Sep 2019 23:44:28 +0530 Subject: [PATCH] SITL: Yaw rate calculation uses wheel offset --- libraries/SITL/SIM_BalanceBot.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/libraries/SITL/SIM_BalanceBot.cpp b/libraries/SITL/SIM_BalanceBot.cpp index 38f2986d78..967b122248 100644 --- a/libraries/SITL/SIM_BalanceBot.cpp +++ b/libraries/SITL/SIM_BalanceBot.cpp @@ -25,7 +25,7 @@ namespace SITL { BalanceBot::BalanceBot(const char *frame_str) : Aircraft(frame_str), - skid_turn_rate(140) // degrees/sec + skid_turn_rate(0.15708) // meters/sec { dcm.from_euler(0,0,0); // initial yaw, pitch and roll in radians printf("Balance Bot Simulation Started\n"); @@ -36,7 +36,8 @@ BalanceBot::BalanceBot(const char *frame_str) : */ float BalanceBot::calc_yaw_rate(float steering) { - return steering * skid_turn_rate; + float wheel_base_length = 0.15f; + return steering * degrees( skid_turn_rate/wheel_base_length ); }