Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: added ship offset and ATTITUDE

Browse Source
This commit is contained in:
Andrew Tridgell 2022-01-09 11:20:46 +11:00
parent 33bc067edf
commit 18782c4990
2 changed files with 19 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/SITL/SIM_Ship.cpp
View File

@ -41,6 +41,7 @@ const AP_Param::GroupInfo ShipSim::var_info[] = {
AP_GROUPINFO("PSIZE", 3, ShipSim, path_size, 1000), AP_GROUPINFO("PSIZE", 3, ShipSim, path_size, 1000),
AP_GROUPINFO("SYSID", 4, ShipSim, sys_id, 17), AP_GROUPINFO("SYSID", 4, ShipSim, sys_id, 17),
AP_GROUPINFO("DSIZE", 5, ShipSim, deck_size, 10), AP_GROUPINFO("DSIZE", 5, ShipSim, deck_size, 10),
AP_GROUPINFO("OFS", 7, ShipSim, offset, 0),
AP_GROUPEND AP_GROUPEND
}; };
@ -117,6 +118,10 @@ void ShipSim::update(void)
if (home.lat == 0 && home.lng == 0) { if (home.lat == 0 && home.lng == 0) {
return; return;
} }
const Vector3f &ofs = offset.get();
home.offset(ofs.x, ofs.y);
home.alt -= ofs.z*100;
initialised = true; initialised = true;
::printf("ShipSim home %f %f\n", home.lat*1.0e-7, home.lng*1.0e-7); ::printf("ShipSim home %f %f\n", home.lat*1.0e-7, home.lng*1.0e-7);
ship.sim = this; ship.sim = this;
@ -208,6 +213,19 @@ void ShipSim::send_report(void)
if (len > 0) { if (len > 0) {
mav_socket.send(buf, len); 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 #endif

1
libraries/SITL/SIM_Ship.h
View File

@ -71,6 +71,7 @@ private:
AP_Float path_size; AP_Float path_size;
AP_Float deck_size; AP_Float deck_size;
AP_Int8 sys_id; AP_Int8 sys_id;
AP_Vector3f offset;
Location home; Location home;
const char *target_address = "127.0.0.1"; const char *target_address = "127.0.0.1";