first version of yaw control loop, needs testing

1 files changed, 22 insertions, 1 deletions

diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c
index 3e485274e..45fe1ccb5 100644
--- a/apps/mavlink/mavlink_receiver.c
+++ b/apps/mavlink/mavlink_receiver.c
@@ -292,6 +292,26 @@ handle_message(mavlink_message_t *msg)
 			mavlink_hil_state_t hil_state;
 			mavlink_msg_hil_state_decode(msg, &hil_state);
 
+			/* Calculate Rotation Matrix */
+			//TODO: better clarification which app does this, atm we have a ekf for quadrotors which does this, but there is no such thing if fly in fixed wing mode
+
+			if(mavlink_system.type == MAV_TYPE_FIXED_WING) {
+				//TODO: asuming low pitch and roll for now
+				hil_attitude.R[0][0] = cosf(hil_state.yaw);
+				hil_attitude.R[0][1] = sinf(hil_state.yaw);
+				hil_attitude.R[0][2] = 0.0f;
+
+				hil_attitude.R[1][0] = -sinf(hil_state.yaw);
+				hil_attitude.R[1][1] = cosf(hil_state.yaw);
+				hil_attitude.R[1][2] = 0.0f;
+
+				hil_attitude.R[2][0] = 0.0f;
+				hil_attitude.R[2][1] = 0.0f;
+				hil_attitude.R[2][2] = 1.0f;
+
+				hil_attitude.R_valid = true;
+			}
+
 			hil_global_pos.lat = hil_state.lat;
 			hil_global_pos.lon = hil_state.lon;
 			hil_global_pos.alt = hil_state.alt / 1000.0f;
@@ -299,6 +319,7 @@ handle_message(mavlink_message_t *msg)
 			hil_global_pos.vy = hil_state.vy / 100.0f;
 			hil_global_pos.vz = hil_state.vz / 100.0f;
 
+
 			/* set timestamp and notify processes (broadcast) */
 			hil_global_pos.timestamp = hrt_absolute_time();
 			orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos);
@@ -418,4 +439,4 @@ receive_start(int uart)
 	pthread_t thread;
 	pthread_create(&thread, &receiveloop_attr, receive_thread, &uart);
 	return thread;
-}
\ No newline at end of file
+}