aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: Add function for sending POSITION_TARGET_LOCAL_NED message

Browse Source
This commit is contained in:
Rajat Singhal 2019-03-28 15:12:26 +05:30 committed by Peter Barker
parent b7a40f2bfe
commit 49554bf8d9
2 changed files with 42 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
ArduCopter/GCS_Mavlink.cpp
View File

@ -117,6 +117,47 @@ void GCS_MAVLINK_Copter::send_position_target_global_int()
0.0f); // yaw_rate
}
void GCS_MAVLINK_Copter::send_position_target_local_ned()
{
if (!copter.flightmode->in_guided_mode()) {
return;
}
const GuidedMode guided_mode = copter.mode_guided.mode();
Vector3f target_pos;
Vector3f target_vel;
uint16_t type_mask;
if (guided_mode == Guided_WP) {
type_mask = 0x0FF8; // ignore everything except position
target_pos = copter.wp_nav->get_wp_destination() * 0.01f; // convert to metres
} else if (guided_mode == Guided_Velocity) {
type_mask = 0x0FC7; // ignore everything except velocity
target_vel = copter.flightmode->pos_control->get_desired_velocity() * 0.01f; // convert to m/s
} else {
type_mask = 0x0FC0; // ignore everything except position & velocity
target_pos = copter.wp_nav->get_wp_destination() * 0.01f;
target_vel = copter.flightmode->pos_control->get_desired_velocity() * 0.01f;
}
mavlink_msg_position_target_local_ned_send(
chan,
AP_HAL::millis(), // time boot ms
MAV_FRAME_LOCAL_NED,
type_mask,
target_pos.x, // x in metres
target_pos.y, // y in metres
-target_pos.z, // z in metres NED frame
target_vel.x, // vx in m/s
target_vel.y, // vy in m/s
-target_vel.z, // vz in m/s NED frame
0.0f, // afx
0.0f, // afy
0.0f, // afz
0.0f, // yaw
0.0f); // yaw_rate
}
void GCS_MAVLINK_Copter::send_nav_controller_output() const
{
if (!copter.ap.initialised) {

1
ArduCopter/GCS_Mavlink.h
View File

@ -25,6 +25,7 @@ protected:
MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet) override;
void send_position_target_global_int() override;
void send_position_target_local_ned() override;
MAV_RESULT handle_command_do_set_roi(const Location &roi_loc) override;