// input : degree
void stablize_mode(float ef_desire_roll, float ef_desire_pitch, float bf_desire_yaw_rate, float hover){
float d_q[4], e_q[4];
float bf_desire_rate[3];
float norm, angle_error;
float phi = ef_desire_roll*0.008726646; // degree to radian
float theta = ef_desire_pitch*0.008726646;
float c_phi = cosf(phi); float s_phi = sinf(phi);
float c_theta = cosf(theta); float s_theta = sinf(theta);
d_q[0] = c_phi*c_theta;
d_q[1] = s_phi*c_theta;
d_q[2] = c_phi*s_theta;
d_q[3] = -s_phi*s_theta;
e_q[0] = d_q[0]*_quaternion[0] + d_q[1]*_quaternion[1] + d_q[2]*_quaternion[2] + d_q[3]*_quaternion[3];
e_q[1] = d_q[1]*_quaternion[0] - d_q[0]*_quaternion[1] + d_q[3]*_quaternion[2] - d_q[2]*_quaternion[3];
e_q[2] = d_q[2]*_quaternion[0] - d_q[3]*_quaternion[1] - d_q[0]*_quaternion[2] + d_q[1]*_quaternion[3];
e_q[3] = d_q[3]*_quaternion[0] + d_q[2]*_quaternion[1] - d_q[1]*_quaternion[2] - d_q[0]*_quaternion[3];
norm = sqrtf(e_q[0]*e_q[0]+e_q[1]*e_q[1]+e_q[2]*e_q[2]+e_q[3]*e_q[3]);
e_q[0] /= norm;
if(e_q[0] < 0){
e_q[0] *= -1;
e_q[1] *= -1;
e_q[2] *= -1;
e_q[3] *= -1;
}
angle_error = acosf(e_q[0])*180.0/3.14159;
if(_prev_angle_error - angle_error > 3){
angle_error = _prev_angle_error - 3;
}else if(angle_error - _prev_angle_error > 3){
angle_error = _prev_angle_error + 3;
}
_prev_angle_error = angle_error;
norm = sqrtf(e_q[1]*e_q[1]+e_q[2]*e_q[2]+e_q[3]*e_q[3]);
bf_desire_rate[0] = e_q[1]/norm;
bf_desire_rate[1] = e_q[2]/norm;
bf_desire_rate[2] = e_q[3]/norm;
angle_error = stable_Kp*angle_error*2; // Kp*2*angle*180/3.14
angle_error = constraints(angle_error,0.0, 45.0); // max 20deg/s
bf_desire_rate[0] *= angle_error;
bf_desire_rate[1] *= angle_error;
//bf_desire_rate[2] *= angle_error;
bf_desire_rate[2] *= bf_desire_yaw_rate;
rate_controller(bf_desire_rate, 0, hover);
}
/* Called from main context */
static void adjust_rates(struct userdata *u) {
size_t buffer;
uint32_t old_rate, base_rate, new_rate;
int32_t latency_difference;
pa_usec_t current_buffer_latency, snapshot_delay, current_source_sink_latency, current_latency, latency_at_optimum_rate;
pa_usec_t final_latency;
pa_assert(u);
pa_assert_ctl_context();
/* Rates and latencies*/
old_rate = u->sink_input->sample_spec.rate;
base_rate = u->source_output->sample_spec.rate;
buffer = u->latency_snapshot.sink_input_buffer;
if (u->latency_snapshot.recv_counter <= u->latency_snapshot.send_counter)
buffer += (size_t) (u->latency_snapshot.send_counter - u->latency_snapshot.recv_counter);
else
buffer = PA_CLIP_SUB(buffer, (size_t) (u->latency_snapshot.recv_counter - u->latency_snapshot.send_counter));
current_buffer_latency = pa_bytes_to_usec(buffer, &u->sink_input->sample_spec);
snapshot_delay = u->latency_snapshot.source_timestamp - u->latency_snapshot.sink_timestamp;
current_source_sink_latency = u->latency_snapshot.sink_latency + u->latency_snapshot.source_latency - snapshot_delay;
/* Current latency */
current_latency = current_source_sink_latency + current_buffer_latency;
/* Latency at base rate */
latency_at_optimum_rate = current_source_sink_latency + current_buffer_latency * old_rate / base_rate;
final_latency = u->latency;
latency_difference = (int32_t)((int64_t)latency_at_optimum_rate - final_latency);
pa_log_debug("Loopback overall latency is %0.2f ms + %0.2f ms + %0.2f ms = %0.2f ms",
(double) u->latency_snapshot.sink_latency / PA_USEC_PER_MSEC,
(double) current_buffer_latency / PA_USEC_PER_MSEC,
(double) u->latency_snapshot.source_latency / PA_USEC_PER_MSEC,
(double) current_latency / PA_USEC_PER_MSEC);
pa_log_debug("Loopback latency at base rate is %0.2f ms", (double)latency_at_optimum_rate / PA_USEC_PER_MSEC);
/* Calculate new rate */
new_rate = rate_controller(base_rate, u->adjust_time, latency_difference);
/* Set rate */
pa_sink_input_set_rate(u->sink_input, new_rate);
pa_log_debug("[%s] Updated sampling rate to %lu Hz.", u->sink_input->sink->name, (unsigned long) new_rate);
}
