mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-05 07:28:29 -04:00
Rover: remove trim radio functionality
This commit is contained in:
parent
e158bab893
commit
72b821d9da
@ -572,13 +572,7 @@ void GCS_MAVLINK_Rover::handle_change_alt_request(AP_Mission::Mission_Command &c
|
|||||||
|
|
||||||
MAV_RESULT GCS_MAVLINK_Rover::_handle_command_preflight_calibration(const mavlink_command_long_t &packet)
|
MAV_RESULT GCS_MAVLINK_Rover::_handle_command_preflight_calibration(const mavlink_command_long_t &packet)
|
||||||
{
|
{
|
||||||
if (is_equal(packet.param4, 1.0f)) {
|
if (is_equal(packet.param6, 1.0f)) {
|
||||||
if (rover.trim_radio()) {
|
|
||||||
return MAV_RESULT_ACCEPTED;
|
|
||||||
} else {
|
|
||||||
return MAV_RESULT_FAILED;
|
|
||||||
}
|
|
||||||
} else if (is_equal(packet.param6, 1.0f)) {
|
|
||||||
if (rover.g2.windvane.start_direction_calibration()) {
|
if (rover.g2.windvane.start_direction_calibration()) {
|
||||||
return MAV_RESULT_ACCEPTED;
|
return MAV_RESULT_ACCEPTED;
|
||||||
} else {
|
} else {
|
||||||
|
@ -345,7 +345,6 @@ private:
|
|||||||
void rudder_arm_disarm_check();
|
void rudder_arm_disarm_check();
|
||||||
void read_radio();
|
void read_radio();
|
||||||
void radio_failsafe_check(uint16_t pwm);
|
void radio_failsafe_check(uint16_t pwm);
|
||||||
bool trim_radio();
|
|
||||||
|
|
||||||
// sensors.cpp
|
// sensors.cpp
|
||||||
void update_compass(void);
|
void update_compass(void);
|
||||||
|
@ -154,21 +154,3 @@ void Rover::radio_failsafe_check(uint16_t pwm)
|
|||||||
}
|
}
|
||||||
failsafe_trigger(FAILSAFE_EVENT_THROTTLE, "Radio", failed);
|
failsafe_trigger(FAILSAFE_EVENT_THROTTLE, "Radio", failed);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool Rover::trim_radio()
|
|
||||||
{
|
|
||||||
if (!rc().has_valid_input()) {
|
|
||||||
// can't trim without valid input
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Store control surface trim values
|
|
||||||
// ---------------------------------
|
|
||||||
if ((channel_steer->get_radio_in() > 1400) && (channel_steer->get_radio_in() < 1600)) {
|
|
||||||
channel_steer->set_and_save_radio_trim(channel_steer->get_radio_in());
|
|
||||||
} else {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
Loading…
Reference in New Issue
Block a user