From f336542dc673bd93e701204dceaf13e843c4c229 Mon Sep 17 00:00:00 2001
From: Timo Korthals <tkorthals@cit-ec.uni-bielefeld.de>
Date: Fri, 2 Feb 2018 12:21:29 +0100
Subject: [PATCH] Fix stack overflow in DiWheelDrive due to access of
non-started gyro thread
Signed-off-by: Timo Korthals <tkorthals@cit-ec.uni-bielefeld.de>
---
components/Odometry.cpp | 32 ++++++++++++++++----------------
components/gyro/l3g4200d.cpp | 9 +++++----
devices/DiWheelDrive/main.cpp | 8 ++++----
include/amiro/Odometry.h | 2 +-
4 files changed, 26 insertions(+), 25 deletions(-)
diff --git a/components/Odometry.cpp b/components/Odometry.cpp
index 071654ea..2083bd1d 100644
--- a/components/Odometry.cpp
+++ b/components/Odometry.cpp
@@ -19,7 +19,7 @@ extern Global global;
Odometry::Odometry(MotorIncrements* mi, L3G4200D* gyroscope)
- : BaseStaticThread<1024>(),
+ : BaseStaticThread<512>(),
motorIncrements(mi),
gyro(gyroscope),
eventSource(),
@@ -82,15 +82,11 @@ void Odometry::resetPosition() {
void Odometry::setError(float* Cp3x3) {
chSysLock();
-// float** test;
Matrix::copy<float>(Cp3x3,3,3, &(this->Cp3x3[0]),3,3);
-// Matrix::copy<float>(Cp3x3,3,3, test,3,3);
chSysUnlock();
}
void Odometry::resetError() {
-// float zeroMatrix[9] = {};
-// this->setError(&(zeroMatrix[0]));
Matrix::init<float>(&(this->Cp3x3[0]),3,3,0.0f);
}
@@ -105,7 +101,7 @@ msg_t Odometry::main(void) {
while (!this->shouldTerminate()) {
time += MS2ST(this->period);
- // Update the base distance, because it may change after a calibration
+ // Update the base distance, because it may have changed after a calibration
this->updateWheelBaseDistance();
// Get the actual speed
@@ -120,11 +116,11 @@ msg_t Odometry::main(void) {
// chprintf((BaseSequentialStream*) &global.sercanmux1, "distance_left:%f distance_right:%f", this->distance[0],this->distance[1]);
// chprintf((BaseSequentialStream*) &global.sercanmux1, "\r\n");
- if (time >= System::getTime()) {
- chThdSleepUntil(time);
+ if (time >= System::getTime()) {
+ chThdSleepUntil(time);
} else {
- chprintf((BaseSequentialStream*) &global.sercanmux1, "WARNING Odometry: Unable to keep track\r\n");
- }
+ chprintf((BaseSequentialStream*) &global.sercanmux1, "WARNING Odometry: Unable to keep track\r\n");
+ }
}
return true;
@@ -134,15 +130,16 @@ void Odometry::updateOdometry() {
// Get the temporary position and error
float Cp3x3[9];
- uint32_t angular_ud;
+ int32_t angular_ud;
+ int32_t angularRate_udps;
chSysLock();
float pX = this->pX;
float pY = this->pY;
float pPhi = this->pPhi;
Matrix::copy<float>(this->Cp3x3,3,3,Cp3x3,3,3);
- // Get the differentail gyro information and reset it
- angular_ud = gyro->getAngular_ud(L3G4200D::AXIS_Z);
- gyro->angularReset();
+ // TODO Get the gyro (or gyro rate) information and do something with it
+ // angular_ud = gyro->getAngular_ud(L3G4200D::AXIS_Z);
+ // angularRate_udps = gyro->getAngularRate_udps(L3G4200D::AXIS_Z);
chSysUnlock();
////////////////
@@ -150,8 +147,11 @@ void Odometry::updateOdometry() {
////////////////
// TMP: Rotated angular
- // float dPhi = (this->distance[RIGHT_WHEEL] - this->distance[LEFT_WHEEL]) / this->wheelBaseDistanceSI;
- float dPhi = ((float(angular_ud * 1e-3) * M_PI ) * 1e-3) / 180.0f;
+ float dPhi = (this->distance[RIGHT_WHEEL] - this->distance[LEFT_WHEEL]) / this->wheelBaseDistanceSI;
+ // TODO Calculate the differential angel dPhi from either the angular (1. line) or angular rate (2.+3. line)
+ // float dPhi = ((float(angular_ud * 1e-3) * M_PI ) * 1e-3) / 180.0f;
+ // const float angular_md = float((angularRate_udps * this->period / constants::millisecondsPerSecond) * 1e-3);
+ // float dPhi = ((angular_md * M_PI) * 1e-3) / 180.0f;
// TMP: Moved distance
float dDistance = (this->distance[RIGHT_WHEEL] + this->distance[LEFT_WHEEL]) / 2.0f;
diff --git a/components/gyro/l3g4200d.cpp b/components/gyro/l3g4200d.cpp
index 2201677f..a8e43c17 100644
--- a/components/gyro/l3g4200d.cpp
+++ b/components/gyro/l3g4200d.cpp
@@ -37,7 +37,7 @@ msg_t L3G4200D::main() {
this->setName("l3g4200d");
while (!this->shouldTerminate()) {
- time += this->period_st;
+ time += this->period_st;
updateSensorData();
calcAngular();
@@ -48,7 +48,7 @@ msg_t L3G4200D::main() {
chThdSleepUntil(time);
} else {
chprintf((BaseSequentialStream*) &global.sercanmux1, "WARNING l3g4200d: Unable to keep track\r\n");
- }
+ }
}
return RDY_OK;
}
@@ -62,7 +62,7 @@ getAngularRate(const uint8_t axis) {
int32_t
L3G4200D::
getAngularRate_udps(const uint8_t axis) {
- return uint32_t(this->angularRate[axis]) * this->udpsPerTic;
+ return int32_t(this->angularRate[axis]) * this->udpsPerTic;
}
int32_t
@@ -160,7 +160,8 @@ msg_t L3G4200D::configure(const L3G4200DConfig *config) {
case L3G4200D::DR_400_HZ: this->period_us = 2500; break;
case L3G4200D::DR_800_HZ: this->period_us = 1250; break;
}
- this->period_st = US2ST(this->period_st);
+ this->period_st = US2ST(this->period_us);
+ this->period_ms = this->period_us * 1e-3;
// Handle the new full scale
switch(config->ctrl1 & L3G4200D::FS_MASK) {
diff --git a/devices/DiWheelDrive/main.cpp b/devices/DiWheelDrive/main.cpp
index 4cc08215..ee7dad32 100644
--- a/devices/DiWheelDrive/main.cpp
+++ b/devices/DiWheelDrive/main.cpp
@@ -849,6 +849,10 @@ int main(void) {
global.increments.start(); // Start the qei driver
+ // Start the three axes gyroscope
+ global.l3g4200d.configure(&global.gyro_run_config);
+ global.l3g4200d.start(NORMALPRIO+5);
+
global.odometry.start(NORMALPRIO + 20);
global.robot.start(HIGHPRIO - 1);
@@ -867,10 +871,6 @@ int main(void) {
global.lis331dlh.configure(&global.accel_run_config);
global.lis331dlh.start(NORMALPRIO+4);
- // Start the three axes gyroscope
- global.l3g4200d.configure(&global.gyro_run_config);
- global.l3g4200d.start(NORMALPRIO+5);
-
// Start the user thread
global.userThread.start(NORMALPRIO);
diff --git a/include/amiro/Odometry.h b/include/amiro/Odometry.h
index 0f44ddb9..af4a513d 100644
--- a/include/amiro/Odometry.h
+++ b/include/amiro/Odometry.h
@@ -8,7 +8,7 @@
namespace amiro {
- class Odometry : public chibios_rt::BaseStaticThread<1024> {
+ class Odometry : public chibios_rt::BaseStaticThread<512> {
public:
/**
* Constructor
--
GitLab