From c1e28f5f13beda2c3074e3c470293887d19d8071 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 11 Nov 2012 19:10:26 +0100 Subject: first version of yaw control loop, needs testing --- apps/mavlink/mavlink_receiver.c | 23 ++++++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) (limited to 'apps/mavlink/mavlink_receiver.c') 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 +} -- cgit v1.2.3