Ngo Kien / Mbed OS RadarDemoT

Optimaze with new mbed os for study

Dependencies:   TS_DISCO_F746NG BSP_DISCO_F746NG Graphics

RadarDemo/Radar.cpp@0:d8b9955d2b36, 2016-11-04 (annotated)

Committer:
karpent
Date:
Fri Nov 04 01:02:37 2016 +0000
Revision:
0:d8b9955d2b36
Child:
1:5e49b46de1b0
Initial revision

Who changed what in which revision?

UserRevisionLine numberNew contents of line
karpent 0:d8b9955d2b36 1 //
karpent 0:d8b9955d2b36 2 // Radar.h - example of graphic commands usage to create 2D graphics.
karpent 0:d8b9955d2b36 3 //
karpent 0:d8b9955d2b36 4
karpent 0:d8b9955d2b36 5 #include "Radar.h"
karpent 0:d8b9955d2b36 6 #include "math.h"
karpent 0:d8b9955d2b36 7 #include <cstdlib>
karpent 0:d8b9955d2b36 8
karpent 0:d8b9955d2b36 9 Radar::Radar(Display* display)
karpent 0:d8b9955d2b36 10 {
karpent 0:d8b9955d2b36 11 _display = display;
karpent 0:d8b9955d2b36 12
karpent 0:d8b9955d2b36 13 Initialize();
karpent 0:d8b9955d2b36 14 }
karpent 0:d8b9955d2b36 15
karpent 0:d8b9955d2b36 16 Radar::~Radar()
karpent 0:d8b9955d2b36 17 {
karpent 0:d8b9955d2b36 18 for (std::list<Target>::iterator it = targetsList.begin(); it != targetsList.end(); ++it) {
karpent 0:d8b9955d2b36 19 Target& target = *it;
karpent 0:d8b9955d2b36 20 it = targetsList.erase(it);
karpent 0:d8b9955d2b36 21 }
karpent 0:d8b9955d2b36 22 targetsList.clear();
karpent 0:d8b9955d2b36 23 }
karpent 0:d8b9955d2b36 24
karpent 0:d8b9955d2b36 25
karpent 0:d8b9955d2b36 26 void Radar::Initialize()
karpent 0:d8b9955d2b36 27 {
karpent 0:d8b9955d2b36 28 int margin = 4;
karpent 0:d8b9955d2b36 29
karpent 0:d8b9955d2b36 30 // Set the size of display window
karpent 0:d8b9955d2b36 31 window.y1 = margin;
karpent 0:d8b9955d2b36 32 window.x1 = margin;
karpent 0:d8b9955d2b36 33
karpent 0:d8b9955d2b36 34 window.y2 = _display->DisplayHeight() - 1 - margin;
karpent 0:d8b9955d2b36 35 window.x2 = _display->DisplayWidth() - 1 - margin;
karpent 0:d8b9955d2b36 36
karpent 0:d8b9955d2b36 37 // Set default center position
karpent 0:d8b9955d2b36 38 SetCenter((window.x2 - window.x1) / 2 + window.x1, (window.y2 - window.y1) / 2 + window.y1);
karpent 0:d8b9955d2b36 39
karpent 0:d8b9955d2b36 40 // Set default range
karpent 0:d8b9955d2b36 41 SetRange(MAX_RANGE_INDEX);
karpent 0:d8b9955d2b36 42 }
karpent 0:d8b9955d2b36 43
karpent 0:d8b9955d2b36 44
karpent 0:d8b9955d2b36 45 void Radar::DrawTracks()
karpent 0:d8b9955d2b36 46 {
karpent 0:d8b9955d2b36 47 // Draw every track on the list if in range
karpent 0:d8b9955d2b36 48 for (std::list<Target>::iterator it = targetsList.begin(); it != targetsList.end(); ++it) {
karpent 0:d8b9955d2b36 49 Target& target = *it;
karpent 0:d8b9955d2b36 50
karpent 0:d8b9955d2b36 51 if (target.GetDistance() <= GetMaxRange()) {
karpent 0:d8b9955d2b36 52 DrawTarget(&target);
karpent 0:d8b9955d2b36 53 }
karpent 0:d8b9955d2b36 54 }
karpent 0:d8b9955d2b36 55 }
karpent 0:d8b9955d2b36 56
karpent 0:d8b9955d2b36 57
karpent 0:d8b9955d2b36 58 void Radar::DrawMarkers()
karpent 0:d8b9955d2b36 59 {
karpent 0:d8b9955d2b36 60 // Range markers
karpent 0:d8b9955d2b36 61 //
karpent 0:d8b9955d2b36 62 int localRange = (window.y2 - window.y1) / 2;
karpent 0:d8b9955d2b36 63 float scale = localRange / GetRange();
karpent 0:d8b9955d2b36 64
karpent 0:d8b9955d2b36 65 // First distance marker in local coordinates.
karpent 0:d8b9955d2b36 66 int maxRadius = floor( 0.5f + GetMaxRange() * scale);
karpent 0:d8b9955d2b36 67 int minRadius = floor( 0.5f + _rangeMarkersDelta[_rangeNumber] * scale);
karpent 0:d8b9955d2b36 68 float radius = minRadius;
karpent 0:d8b9955d2b36 69 while (radius <= maxRadius + 1) {
karpent 0:d8b9955d2b36 70 _display->DrawCircle(_centerX, _centerY, floor( 0.5f + radius));
karpent 0:d8b9955d2b36 71 radius += _rangeMarkersDelta[_rangeNumber] * scale;
karpent 0:d8b9955d2b36 72 }
karpent 0:d8b9955d2b36 73 _display->DrawCircle(_centerX, _centerY, maxRadius + 4);
karpent 0:d8b9955d2b36 74
karpent 0:d8b9955d2b36 75 // DrawPoint in the center
karpent 0:d8b9955d2b36 76 _display->DrawPoint(_centerX, _centerY, _display->GetDrawColor());
karpent 0:d8b9955d2b36 77
karpent 0:d8b9955d2b36 78 // Azimuth markers, use the first and last marker radius calculated above.
karpent 0:d8b9955d2b36 79 //
karpent 0:d8b9955d2b36 80 double angle = 0;
karpent 0:d8b9955d2b36 81 double angleDelta = M_PI / (double)azimuthMarkersCount;
karpent 0:d8b9955d2b36 82 for (int i = 0; i<azimuthMarkersCount; i++) {
karpent 0:d8b9955d2b36 83 float x = sin(angle);
karpent 0:d8b9955d2b36 84 float y = cos(angle);
karpent 0:d8b9955d2b36 85 _display->DrawLine(_centerX - x * minRadius, _centerY - y * minRadius, _centerX - x * maxRadius, _centerY - y * maxRadius);
karpent 0:d8b9955d2b36 86 _display->DrawLine(_centerX + x * minRadius, _centerY + y * minRadius, _centerX + x * maxRadius, _centerY + y * maxRadius);
karpent 0:d8b9955d2b36 87 angle += angleDelta;
karpent 0:d8b9955d2b36 88 }
karpent 0:d8b9955d2b36 89 }
karpent 0:d8b9955d2b36 90
karpent 0:d8b9955d2b36 91
karpent 0:d8b9955d2b36 92 void Radar::DrawTarget(Target * target)
karpent 0:d8b9955d2b36 93 {
karpent 0:d8b9955d2b36 94 const int radius = 6;
karpent 0:d8b9955d2b36 95 uint32_t colorMask = 0xFF00FF00;
karpent 0:d8b9955d2b36 96 _display->SetDrawColor((colorMask & 0x00FF0000) >> 16, (colorMask & 0x0000FF00) >> 8, (colorMask & 0x000000FF), (colorMask & 0xFF000000) >> 24);
karpent 0:d8b9955d2b36 97
karpent 0:d8b9955d2b36 98 // Convert world location to local coordinates
karpent 0:d8b9955d2b36 99 int x = _centerX + target->GetX() * _scaleRatio;
karpent 0:d8b9955d2b36 100 int y = _centerY - target->GetY() * _scaleRatio;
karpent 0:d8b9955d2b36 101
karpent 0:d8b9955d2b36 102 // Draw track
karpent 0:d8b9955d2b36 103 //_display->DrawLine(x-radius, y, x+radius, y);
karpent 0:d8b9955d2b36 104 _display->DrawPoint(x - radius, y, colorMask);
karpent 0:d8b9955d2b36 105 _display->DrawPoint(x + radius, y, colorMask);
karpent 0:d8b9955d2b36 106 for (int i = 1; i < radius; i++) {
karpent 0:d8b9955d2b36 107 //_display->DrawLine(x - radius + i, y-i, x + radius - i, y-i);
karpent 0:d8b9955d2b36 108 _display->DrawPoint(x - radius + i, y - i, colorMask);
karpent 0:d8b9955d2b36 109 _display->DrawPoint(x + radius - i, y - i, colorMask);
karpent 0:d8b9955d2b36 110 //_display->DrawLine(x - radius + i, y+i, x + radius - i, y+i);
karpent 0:d8b9955d2b36 111 _display->DrawPoint(x - radius + i, y + i, colorMask);
karpent 0:d8b9955d2b36 112 _display->DrawPoint(x + radius - i, y + i, colorMask);
karpent 0:d8b9955d2b36 113 }
karpent 0:d8b9955d2b36 114 _display->DrawPoint(x, y + radius, colorMask);
karpent 0:d8b9955d2b36 115 _display->DrawPoint(x, y - radius, colorMask);
karpent 0:d8b9955d2b36 116
karpent 0:d8b9955d2b36 117 // Draw vector
karpent 0:d8b9955d2b36 118 const float maxSpeed = 800; // [km/h]
karpent 0:d8b9955d2b36 119 float maxVectorLen = 20;
karpent 0:d8b9955d2b36 120 float vectorLen = target->GetSpeed() * maxVectorLen / maxSpeed;
karpent 0:d8b9955d2b36 121
karpent 0:d8b9955d2b36 122 // Convert world location to local coordinates
karpent 0:d8b9955d2b36 123 int x1 = x + sin(target->GetDirection()) * vectorLen;
karpent 0:d8b9955d2b36 124 int y1 = y - cos(target->GetDirection()) * vectorLen;
karpent 0:d8b9955d2b36 125 _display->DrawLine(x, y, x1, y1);
karpent 0:d8b9955d2b36 126 }
karpent 0:d8b9955d2b36 127
karpent 0:d8b9955d2b36 128
karpent 0:d8b9955d2b36 129 void Radar::DrawPlot(Location* plot)
karpent 0:d8b9955d2b36 130 {
karpent 0:d8b9955d2b36 131 const int size = 5;
karpent 0:d8b9955d2b36 132
karpent 0:d8b9955d2b36 133 // Convert world location to local coordinates
karpent 0:d8b9955d2b36 134 int x = _centerX + plot->GetX() * _scaleRatio;
karpent 0:d8b9955d2b36 135 int y = _centerY - plot->GetY() * _scaleRatio;
karpent 0:d8b9955d2b36 136
karpent 0:d8b9955d2b36 137 // Draw plot
karpent 0:d8b9955d2b36 138 _display->DrawLine(x - size / 2, y, x + size / 2, y);
karpent 0:d8b9955d2b36 139 _display->DrawLine(x, y - size / 2, x, y + size / 2);
karpent 0:d8b9955d2b36 140 }
karpent 0:d8b9955d2b36 141
karpent 0:d8b9955d2b36 142
karpent 0:d8b9955d2b36 143 void Radar::UpdateTargetsLocation(float startAngle, float endAngle, uint32_t currentTime)
karpent 0:d8b9955d2b36 144 {
karpent 0:d8b9955d2b36 145 int count = 0;
karpent 0:d8b9955d2b36 146 for (std::list<Target>::iterator it = targetsList.begin(); it != targetsList.end(); ++it) {
karpent 0:d8b9955d2b36 147 Target& target = *it;
karpent 0:d8b9955d2b36 148
karpent 0:d8b9955d2b36 149 if( startAngle < endAngle ) {
karpent 0:d8b9955d2b36 150 if (target.GetAzimuth() > startAngle && target.GetAzimuth() <= endAngle) {
karpent 0:d8b9955d2b36 151 count++;
karpent 0:d8b9955d2b36 152 target.UpdateLocationForTime(currentTime);
karpent 0:d8b9955d2b36 153 }
karpent 0:d8b9955d2b36 154 } else {
karpent 0:d8b9955d2b36 155 if (target.GetAzimuth() > startAngle || target.GetAzimuth() <= endAngle) {
karpent 0:d8b9955d2b36 156 count++;
karpent 0:d8b9955d2b36 157 target.UpdateLocationForTime(currentTime);
karpent 0:d8b9955d2b36 158 }
karpent 0:d8b9955d2b36 159 }
karpent 0:d8b9955d2b36 160 }
karpent 0:d8b9955d2b36 161
karpent 0:d8b9955d2b36 162 // if(count > 0) {
karpent 0:d8b9955d2b36 163 // pc.printf("%d targets updated for angle (%3.1f, %3.1f>\r\n", count, startAngle * 180.f/M_PI, endAngle * 180.f/M_PI);
karpent 0:d8b9955d2b36 164 // }
karpent 0:d8b9955d2b36 165 }
karpent 0:d8b9955d2b36 166
karpent 0:d8b9955d2b36 167
karpent 0:d8b9955d2b36 168 void Radar::DrawBorder()
karpent 0:d8b9955d2b36 169 {
karpent 0:d8b9955d2b36 170 // Set display window coordinates
karpent 0:d8b9955d2b36 171 int x1 = 0;
karpent 0:d8b9955d2b36 172 int y1 = 0;
karpent 0:d8b9955d2b36 173 int x2 = _display->DisplayWidth() - 1;
karpent 0:d8b9955d2b36 174 int y2 = _display->DisplayHeight() - 1;
karpent 0:d8b9955d2b36 175
karpent 0:d8b9955d2b36 176 _display->DrawRectangle(x1, y1, x2, y2);
karpent 0:d8b9955d2b36 177 _display->DrawRectangle(window.x1, window.y1, window.x2, window.y2);
karpent 0:d8b9955d2b36 178
karpent 0:d8b9955d2b36 179 float maxLen = 3;
karpent 0:d8b9955d2b36 180 float delta = M_PI / (8 * 5);
karpent 0:d8b9955d2b36 181
karpent 0:d8b9955d2b36 182 // top scale, y = 0;
karpent 0:d8b9955d2b36 183 float angle = -M_PI / 2;
karpent 0:d8b9955d2b36 184 while ((angle += delta) < M_PI / 2) {
karpent 0:d8b9955d2b36 185 int yStart = y1;
karpent 0:d8b9955d2b36 186 int xStart = _centerX + (yStart - _centerY) * tan(angle);
karpent 0:d8b9955d2b36 187
karpent 0:d8b9955d2b36 188 if (xStart >= window.x1 && xStart <= window.x2) {
karpent 0:d8b9955d2b36 189 int yEnd = yStart + maxLen;
karpent 0:d8b9955d2b36 190 int xEnd = _centerX + (yEnd - _centerY) * tan(angle);
karpent 0:d8b9955d2b36 191
karpent 0:d8b9955d2b36 192 _display->DrawLine(xStart, yStart, xEnd, yEnd);
karpent 0:d8b9955d2b36 193 }
karpent 0:d8b9955d2b36 194 }
karpent 0:d8b9955d2b36 195
karpent 0:d8b9955d2b36 196 // bottom scale, y = y2;
karpent 0:d8b9955d2b36 197 angle = M_PI / 2;
karpent 0:d8b9955d2b36 198 while ((angle += delta) < 3 * M_PI / 2) {
karpent 0:d8b9955d2b36 199 int yStart = y2;
karpent 0:d8b9955d2b36 200 int xStart = _centerX + (yStart - _centerY) * tan(angle);
karpent 0:d8b9955d2b36 201
karpent 0:d8b9955d2b36 202 if (xStart >= window.x1 && xStart <= window.x2) {
karpent 0:d8b9955d2b36 203 int yEnd = yStart - maxLen;
karpent 0:d8b9955d2b36 204 int xEnd = _centerX + (yEnd - _centerY) * tan(angle);
karpent 0:d8b9955d2b36 205
karpent 0:d8b9955d2b36 206 _display->DrawLine(xStart, yStart, xEnd, yEnd);
karpent 0:d8b9955d2b36 207 }
karpent 0:d8b9955d2b36 208 }
karpent 0:d8b9955d2b36 209
karpent 0:d8b9955d2b36 210 // left scale, x = 0;
karpent 0:d8b9955d2b36 211 angle = -M_PI / 2;
karpent 0:d8b9955d2b36 212 while ((angle += delta) < M_PI / 2) {
karpent 0:d8b9955d2b36 213 int xStart = x1;
karpent 0:d8b9955d2b36 214 int yStart = _centerY + (xStart - _centerX) * tan(angle);
karpent 0:d8b9955d2b36 215
karpent 0:d8b9955d2b36 216 if (yStart >= window.y1 && yStart <= window.y2) {
karpent 0:d8b9955d2b36 217 int xEnd = xStart + maxLen;
karpent 0:d8b9955d2b36 218 int yEnd = _centerY + (xEnd - _centerX) * tan(angle);
karpent 0:d8b9955d2b36 219
karpent 0:d8b9955d2b36 220 _display->DrawLine(xStart, yStart, xEnd, yEnd);
karpent 0:d8b9955d2b36 221 }
karpent 0:d8b9955d2b36 222 }
karpent 0:d8b9955d2b36 223
karpent 0:d8b9955d2b36 224 // right scale, x = x2;
karpent 0:d8b9955d2b36 225 angle = M_PI / 2;
karpent 0:d8b9955d2b36 226 while ((angle += delta) < 3 * M_PI / 2) {
karpent 0:d8b9955d2b36 227 int xStart = x2;
karpent 0:d8b9955d2b36 228 int yStart = _centerY + (xStart - _centerX) * tan(angle);
karpent 0:d8b9955d2b36 229
karpent 0:d8b9955d2b36 230 if (yStart >= window.y1 && yStart <= window.y2) {
karpent 0:d8b9955d2b36 231 int xEnd = xStart - maxLen;
karpent 0:d8b9955d2b36 232 int yEnd = _centerY + (xEnd - _centerX) * tan(angle);
karpent 0:d8b9955d2b36 233
karpent 0:d8b9955d2b36 234 _display->DrawLine(xStart, yStart, xEnd, yEnd);
karpent 0:d8b9955d2b36 235 }
karpent 0:d8b9955d2b36 236 }
karpent 0:d8b9955d2b36 237 }
karpent 0:d8b9955d2b36 238
karpent 0:d8b9955d2b36 239
karpent 0:d8b9955d2b36 240 void Radar::DrawRadarBeam(float azimuth)
karpent 0:d8b9955d2b36 241 {
karpent 0:d8b9955d2b36 242 // Maximum radius length i local coordinates
karpent 0:d8b9955d2b36 243 int maxRadius = floor( 0.5f + GetMaxRange() * _scaleRatio);
karpent 0:d8b9955d2b36 244
karpent 0:d8b9955d2b36 245 int endX = GetCenterX() + maxRadius * sin(azimuth);
karpent 0:d8b9955d2b36 246 int endY = GetCenterY() - maxRadius * cos(azimuth);
karpent 0:d8b9955d2b36 247
karpent 0:d8b9955d2b36 248 _display->SetDrawColor(0xFF, 0xFF, 0xFF, 0xFF);
karpent 0:d8b9955d2b36 249 _display->DrawLine(_centerX, _centerY, endX, endY);
karpent 0:d8b9955d2b36 250 }
karpent 0:d8b9955d2b36 251
karpent 0:d8b9955d2b36 252 Display* Radar::GetDisplay()
karpent 0:d8b9955d2b36 253 {
karpent 0:d8b9955d2b36 254 return _display;
karpent 0:d8b9955d2b36 255 }
karpent 0:d8b9955d2b36 256
karpent 0:d8b9955d2b36 257
karpent 0:d8b9955d2b36 258 int Radar::_centerX;
karpent 0:d8b9955d2b36 259 int Radar::_centerY;
karpent 0:d8b9955d2b36 260 uint8_t Radar::_rangeNumber;
karpent 0:d8b9955d2b36 261 float Radar::_scaleRatio;