Ngo Kien / Mbed OS RadarDemoT

Optimaze with new mbed os for study

Dependencies:   TS_DISCO_F746NG BSP_DISCO_F746NG Graphics

RadarDemo/Radar.cpp@1:5e49b46de1b0, 2016-11-04 (annotated)

Committer:
karpent
Date:
Fri Nov 04 17:10:50 2016 +0000
Revision:
1:5e49b46de1b0
Parent:
0:d8b9955d2b36
Child:
2:8db224cc1fcb
Graphics folder published as a library, minor comments update.

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 1:5e49b46de1b0 143 bool 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 1:5e49b46de1b0 162 return count > 0;
karpent 0:d8b9955d2b36 163 }
karpent 0:d8b9955d2b36 164
karpent 0:d8b9955d2b36 165
karpent 0:d8b9955d2b36 166 void Radar::DrawBorder()
karpent 0:d8b9955d2b36 167 {
karpent 0:d8b9955d2b36 168 // Set display window coordinates
karpent 0:d8b9955d2b36 169 int x1 = 0;
karpent 0:d8b9955d2b36 170 int y1 = 0;
karpent 0:d8b9955d2b36 171 int x2 = _display->DisplayWidth() - 1;
karpent 0:d8b9955d2b36 172 int y2 = _display->DisplayHeight() - 1;
karpent 0:d8b9955d2b36 173
karpent 0:d8b9955d2b36 174 _display->DrawRectangle(x1, y1, x2, y2);
karpent 0:d8b9955d2b36 175 _display->DrawRectangle(window.x1, window.y1, window.x2, window.y2);
karpent 0:d8b9955d2b36 176
karpent 0:d8b9955d2b36 177 float maxLen = 3;
karpent 0:d8b9955d2b36 178 float delta = M_PI / (8 * 5);
karpent 0:d8b9955d2b36 179
karpent 0:d8b9955d2b36 180 // top scale, y = 0;
karpent 0:d8b9955d2b36 181 float angle = -M_PI / 2;
karpent 0:d8b9955d2b36 182 while ((angle += delta) < M_PI / 2) {
karpent 0:d8b9955d2b36 183 int yStart = y1;
karpent 0:d8b9955d2b36 184 int xStart = _centerX + (yStart - _centerY) * tan(angle);
karpent 0:d8b9955d2b36 185
karpent 0:d8b9955d2b36 186 if (xStart >= window.x1 && xStart <= window.x2) {
karpent 0:d8b9955d2b36 187 int yEnd = yStart + maxLen;
karpent 0:d8b9955d2b36 188 int xEnd = _centerX + (yEnd - _centerY) * tan(angle);
karpent 0:d8b9955d2b36 189
karpent 0:d8b9955d2b36 190 _display->DrawLine(xStart, yStart, xEnd, yEnd);
karpent 0:d8b9955d2b36 191 }
karpent 0:d8b9955d2b36 192 }
karpent 0:d8b9955d2b36 193
karpent 0:d8b9955d2b36 194 // bottom scale, y = y2;
karpent 0:d8b9955d2b36 195 angle = M_PI / 2;
karpent 0:d8b9955d2b36 196 while ((angle += delta) < 3 * M_PI / 2) {
karpent 0:d8b9955d2b36 197 int yStart = y2;
karpent 0:d8b9955d2b36 198 int xStart = _centerX + (yStart - _centerY) * tan(angle);
karpent 0:d8b9955d2b36 199
karpent 0:d8b9955d2b36 200 if (xStart >= window.x1 && xStart <= window.x2) {
karpent 0:d8b9955d2b36 201 int yEnd = yStart - maxLen;
karpent 0:d8b9955d2b36 202 int xEnd = _centerX + (yEnd - _centerY) * tan(angle);
karpent 0:d8b9955d2b36 203
karpent 0:d8b9955d2b36 204 _display->DrawLine(xStart, yStart, xEnd, yEnd);
karpent 0:d8b9955d2b36 205 }
karpent 0:d8b9955d2b36 206 }
karpent 0:d8b9955d2b36 207
karpent 0:d8b9955d2b36 208 // left scale, x = 0;
karpent 0:d8b9955d2b36 209 angle = -M_PI / 2;
karpent 0:d8b9955d2b36 210 while ((angle += delta) < M_PI / 2) {
karpent 0:d8b9955d2b36 211 int xStart = x1;
karpent 0:d8b9955d2b36 212 int yStart = _centerY + (xStart - _centerX) * tan(angle);
karpent 0:d8b9955d2b36 213
karpent 0:d8b9955d2b36 214 if (yStart >= window.y1 && yStart <= window.y2) {
karpent 0:d8b9955d2b36 215 int xEnd = xStart + maxLen;
karpent 0:d8b9955d2b36 216 int yEnd = _centerY + (xEnd - _centerX) * tan(angle);
karpent 0:d8b9955d2b36 217
karpent 0:d8b9955d2b36 218 _display->DrawLine(xStart, yStart, xEnd, yEnd);
karpent 0:d8b9955d2b36 219 }
karpent 0:d8b9955d2b36 220 }
karpent 0:d8b9955d2b36 221
karpent 0:d8b9955d2b36 222 // right scale, x = x2;
karpent 0:d8b9955d2b36 223 angle = M_PI / 2;
karpent 0:d8b9955d2b36 224 while ((angle += delta) < 3 * M_PI / 2) {
karpent 0:d8b9955d2b36 225 int xStart = x2;
karpent 0:d8b9955d2b36 226 int yStart = _centerY + (xStart - _centerX) * tan(angle);
karpent 0:d8b9955d2b36 227
karpent 0:d8b9955d2b36 228 if (yStart >= window.y1 && yStart <= window.y2) {
karpent 0:d8b9955d2b36 229 int xEnd = xStart - maxLen;
karpent 0:d8b9955d2b36 230 int yEnd = _centerY + (xEnd - _centerX) * tan(angle);
karpent 0:d8b9955d2b36 231
karpent 0:d8b9955d2b36 232 _display->DrawLine(xStart, yStart, xEnd, yEnd);
karpent 0:d8b9955d2b36 233 }
karpent 0:d8b9955d2b36 234 }
karpent 0:d8b9955d2b36 235 }
karpent 0:d8b9955d2b36 236
karpent 0:d8b9955d2b36 237
karpent 0:d8b9955d2b36 238 void Radar::DrawRadarBeam(float azimuth)
karpent 0:d8b9955d2b36 239 {
karpent 0:d8b9955d2b36 240 // Maximum radius length i local coordinates
karpent 0:d8b9955d2b36 241 int maxRadius = floor( 0.5f + GetMaxRange() * _scaleRatio);
karpent 0:d8b9955d2b36 242
karpent 0:d8b9955d2b36 243 int endX = GetCenterX() + maxRadius * sin(azimuth);
karpent 0:d8b9955d2b36 244 int endY = GetCenterY() - maxRadius * cos(azimuth);
karpent 0:d8b9955d2b36 245
karpent 0:d8b9955d2b36 246 _display->SetDrawColor(0xFF, 0xFF, 0xFF, 0xFF);
karpent 0:d8b9955d2b36 247 _display->DrawLine(_centerX, _centerY, endX, endY);
karpent 0:d8b9955d2b36 248 }
karpent 0:d8b9955d2b36 249
karpent 0:d8b9955d2b36 250 Display* Radar::GetDisplay()
karpent 0:d8b9955d2b36 251 {
karpent 0:d8b9955d2b36 252 return _display;
karpent 0:d8b9955d2b36 253 }
karpent 0:d8b9955d2b36 254
karpent 0:d8b9955d2b36 255
karpent 0:d8b9955d2b36 256 int Radar::_centerX;
karpent 0:d8b9955d2b36 257 int Radar::_centerY;
karpent 0:d8b9955d2b36 258 uint8_t Radar::_rangeNumber;
karpent 0:d8b9955d2b36 259 float Radar::_scaleRatio;