mirror of https://github.com/ArduPilot/ardupilot
36 lines
1.1 KiB
C++
36 lines
1.1 KiB
C++
|
#include "mode.h"
|
||
|
|
#include "Plane.h"
|
||
|
|
|
||
|
|
void ModeTraining::update()
|
||
|
|
{
|
||
|
|
plane.training_manual_roll = false;
|
||
|
|
plane.training_manual_pitch = false;
|
||
|
|
plane.update_load_factor();
|
||
|
|
|
||
|
|
// if the roll is past the set roll limit, then
|
||
|
|
// we set target roll to the limit
|
||
|
|
if (plane.ahrs.roll_sensor >= plane.roll_limit_cd) {
|
||
|
|
plane.nav_roll_cd = plane.roll_limit_cd;
|
||
|
|
} else if (plane.ahrs.roll_sensor <= -plane.roll_limit_cd) {
|
||
|
|
plane.nav_roll_cd = -plane.roll_limit_cd;
|
||
|
|
} else {
|
||
|
|
plane.training_manual_roll = true;
|
||
|
|
plane.nav_roll_cd = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
// if the pitch is past the set pitch limits, then
|
||
|
|
// we set target pitch to the limit
|
||
|
|
if (plane.ahrs.pitch_sensor >= plane.aparm.pitch_limit_max_cd) {
|
||
|
|
plane.nav_pitch_cd = plane.aparm.pitch_limit_max_cd;
|
||
|
|
} else if (plane.ahrs.pitch_sensor <= plane.pitch_limit_min_cd) {
|
||
|
|
plane.nav_pitch_cd = plane.pitch_limit_min_cd;
|
||
|
|
} else {
|
||
|
|
plane.training_manual_pitch = true;
|
||
|
|
plane.nav_pitch_cd = 0;
|
||
|
|
}
|
||
|
|
if (plane.fly_inverted()) {
|
||
|
|
plane.nav_pitch_cd = -plane.nav_pitch_cd;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|