USNA WSE ES456
Emaxx Navigation code ported for MBED
Dependencies: BNO055_fusion Emaxx_Navigation_Dynamic_HIL MODSERIAL ServoIn ServoOut Vehicle_Model mbed
Fork of
Emaxx_Navigation_Dynamic_HIL
by 
Emaxx Navigation Group
utilityFunctions.h@9:3aa9b689bda5, 2018-04-03 (annotated)
- Committer:
- jdawkins
- Date:
- Tue Apr 03 12:52:52 2018 +0000
- Revision:
- 9:3aa9b689bda5
- Parent:
- 7:a8c2e9d049e8
Publish Code for MIDS
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| jdawkins | 7:a8c2e9d049e8 | 1 | #ifndef UTIL_FUNC_H |
| jdawkins | 7:a8c2e9d049e8 | 2 | #define UTIL_FUNC_H |
| jdawkins | 7:a8c2e9d049e8 | 3 | |
| jdawkins | 7:a8c2e9d049e8 | 4 | #define Pi (3.14159) |
| jdawkins | 7:a8c2e9d049e8 | 5 | #define DEG2RAD (Pi/180) |
| jdawkins | 7:a8c2e9d049e8 | 6 | #define RAD2DEG (180/Pi) |
| jdawkins | 7:a8c2e9d049e8 | 7 | float saturateCmd(float cmd, float max,float min) |
| jdawkins | 7:a8c2e9d049e8 | 8 | { |
| jdawkins | 7:a8c2e9d049e8 | 9 | if(cmd>max) { |
| jdawkins | 7:a8c2e9d049e8 | 10 | cmd = max; |
| jdawkins | 7:a8c2e9d049e8 | 11 | } |
| jdawkins | 7:a8c2e9d049e8 | 12 | if(cmd < min) { |
| jdawkins | 7:a8c2e9d049e8 | 13 | cmd = min; |
| jdawkins | 7:a8c2e9d049e8 | 14 | } |
| jdawkins | 7:a8c2e9d049e8 | 15 | return cmd; |
| jdawkins | 7:a8c2e9d049e8 | 16 | } |
| jdawkins | 7:a8c2e9d049e8 | 17 | float saturateCmd(float cmd) |
| jdawkins | 7:a8c2e9d049e8 | 18 | { |
| jdawkins | 7:a8c2e9d049e8 | 19 | if(cmd>1.0) { |
| jdawkins | 7:a8c2e9d049e8 | 20 | cmd = 1.0; |
| jdawkins | 7:a8c2e9d049e8 | 21 | } |
| jdawkins | 7:a8c2e9d049e8 | 22 | if(cmd < -1.0) { |
| jdawkins | 7:a8c2e9d049e8 | 23 | cmd = -1.0; |
| jdawkins | 7:a8c2e9d049e8 | 24 | } |
| jdawkins | 7:a8c2e9d049e8 | 25 | return cmd; |
| jdawkins | 7:a8c2e9d049e8 | 26 | } |
| jdawkins | 7:a8c2e9d049e8 | 27 | float wrapToPi(float ang) |
| jdawkins | 7:a8c2e9d049e8 | 28 | { |
| jdawkins | 7:a8c2e9d049e8 | 29 | |
| jdawkins | 7:a8c2e9d049e8 | 30 | while(ang > Pi) { |
| jdawkins | 7:a8c2e9d049e8 | 31 | |
| jdawkins | 7:a8c2e9d049e8 | 32 | ang = ang - 2*Pi; |
| jdawkins | 7:a8c2e9d049e8 | 33 | } |
| jdawkins | 7:a8c2e9d049e8 | 34 | while(ang < -Pi) { |
| jdawkins | 7:a8c2e9d049e8 | 35 | ang = ang + 2*Pi; |
| jdawkins | 7:a8c2e9d049e8 | 36 | } |
| jdawkins | 7:a8c2e9d049e8 | 37 | |
| jdawkins | 7:a8c2e9d049e8 | 38 | return ang; |
| jdawkins | 7:a8c2e9d049e8 | 39 | } |
| jdawkins | 7:a8c2e9d049e8 | 40 | |
| jdawkins | 7:a8c2e9d049e8 | 41 | #endif |