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

Tracker: handle Tracker fixed position

This commit is contained in:
IamPete1 2019-02-28 21:38:28 +00:00 committed by Peter Barker
parent 20d79207d8
commit 5399b659ea
4 changed files with 29 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
AntennaTracker/GCS_Mavlink.cpp
View File

@ -588,6 +588,28 @@ bool GCS_MAVLINK_Tracker::set_mode(uint8_t mode)
return false;
}
// send position tracker is using
void GCS_MAVLINK_Tracker::send_global_position_int()
{
if (!tracker.stationary) {
GCS_MAVLINK::send_global_position_int();
return;
}
mavlink_msg_global_position_int_send(
chan,
AP_HAL::millis(),
tracker.current_loc.lat, // in 1E7 degrees
tracker.current_loc.lng, // in 1E7 degrees
tracker.current_loc.alt, // millimeters above ground/sea level
0, // millimeters above home
0, // X speed cm/s (+ve North)
0, // Y speed cm/s (+ve East)
0, // Z speed cm/s (+ve Down)
tracker.ahrs.yaw_sensor); // compass heading in 1/100 degree
}
/* dummy methods to avoid having to link against AP_Camera */
void AP_Camera::control_msg(mavlink_message_t const*) {}
void AP_Camera::configure(float, float, float, float, float, float, float) {}

1
AntennaTracker/GCS_Mavlink.h
View File

@ -41,6 +41,7 @@ private:
void handleMessage(mavlink_message_t * msg) override;
bool handle_guided_request(AP_Mission::Mission_Command &cmd) override;
void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override;
void send_global_position_int() override;
MAV_TYPE frame_type() const override;
MAV_MODE base_mode() const override;

1
AntennaTracker/Tracker.h
View File

@ -188,6 +188,7 @@ private:
uint8_t one_second_counter = 0;
bool target_set = false;
bool stationary = true; // are we using the start lat and log?
static const AP_Scheduler::Task scheduler_tasks[];
static const AP_Param::Info var_info[];

8
AntennaTracker/tracking.cpp
View File

@ -31,10 +31,12 @@ void Tracker::update_vehicle_pos_estimate()
*/
void Tracker::update_tracker_position()
{
// update our position if we have at least a 2D fix
Location temp_loc;
// REVISIT: what if we lose lock during a mission and the antenna is moving?
if (!ahrs.get_position(current_loc) && (gps.status() >= AP_GPS::GPS_OK_FIX_2D)) {
current_loc = gps.location();
if (ahrs.get_position(temp_loc)) {
stationary = false;
current_loc = temp_loc;
}
}