double CDKF::getImuAngleChange(const IMUList& imu_rotations,
const ros::Time& start_stamp,
const ros::Time& end_stamp) {
Eigen::Quaterniond start_angle =
getInterpolatedImuAngle(imu_rotations, start_stamp);
Eigen::Quaterniond end_angle =
getInterpolatedImuAngle(imu_rotations, end_stamp);
Eigen::AngleAxisd diff_angle(start_angle.inverse() * end_angle);
return diff_angle.angle();
}
void controller(float dx, float dy, float dz, float angle_drone_cons, float angle_drone, float * pitch_cmd, float * roll_cmd, float * angular_speed_cmd, float * vertical_speed_cmd)
{
// Angle must be between -180 and +180°
convert_angle(&angle_drone_cons);
convert_angle(&angle_drone);
// CHange of reference (from the room reference to the drone reference)
room_to_drone(dx,dy,angle_drone,pitch_cmd,roll_cmd);
// Apply gain
*pitch_cmd *= -GAIN_PITCH;
*roll_cmd *= GAIN_ROLL;
*angular_speed_cmd = -GAIN_ANGULAR * diff_angle(angle_drone,angle_drone_cons);
*vertical_speed_cmd = GAIN_VERTICAL * dz;
//printf("pitch=%f\troll=%f\tangular_speed=%f\tvert_speed=%f\n",*pitch_cmd,*roll_cmd,*angular_speed_cmd,*vertical_speed_cmd);
// Saturate the command if necessary
if(*pitch_cmd>PITCH_CMD_MAX) {
*pitch_cmd = PITCH_CMD_MAX;
}
else if(*pitch_cmd<-PITCH_CMD_MAX) {
*pitch_cmd = -PITCH_CMD_MAX;
}
if(*roll_cmd>ROLL_CMD_MAX) {
*roll_cmd = ROLL_CMD_MAX;
}
else if(*roll_cmd<-ROLL_CMD_MAX) {
*roll_cmd = -ROLL_CMD_MAX;
}
if(*angular_speed_cmd>ANGULAR_CMD_MAX) {
*angular_speed_cmd = ANGULAR_CMD_MAX;
}
else if(*angular_speed_cmd<-ANGULAR_CMD_MAX) {
*angular_speed_cmd = -ANGULAR_CMD_MAX;
}
if(*vertical_speed_cmd>VERTICAL_CMD_MAX) {
*vertical_speed_cmd = VERTICAL_CMD_MAX;
}
else if(*vertical_speed_cmd<-VERTICAL_CMD_MAX) {
*vertical_speed_cmd = -VERTICAL_CMD_MAX;
}
}
