Ngo Kien / Mbed OS RadarDemoT

Optimaze with new mbed os for study

Dependencies:   TS_DISCO_F746NG BSP_DISCO_F746NG Graphics

RadarDemo/Location.cpp@5:64cafb3c6e7b, 2019-08-23 (annotated)

Committer:
ngtkien
Date:
Fri Aug 23 16:59:22 2019 +0000
Revision:
5:64cafb3c6e7b
Parent:
0:d8b9955d2b36 
KienNgo optimaze

Who changed what in which revision?

UserRevisionLine numberNew contents of line
karpent 0:d8b9955d2b36 1 //
karpent 0:d8b9955d2b36 2 // Location.cpp
karpent 0:d8b9955d2b36 3 //
karpent 0:d8b9955d2b36 4
karpent 0:d8b9955d2b36 5 #include "Location.h"
karpent 0:d8b9955d2b36 6 #include "math.h"
karpent 0:d8b9955d2b36 7 #include "Commons.h"
karpent 0:d8b9955d2b36 8
karpent 0:d8b9955d2b36 9 Location::Location()
karpent 0:d8b9955d2b36 10 {
karpent 0:d8b9955d2b36 11 SetLocation(0,0,0);
karpent 0:d8b9955d2b36 12 }
karpent 0:d8b9955d2b36 13
karpent 0:d8b9955d2b36 14 Location::Location(float x, float y, float height)
karpent 0:d8b9955d2b36 15 {
karpent 0:d8b9955d2b36 16 SetLocation(x, y, height);
karpent 0:d8b9955d2b36 17 }
karpent 0:d8b9955d2b36 18
karpent 0:d8b9955d2b36 19 void Location::SetLocation(float x, float y, float height)
karpent 0:d8b9955d2b36 20 {
karpent 0:d8b9955d2b36 21 _x = x;
karpent 0:d8b9955d2b36 22 _y = y;
karpent 0:d8b9955d2b36 23 _h = height;
karpent 0:d8b9955d2b36 24
karpent 0:d8b9955d2b36 25 ToAngular();
karpent 0:d8b9955d2b36 26 }
karpent 0:d8b9955d2b36 27
karpent 0:d8b9955d2b36 28 void Location::SetLocationAngular(float distance, float azimuth, float elevation)
karpent 0:d8b9955d2b36 29 {
karpent 0:d8b9955d2b36 30 _distance = distance;
karpent 0:d8b9955d2b36 31 _azimuth = azimuth;
karpent 0:d8b9955d2b36 32 _elevation = elevation;
karpent 0:d8b9955d2b36 33
karpent 0:d8b9955d2b36 34 ToCartesian();
karpent 0:d8b9955d2b36 35 }
karpent 0:d8b9955d2b36 36
karpent 0:d8b9955d2b36 37 float Location::GetX()
karpent 0:d8b9955d2b36 38 {
karpent 0:d8b9955d2b36 39 return _x;
karpent 0:d8b9955d2b36 40 }
karpent 0:d8b9955d2b36 41
karpent 0:d8b9955d2b36 42 float Location::GetY()
karpent 0:d8b9955d2b36 43 {
karpent 0:d8b9955d2b36 44 return _y;
karpent 0:d8b9955d2b36 45 }
karpent 0:d8b9955d2b36 46
karpent 0:d8b9955d2b36 47 float Location::GetHeight()
karpent 0:d8b9955d2b36 48 {
karpent 0:d8b9955d2b36 49 return _h;
karpent 0:d8b9955d2b36 50 }
karpent 0:d8b9955d2b36 51
karpent 0:d8b9955d2b36 52 float Location::GetDistance()
karpent 0:d8b9955d2b36 53 {
karpent 0:d8b9955d2b36 54 return _distance;
karpent 0:d8b9955d2b36 55 }
karpent 0:d8b9955d2b36 56
karpent 0:d8b9955d2b36 57 float Location::GetAzimuth()
karpent 0:d8b9955d2b36 58 {
karpent 0:d8b9955d2b36 59 return _azimuth;
karpent 0:d8b9955d2b36 60 }
karpent 0:d8b9955d2b36 61
karpent 0:d8b9955d2b36 62 float Location::GetElevation()
karpent 0:d8b9955d2b36 63 {
karpent 0:d8b9955d2b36 64 return _elevation;
karpent 0:d8b9955d2b36 65 }
karpent 0:d8b9955d2b36 66
karpent 0:d8b9955d2b36 67 void Location::ToAngular()
karpent 0:d8b9955d2b36 68 {
karpent 0:d8b9955d2b36 69 _distance = (float)sqrt(_x * _x + _y * _y + _h * _h);
karpent 0:d8b9955d2b36 70 if (_distance > 0)
karpent 0:d8b9955d2b36 71 {
karpent 0:d8b9955d2b36 72 if (_x >= 0)
karpent 0:d8b9955d2b36 73 _azimuth = ((float)M_PI_2 - asinf(_y / _distance));
karpent 0:d8b9955d2b36 74 else
karpent 0:d8b9955d2b36 75 _azimuth = (float)(M_PI + M_PI_2 + asinf(_y / _distance));
karpent 0:d8b9955d2b36 76 }
karpent 0:d8b9955d2b36 77 else
karpent 0:d8b9955d2b36 78 _azimuth = 0.f;
karpent 0:d8b9955d2b36 79
karpent 0:d8b9955d2b36 80 _elevation = _distance > 0 ? asinf(_h / _distance) : 0.f;
karpent 0:d8b9955d2b36 81 }
karpent 0:d8b9955d2b36 82
karpent 0:d8b9955d2b36 83 void Location::ToCartesian()
karpent 0:d8b9955d2b36 84 {
karpent 0:d8b9955d2b36 85 _x = _distance * sinf(_azimuth);
karpent 0:d8b9955d2b36 86 _y = _distance * cosf(_azimuth);
karpent 0:d8b9955d2b36 87 _h = _distance * sinf(_elevation);
karpent 0:d8b9955d2b36 88 }
karpent 0:d8b9955d2b36 89
karpent 0:d8b9955d2b36 90