You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
35 lines
1.1 KiB
35 lines
1.1 KiB
#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; |
|
} |
|
} |
|
|
|
|