diff --git a/docs/troubleshooting.md b/docs/troubleshooting.md index 25f130c..756bf71 100644 --- a/docs/troubleshooting.md +++ b/docs/troubleshooting.md @@ -23,7 +23,7 @@ Do the following: * **Check the RC data** if you use it. Use `rc` command, `Control` should show correct values between -1 and 1, and between 0 and 1 for the throttle. * **Configure QGroundControl correctly before connecting to the drone** if you use it to control the drone. Go to the settings and enable *Virtual Joystick*. *Auto-Center Throttle* setting **should be disabled**. * **Make sure you're not moving the drone several seconds after the power on**. The drone calibrates its gyroscope on the start so it should stay still for a while. -* **Check the IMU frequency**. Perform `imu` command. The `frequency` field should be about 1000 (Hz). +* **Check the IMU sample rate**. Perform `imu` command. The `rate` field should be about 1000 (Hz). * **Check the IMU data**. Perform `imu` command, check raw accelerometer and gyro output. The output should change as you move the drone. * **Calibrate the accelerometer.** if is wasn't done before. Perform `ca` command and put the results to `imu.ino` file. * **Check the attitude estimation**. Connect to the drone using QGroundControl. Rotate the drone in different orientations and check if the attitude estimation shown in QGroundControl is correct. diff --git a/flix/cli.ino b/flix/cli.ino index 0bcfbb8..824d8b3 100644 --- a/flix/cli.ino +++ b/flix/cli.ino @@ -53,7 +53,7 @@ const struct Param { {"lpr", &ratesFilter.alpha, nullptr}, {"lpd", &rollRatePID.lpf.alpha, &pitchRatePID.lpf.alpha}, - {"ss", &loopFreq, nullptr}, + {"ss", &loopRate, nullptr}, {"dt", &dt, nullptr}, {"t", &t, nullptr}, }; @@ -73,7 +73,7 @@ void doCommand(String& command, String& value) { Serial.printf("gyro: %f %f %f\n", rates.x, rates.y, rates.z); Serial.printf("acc: %f %f %f\n", acc.x, acc.y, acc.z); printIMUCal(); - Serial.printf("frequency: %f\n", loopFreq); + Serial.printf("rate: %f\n", loopRate); } else if (command == "rc") { Serial.printf("Raw: throttle %d yaw %d pitch %d roll %d armed %d mode %d\n", channels[RC_CHANNEL_THROTTLE], channels[RC_CHANNEL_YAW], channels[RC_CHANNEL_PITCH], diff --git a/flix/flix.ino b/flix/flix.ino index 8183e55..dea10d9 100644 --- a/flix/flix.ino +++ b/flix/flix.ino @@ -25,7 +25,7 @@ float t = NAN; // current step time, s float dt; // time delta from previous step, s -float loopFreq; // loop frequency, Hz +float loopRate; // loop rate, Hz int16_t channels[RC_CHANNELS]; // raw rc channels float controls[RC_CHANNELS]; // normalized controls in range [-1..1] ([0..1] for throttle) float controlsTime; // time of the last controls update diff --git a/flix/time.ino b/flix/time.ino index 4632bfb..da9f1c8 100644 --- a/flix/time.ino +++ b/flix/time.ino @@ -12,16 +12,16 @@ void step() { dt = 0; // assume dt to be zero on first step and on reset } - computeLoopFreq(); + computeLoopRate(); } -void computeLoopFreq() { +void computeLoopRate() { static float windowStart = 0; - static uint32_t freq = 0; - freq++; + static uint32_t rate = 0; + rate++; if (t - windowStart >= 1) { // 1 second window - loopFreq = freq; + loopRate = rate; windowStart = t; - freq = 0; + rate = 0; } } diff --git a/gazebo/flix.h b/gazebo/flix.h index 4c59561..72787f5 100644 --- a/gazebo/flix.h +++ b/gazebo/flix.h @@ -21,7 +21,7 @@ float t = NAN; float dt; -float loopFreq; +float loopRate; float motors[4]; int16_t channels[16]; // raw rc channels float controls[RC_CHANNELS]; @@ -32,7 +32,7 @@ Vector rates; Quaternion attitude; // declarations -void computeLoopFreq(); +void computeLoopRate(); void applyGyro(); void applyAcc(); void control();