mirror of https://github.com/ArduPilot/ardupilot
110 lines
3.4 KiB
C++
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;
|
|
}
|