Line data Source code
1 : /*
2 : per-motor compass compensation
3 : */
4 :
5 : #include "AP_Compass.h"
6 : #include <GCS_MAVLink/GCS.h>
7 : #include <SRV_Channel/SRV_Channel.h>
8 :
9 : extern const AP_HAL::HAL &hal;
10 :
11 : const AP_Param::GroupInfo Compass_PerMotor::var_info[] = {
12 : // @Param: _EN
13 : // @DisplayName: per-motor compass correction enable
14 : // @Description: This enables per-motor compass corrections
15 : // @Values: 0:Disabled,1:Enabled
16 : // @User: Advanced
17 : AP_GROUPINFO_FLAGS("_EN", 1, Compass_PerMotor, enable, 0, AP_PARAM_FLAG_ENABLE),
18 :
19 : // @Param: _EXP
20 : // @DisplayName: per-motor exponential correction
21 : // @Description: This is the exponential correction for the power output of the motor for per-motor compass correction
22 : // @Range: 0 2
23 : // @Increment: 0.01
24 : // @User: Advanced
25 : AP_GROUPINFO("_EXP", 2, Compass_PerMotor, expo, 0.65),
26 :
27 : // @Param: 1_X
28 : // @DisplayName: Compass per-motor1 X
29 : // @Description: Compensation for X axis of motor1
30 : // @User: Advanced
31 :
32 : // @Param: 1_Y
33 : // @DisplayName: Compass per-motor1 Y
34 : // @Description: Compensation for Y axis of motor1
35 : // @User: Advanced
36 :
37 : // @Param: 1_Z
38 : // @DisplayName: Compass per-motor1 Z
39 : // @Description: Compensation for Z axis of motor1
40 : // @User: Advanced
41 : AP_GROUPINFO("1", 3, Compass_PerMotor, compensation[0], 0),
42 :
43 : // @Param: 2_X
44 : // @DisplayName: Compass per-motor2 X
45 : // @Description: Compensation for X axis of motor2
46 : // @User: Advanced
47 :
48 : // @Param: 2_Y
49 : // @DisplayName: Compass per-motor2 Y
50 : // @Description: Compensation for Y axis of motor2
51 : // @User: Advanced
52 :
53 : // @Param: 2_Z
54 : // @DisplayName: Compass per-motor2 Z
55 : // @Description: Compensation for Z axis of motor2
56 : // @User: Advanced
57 : AP_GROUPINFO("2", 4, Compass_PerMotor, compensation[1], 0),
58 :
59 : // @Param: 3_X
60 : // @DisplayName: Compass per-motor3 X
61 : // @Description: Compensation for X axis of motor3
62 : // @User: Advanced
63 :
64 : // @Param: 3_Y
65 : // @DisplayName: Compass per-motor3 Y
66 : // @Description: Compensation for Y axis of motor3
67 : // @User: Advanced
68 :
69 : // @Param: 3_Z
70 : // @DisplayName: Compass per-motor3 Z
71 : // @Description: Compensation for Z axis of motor3
72 : // @User: Advanced
73 : AP_GROUPINFO("3", 5, Compass_PerMotor, compensation[2], 0),
74 :
75 : // @Param: 4_X
76 : // @DisplayName: Compass per-motor4 X
77 : // @Description: Compensation for X axis of motor4
78 : // @User: Advanced
79 :
80 : // @Param: 4_Y
81 : // @DisplayName: Compass per-motor4 Y
82 : // @Description: Compensation for Y axis of motor4
83 : // @User: Advanced
84 :
85 : // @Param: 4_Z
86 : // @DisplayName: Compass per-motor4 Z
87 : // @Description: Compensation for Z axis of motor4
88 : // @User: Advanced
89 : AP_GROUPINFO("4", 6, Compass_PerMotor, compensation[3], 0),
90 :
91 : AP_GROUPEND
92 : };
93 :
94 : // constructor
95 2098 : Compass_PerMotor::Compass_PerMotor(Compass &_compass) :
96 10490 : compass(_compass)
97 : {
98 2098 : AP_Param::setup_object_defaults(this, var_info);
99 2098 : }
100 :
101 : // return current scaled motor output
102 0 : float Compass_PerMotor::scaled_output(uint8_t motor)
103 : {
104 0 : if (!have_motor_map) {
105 0 : if (SRV_Channels::find_channel(SRV_Channel::k_motor1, motor_map[0]) &&
106 0 : SRV_Channels::find_channel(SRV_Channel::k_motor2, motor_map[1]) &&
107 0 : SRV_Channels::find_channel(SRV_Channel::k_motor3, motor_map[2]) &&
108 0 : SRV_Channels::find_channel(SRV_Channel::k_motor4, motor_map[3])) {
109 0 : have_motor_map = true;
110 : }
111 : }
112 0 : if (!have_motor_map) {
113 0 : return 0;
114 : }
115 :
116 : // this currently assumes first 4 channels.
117 0 : uint16_t pwm = hal.rcout->read_last_sent(motor_map[motor]);
118 :
119 : // get 0 to 1 motor demand
120 0 : float output = (hal.rcout->scale_esc_to_unity(pwm)+1) * 0.5f;
121 :
122 0 : if (output <= 0) {
123 0 : return 0;
124 : }
125 :
126 : // scale for voltage
127 0 : output *= voltage;
128 :
129 : // apply expo correction
130 0 : output = powf(output, expo);
131 0 : return output;
132 : }
133 :
134 : // per-motor calibration update
135 0 : void Compass_PerMotor::calibration_start(void)
136 : {
137 0 : for (uint8_t i=0; i<4; i++) {
138 0 : field_sum[i].zero();
139 0 : output_sum[i] = 0;
140 0 : count[i] = 0;
141 0 : start_ms[i] = 0;
142 : }
143 :
144 : // we need to ensure we get current data by throwing away several
145 : // samples. The offsets may have just changed from an offset
146 : // calibration
147 0 : for (uint8_t i=0; i<4; i++) {
148 0 : compass.read();
149 0 : hal.scheduler->delay(50);
150 : }
151 :
152 0 : base_field = compass.get_field(0);
153 0 : running = true;
154 0 : }
155 :
156 : // per-motor calibration update
157 0 : void Compass_PerMotor::calibration_update(void)
158 : {
159 0 : uint32_t now = AP_HAL::millis();
160 :
161 : // accumulate per-motor sums
162 0 : for (uint8_t i=0; i<4; i++) {
163 0 : float output = scaled_output(i);
164 :
165 0 : if (output <= 0) {
166 : // motor is off
167 0 : start_ms[i] = 0;
168 0 : continue;
169 : }
170 0 : if (start_ms[i] == 0) {
171 0 : start_ms[i] = now;
172 : }
173 0 : if (now - start_ms[i] < 500) {
174 : // motor must run for 0.5s to settle
175 0 : continue;
176 : }
177 :
178 : // accumulate a sample
179 0 : field_sum[i] += compass.get_field(0);
180 0 : output_sum[i] += output;
181 0 : count[i]++;
182 : }
183 0 : }
184 :
185 : // calculate per-motor calibration values
186 0 : void Compass_PerMotor::calibration_end(void)
187 : {
188 0 : for (uint8_t i=0; i<4; i++) {
189 0 : if (count[i] == 0) {
190 0 : continue;
191 : }
192 :
193 : // calculate effective output
194 0 : float output = output_sum[i] / count[i];
195 :
196 : // calculate amount that field changed from base field
197 0 : Vector3f field_change = base_field - (field_sum[i] / count[i]);
198 0 : if (output <= 0) {
199 0 : continue;
200 : }
201 :
202 0 : Vector3f c = field_change / output;
203 0 : compensation[i].set_and_save(c);
204 : }
205 :
206 : // enable per-motor compensation
207 0 : enable.set_and_save(1);
208 :
209 0 : running = false;
210 0 : }
211 :
212 : /*
213 : calculate total offset for per-motor compensation
214 : */
215 0 : void Compass_PerMotor::compensate(Vector3f &offset)
216 : {
217 0 : offset.zero();
218 :
219 0 : if (running) {
220 : // don't compensate while calibrating
221 0 : return;
222 : }
223 :
224 0 : for (uint8_t i=0; i<4; i++) {
225 0 : float output = scaled_output(i);
226 :
227 0 : const Vector3f &c = compensation[i].get();
228 :
229 0 : offset += c * output;
230 : }
231 : }
|
