void showBatteryPage(void)
{
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
if (feature(FEATURE_VBAT)) {
tfp_sprintf(lineBuffer, "Volts: %d.%1d Cells: %d", vbat / 10, vbat % 10, batteryCellCount);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
uint8_t batteryPercentage = calculateBatteryPercentage();
i2c_OLED_set_line(rowIndex++);
drawHorizonalPercentageBar(SCREEN_CHARACTER_COLUMN_COUNT, batteryPercentage);
}
if (feature(FEATURE_CURRENT_METER)) {
tfp_sprintf(lineBuffer, "Amps: %d.%2d mAh: %d", amperage / 100, amperage % 100, mAhDrawn);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
uint8_t capacityPercentage = calculateBatteryCapacityRemainingPercentage();
i2c_OLED_set_line(rowIndex++);
drawHorizonalPercentageBar(SCREEN_CHARACTER_COLUMN_COUNT, capacityPercentage);
}
}
static void sendFuelLevel(void)
{
sendDataHead(ID_FUEL_LEVEL);
if (batteryConfig->batteryCapacity > 0) {
serialize16((uint16_t)calculateBatteryPercentage());
} else {
serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF));
}
}
void mavlinkSendSystemStatus(void)
{
uint16_t msgLength;
uint32_t onboardControlAndSensors = 35843;
/*
onboard_control_sensors_present Bitmask
fedcba9876543210
1000110000000011 For all = 35843
0001000000000100 With Mag = 4100
0010000000001000 With Baro = 8200
0100000000100000 With GPS = 16416
0000001111111111
*/
if (sensors(SENSOR_MAG)) onboardControlAndSensors |= 4100;
if (sensors(SENSOR_BARO)) onboardControlAndSensors |= 8200;
if (sensors(SENSOR_GPS)) onboardControlAndSensors |= 16416;
mavlink_msg_sys_status_pack(0, 200, &mavMsg,
// onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present.
//Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure,
// 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position,
// 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization,
// 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
onboardControlAndSensors,
// onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled
onboardControlAndSensors,
// onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error.
onboardControlAndSensors & 1023,
// load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
0,
// voltage_battery Battery voltage, in millivolts (1 = 1 millivolt)
feature(FEATURE_VBAT) ? vbat * 100 : 0,
// current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
feature(FEATURE_VBAT) ? amperage : -1,
// battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery
feature(FEATURE_VBAT) ? calculateBatteryPercentage() : 100,
// drop_rate_comm Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
0,
// errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
0,
// errors_count1 Autopilot-specific errors
0,
// errors_count2 Autopilot-specific errors
0,
// errors_count3 Autopilot-specific errors
0,
// errors_count4 Autopilot-specific errors
0);
msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
mavlinkSerialWrite(mavBuffer, msgLength);
}
static void applyLedFixedLayers()
{
for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) {
const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[ledIndex];
hsvColor_t color = *getSC(LED_SCOLOR_BACKGROUND);
int fn = ledGetFunction(ledConfig);
int hOffset = HSV_HUE_MAX;
switch (fn) {
case LED_FUNCTION_COLOR:
color = ledStripConfig()->colors[ledGetColor(ledConfig)];
break;
case LED_FUNCTION_FLIGHT_MODE:
for (unsigned i = 0; i < ARRAYLEN(flightModeToLed); i++)
if (!flightModeToLed[i].flightMode || FLIGHT_MODE(flightModeToLed[i].flightMode)) {
const hsvColor_t *directionalColor = getDirectionalModeColor(ledIndex, &ledStripConfig()->modeColors[flightModeToLed[i].ledMode]);
if (directionalColor) {
color = *directionalColor;
}
break; // stop on first match
}
break;
case LED_FUNCTION_ARM_STATE:
color = ARMING_FLAG(ARMED) ? *getSC(LED_SCOLOR_ARMED) : *getSC(LED_SCOLOR_DISARMED);
break;
case LED_FUNCTION_BATTERY:
color = HSV(RED);
hOffset += scaleRange(calculateBatteryPercentage(), 0, 100, -30, 120);
break;
case LED_FUNCTION_RSSI:
color = HSV(RED);
hOffset += scaleRange(rssi * 100, 0, 1023, -30, 120);
break;
default:
break;
}
if (ledGetOverlayBit(ledConfig, LED_OVERLAY_THROTTLE)) {
hOffset += scaledAux;
}
color.h = (color.h + hOffset) % (HSV_HUE_MAX + 1);
setLedHsv(ledIndex, &color);
}
}
static void showStatusPage(void)
{
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
if (feature(FEATURE_VBAT)) {
i2c_OLED_set_line(rowIndex++);
tfp_sprintf(lineBuffer, "V: %d.%1d ", vbat / 10, vbat % 10);
padLineBufferToChar(12);
i2c_OLED_send_string(lineBuffer);
uint8_t batteryPercentage = calculateBatteryPercentage();
drawHorizonalPercentageBar(10, batteryPercentage);
}
if (feature(FEATURE_CURRENT_METER)) {
i2c_OLED_set_line(rowIndex++);
tfp_sprintf(lineBuffer, "mAh: %d", mAhDrawn);
padLineBufferToChar(12);
i2c_OLED_send_string(lineBuffer);
uint8_t capacityPercentage = calculateBatteryCapacityRemainingPercentage();
drawHorizonalPercentageBar(10, capacityPercentage);
}
#ifdef GPS
if (feature(FEATURE_GPS)) {
tfp_sprintf(lineBuffer, "Sats: %d", gpsSol.numSat);
padHalfLineBuffer();
i2c_OLED_set_line(rowIndex);
i2c_OLED_send_string(lineBuffer);
tfp_sprintf(lineBuffer, "Fix: %s", gpsFixTypeText[gpsSol.fixType]);
padHalfLineBuffer();
i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
i2c_OLED_send_string(lineBuffer);
tfp_sprintf(lineBuffer, "HDOP: %d.%1d", gpsSol.hdop / 100, gpsSol.hdop % 100);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
tfp_sprintf(lineBuffer, "La/Lo: %d/%d", gpsSol.llh.lat / GPS_DEGREES_DIVIDER, gpsSol.llh.lon / GPS_DEGREES_DIVIDER);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
}
#endif
#ifdef MAG
if (sensors(SENSOR_MAG)) {
tfp_sprintf(lineBuffer, "HDG: %d", DECIDEGREES_TO_DEGREES(attitude.values.yaw));
padHalfLineBuffer();
i2c_OLED_set_line(rowIndex);
i2c_OLED_send_string(lineBuffer);
}
#endif
#ifdef BARO
if (sensors(SENSOR_BARO)) {
int32_t alt = baroCalculateAltitude();
tfp_sprintf(lineBuffer, "Alt: %d", alt / 100);
padHalfLineBuffer();
i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex);
i2c_OLED_send_string(lineBuffer);
}
#endif
rowIndex++;
char rollTrim[7], pitchTrim[7];
formatTrimDegrees(rollTrim, boardAlignment()->rollDeciDegrees);
formatTrimDegrees(pitchTrim, boardAlignment()->pitchDeciDegrees);
tfp_sprintf(lineBuffer, "Acc: %sR, %sP", rollTrim, pitchTrim );
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
}