mirror of https://github.com/ArduPilot/ardupilot
SITL: add SIM_Calibration
That model can be used to run compass and accelerometer calibration by rotating the vehicle through the servos channels.
This commit is contained in:
parent
45754ef37c
commit
9fa85d9bcf
|
@ -0,0 +1,109 @@
|
|||
/*
|
||||
* Copyright (C) 2015-2016 Intel Corporation. All rights reserved.
|
||||
*
|
||||
* This file is free software: you can redistribute it and/or modify it
|
||||
* under the terms of the GNU General Public License as published by the
|
||||
* Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This file is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||
* See the GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#include <assert.h>
|
||||
|
||||
#include <AP_Math/AP_Math.h>
|
||||
|
||||
#include "SIM_Calibration.h"
|
||||
|
||||
#define MAX_ANGULAR_SPEED (2 * M_PI)
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
SITL::Calibration::Calibration(const char *home_str, const char *frame_str)
|
||||
: Aircraft(home_str, frame_str)
|
||||
{
|
||||
mass = 1.5f;
|
||||
}
|
||||
|
||||
void SITL::Calibration::update(const struct sitl_input& input)
|
||||
{
|
||||
Vector3f rot_accel{0, 0, 0};
|
||||
|
||||
float switcher_pwm = input.servos[4];
|
||||
if (switcher_pwm < 1100) {
|
||||
_stop_control(input, rot_accel);
|
||||
} else if (switcher_pwm < 1200) {
|
||||
_attitude_control(input, rot_accel);
|
||||
} else {
|
||||
_angular_velocity_control(input, rot_accel);
|
||||
}
|
||||
|
||||
accel_body(0, 0, 0);
|
||||
update_dynamics(rot_accel);
|
||||
update_position();
|
||||
}
|
||||
|
||||
void SITL::Calibration::_stop_control(const struct sitl_input& input,
|
||||
Vector3f& rot_accel)
|
||||
{
|
||||
Vector3f desired_angvel{0, 0, 0};
|
||||
Vector3f error = desired_angvel - gyro;
|
||||
float dt = frame_time_us * 1.0e-6f;
|
||||
rot_accel = error * (1.0f / dt);
|
||||
/* Provide a somewhat "smooth" transition */
|
||||
rot_accel *= 0.002f;
|
||||
}
|
||||
|
||||
void SITL::Calibration::_attitude_control(const struct sitl_input& input,
|
||||
Vector3f& rot_accel)
|
||||
{
|
||||
float desired_roll = -M_PI + 2 * M_PI * (input.servos[5] - 1000) / 1000.f;
|
||||
float desired_pitch = -M_PI + 2 * M_PI * (input.servos[6] - 1000) / 1000.f;
|
||||
float desired_yaw = -M_PI + 2 * M_PI * (input.servos[7] - 1000) / 1000.f;
|
||||
float dt = frame_time_us * 1.0e-6f;
|
||||
|
||||
Quaternion desired_q;
|
||||
desired_q.from_euler(desired_roll, desired_pitch, desired_yaw);
|
||||
desired_q.normalize();
|
||||
|
||||
Quaternion current_q;
|
||||
current_q.from_rotation_matrix(dcm);
|
||||
current_q.normalize();
|
||||
|
||||
Quaternion error_q = desired_q / current_q;
|
||||
|
||||
Vector3f angle_differential;
|
||||
error_q.normalize();
|
||||
error_q.to_axis_angle(angle_differential);
|
||||
|
||||
Vector3f desired_angvel = angle_differential * (1 / dt);
|
||||
/* Provide a somewhat "smooth" transition */
|
||||
desired_angvel *= .005f;
|
||||
|
||||
Vector3f error = desired_angvel - gyro;
|
||||
rot_accel = error * (1.0f / dt);
|
||||
}
|
||||
|
||||
void SITL::Calibration::_angular_velocity_control(const struct sitl_input& in,
|
||||
Vector3f& rot_accel)
|
||||
{
|
||||
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 dt = frame_time_us * 1.0e-6f;
|
||||
|
||||
axis.normalize();
|
||||
|
||||
Vector3f desired_angvel = axis * theta;
|
||||
Vector3f error = desired_angvel - gyro;
|
||||
|
||||
rot_accel = error * (1.0f / dt);
|
||||
/* Provide a somewhat "smooth" transition */
|
||||
rot_accel *= .05f;
|
||||
}
|
|
@ -0,0 +1,80 @@
|
|||
/*
|
||||
* Copyright (C) 2015-2016 Intel Corporation. All rights reserved.
|
||||
*
|
||||
* This file is free software: you can redistribute it and/or modify it
|
||||
* under the terms of the GNU General Public License as published by the
|
||||
* Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This file is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||
* See the GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include "SIM_Aircraft.h"
|
||||
|
||||
#include <AP_Math/AP_Math.h>
|
||||
|
||||
namespace SITL {
|
||||
|
||||
/**
|
||||
* Simulation model to simulate calibration of accelerometers and compasses.
|
||||
*
|
||||
* The vehicle rotation can be controlled by sending PWM values to the servos
|
||||
* input, denoted by PWM[i] for the i-th channel (starting by zero). All PWM
|
||||
* values must be in [1000, 2000], otherwise that will cause undefined
|
||||
* behavior.
|
||||
*
|
||||
* There are three 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.
|
||||
*
|
||||
* 2) Attitude (1100 <= PWM[4] < 1200):
|
||||
* Rotate the vehicle to the specified attitude. The attitude is defined
|
||||
* with the PWM channels 5, 6 and 7 for roll, pitch and yaw angles,
|
||||
* respectively. The PWM value for a desired angle in radians is given by:
|
||||
*
|
||||
* pwm(theta) = 1500 + 500 * round(theta / pi)
|
||||
* where -pi <= theta <= pi
|
||||
*
|
||||
* 3) Angular Velocity (1200 <= PWM[4] <= 2000):
|
||||
* Rotate the vehicle at a desired angular velocity. The angular velocity is
|
||||
* specified by a rotation axis and an angular speed.
|
||||
*
|
||||
* The x, y and z components of the rotation axis is given, respectively, by
|
||||
* the PWM channels 5, 6 and 7 with an offset of 1500. The rotation axis is
|
||||
* normalized internally, so that PWM[5,6,7] = [1600, 1300, 0] and
|
||||
* PWM[5,6,7] = [1700, 1100, 0] means the same normalized rotation axis.
|
||||
*
|
||||
* 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)),
|
||||
* where 0 <= theta <= 2 * pi
|
||||
*/
|
||||
class Calibration : public Aircraft {
|
||||
public:
|
||||
Calibration(const char *home_str, const char *frame_str);
|
||||
|
||||
void update(const struct sitl_input& input);
|
||||
|
||||
static Aircraft *create(const char *home_str, const char *frame_str) {
|
||||
return new Calibration(home_str, frame_str);
|
||||
}
|
||||
|
||||
private:
|
||||
void _stop_control(const struct sitl_input& input, Vector3f& rot_accel);
|
||||
|
||||
void _attitude_control(const struct sitl_input& input,
|
||||
Vector3f& rot_accel);
|
||||
|
||||
void _angular_velocity_control(const struct sitl_input& input,
|
||||
Vector3f& rot_accel);
|
||||
};
|
||||
}
|
Loading…
Reference in New Issue