static void update_rc_control(MR_CONTROL_STATE *ctrl, MR_RC_STATE *rc, POS_STATE *pos, float dt) {
if (fabsf(rc->yaw) < 0.05) {
rc->yaw = 0.0;
}
float roll = ctrl->roll_goal + rc->roll * main_config.mr.js_gain_tilt * dt * 250.0;
float pitch = ctrl->pitch_goal + rc->pitch * main_config.mr.js_gain_tilt * dt * 250.0;
float yaw = ctrl->yaw_goal + rc->yaw * main_config.mr.js_gain_yaw * dt * 250.0;
if (main_config.mr.js_mode_rate) {
// Integrate towards the current angle slowly
roll = utils_weight_angle(roll, pos->roll, 0.995);
pitch = utils_weight_angle(pitch, pos->pitch, 0.995);
} else {
// A trick!
roll = utils_weight_angle(roll, -pos->roll, 0.995);
pitch = utils_weight_angle(pitch, -pos->pitch, 0.995);
}
utils_norm_angle(&roll);
utils_norm_angle(&pitch);
utils_norm_angle(&yaw);
ctrl->roll_goal = roll;
ctrl->pitch_goal = pitch;
ctrl->yaw_goal = yaw;
}
/**
* Get the difference between two angles. Will always be between -180 and +180 degrees.
* @param angle1
* The first angle
* @param angle2
* The second angle
* @return
* The difference between the angles
*/
float utils_angle_difference(float angle1, float angle2) {
utils_norm_angle(&angle1);
utils_norm_angle(&angle2);
if (fabsf(angle1 - angle2) > 180.0) {
if (angle1 < angle2) {
angle1 += 360.0;
} else {
angle2 += 360.0;
}
}
return angle1 - angle2;
}
/**
* Run a "complementary filter" on two angles
* @param angle1
* The first angle
* @param angle2
* The second angle
* @param ratio
* The ratio between the first and second angle.
* @return
*/
float utils_weight_angle(float angle1, float angle2, float ratio) {
utils_norm_angle(&angle1);
utils_norm_angle(&angle2);
if (fabsf(angle1 - angle2) > 180.0) {
if (angle1 < angle2) {
angle1 += 360.0;
} else {
angle2 += 360.0;
}
}
float angle_weighted = angle1 * ratio + angle2 * (1 - ratio);
utils_norm_angle(&angle_weighted);
return angle_weighted;
}
