diff --git a/src/AutoPilotPlugins/Common/RadioComponentController.cc b/src/AutoPilotPlugins/Common/RadioComponentController.cc index ee8a3969b11225bb56a2a3fe91a337f40d5af2d0..9a222d15a56462732e7f62210bf753a15a8159a9 100644 --- a/src/AutoPilotPlugins/Common/RadioComponentController.cc +++ b/src/AutoPilotPlugins/Common/RadioComponentController.cc @@ -206,11 +206,20 @@ void RadioComponentController::_advanceState(void) /// @brief Sets up the state machine according to the current step from _currentStep. void RadioComponentController::_setupCurrentState(void) { - const stateMachineEntry* state = _getStateMachineEntry(_currentStep); - - _statusText->setProperty("text", state->instructions); - - _setHelpImage(state->image); + static const char* msgBeginAPMRover = "Center the Throttle stick as shown in diagram.\nReset all transmitter trims to center.\n\n" + "Please ensure all motor power is disconnected from the vehicle.\n\n" + "Click Next to continue"; + const stateMachineEntry* state = _getStateMachineEntry(_currentStep); + + const char* instructions = state->instructions; + const char* helpImage = state->image; + if (_vehicle->rover() && _currentStep == 0) { + // Hack in center throttle start for Rover. This is to set the correct centered trim for throttle. + instructions = msgBeginAPMRover; + helpImage = _imageCenter; + } + _statusText->setProperty("text", instructions); + _setHelpImage(helpImage); _stickDetectChannel = _chanMax(); _stickDetectSettleStarted = false; diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc index 612bd8e79cefde0378f3f0b79ed5fedb04a853b4..12b72c11bce139cb83dcb051150a2da4b43e021d 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc @@ -148,7 +148,6 @@ void ArduCopterFirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QG return; } - vehicle->setFlightMode("Guided"); QGeoCoordinate coordWithAltitude = gotoCoord; coordWithAltitude.setAltitude(vehicle->altitudeRelative()->rawValue().toDouble()); vehicle->missionManager()->writeArduPilotGuidedMissionItem(coordWithAltitude, false /* altChangeOnly */);