Browse Source

Copter: send home position when home is set or get-home msg received

master
Randy Mackay 10 years ago
parent
commit
330961b524
  1. 4
      ArduCopter/GCS_Mavlink.cpp
  2. 3
      ArduCopter/commands.cpp

4
ArduCopter/GCS_Mavlink.cpp

@ -1386,6 +1386,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) @@ -1386,6 +1386,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
}
break;
case MAV_CMD_GET_HOME_POSITION:
send_home(copter.ahrs.get_home());
break;
case MAV_CMD_DO_SET_SERVO:
if (copter.ServoRelayEvents.do_set_servo(packet.param1, packet.param2)) {
result = MAV_RESULT_ACCEPTED;

3
ArduCopter/commands.cpp

@ -104,6 +104,9 @@ bool Copter::set_home(const Location& loc) @@ -104,6 +104,9 @@ bool Copter::set_home(const Location& loc)
// log ahrs home and ekf origin dataflash
Log_Write_Home_And_Origin();
// send new home location to GCS
GCS_MAVLINK::send_home_all(loc);
// return success
return true;
}

Loading…
Cancel
Save