diff --git a/src/boards/HeliTest/ScreenManager.cpp b/src/boards/HeliTest/ScreenManager.cpp
index 2a83ee57d0bcf05d13cd4c8f47a202d86984bb93..d135babc96e9a7550e7dc23491cb1e6fdaa7f255 100644
--- a/src/boards/HeliTest/ScreenManager.cpp
+++ b/src/boards/HeliTest/ScreenManager.cpp
@@ -27,25 +27,28 @@
#include <miosix.h>
#include <stdio.h>
#include <sstream>
-
-using std::stringstream;
+#include <string>
using namespace miosix;
+using namespace mxgui;
+using std::string;
using miosix::Thread;
-using namespace mxgui;
+
const int MARGIN = 5;
const int INTERSPACE = 13;
-// 23 Available text lines
+static char buffer[256];
+// 23 Available text lines
// Screen is 240x320 pixels
namespace RogallinaBoard
{
ScreenManager::ScreenManager(SensorManager* sm)
- : display(DisplayManager::instance().getDisplay()), dc(display), dw(dc),sm(sm)
+ : display(DisplayManager::instance().getDisplay()), dc(display), dw(dc),
+ sm(sm)
{
D(printf("Screenmanager constructor\n"));
clearScreen();
@@ -61,15 +64,18 @@ void ScreenManager::update()
}
void ScreenManager::fetchData()
{
- FastInterruptDisableLock iLock;
- status_logger = logger.getLogStats();
- logger_started = logger.isStarted();
- logger_file = logger.getFileName();
+ {
+ FastInterruptDisableLock iLock;
- sensorData = sm->getSensorData();
- logging_sensors = sm->loggingSensors();
+ status_logger = logger.getLogStats();
+ logger_started = logger.isStarted();
+ logger_file = logger.getFileName();
- status_fmm = sHeliFMM->getStatus();
+ sensorData = sm->getSensorData();
+ logging_sensors = sm->loggingSensors();
+
+ status_fmm = sHeliFMM->getStatus();
+ }
cpu_usage = sSystemDiagnostics->getCpuUsage();
min_stack = sSystemDiagnostics->getWorstStack();
@@ -86,8 +92,7 @@ void ScreenManager::drawScreen()
// Disegna titolo
writeLine(0, 0, "ROGALLINA CONTROLLER", green, black);
- char buffer[50];
- sprintf(buffer, "T: %.3f s", miosix::getTick() / 1000.0f);
+ sprintf(buffer, "T: %-10.3f", miosix::getTick() / 1000.0f);
writeLine(0, 140, buffer);
drawRogallinaFMM();
@@ -98,16 +103,16 @@ void ScreenManager::drawScreen()
void ScreenManager::drawRogallinaFMM()
{
- char buffer[50];
+ string state;
Color bgcolor = white, fgcolor = black;
switch (status_fmm.state)
{
case HELISTATE_BOOTED:
- sprintf(buffer, "State: BOOTED");
+ state = "BOOTED";
break;
case HELISTATE_CALIBRATION:
- sprintf(buffer, "State: CALIBRATION");
+ state = "CALIBRATION";
break;
case HELISTATE_FAILED:
{
@@ -116,52 +121,52 @@ void ScreenManager::drawRogallinaFMM()
switch (status_fmm.failReason)
{
case FMMFAIL_LOGGER:
- sprintf(buffer, "State: LOGGER ERROR!!!!");
+ state = "LOGGER ERROR";
break;
case FMMFAIL_SENSOR:
- sprintf(buffer, "State: SENSOR INIT ERROR!!!!");
+ state = "SENSOR INIT ERROR";
break;
default:
- sprintf(buffer, "State: UNKNOWN ERROR!!!!");
+ state = "UNKNOWN ERROR";
break;
}
break;
}
case HELISTATE_READY:
- sprintf(buffer, "State: READY ");
+ state = "READY";
break;
case HELISTATE_FREEFALL:
- sprintf(buffer, "State: FREE FALL ");
+ state = "State: FREE FALL";
break;
case HELISTATE_CUTTING:
- sprintf(buffer, "State: CUTTING ");
+ state = "State: CUTTING";
break;
case HELISTATE_DESCENDING:
- sprintf(buffer, "State: DESCENDING ");
+ state = "State: DESCENDING";
break;
case HELISTATE_STOPPING:
- sprintf(buffer, "State: STOPPING ");
+ state = "State: STOPPING";
break;
case HELISTATE_STOPPED:
bgcolor = blue;
fgcolor = white;
- sprintf(buffer, "State: STOPPED ");
+ state = "State: STOPPED";
break;
}
+ sprintf(buffer, "State: %-20s", state.c_str());
writeLine(1, 0, "STATE MACHINE");
writeLine(2, 0, buffer, fgcolor, bgcolor);
}
+
void ScreenManager::drawLoggerStatus()
{
const int sl = 3;
const int x1 = 0;
const int x2 = 120;
- char buffer[50];
-
writeLine(sl, x1, "LOGGER");
- sprintf(buffer, "File: %s", logger_file.c_str());
+ sprintf(buffer, "File: %-10s", logger_file.c_str());
writeLine(sl, x2, buffer);
int logger_state = 0; // 0: ready, -1: disabled, 1: started
@@ -174,35 +179,45 @@ void ScreenManager::drawLoggerStatus()
else
logger_state = logger_started ? 1 : 0;
#endif
-
+ string state;
+ Color fgColor = black;
+ Color bgColor = white;
switch (logger_state)
{
case 0:
- writeLine(sl + 1, x1, "Status: READY ");
+ state = "READY";
break;
case 1:
+ bgColor = green;
if (logging_sensors)
- writeLine(sl + 1, x1, "Status: LOGGING (sens on) ", black,
- green);
+ {
+ state = "LOGGING (sens on)";
+ }
else
- writeLine(sl + 1, x1, "Status: LOGGING (sens off) ", black,
- green);
+ {
+ state = "LOGGING (sens off)";
+ }
break;
case 2:
- writeLine(sl + 1, x1, "Status: STOPPED ", white, blue);
+ bgColor = blue;
+ state = "STOPPED";
break;
case -1:
- writeLine(sl + 1, x1, "Status: DISABLED ", white, red);
+ bgColor = red;
+ state = "DISABLED";
break;
default:
- writeLine(sl + 1, x1, "Status: UNKNOWN ", white, red);
+ bgColor = red;
+ state = "UNKNOWN";
break;
}
+ sprintf(buffer, "Status: %-20s", state.c_str());
+ writeLine(sl + 1, x1, buffer, fgColor, bgColor);
- sprintf(buffer, "Dropped: %d", status_logger.statDroppedSamples);
+ sprintf(buffer, "Dropped: %8d", status_logger.statDroppedSamples);
writeLine(sl + 2, x1, buffer);
- sprintf(buffer, "Last error: %d", status_logger.statWriteError);
+ sprintf(buffer, "Last error: %8d", status_logger.statWriteError);
writeLine(sl + 2, x2, buffer);
}
@@ -214,10 +229,8 @@ void ScreenManager::drawSensorSamples()
const int x2 = 80;
const int x3 = 160;
- char buffer[30];
-
writeLine(sl + 1, 0, "IMU");
- sprintf(buffer, "Compass: %.2f ", sensorData.mpu9255_data.compass);
+ sprintf(buffer, "Compass: %-8.2f", sensorData.mpu9255_data.compass);
writeLine(sl + 1, (x2 + x3) / 2, buffer);
writeLine(sl + 2, x1, "ACC");
@@ -225,90 +238,89 @@ void ScreenManager::drawSensorSamples()
writeLine(sl + 2, x3, "MAG");
// X
- sprintf(buffer, "X: %.2f ", sensorData.mpu9255_data.accel.getX());
+ sprintf(buffer, "X: %-8.2f", sensorData.mpu9255_data.accel.getX());
writeLine(sl + 3, x1, buffer);
- sprintf(buffer, "X: %.2f ", sensorData.mpu9255_data.gyro.getX());
+ sprintf(buffer, "X: %-8.2f", sensorData.mpu9255_data.gyro.getX());
writeLine(sl + 3, x2, buffer);
- sprintf(buffer, "X: %.2f ", sensorData.mpu9255_data.magnet.getX());
+ sprintf(buffer, "X: %-8.2f", sensorData.mpu9255_data.magnet.getX());
writeLine(sl + 3, x3, buffer);
// Y
- sprintf(buffer, "X: %.2f ", sensorData.mpu9255_data.accel.getY());
+ sprintf(buffer, "Y: %-8.2f", sensorData.mpu9255_data.accel.getY());
writeLine(sl + 4, x1, buffer);
- sprintf(buffer, "X: %.2f ", sensorData.mpu9255_data.gyro.getY());
+ sprintf(buffer, "Y: %-8.2f", sensorData.mpu9255_data.gyro.getY());
writeLine(sl + 4, x2, buffer);
- sprintf(buffer, "X: %.2f ", sensorData.mpu9255_data.magnet.getY());
+ sprintf(buffer, "Y: %-8.2f", sensorData.mpu9255_data.magnet.getY());
writeLine(sl + 4, x3, buffer);
// Z
- sprintf(buffer, "X: %.2f ", sensorData.mpu9255_data.accel.getZ());
+ sprintf(buffer, "Z: %-8.2f", sensorData.mpu9255_data.accel.getZ());
writeLine(sl + 5, x1, buffer);
- sprintf(buffer, "X: %.2f ", sensorData.mpu9255_data.gyro.getZ());
+ sprintf(buffer, "Z: %-8.2f", sensorData.mpu9255_data.gyro.getZ());
writeLine(sl + 5, x2, buffer);
- sprintf(buffer, "X: %.2f ", sensorData.mpu9255_data.magnet.getZ());
+ sprintf(buffer, "Z: %-8.2f", sensorData.mpu9255_data.magnet.getZ());
writeLine(sl + 5, x3, buffer);
// YAW, PITCH, ROLL
- sprintf(buffer, "YAW: %.2f ", sensorData.mpu9255_data.yaw);
+ sprintf(buffer, "YAW: %-8.2f", sensorData.mpu9255_data.yaw);
writeLine(sl + 6, x1, buffer);
- sprintf(buffer, "PTCH: %.2f ", sensorData.mpu9255_data.pitch);
+ sprintf(buffer, "PTCH: %-8.2f", sensorData.mpu9255_data.pitch);
writeLine(sl + 6, x2, buffer);
- sprintf(buffer, "ROLL: %.2f ", sensorData.mpu9255_data.roll);
+ sprintf(buffer, "ROLL: %-8.2f", sensorData.mpu9255_data.roll);
writeLine(sl + 6, x3, buffer);
// GPS
writeLine(sl + 7, 0, "GPS");
- sprintf(buffer, "BUF COUNT: %d ", (int)sensorData.gps_data.buf_count);
+ sprintf(buffer, "BUF COUNT: %-3d", (int)sensorData.gps_data.buf_count);
writeLine(sl + 7, x2, buffer);
// INFO
switch (sensorData.gps_data.fix)
{
case 0:
- sprintf(buffer, "FIX: NOPE");
+ sprintf(buffer, "FIX: NOPE ");
break;
case 1:
- sprintf(buffer, "FIX: GPS FIX");
+ sprintf(buffer, "FIX: GPS FIX ");
break;
case 2:
sprintf(buffer, "FIX: AGPS FIX");
break;
}
writeLine(sl + 8, x1, buffer);
- sprintf(buffer, "SATS: %d ", sensorData.gps_data.sats);
+ sprintf(buffer, "SATS: %-3d", sensorData.gps_data.sats);
writeLine(sl + 9, x1, buffer);
- sprintf(buffer, "TIME: %2d:%2d:%2d ", sensorData.gps_data.time.hours,
+ sprintf(buffer, "TIME: %2d:%2d:%2d", sensorData.gps_data.time.hours,
sensorData.gps_data.time.minutes, sensorData.gps_data.time.seconds);
writeLine(sl + 10, x1, buffer);
// COORDS
- sprintf(buffer, "LAT: %.2f ", sensorData.gps_data.lat);
+ sprintf(buffer, "LAT: %-8.2f", sensorData.gps_data.lat);
writeLine(sl + 8, x2, buffer);
- sprintf(buffer, "LON: %.2f ", sensorData.gps_data.lon);
+ sprintf(buffer, "LON: %-8.2f", sensorData.gps_data.lon);
writeLine(sl + 9, x2, buffer);
- sprintf(buffer, "ALT: %.2f ", sensorData.gps_data.alt);
+ sprintf(buffer, "ALT: %-8.2f", sensorData.gps_data.alt);
writeLine(sl + 10, x2, buffer);
// DIR, SPEED
- sprintf(buffer, "SPD: %.2f ", sensorData.gps_data.speed);
+ sprintf(buffer, "SPD: %-8.2f", sensorData.gps_data.speed);
writeLine(sl + 8, x3, buffer);
- sprintf(buffer, "DIR: %.2f ", sensorData.gps_data.bearing);
+ sprintf(buffer, "DIR: %-8.2f", sensorData.gps_data.bearing);
writeLine(sl + 9, x3, buffer);
- sprintf(buffer, "L_U: %.1f s", sensorData.gps_data.timestamp / 1000.0f);
+ sprintf(buffer, "L_U: %-8.1f", sensorData.gps_data.timestamp / 1000.0f);
writeLine(sl + 10, x3, buffer);
// BAROMETER
writeLine(sl + 11, 0, "BAROMETER");
- sprintf(buffer, "PRES: %.2f ", sensorData.bme280_data.pressure);
+ sprintf(buffer, "PRES: %-8.2f", sensorData.bme280_data.pressure);
writeLine(sl + 12, x1, buffer);
- sprintf(buffer, "TEMP: %.2f ", sensorData.bme280_data.temperature);
- writeLine(sl + 12, x2, buffer);
+ sprintf(buffer, "TEMP: %-8.2f", sensorData.bme280_data.temperature);
+ writeLine(sl + 12, (x2 + x3)/2, buffer);
}
void ScreenManager::drawSystemStats()
{
- char buffer[70];
- sprintf(buffer, "CpuUsage: %3.1f Worst stack:%d %d bytes ",
+ sprintf(buffer, "CpuUsage: %4.1f Worst stack:%2d %6d bytes",
cpu_usage, min_stack.thread_id, min_stack.abs_free_stack);
writeLine(23, 0, buffer);
}