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