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

Sub: add method to translate wpnav roll/pitch output to forward/lateral

Browse Source
This commit is contained in:
Jacob Walser 2016-10-10 23:10:56 -04:00 committed by Andrew Tridgell
parent d3442ae16b
commit ad60e8476f
1 changed files with 16 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
ArduSub/motors.cpp
View File

@ -314,3 +314,19 @@ void Sub::lost_vehicle_check()
}
}
}
// translate wpnav roll/pitch outputs to lateral/forward
void Sub::translate_wpnav_rp(float &lateral_out, float &forward_out) {
// get roll and pitch targets in centidegrees
int32_t lateral = wp_nav.get_roll();
int32_t forward = -wp_nav.get_pitch(); // output is reversed
// constrain target forward/lateral values
// The outputs of wp_nav.get_roll and get_pitch should already be constrained to these values
lateral = constrain_int16(lateral, -aparm.angle_max, aparm.angle_max);
forward = constrain_int16(forward, -aparm.angle_max, aparm.angle_max);
// Normalize
lateral_out = (float)lateral/(float)aparm.angle_max;
forward_out = (float)forward/(float)aparm.angle_max;
}