mirror of https://github.com/ArduPilot/ardupilot
Copter: implement sending of position_target_global_int
This commit is contained in:
parent
cdf4128900
commit
7c8583d51d
|
@ -98,6 +98,30 @@ MAV_STATE GCS_MAVLINK_Copter::system_status() const
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void GCS_MAVLINK_Copter::send_position_target_global_int()
|
||||||
|
{
|
||||||
|
Location_Class target;
|
||||||
|
if (!copter.flightmode->get_wp(target)) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
mavlink_msg_position_target_global_int_send(
|
||||||
|
chan,
|
||||||
|
AP_HAL::millis(), // time_boot_ms
|
||||||
|
MAV_FRAME_GLOBAL_INT, // targets are always global altitude
|
||||||
|
0xFFF8, // ignore everything except the x/y/z components
|
||||||
|
target.lat, // latitude as 1e7
|
||||||
|
target.lng, // longitude as 1e7
|
||||||
|
target.alt * 0.01f, // altitude is sent as a float
|
||||||
|
0.0f, // vx
|
||||||
|
0.0f, // vy
|
||||||
|
0.0f, // vz
|
||||||
|
0.0f, // afx
|
||||||
|
0.0f, // afy
|
||||||
|
0.0f, // afz
|
||||||
|
0.0f, // yaw
|
||||||
|
0.0f); // yaw_rate
|
||||||
|
}
|
||||||
|
|
||||||
#if AC_FENCE == ENABLED
|
#if AC_FENCE == ENABLED
|
||||||
NOINLINE void Copter::send_fence_status(mavlink_channel_t chan)
|
NOINLINE void Copter::send_fence_status(mavlink_channel_t chan)
|
||||||
{
|
{
|
||||||
|
@ -360,7 +384,6 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id)
|
||||||
|
|
||||||
case MSG_LIMITS_STATUS:
|
case MSG_LIMITS_STATUS:
|
||||||
case MSG_WIND:
|
case MSG_WIND:
|
||||||
case MSG_POSITION_TARGET_GLOBAL_INT:
|
|
||||||
case MSG_SERVO_OUT:
|
case MSG_SERVO_OUT:
|
||||||
case MSG_AOA_SSA:
|
case MSG_AOA_SSA:
|
||||||
case MSG_LANDING:
|
case MSG_LANDING:
|
||||||
|
@ -493,7 +516,8 @@ static const uint8_t STREAM_EXTENDED_STATUS_msgs[] = {
|
||||||
MSG_GPS2_RAW,
|
MSG_GPS2_RAW,
|
||||||
MSG_GPS2_RTK,
|
MSG_GPS2_RTK,
|
||||||
MSG_NAV_CONTROLLER_OUTPUT,
|
MSG_NAV_CONTROLLER_OUTPUT,
|
||||||
MSG_FENCE_STATUS
|
MSG_FENCE_STATUS,
|
||||||
|
MSG_POSITION_TARGET_GLOBAL_INT,
|
||||||
};
|
};
|
||||||
static const uint8_t STREAM_POSITION_msgs[] = {
|
static const uint8_t STREAM_POSITION_msgs[] = {
|
||||||
MSG_LOCATION,
|
MSG_LOCATION,
|
||||||
|
|
|
@ -34,6 +34,8 @@ protected:
|
||||||
|
|
||||||
MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet) override;
|
MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet) override;
|
||||||
|
|
||||||
|
void send_position_target_global_int() override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
void handleMessage(mavlink_message_t * msg) override;
|
void handleMessage(mavlink_message_t * msg) override;
|
||||||
|
|
|
@ -93,6 +93,7 @@ protected:
|
||||||
virtual void run_autopilot() {}
|
virtual void run_autopilot() {}
|
||||||
virtual uint32_t wp_distance() const { return 0; }
|
virtual uint32_t wp_distance() const { return 0; }
|
||||||
virtual int32_t wp_bearing() const { return 0; }
|
virtual int32_t wp_bearing() const { return 0; }
|
||||||
|
virtual bool get_wp(Location_Class &loc) { return false; };
|
||||||
virtual bool in_guided_mode() const { return false; }
|
virtual bool in_guided_mode() const { return false; }
|
||||||
|
|
||||||
// pilot input processing
|
// pilot input processing
|
||||||
|
@ -279,6 +280,7 @@ protected:
|
||||||
|
|
||||||
uint32_t wp_distance() const override;
|
uint32_t wp_distance() const override;
|
||||||
int32_t wp_bearing() const override;
|
int32_t wp_bearing() const override;
|
||||||
|
bool get_wp(Location_Class &loc) override;
|
||||||
void run_autopilot() override;
|
void run_autopilot() override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
@ -755,6 +757,7 @@ public:
|
||||||
void set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads);
|
void set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads);
|
||||||
bool set_destination(const Vector3f& destination, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
|
bool set_destination(const Vector3f& destination, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
|
||||||
bool set_destination(const Location_Class& dest_loc, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
|
bool set_destination(const Location_Class& dest_loc, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
|
||||||
|
bool get_wp(Location_Class &loc) override;
|
||||||
void set_velocity(const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool log_request = true);
|
void set_velocity(const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool log_request = true);
|
||||||
bool set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
|
bool set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
|
||||||
|
|
||||||
|
|
|
@ -616,6 +616,18 @@ int32_t Copter::ModeAuto::wp_bearing() const
|
||||||
return wp_nav->get_wp_bearing_to_destination();
|
return wp_nav->get_wp_bearing_to_destination();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool Copter::ModeAuto::get_wp(Location_Class& destination)
|
||||||
|
{
|
||||||
|
switch (_mode) {
|
||||||
|
case Auto_NavGuided:
|
||||||
|
return copter.mode_guided.get_wp(destination);
|
||||||
|
case Auto_WP:
|
||||||
|
return wp_nav->get_wp_destination(destination);
|
||||||
|
default:
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
// update mission
|
// update mission
|
||||||
void Copter::ModeAuto::run_autopilot()
|
void Copter::ModeAuto::run_autopilot()
|
||||||
{
|
{
|
||||||
|
|
|
@ -203,6 +203,14 @@ bool Copter::ModeGuided::set_destination(const Vector3f& destination, bool use_y
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool Copter::ModeGuided::get_wp(Location_Class& destination)
|
||||||
|
{
|
||||||
|
if (guided_mode != Guided_WP) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
return wp_nav->get_wp_destination(destination);
|
||||||
|
}
|
||||||
|
|
||||||
// sets guided mode's target from a Location object
|
// sets guided mode's target from a Location object
|
||||||
// returns false if destination could not be set (probably caused by missing terrain data)
|
// returns false if destination could not be set (probably caused by missing terrain data)
|
||||||
// or if the fence is enabled and guided waypoint is outside the fence
|
// or if the fence is enabled and guided waypoint is outside the fence
|
||||||
|
|
Loading…
Reference in New Issue