Tracker: let GCS_MAVLink handle get_home_position

This commit is contained in:
Peter Barker 2018-05-16 20:23:29 +10:00 committed by Peter Barker
parent 62a3faf448
commit cbbd28aeda
3 changed files with 10 additions and 6 deletions

View File

@ -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()

View File

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

View File

@ -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();