void acoustic_set_speed_command(audio_t* audio_data, float rel_pos[], float dist2wpSqr)
{
float norm_rel_dist, v_desiredxy, v_desiredz;
float dir_desired_bf[3], new_velocity[3];
norm_rel_dist = rel_pos[2];
dir_desired_bf[2] = rel_pos[2];
v_desiredz = maths_f_min(audio_data->navigation->cruise_speed, (audio_data->navigation->dist2vel_gain * maths_soft_zone(norm_rel_dist, audio_data->stabilisation_copter->stabiliser_stack.velocity_stabiliser.thrust_controller.soft_zone_width)));
if (audio_data->reliabe_el < 1.35)
{
v_desiredxy = maths_f_min(audio_data->navigation->cruise_speed, (maths_center_window_2(4.0f * audio_data->reliabe_az) * (-9.0f * audio_data->reliabe_el + (1.35 * 9))));
}
else
{
v_desiredxy = 0;
}
if (v_desiredz * maths_f_abs(dir_desired_bf[Z]) > audio_data->navigation->max_climb_rate * norm_rel_dist)
{
v_desiredz = audio_data->navigation->max_climb_rate * norm_rel_dist / maths_f_abs(dir_desired_bf[Z]);
}
dir_desired_bf[X] = v_desiredxy * quick_trig_cos(audio_data->reliabe_az);
dir_desired_bf[Y] = v_desiredxy * quick_trig_sin(audio_data->reliabe_az);
dir_desired_bf[Z] = v_desiredz * dir_desired_bf[Z] / norm_rel_dist;
for (uint8_t i = 0; i < 3; i++)
{
new_velocity[i] = dir_desired_bf[i];
}
audio_data->controls_nav->tvel[X] = new_velocity[X];
audio_data->controls_nav->tvel[Y] = new_velocity[Y];
audio_data->controls_nav->tvel[Z] = new_velocity[Z];
audio_data->controls_nav->rpy[YAW] = KP_YAW * audio_data->reliabe_az;
}
static void imu_raw2oriented(imu_t *imu)
{
uint16_t i;
for (i=0; i<3; i++)
{
imu->oriented_gyro.data[i] = imu->raw_gyro.data[imu->calib_gyro.axis[i]] * imu->calib_gyro.orientation[i];
imu->oriented_accelero.data[i] = imu->raw_accelero.data[imu->calib_accelero.axis[i]] * imu->calib_accelero.orientation[i];
imu->oriented_compass.data[i] = imu->raw_compass.data[imu->calib_compass.axis[i]] * imu->calib_compass.orientation[i];
}
/*if (imu->calib_gyro.calibration)
{
for (i=0; i<3; i++)
{
imu->calib_gyro.max_oriented_values[i] = maths_f_max(imu->calib_gyro.max_oriented_values[i],imu->oriented_gyro.data[i]);
imu->calib_gyro.min_oriented_values[i] = maths_f_min(imu->calib_gyro.min_oriented_values[i],imu->oriented_gyro.data[i]);
}
}*/
/*if (imu->calib_accelero.calibration)
{
for (i=0; i<3; i++)
{
imu->calib_accelero.max_oriented_values[i] = maths_f_max(imu->calib_accelero.max_oriented_values[i],imu->oriented_accelero.data[i]);
imu->calib_accelero.min_oriented_values[i] = maths_f_min(imu->calib_accelero.min_oriented_values[i],imu->oriented_accelero.data[i]);
}
}*/
if (imu->calib_compass.calibration)
{
for (i=0; i<3; i++)
{
imu->calib_compass.max_oriented_values[i] = maths_f_max(imu->calib_compass.max_oriented_values[i],imu->oriented_compass.data[i]);
imu->calib_compass.min_oriented_values[i] = maths_f_min(imu->calib_compass.min_oriented_values[i],imu->oriented_compass.data[i]);
}
}
}
