/* skew_midpoint() finds the middle of the minimal distance segment between
skew rays. Undefined for parallel rays.
Reference for algorithm:
docs/skew_midpoint.pdf
Arguments:
vec3d vert1, direct1 - vertex and direction unit vector of one ray.
vec3d vert2, direct2 - vertex and direction unit vector of other ray.
vec3d res - output buffer for midpoint result.
Returns:
The minimal distance between the skew lines.
*/
double skew_midpoint(vec3d vert1, vec3d direct1, vec3d vert2, vec3d direct2,
vec3d res)
{
vec3d perp_both, sp_diff, on1, on2, temp;
double scale;
/* vector between starting points */
vec_subt(vert2, vert1, sp_diff);
/* The shortest distance is on a line perpendicular to both rays. */
vec_cross(direct1, direct2, perp_both);
scale = vec_dot(perp_both, perp_both);
/* position along each ray */
vec_cross(sp_diff, direct2, temp);
vec_scalar_mul(direct1, vec_dot(perp_both, temp)/scale, temp);
vec_add(vert1, temp, on1);
vec_cross(sp_diff, direct1, temp);
vec_scalar_mul(direct2, vec_dot(perp_both, temp)/scale, temp);
vec_add(vert2, temp, on2);
/* Distance: */
scale = vec_diff_norm(on1, on2);
/* Average */
vec_add(on1, on2, res);
vec_scalar_mul(res, 0.5, res);
return scale;
}
static void ahrs_update_imu(ahrs_t *ahrs, real_t gx, real_t gy, real_t gz,
real_t ax, real_t ay, real_t az, real_t dt)
{
/* get rate of change of quaternion from gyroscope: */
vec4_t qdot;
vec4_init(&qdot);
qdot.ve[0] = REAL(0.5) * (-ahrs->quat.q1 * gx - ahrs->quat.q2 * gy - ahrs->quat.q3 * gz);
qdot.ve[1] = REAL(0.5) * ( ahrs->quat.q0 * gx + ahrs->quat.q2 * gz - ahrs->quat.q3 * gy);
qdot.ve[2] = REAL(0.5) * ( ahrs->quat.q0 * gy - ahrs->quat.q1 * gz + ahrs->quat.q3 * gx);
qdot.ve[3] = REAL(0.5) * ( ahrs->quat.q0 * gz + ahrs->quat.q1 * gy - ahrs->quat.q2 * gx);
if (!((ax == REAL(0.0)) && (ay == REAL(0.0)) && (az == REAL(0.0))))
{
/* normalize accelerometer measurements: */
ahrs_normalize_3(&ax, &ay, &az);
/* auxiliary variables to avoid repeated arithmetic: */
real_t _2q0 = REAL(2.0) * ahrs->quat.q0;
real_t _2q1 = REAL(2.0) * ahrs->quat.q1;
real_t _2q2 = REAL(2.0) * ahrs->quat.q2;
real_t _2q3 = REAL(2.0) * ahrs->quat.q3;
real_t _4q0 = REAL(4.0) * ahrs->quat.q0;
real_t _4q1 = REAL(4.0) * ahrs->quat.q1;
real_t _4q2 = REAL(4.0) * ahrs->quat.q2;
real_t _8q1 = REAL(8.0) * ahrs->quat.q1;
real_t _8q2 = REAL(8.0) * ahrs->quat.q2;
real_t q0q0 = ahrs->quat.q0 * ahrs->quat.q0;
real_t q1q1 = ahrs->quat.q1 * ahrs->quat.q1;
real_t q2q2 = ahrs->quat.q2 * ahrs->quat.q2;
real_t q3q3 = ahrs->quat.q3 * ahrs->quat.q3;
/* gradient decent algorithm corrective step: */
vec4_t s;
vec4_init(&s);
s.ve[0] = _4q0 * q2q2 - _2q2 * ax + _4q0 * q1q1 + _2q1 * ay;
s.ve[1] = _4q1 * q3q3 + _2q3 * ax + REAL(4.0) * q0q0 * ahrs->quat.q1 + _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 - _4q1 * az;
s.ve[2] = REAL(4.0) * q0q0 * ahrs->quat.q2 - _2q0 * ax + _4q2 * q3q3 + _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 - _4q2 * az;
s.ve[3] = REAL(4.0) * q1q1 * ahrs->quat.q3 + _2q1 * ax + REAL(4.0) * q2q2 * ahrs->quat.q3 + _2q2 * ay;
vec_normalize(&s);
/* apply feedback step: */
vec_scalar_mul(&s, &s, ahrs->beta);
vec_sub(&qdot, &qdot, &s);
}
/* integrate rate of change to yield quaternion: */
vec_scalar_mul(&qdot, &qdot, dt);
vec_add(&ahrs->quat, &ahrs->quat, &qdot);
/* normalise quaternion: */
quat_normalize(&ahrs->quat);
}
END_TEST
START_TEST(test_scalar_mul)
{
vec3d v1 = {1., 2., 3.}, v2 = {4., 8., 12.}, out;
vec_scalar_mul(v1, 4., out);
fail_unless(vec_cmp(out, v2));
}
/* point_position() calculates an average 3D position implied by the rays
sent toward it from cameras through the image projections of the point.
Arguments:
vec2d targets[] - for each camera, the 2D metric, flat, centred coordinates
of the identified point projection.
int num_cams - number of cameras ( = number of elements in ``targets``).
mm_np *multimed_pars - multimedia parameters struct for ray tracing through
several layers.
Calibration* cals[] - each camera's calibration object.
vec3d res - the result average point.
Returns:
The ray convergence measure, an average of skew ray distance across all
ray pairs.
*/
double point_position(vec2d targets[], int num_cams, mm_np *multimed_pars,
Calibration* cals[], vec3d res)
{
int cam, pair; /* loop counters */
int num_used_pairs = 0; /* averaging accumulators */
double dtot = 0;
vec3d point_tot = {0., 0., 0.};
vec3d* vertices = (vec3d *) calloc(num_cams, sizeof(vec3d));
vec3d* directs = (vec3d *) calloc(num_cams, sizeof(vec3d));
vec3d point;
/* Shoot rays from all cameras. */
for (cam = 0; cam < num_cams; cam++) {
if (targets[cam][0] != PT_UNUSED) {
ray_tracing(targets[cam][0], targets[cam][1], cals[cam],
*multimed_pars, vertices[cam], directs[cam]);
}
}
/* Check intersection distance for each pair of rays and find position */
for (cam = 0; cam < num_cams; cam++) {
if (targets[cam][0] == PT_UNUSED) continue;
for (pair = cam + 1; pair < num_cams; pair++) {
if (targets[pair][0] == PT_UNUSED) continue;
num_used_pairs++;
dtot += skew_midpoint(vertices[cam], directs[cam],
vertices[pair], directs[pair], point);
vec_add(point_tot, point, point_tot);
}
}
free(vertices);
free(directs);
vec_scalar_mul(point_tot, 1./num_used_pairs, res);
return (dtot / num_used_pairs);
}
/* search_volume_center_moving() finds the position of the center of the search
* volume for a moving particle using the velocity of last step.
*
* Arguments:
* vec3d prev_pos - previous position
* vec3d curr_pos - current position
* vec3d *output - output variable, for the calculated
* position.
*/
void search_volume_center_moving(vec3d prev_pos, vec3d curr_pos, vec3d output)
{
vec_scalar_mul(curr_pos, 2, output);
vec_subt(output, prev_pos, output);
}
int ahrs_update(ahrs_t *ahrs, const marg_data_t *marg_data, const real_t dt)
{
int ret;
if (ahrs->beta > ahrs->beta_end)
{
ahrs->beta -= ahrs->beta_step * dt;
if (ahrs->beta < ahrs->beta_end)
{
ahrs->beta = ahrs->beta_end;
ret = 1;
}
else
{
ret = -1;
}
}
else
{
ret = 0;
}
real_t gx = marg_data->gyro.x;
real_t gy = marg_data->gyro.y;
real_t gz = marg_data->gyro.z;
real_t ax = marg_data->acc.x;
real_t ay = marg_data->acc.y;
real_t az = marg_data->acc.z;
real_t mx = marg_data->mag.x;
real_t my = marg_data->mag.y;
real_t mz = marg_data->mag.z;
/* use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
or if requested upon initialization: */
if (((mx == REAL(0.0)) && (my == REAL(0.0)) && (mz == REAL(0.0))) || ahrs->type == AHRS_ACC)
{
ahrs_update_imu(ahrs, gx, gy, gz, ax, ay, az, dt);
goto out;
}
/* Rate of change of quaternion from gyroscope */
vec4_t qdot;
vec4_init(&qdot);
qdot.ve[0] = REAL(0.5) * (-ahrs->quat.q1 * gx - ahrs->quat.q2 * gy - ahrs->quat.q3 * gz);
qdot.ve[1] = REAL(0.5) * ( ahrs->quat.q0 * gx + ahrs->quat.q2 * gz - ahrs->quat.q3 * gy);
qdot.ve[2] = REAL(0.5) * ( ahrs->quat.q0 * gy - ahrs->quat.q1 * gz + ahrs->quat.q3 * gx);
qdot.ve[3] = REAL(0.5) * ( ahrs->quat.q0 * gz + ahrs->quat.q1 * gy - ahrs->quat.q2 * gx);
if (!((ax == REAL(0.0)) && (ay == REAL(0.0)) && (az == REAL(0.0))))
{
/* normalise accelerometer and magnetometer measurements: */
ahrs_normalize_3(&ax, &ay, &az);
ahrs_normalize_3(&mx, &my, &mz);
/* auxiliary variables to avoid repeated arithmetic: */
real_t _2q0mx = REAL(2.0) * ahrs->quat.q0 * mx;
real_t _2q0my = REAL(2.0) * ahrs->quat.q0 * my;
real_t _2q0mz = REAL(2.0) * ahrs->quat.q0 * mz;
real_t _2q1mx = REAL(2.0) * ahrs->quat.q1 * mx;
real_t _2q0 = REAL(2.0) * ahrs->quat.q0;
real_t _2q1 = REAL(2.0) * ahrs->quat.q1;
real_t _2q2 = REAL(2.0) * ahrs->quat.q2;
real_t _2q3 = REAL(2.0) * ahrs->quat.q3;
real_t _2q0q2 = REAL(2.0) * ahrs->quat.q0 * ahrs->quat.q2;
real_t _2q2q3 = REAL(2.0) * ahrs->quat.q2 * ahrs->quat.q3;
real_t q0q0 = ahrs->quat.q0 * ahrs->quat.q0;
real_t q0q1 = ahrs->quat.q0 * ahrs->quat.q1;
real_t q0q2 = ahrs->quat.q0 * ahrs->quat.q2;
real_t q0q3 = ahrs->quat.q0 * ahrs->quat.q3;
real_t q1q1 = ahrs->quat.q1 * ahrs->quat.q1;
real_t q1q2 = ahrs->quat.q1 * ahrs->quat.q2;
real_t q1q3 = ahrs->quat.q1 * ahrs->quat.q3;
real_t q2q2 = ahrs->quat.q2 * ahrs->quat.q2;
real_t q2q3 = ahrs->quat.q2 * ahrs->quat.q3;
real_t q3q3 = ahrs->quat.q3 * ahrs->quat.q3;
float q0 = ahrs->quat.q0;
float q1 = ahrs->quat.q1;
float q2 = ahrs->quat.q2;
float q3 = ahrs->quat.q3;
/* reference direction of Earth's magnetic field: */
real_t hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
real_t hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
real_t _2bx = sqrt(hx * hx + hy * hy);
real_t _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
real_t _4bx = 2.0f * _2bx;
real_t _4bz = 2.0f * _2bz;
real_t _8bx = 2.0f * _4bx;
real_t _8bz = 2.0f * _4bz;
// Gradient decent algorithm corrective step
vec4_t s;
vec4_init(&s);
s.ve[0] = -_2q2*(2.0f*(q1q3 - q0q2) - ax) + _2q1*(2.0f*(q0q1 + q2q3) - ay) + -_4bz*q2*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx) + (-_4bx*q3+_4bz*q1)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my) + _4bx*q2*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
s.ve[1] = _2q3*(2.0f*(q1q3 - q0q2) - ax) + _2q0*(2.0f*(q0q1 + q2q3) - ay) + -4.0f*q1*(2.0f*(0.5 - q1q1 - q2q2) - az) + _4bz*q3*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx) + (_4bx*q2+_4bz*q0)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my) + (_4bx*q3-_8bz*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
s.ve[2] = -_2q0*(2.0f*(q1q3 - q0q2) - ax) + _2q3*(2.0f*(q0q1 + q2q3) - ay) + (-4.0f*q2)*(2.0f*(0.5 - q1q1 - q2q2) - az) + (-_8bx*q2-_4bz*q0)*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(_4bx*q1+_4bz*q3)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q0-_8bz*q2)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
s.ve[3] = _2q1*(2.0f*(q1q3 - q0q2) - ax) + _2q2*(2.0f*(q0q1 + q2q3) - ay)+(-_8bx*q3+_4bz*q1)*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(-_4bx*q0+_4bz*q2)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
vec_normalize(&s);
/* apply feedback step: */
vec_scalar_mul(&s, &s, ahrs->beta);
vec_sub(&qdot, &qdot, &s);
}
/* integrate rate of change to yield quaternion: */
vec_scalar_mul(&qdot, &qdot, dt);
vec_add(&ahrs->quat, &ahrs->quat, &qdot);
/* normalise quaternion: */
quat_normalize(&ahrs->quat);
out:
return ret;
}
void main_step(const float dt,
const marg_data_t *marg_data,
const gps_data_t *gps_data,
const float ultra,
const float baro,
const float voltage,
const float current,
const float channels[MAX_CHANNELS],
const uint16_t sensor_status,
const bool override_hw)
{
vec2_t ne_pos_err, ne_speed_sp, ne_spd_err;
vec2_init(&ne_pos_err);
vec2_init(&ne_speed_sp);
vec2_init(&ne_spd_err);
vec3_t mag_normal;
vec3_init(&mag_normal);
float u_pos_err = 0.0f;
float u_spd_err = 0.0f;
int bb_rate;
if (calibrate)
{
ONCE(LOG(LL_INFO, "publishing calibration data; actuators disabled"));
bb_rate = 20;
goto out;
}
else
bb_rate = 1;
pos_in.dt = dt;
pos_in.ultra_u = ultra;
pos_in.baro_u = baro;
if (!(sensor_status & MARG_VALID))
{
marg_err += 1;
if (marg_err > 500)
{
/* we are in serious trouble */
memset(setpoints, 0, sizeof(float) * platform.n_motors);
platform_write_motors(setpoints);
die();
}
goto out;
}
marg_err = 0;
float cal_channels[MAX_CHANNELS];
memcpy(cal_channels, channels, sizeof(cal_channels));
if (sensor_status & RC_VALID)
{
/* apply calibration if remote control input is valid: */
float cal_data[3] = {channels[CH_PITCH], channels[CH_ROLL], channels[CH_YAW]};
cal_sample_apply(&rc_cal, cal_data);
cal_channels[CH_PITCH] = cal_data[0];
cal_channels[CH_ROLL] = cal_data[1];
cal_channels[CH_YAW] = cal_data[2];
}
/* perform gyro calibration: */
marg_data_t cal_marg_data;
marg_data_init(&cal_marg_data);
marg_data_copy(&cal_marg_data, marg_data);
if (cal_sample_apply(&gyro_cal, cal_marg_data.gyro.ve) == 0 && gyro_moved(&marg_data->gyro))
{
if (interval_measure(&gyro_move_interval) > 1.0)
LOG(LL_ERROR, "gyro moved during calibration, retrying");
cal_reset(&gyro_cal);
}
/* update relative gps position, if we have a fix: */
float mag_decl;
if (sensor_status & GPS_VALID)
{
gps_util_update(&gps_rel_data, gps_data);
pos_in.pos_n = gps_rel_data.dn;
pos_in.pos_e = gps_rel_data.de;
pos_in.speed_n = gps_rel_data.speed_n;
pos_in.speed_e = gps_rel_data.speed_e;
ONCE(gps_start_set(gps_data));
mag_decl = mag_decl_get();
}
else
{
pos_in.pos_n = 0.0f;
pos_in.pos_e = 0.0f;
pos_in.speed_n = 0.0f;
pos_in.speed_e = 0.0f;
mag_decl = 0.0f;
}
/* apply acc/mag calibration: */
acc_mag_cal_apply(&cal_marg_data.acc, &cal_marg_data.mag);
vec_copy(&mag_normal, &cal_marg_data.mag);
/* apply current magnetometer compensation: */
cmc_apply(&cal_marg_data.mag, current);
//printf("%f %f %f %f\n", current, cal_marg_data.mag.x, cal_marg_data.mag.y, cal_marg_data.mag.z);
/* determine flight state: */
bool flying = flight_state_update(&cal_marg_data.acc.ve[0]);
if (!flying && pos_in.ultra_u == 7.0)
pos_in.ultra_u = 0.2;
/* compute orientation estimate: */
euler_t euler;
int ahrs_state = cal_ahrs_update(&euler, &cal_marg_data, mag_decl, dt);
if (ahrs_state < 0 || !cal_complete(&gyro_cal))
goto out;
ONCE(init = 1; LOG(LL_DEBUG, "system initialized; orientation = yaw: %f pitch: %f roll: %f", euler.yaw, euler.pitch, euler.roll));
/* rotate local ACC measurements into global NEU reference frame: */
vec3_t world_acc;
vec3_init(&world_acc);
body_to_neu(&world_acc, &euler, &cal_marg_data.acc);
/* center global ACC readings: */
filter1_run(&lp_filter, &world_acc.ve[0], &acc_vec[0]);
FOR_N(i, 3)
pos_in.acc.ve[i] = world_acc.ve[i] - acc_vec[i];
/* compute next 3d position estimate using Kalman filters: */
pos_t pos_est;
vec2_init(&pos_est.ne_pos);
vec2_init(&pos_est.ne_speed);
pos_update(&pos_est, &pos_in);
/* execute flight logic (sets cm_x parameters used below): */
bool hard_off = false;
bool motors_enabled = flight_logic_run(&hard_off, sensor_status, flying, cal_channels, euler.yaw, &pos_est.ne_pos, pos_est.baro_u.pos, pos_est.ultra_u.pos, platform.max_thrust_n, platform.mass_kg, dt);
/* execute up position/speed controller(s): */
float a_u = 0.0f;
if (cm_u_is_pos())
{
if (cm_u_is_baro_pos())
a_u = u_ctrl_step(&u_pos_err, cm_u_sp(), pos_est.baro_u.pos, pos_est.baro_u.speed, dt);
else /* ultra pos */
a_u = u_ctrl_step(&u_pos_err, cm_u_sp(), pos_est.ultra_u.pos, pos_est.ultra_u.speed, dt);
}
else if (cm_u_is_spd())
a_u = u_speed_step(&u_spd_err, cm_u_sp(), pos_est.baro_u.speed, dt);
else
a_u = cm_u_sp();
/* execute north/east navigation and/or read speed vector input: */
if (cm_att_is_gps_pos())
{
vec2_t pos_sp;
vec2_init(&pos_sp);
cm_att_sp(&pos_sp);
navi_run(&ne_speed_sp, &ne_pos_err, &pos_sp, &pos_est.ne_pos, dt);
}
else if (cm_att_is_gps_spd())
cm_att_sp(&ne_speed_sp);
/* execute north/east speed controller: */
vec2_t a_ne;
vec2_init(&a_ne);
ne_speed_ctrl_run(&a_ne, &ne_spd_err, &ne_speed_sp, dt, &pos_est.ne_speed);
vec3_t a_neu;
vec3_set(&a_neu, a_ne.x, a_ne.y, a_u);
vec3_t f_neu;
vec3_init(&f_neu);
vec_scalar_mul(&f_neu, &a_neu, platform.mass_kg); /* f[i] = a[i] * m, makes ctrl device-independent */
float hover_force = platform.mass_kg * 9.81f;
f_neu.z += hover_force;
/* execute NEU forces optimizer: */
float thrust;
vec2_t pr_pos_sp;
vec2_init(&pr_pos_sp);
att_thrust_calc(&pr_pos_sp, &thrust, &f_neu, euler.yaw, platform.max_thrust_n, 0);
/* execute direct attitude angle control, if requested: */
if (cm_att_is_angles())
cm_att_sp(&pr_pos_sp);
/* execute attitude angles controller: */
vec2_t att_err;
vec2_init(&att_err);
vec2_t pr_spd;
vec2_set(&pr_spd, -cal_marg_data.gyro.y, cal_marg_data.gyro.x);
vec2_t pr_pos, pr_pos_ctrl;
vec2_set(&pr_pos, -euler.pitch, euler.roll);
vec2_init(&pr_pos_ctrl);
att_ctrl_step(&pr_pos_ctrl, &att_err, dt, &pr_pos, &pr_spd, &pr_pos_sp);
float piid_sp[3] = {0.0f, 0.0f, 0.0f};
piid_sp[PIID_PITCH] = pr_pos_ctrl.ve[0];
piid_sp[PIID_ROLL] = pr_pos_ctrl.ve[1];
/* execute direct attitude rate control, if requested:*/
if (cm_att_is_rates())
{
vec2_t rates_sp;
vec2_init(&rates_sp);
cm_att_sp(&rates_sp);
piid_sp[PIID_PITCH] = rates_sp.ve[0];
piid_sp[PIID_ROLL] = rates_sp.ve[1];
}
/* execute yaw position controller: */
float yaw_speed_sp, yaw_err;
if (cm_yaw_is_pos())
yaw_speed_sp = yaw_ctrl_step(&yaw_err, cm_yaw_sp(), euler.yaw, cal_marg_data.gyro.z, dt);
else
yaw_speed_sp = cm_yaw_sp(); /* direct yaw speed control */
//yaw_speed_sp = yaw_ctrl_step(&yaw_err, 0.0, euler.yaw, cal_marg_data.gyro.z, dt);
piid_sp[PIID_YAW] = yaw_speed_sp;
/* execute stabilizing PIID controller: */
f_local_t f_local = {{thrust, 0.0f, 0.0f, 0.0f}};
float piid_gyros[3] = {cal_marg_data.gyro.x, -cal_marg_data.gyro.y, cal_marg_data.gyro.z};
piid_run(&f_local.ve[1], piid_gyros, piid_sp, dt);
/* computate rpm ^ 2 out of the desired forces: */
inv_coupling_calc(rpm_square, f_local.ve);
/* compute motor setpoints out of rpm ^ 2: */
piid_int_enable(platform_ac_calc(setpoints, motors_spinning(), voltage, rpm_square));
/* enables motors, if flight logic requests it: */
motors_state_update(flying, dt, motors_enabled);
/* reset controllers, if motors are not controllable: */
if (!motors_controllable())
{
navi_reset();
ne_speed_ctrl_reset();
u_ctrl_reset();
att_ctrl_reset();
piid_reset();
}
/* handle special cases for motor setpoints: */
if (motors_starting())
FOR_N(i, platform.n_motors) setpoints[i] = platform.ac.min;
if (hard_off || motors_stopping())
FOR_N(i, platform.n_motors) setpoints[i] = platform.ac.off_val;
printf("%f %f %f\n", rad2deg(euler.pitch), rad2deg(euler.roll), rad2deg(euler.yaw));
/* write motors: */
if (!override_hw)
{
//platform_write_motors(setpoints);
}
/* set monitoring data: */
mon_data_set(ne_pos_err.x, ne_pos_err.y, u_pos_err, yaw_err);
out:
EVERY_N_TIMES(bb_rate, blackbox_record(dt, marg_data, gps_data, ultra, baro, voltage, current, channels, sensor_status, /* sensor inputs */
&ne_pos_err, u_pos_err, /* position errors */
&ne_spd_err, u_spd_err /* speed errors */,
&mag_normal));
}