|
|
|
@ -744,15 +744,15 @@ FixedwingAttitudeControl::task_main()
@@ -744,15 +744,15 @@ FixedwingAttitudeControl::task_main()
|
|
|
|
|
_att.roll = euler_angles(0); |
|
|
|
|
_att.pitch = euler_angles(1); |
|
|
|
|
_att.yaw = euler_angles(2); |
|
|
|
|
_att.R[0][0] = R_adapted(0, 0); |
|
|
|
|
_att.R[0][1] = R_adapted(0, 1); |
|
|
|
|
_att.R[0][2] = R_adapted(0, 2); |
|
|
|
|
_att.R[1][0] = R_adapted(1, 0); |
|
|
|
|
_att.R[1][1] = R_adapted(1, 1); |
|
|
|
|
_att.R[1][2] = R_adapted(1, 2); |
|
|
|
|
_att.R[2][0] = R_adapted(2, 0); |
|
|
|
|
_att.R[2][1] = R_adapted(2, 1); |
|
|
|
|
_att.R[2][2] = R_adapted(2, 2); |
|
|
|
|
PX4_R(_att.R, 0, 0) = R_adapted(0, 0); |
|
|
|
|
PX4_R(_att.R, 0, 1) = R_adapted(0, 1); |
|
|
|
|
PX4_R(_att.R, 0, 2) = R_adapted(0, 2); |
|
|
|
|
PX4_R(_att.R, 1, 0) = R_adapted(1, 0); |
|
|
|
|
PX4_R(_att.R, 1, 1) = R_adapted(1, 1); |
|
|
|
|
PX4_R(_att.R, 1, 2) = R_adapted(1, 2); |
|
|
|
|
PX4_R(_att.R, 2, 0) = R_adapted(2, 0); |
|
|
|
|
PX4_R(_att.R, 2, 1) = R_adapted(2, 1); |
|
|
|
|
PX4_R(_att.R, 2, 2) = R_adapted(2, 2); |
|
|
|
|
|
|
|
|
|
// lastly, roll- and yawspeed have to be swaped
|
|
|
|
|
float helper = _att.rollspeed; |
|
|
|
|