ardupilot/libraries/SITL/SIM_Calibration.cpp

110 lines
3.4 KiB
C++

/*
* 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;
}