Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-24 01:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: trim_radio: check manual mode and rotation rates and add mesages

Browse Source
This commit is contained in:
Iampete1 2021-09-21 22:41:35 +01:00 committed by Andrew Tridgell
parent 90dd36285e
commit dc2ba8236a
2 changed files with 21 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduPlane/Plane.h
View File

@ -1024,7 +1024,7 @@ private:
void read_radio(); void read_radio();
int16_t rudder_input(void); int16_t rudder_input(void);
void control_failsafe(); void control_failsafe();
bool trim_radio(); void trim_radio();
bool rc_throttle_value_ok(void) const; bool rc_throttle_value_ok(void) const;
bool rc_failsafe_active(void) const; bool rc_failsafe_active(void) const;

32
ArduPlane/radio.cpp
View File

@ -299,24 +299,32 @@ void Plane::control_failsafe()
} }
} }
bool Plane::trim_radio() void Plane::trim_radio()
{ {
if (failsafe.rc_failsafe) { if (failsafe.rc_failsafe) {
// can't trim if we don't have valid input // can't trim if we don't have valid input
return false; return;
} }
int16_t trim_roll_range = (channel_roll->get_radio_max() - channel_roll->get_radio_min())/5; if (plane.control_mode != &mode_manual) {
int16_t trim_pitch_range = (channel_pitch->get_radio_max() - channel_pitch->get_radio_min())/5; gcs().send_text(MAV_SEVERITY_ERROR, "trim failed, not in manual mode");
if (channel_roll->get_radio_in() < channel_roll->get_radio_min()+trim_roll_range || return;
channel_roll->get_radio_in() > channel_roll->get_radio_max()-trim_roll_range || }
channel_pitch->get_radio_in() < channel_pitch->get_radio_min()+trim_pitch_range ||
channel_pitch->get_radio_in() > channel_pitch->get_radio_max()-trim_pitch_range) { if (labs(channel_roll->get_control_in()) > (channel_roll->get_range() * 0.2) ||
// don't trim for extreme values - if we attempt to trim so labs(channel_pitch->get_control_in()) > (channel_pitch->get_range() * 0.2)) {
// there is less than 20 percent range left then assume the // don't trim for extreme values - if we attempt to trim
// more than 20 percent range left then assume the
// sticks are not properly centered. This also prevents // sticks are not properly centered. This also prevents
// problems with starting APM with the TX off // problems with starting APM with the TX off
return false; gcs().send_text(MAV_SEVERITY_ERROR, "trim failed, large roll and pitch input");
return;
}
if (degrees(ahrs.get_gyro().length()) > 30.0) {
// rotating more than 30 deg/second
gcs().send_text(MAV_SEVERITY_ERROR, "trim failed, large movement");
return;
} }
// trim main surfaces // trim main surfaces
@ -352,7 +360,7 @@ bool Plane::trim_radio()
channel_pitch->set_and_save_trim(); channel_pitch->set_and_save_trim();
channel_rudder->set_and_save_trim(); channel_rudder->set_and_save_trim();
return true; gcs().send_text(MAV_SEVERITY_NOTICE, "trim complete");
} }
/* /*