mirror of https://github.com/ArduPilot/ardupilot
Copter: support do_set_home command from GCS and mission
This commit is contained in:
parent
d5fd6d2a99
commit
7029b11414
|
@ -1111,6 +1111,29 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||
}
|
||||
break;
|
||||
|
||||
case MAV_CMD_DO_SET_HOME:
|
||||
// param1 : use current (1=use current location, 0=use specified location)
|
||||
// param5 : latitude
|
||||
// param6 : longitude
|
||||
// param7 : altitude (absolute)
|
||||
result = MAV_RESULT_FAILED; // assume failure
|
||||
if(packet.param1 == 1 || (packet.param5 == 0 && packet.param6 == 0 && packet.param7 == 0)) {
|
||||
if (set_home_to_current_location_and_lock()) {
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
}
|
||||
} else {
|
||||
Location new_home_loc;
|
||||
new_home_loc.lat = (int32_t)(packet.param5 * 1.0e7f);
|
||||
new_home_loc.lng = (int32_t)(packet.param6 * 1.0e7f);
|
||||
new_home_loc.alt = (int32_t)(packet.param7 * 100.0f);
|
||||
if (!far_from_EKF_origin(new_home_loc)) {
|
||||
if (set_home_and_lock(new_home_loc)) {
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case MAV_CMD_DO_SET_ROI:
|
||||
// param1 : regional of interest mode (not supported)
|
||||
// param2 : mission index/ target id (not supported)
|
||||
|
@ -1437,11 +1460,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||
packet.time_usec/1000,
|
||||
loc, vel, 10, 0, true);
|
||||
|
||||
if (!ap.home_is_set) {
|
||||
init_home();
|
||||
}
|
||||
|
||||
|
||||
// rad/sec
|
||||
Vector3f gyros;
|
||||
gyros.x = packet.rollspeed;
|
||||
|
|
|
@ -111,7 +111,7 @@ static bool start_command(const AP_Mission::Mission_Command& cmd)
|
|||
break;
|
||||
|
||||
case MAV_CMD_DO_SET_HOME: // 179
|
||||
// unsupported as mission command
|
||||
do_set_home(cmd);
|
||||
break;
|
||||
|
||||
case MAV_CMD_DO_SET_SERVO:
|
||||
|
@ -864,12 +864,12 @@ static void do_change_speed(const AP_Mission::Mission_Command& cmd)
|
|||
|
||||
static void do_set_home(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
if(cmd.p1 == 1) {
|
||||
init_home();
|
||||
if(cmd.p1 == 1 || (cmd.content.location.lat == 0 && cmd.content.location.lng == 0 && cmd.content.location.alt == 0)) {
|
||||
set_home_to_current_location();
|
||||
} else {
|
||||
Location loc = cmd.content.location;
|
||||
ahrs.set_home(loc);
|
||||
set_home_is_set(true);
|
||||
if (!far_from_EKF_origin(cmd.content.location)) {
|
||||
set_home(cmd.content.location);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue