Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 12:38:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: SIM_Calibration: bring angular velocity controller back

Browse Source 
And fix header documentation.
This commit is contained in:
Gustavo Jose de Sousa 2016-09-28 16:10:06 -03:00 committed by Andrew Tridgell
parent 8358809a02
commit 06c3102701
2 changed files with 9 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/SITL/SIM_Calibration.cpp
View File

@ -42,7 +42,7 @@ void SITL::Calibration::update(const struct sitl_input& input)
} else if (switcher_pwm < 1300) {
_calibration_poses(rot_accel);
} else {
_attitude_control(input, rot_accel);
_angular_velocity_control(input, rot_accel);
}
accel_body(0, 0, 0);
@ -108,7 +108,7 @@ void SITL::Calibration::_angular_velocity_control(const struct sitl_input& in,
Vector3f axis{(float)(in.servos[5] - 1500),
(float)(in.servos[6] - 1500),
(float)(in.servos[7] - 1500)};
float theta = MAX_ANGULAR_SPEED * (in.servos[4] - 1200) / 800.f;
float theta = MAX_ANGULAR_SPEED * (in.servos[4] - 1300) / 700.f;
float dt = frame_time_us * 1.0e-6f;
if (axis.length() > 0) {

10
libraries/SITL/SIM_Calibration.h
View File

@ -30,7 +30,7 @@ namespace SITL {
* values must be in [1000, 2000], otherwise that will cause undefined
* behavior.
*
* There are three control modes, that are set with PWM[4]:
* There are four control modes, that are set with PWM[4]:
*
* 1) Stop (1000 <= PWM[4] < 1100):
* Stop the vehicle, i.e., stop the actuation of the other modes.
@ -43,7 +43,11 @@ namespace SITL {
* pwm(theta) = 1500 + 500 * round(theta / pi)
* where -pi <= theta <= pi
*
* 3) Angular Velocity (1200 <= PWM[4] <= 2000):
* 3) Simple autonomous compass calibration (1200 <= PWM[4] < 1300):
* Move continuously the vehicle through six calibration poses and do a
* rotation about each pose over a short period of time.
*
* 4) Angular Velocity (1300 <= PWM[4] <= 2000):
* Rotate the vehicle at a desired angular velocity. The angular velocity is
* specified by a rotation axis and an angular speed.
*
@ -55,7 +59,7 @@ namespace SITL {
* The angular speed value is specified by PWM[4]. The PWM value for a
* desired angular speed in radians/s is given by:
*
* pwm(theta) = 1200 + 800 * round(theta / (2 * pi)),
* pwm(theta) = 1300 + 700 * round(theta / (2 * pi)),
* where 0 <= theta <= 2 * pi
*/
class Calibration : public Aircraft {