mirror of https://github.com/ArduPilot/ardupilot
Tracker: let GCS_MAVLink handle get_home_position
This commit is contained in:
parent
62a3faf448
commit
cbbd28aeda
|
@ -106,6 +106,15 @@ void Tracker::one_second_loop()
|
|||
}
|
||||
one_second_counter = 0;
|
||||
}
|
||||
|
||||
if (!ahrs.home_is_set()) {
|
||||
// set home to current location
|
||||
Location temp_loc;
|
||||
if (ahrs.get_location(temp_loc)) {
|
||||
set_home(temp_loc);
|
||||
}
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
void Tracker::ten_hz_logging_loop()
|
||||
|
|
|
@ -460,12 +460,6 @@ void GCS_MAVLINK_Tracker::handleMessage(mavlink_message_t* msg)
|
|||
}
|
||||
break;
|
||||
|
||||
case MAV_CMD_GET_HOME_POSITION:
|
||||
send_home();
|
||||
send_ekf_origin();
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
break;
|
||||
|
||||
case MAV_CMD_DO_SET_SERVO:
|
||||
if (tracker.servo_test_set_servo(packet.param1, packet.param2)) {
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
|
|
|
@ -163,6 +163,7 @@ void Tracker::set_home(struct Location temp)
|
|||
Location ekf_origin;
|
||||
if (ahrs.get_origin(ekf_origin)) {
|
||||
ahrs.set_home(temp);
|
||||
ahrs.set_home_status(HOME_SET_NOT_LOCKED);
|
||||
}
|
||||
|
||||
gcs().send_home();
|
||||
|
|
Loading…
Reference in New Issue