Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

SITL: added ship offset and ATTITUDE

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("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

1
libraries/SITL/SIM_Ship.h
View File

@ -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";