Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -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();
int16_t rudder_input(void);
void control_failsafe();
bool trim_radio();
void trim_radio();
bool rc_throttle_value_ok(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) {
// 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;
int16_t trim_pitch_range = (channel_pitch->get_radio_max() - channel_pitch->get_radio_min())/5;
if (channel_roll->get_radio_in() < channel_roll->get_radio_min()+trim_roll_range ||
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) {
// don't trim for extreme values - if we attempt to trim so
// there is less than 20 percent range left then assume the
if (plane.control_mode != &mode_manual) {
gcs().send_text(MAV_SEVERITY_ERROR, "trim failed, not in manual mode");
return;
}
if (labs(channel_roll->get_control_in()) > (channel_roll->get_range() * 0.2) ||
labs(channel_pitch->get_control_in()) > (channel_pitch->get_range() * 0.2)) {
// 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
// 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
@ -352,7 +360,7 @@ bool Plane::trim_radio()
channel_pitch->set_and_save_trim();
channel_rudder->set_and_save_trim();
return true;
gcs().send_text(MAV_SEVERITY_NOTICE, "trim complete");
}
/*