diff --git a/libraries/SITL/SIM_Ship.cpp b/libraries/SITL/SIM_Ship.cpp index 3e097f16a8..acd4bd376e 100644 --- a/libraries/SITL/SIM_Ship.cpp +++ b/libraries/SITL/SIM_Ship.cpp @@ -41,6 +41,7 @@ const AP_Param::GroupInfo ShipSim::var_info[] = { AP_GROUPINFO("PSIZE", 3, ShipSim, path_size, 1000), AP_GROUPINFO("SYSID", 4, ShipSim, sys_id, 17), AP_GROUPINFO("DSIZE", 5, ShipSim, deck_size, 10), + AP_GROUPINFO("OFS", 7, ShipSim, offset, 0), AP_GROUPEND }; @@ -117,6 +118,10 @@ void ShipSim::update(void) if (home.lat == 0 && home.lng == 0) { return; } + const Vector3f &ofs = offset.get(); + home.offset(ofs.x, ofs.y); + home.alt -= ofs.z*100; + initialised = true; ::printf("ShipSim home %f %f\n", home.lat*1.0e-7, home.lng*1.0e-7); ship.sim = this; @@ -208,6 +213,19 @@ void ShipSim::send_report(void) if (len > 0) { mav_socket.send(buf, len); } + + // also set ATTITUDE so MissionPlanner can display ship orientation + mavlink_msg_attitude_pack_chan(sys_id, + component_id, + mavlink_ch, + &msg, + now, + 0, 0, radians(ship.heading_deg), + 0, 0, ship.yaw_rate); + len = mavlink_msg_to_send_buffer(buf, &msg); + if (len > 0) { + mav_socket.send(buf, len); + } } #endif diff --git a/libraries/SITL/SIM_Ship.h b/libraries/SITL/SIM_Ship.h index 16e97152a6..2989ffd597 100644 --- a/libraries/SITL/SIM_Ship.h +++ b/libraries/SITL/SIM_Ship.h @@ -71,6 +71,7 @@ private: AP_Float path_size; AP_Float deck_size; AP_Int8 sys_id; + AP_Vector3f offset; Location home; const char *target_address = "127.0.0.1";