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]); } } }