Line data Source code
1 : /*
2 : * Copyright (C) 2015-2016 Intel Corporation. All rights reserved.
3 : *
4 : * This file is free software: you can redistribute it and/or modify it
5 : * under the terms of the GNU General Public License as published by the
6 : * Free Software Foundation, either version 3 of the License, or
7 : * (at your option) any later version.
8 : *
9 : * This file is distributed in the hope that it will be useful, but
10 : * WITHOUT ANY WARRANTY; without even the implied warranty of
11 : * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
12 : * See the GNU General Public License for more details.
13 : *
14 : * You should have received a copy of the GNU General Public License along
15 : * with this program. If not, see <http://www.gnu.org/licenses/>.
16 : */
17 : #include <assert.h>
18 :
19 : #include <AP_Math/AP_Math.h>
20 :
21 : #include "SIM_Calibration.h"
22 :
23 : #define MAX_ANGULAR_SPEED (2 * M_PI)
24 :
25 : #include <stdio.h>
26 :
27 11 : SITL::Calibration::Calibration(const char *frame_str)
28 11 : : Aircraft(frame_str)
29 : {
30 11 : mass = 1.5f;
31 11 : }
32 :
33 3481756 : void SITL::Calibration::update(const struct sitl_input &input)
34 : {
35 3481756 : Vector3f rot_accel{0, 0, 0};
36 :
37 3481756 : float switcher_pwm = input.servos[4];
38 3481756 : if (switcher_pwm < 1100) {
39 561094 : _stop_control(input, rot_accel);
40 2920662 : } else if (switcher_pwm < 1200) {
41 956439 : _attitude_control(input, rot_accel);
42 1964223 : } else if (switcher_pwm < 1300) {
43 0 : _calibration_poses(rot_accel);
44 : } else {
45 1964223 : _angular_velocity_control(input, rot_accel);
46 : }
47 :
48 3481756 : accel_body.zero();
49 3481756 : update_dynamics(rot_accel);
50 3481756 : update_position();
51 3481756 : time_advance();
52 :
53 : // update magnetic field
54 3481756 : update_mag_field_bf();
55 :
56 3481756 : update_battery();
57 3481756 : }
58 :
59 561094 : void SITL::Calibration::_stop_control(const struct sitl_input &input,
60 : Vector3f& rot_accel)
61 : {
62 561094 : Vector3f desired_angvel{0, 0, 0};
63 561094 : Vector3f error = desired_angvel - gyro;
64 561094 : float dt = frame_time_us * 1.0e-6f;
65 561094 : rot_accel = error * (1.0f / dt);
66 : /* Provide a somewhat "smooth" transition */
67 561094 : rot_accel *= 0.002f;
68 561094 : }
69 :
70 956439 : void SITL::Calibration::_attitude_control(const struct sitl_input &input,
71 : Vector3f& rot_accel)
72 : {
73 956439 : float desired_roll = -M_PI + 2 * M_PI * (input.servos[5] - 1000) / 1000.f;
74 956439 : float desired_pitch = -M_PI + 2 * M_PI * (input.servos[6] - 1000) / 1000.f;
75 956439 : float desired_yaw = -M_PI + 2 * M_PI * (input.servos[7] - 1000) / 1000.f;
76 :
77 956439 : _attitude_set(desired_roll, desired_pitch, desired_yaw, rot_accel);
78 956439 : }
79 :
80 :
81 956439 : void SITL::Calibration::_attitude_set(float desired_roll, float desired_pitch, float desired_yaw,
82 : Vector3f& rot_accel)
83 : {
84 956439 : float dt = frame_time_us * 1.0e-6f;
85 :
86 956439 : Quaternion desired_q;
87 956439 : desired_q.from_euler(desired_roll, desired_pitch, desired_yaw);
88 956439 : desired_q.normalize();
89 :
90 956439 : Quaternion current_q;
91 956439 : current_q.from_rotation_matrix(dcm);
92 956439 : current_q.normalize();
93 :
94 956439 : Quaternion error_q = desired_q / current_q;
95 :
96 956439 : Vector3f angle_differential;
97 956439 : error_q.normalize();
98 956439 : error_q.to_axis_angle(angle_differential);
99 :
100 956439 : Vector3f desired_angvel = angle_differential * (1 / dt);
101 : /* Provide a somewhat "smooth" transition */
102 956439 : desired_angvel *= 0.005f;
103 :
104 956439 : Vector3f error = desired_angvel - gyro;
105 956439 : rot_accel = error * (1.0f / dt);
106 956439 : }
107 :
108 1964223 : void SITL::Calibration::_angular_velocity_control(const struct sitl_input &in,
109 : Vector3f& rot_accel)
110 : {
111 1964223 : Vector3f axis{(float)(in.servos[5] - 1500),
112 1964223 : (float)(in.servos[6] - 1500),
113 1964223 : (float)(in.servos[7] - 1500)};
114 1964223 : float theta = MAX_ANGULAR_SPEED * (in.servos[4] - 1300) / 700.f;
115 1964223 : float dt = frame_time_us * 1.0e-6f;
116 :
117 1964223 : if (axis.length() > 0) {
118 1963539 : axis.normalize();
119 : }
120 :
121 1964223 : Vector3f desired_angvel = axis * theta;
122 1964223 : Vector3f error = desired_angvel - gyro;
123 :
124 1964223 : rot_accel = error * (1.0f / dt);
125 : /* Provide a somewhat "smooth" transition */
126 1964223 : rot_accel *= 0.05f;
127 1964223 : }
128 :
129 : /*
130 : move continuously through 6 calibration poses, doing a rotation
131 : about each pose over 3 seconds
132 : */
133 0 : void SITL::Calibration::_calibration_poses(Vector3f& rot_accel)
134 : {
135 : const struct pose {
136 : int16_t roll, pitch, yaw;
137 : uint8_t axis;
138 0 : } poses[] = {
139 : { 0, 0, 0, 0 },
140 : { 0, 0, 0, 1 },
141 : { 0, 0, 0, 2 },
142 : { 90, 0, 0, 1 },
143 : { 0, 90, 0, 1 },
144 : { 0, 180, 0, 2 },
145 : { 45, 0, 0, 1 },
146 : { 0, 45, 0, 2 },
147 : { 0, 0, 45, 0 },
148 : { 30, 0, 0, 1 },
149 : { 0, 30, 0, 0 },
150 : { 30, 0, 0, 1 },
151 : { 0, 0, 30, 0 },
152 : { 0, 0, 30, 1 },
153 : { 60, 20, 0, 1 },
154 : { 0, 50, 10, 0 },
155 : { 0, 30, 50, 1 },
156 : { 0, 30, 50, 2 },
157 : };
158 0 : const float secs_per_pose = 6;
159 0 : const float rate = radians(360 / secs_per_pose);
160 0 : float tnow = AP_HAL::millis() * 1.0e-3;
161 0 : float t_in_pose = fmod(tnow, secs_per_pose);
162 0 : uint8_t pose_num = ((unsigned)(tnow / secs_per_pose)) % ARRAY_SIZE(poses);
163 0 : const struct pose &pose = poses[pose_num];
164 :
165 : // let the sensor smoothing create sensible gyro values
166 0 : use_smoothing = true;
167 0 : dcm.identity();
168 0 : dcm.from_euler(radians(pose.roll), radians(pose.pitch), radians(pose.yaw));
169 :
170 0 : Vector3f axis;
171 0 : axis[pose.axis] = 1;
172 0 : float rot_angle = rate * t_in_pose;
173 0 : Matrix3f r2;
174 0 : r2.from_axis_angle(axis, rot_angle);
175 0 : dcm = r2 * dcm;
176 :
177 0 : accel_body = {0, 0, -GRAVITY_MSS};
178 0 : accel_body = dcm.transposed() * accel_body;
179 0 : }
|
