107 float px_dot __attribute__((unused)) =
gvf_c_state.px_dot;
108 float py_dot __attribute__((unused)) =
gvf_c_state.py_dot;
117 float tx =
s * grad->
ny;
118 float ty = -
s * grad->
nx;
121 float H11 __attribute__((unused)) = hess->
H11;
122 float H12 __attribute__((unused)) = hess->
H12;
123 float H21 __attribute__((unused)) = hess->
H21;
124 float H22 __attribute__((unused)) = hess->
H22;
127 float pdx_dot = tx - ke * e * nx;
128 float pdy_dot = ty - ke * e * ny;
130 float norm_pd_dot = sqrtf(pdx_dot * pdx_dot + pdy_dot * pdy_dot);
131 float md_x = pdx_dot / norm_pd_dot;
132 float md_y = pdy_dot / norm_pd_dot;
134 #if defined(ROTORCRAFT_FIRMWARE)
139 #if defined(GVF_ROTORCRAFT_USE_ACCEL)
143 nav.speed.x = md_x * kn;
144 nav.speed.y = md_y * kn;
147 #warning "Using GVF for rotorcraft is still experimental, proceed with caution"
148 float n_norm = sqrtf(nx*nx+ny*ny);
149 float hess_px_dot = px_dot * H11 + py_dot * H12;
150 float hess_py_dot = px_dot * H21 + py_dot * H22;
152 float hess_pdx_dot = pdx_dot * H11 + pdy_dot * H12;
153 float hess_pdy_dot = pdx_dot * H21 + pdy_dot * H22;
155 float curvature_correction = tx * hess_px_dot + ty * hess_py_dot / (n_norm * n_norm);
156 float accel_correction_x = kn * hess_py_dot / n_norm;
157 float accel_correction_y = - kn * hess_px_dot / n_norm;
158 float accel_cmd_x = accel_correction_x + px_dot * curvature_correction;
159 float accel_cmd_y = accel_correction_y + py_dot * curvature_correction;
161 float speed_cmd_x = kn*tx / n_norm - ke * e * nx / (n_norm);
162 float speed_cmd_y = kn*ty / n_norm - ke * e * ny / (n_norm);
165 nav.accel.x = accel_cmd_x + (speed_cmd_x - px_dot);
166 nav.accel.y = accel_cmd_y + (speed_cmd_y - py_dot);
167 nav.heading = atan2f(md_x,md_y);
180 nav.heading = atan2f(md_x, md_y);
187 float Apd_dot_dot_x = -ke * (nx * px_dot + ny * py_dot) * nx;
188 float Apd_dot_dot_y = -ke * (nx * px_dot + ny * py_dot) * ny;
190 float Bpd_dot_dot_x = ((-ke * e * H11) +
s * H21) * px_dot
191 + ((-ke * e * H12) +
s * H22) * py_dot;
192 float Bpd_dot_dot_y = -(
s * H11 + (ke * e * H21)) * px_dot
193 - (
s * H12 + (ke * e * H22)) * py_dot;
195 float pd_dot_dot_x = Apd_dot_dot_x + Bpd_dot_dot_x;
196 float pd_dot_dot_y = Apd_dot_dot_y + Bpd_dot_dot_y;
198 float md_dot_const = -(md_x * pd_dot_dot_y - md_y * pd_dot_dot_x)
201 float md_dot_x = md_y * md_dot_const;
202 float md_dot_y = -md_x * md_dot_const;
204 float omega_d = -(md_dot_x * md_y - md_dot_y * md_x);
206 float mr_x = sinf(
course);
207 float mr_y = cosf(
course);
209 float e_n = (mr_x * md_y - mr_y * md_x);
210 float omega = omega_d + kn * e_n;
223 gvf_c_info.kappa = (nx*(H12*ny - nx*H22) + ny*(H21*nx - H11*ny))/powf(nx*nx + ny*ny,1.5);