static void showBatteryPage(void)
{
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) {
tfp_sprintf(lineBuffer, "Volts: %d.%1d Cells: %d", getBatteryVoltage() / 10, getBatteryVoltage() % 10, getBatteryCellCount());
padLineBuffer();
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
uint8_t batteryPercentage = calculateBatteryPercentageRemaining();
i2c_OLED_set_line(bus, rowIndex++);
drawHorizonalPercentageBar(SCREEN_CHARACTER_COLUMN_COUNT, batteryPercentage);
}
if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) {
int32_t amperage = getAmperage();
tfp_sprintf(lineBuffer, "Amps: %d.%2d mAh: %d", amperage / 100, amperage % 100, getMAhDrawn());
padLineBuffer();
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
uint8_t capacityPercentage = calculateBatteryPercentageRemaining();
i2c_OLED_set_line(bus, rowIndex++);
drawHorizonalPercentageBar(SCREEN_CHARACTER_COLUMN_COUNT, capacityPercentage);
}
}
inline static void adcInit() {
ADCSRA = _BV(ADEN) | _BV(ADPS0) | _BV(ADPS1) | _BV(ADPS2);
DIDR1 = _BV(ADC11D);
ADMUXA = 11;
ADMUXB = _BV(REFS1); //internal 2.2 reference
delay(10);
getBatteryVoltage();
delay(10);
getBatteryVoltage();
}
int main()
{
init_motors();
init_peripherals();
// find out how many cells are connected
u16 batteryCutoff;
while((batteryCutoff = getBatteryVoltage()) == 0);
if(batteryCutoff > 720) // voltage is higher than 6V -> 2cells
batteryCutoff = 720; // cutoff 6V
else
batteryCutoff = 360; // cutoff 3V
calibrate_sensors();
// enable interrupt driven gyro acquisation
startGyro();
u08 c = 0, maxThrust = (batteryCutoff>360)?70:100;
u08 cutBattery = 0;
while(1) {
if(Thrust > maxThrust)
Thrust = maxThrust;
calculate_balance(Thrust, Yaw, Pitch, Roll);
checkForInput();
// check battery
if(((c%100)==0)) {
u16 bat = getBatteryVoltage();
if(bat > 0 && bat <= batteryCutoff) {
if(maxThrust > 0)
maxThrust -= 5;
cutBattery = 1;
}
}
if(!cutBattery || !(c % 50))
PORTC ^= (1<<2);
c++;
//_delay_ms(100);
}
}
void sendLaserVisiblePacket() {
packet.s.messageId = MESSAGE_LASER_VISIBLE;
packet.s.batteryVoltage = getBatteryVoltage();
packet.s.calibration = calibration;
packet.s.laserTripped = (0 != (PINB & _BV(PB1)));
radio.send(GATEWAY_ADDRESS, packet.buff, 9);
}
void messaging_onMessageRead(unsigned char messageCode, unsigned char* parametersBuffer, unsigned short parametersBytesRead)
{
switch((unsigned short) messageCode)
{
case MESSAGE_CODE_TEST_CONNECTION:
testChannel();
break;
case MESSAGE_CODE_GET_VALUE_FROM_ULTRASONIC_SENSOR:
getValueFromUltrasonicSensor(parametersBuffer[0]);
break;
case MESSAGE_CODE_GET_VALUES_FROM_MAGNETOMETER:
getValuesFromMagnetometer();
break;
case MESSAGE_CODE_GET_VALUES_FROM_ACCELEROMETER:
getValuesFromAccelerometer();
break;
case MESSAGE_CODE_GET_VALUES_FROM_GYROSCOPE:
getValuesFromGyroscope();
break;
case MESSAGE_CODE_GET_BATTERY_VOLTAGE:
getBatteryVoltage();
break;
case MESSAGE_CODE_GET_CHARGER_VOLTAGE:
getChargerVoltage();
break;
case MESSAGE_CODE_CHANGE_MOTOR_STATE:
changeMotorState(parametersBuffer[0], parametersBuffer[1]);
break;
}
}
static void buildTelemetryFrame(uint8_t *packet)
{
uint8_t a1Value;
switch (rxFrSkySpiConfig()->a1Source) {
case FRSKY_SPI_A1_SOURCE_VBAT:
a1Value = (getBatteryVoltage() / 5) & 0xff;
break;
case FRSKY_SPI_A1_SOURCE_EXTADC:
a1Value = (adcGetChannel(ADC_EXTERNAL1) & 0xff0) >> 4;
break;
case FRSKY_SPI_A1_SOURCE_CONST:
a1Value = A1_CONST_D & 0xff;
break;
}
const uint8_t a2Value = (adcGetChannel(ADC_RSSI)) >> 4;
telemetryId = packet[4];
frame[0] = 0x11; // length
frame[1] = rxFrSkySpiConfig()->bindTxId[0];
frame[2] = rxFrSkySpiConfig()->bindTxId[1];
frame[3] = a1Value;
frame[4] = a2Value;
frame[5] = (uint8_t)cc2500getRssiDbm();
uint8_t bytesUsed = 0;
#if defined(USE_TELEMETRY_FRSKY_HUB)
if (telemetryEnabled) {
bytesUsed = appendFrSkyHubData(&frame[8]);
}
#endif
frame[6] = bytesUsed;
frame[7] = telemetryId;
}
bool srxlFrameRpm(sbuf_t *dst, timeUs_t currentTimeUs)
{
uint16_t period_us = SPEKTRUM_RPM_UNUSED;
#ifdef USE_ESC_SENSOR_TELEMETRY
escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
if (escData != NULL) {
period_us = 60000000 / escData->rpm; // revs/minute -> microSeconds
}
#endif
int16_t coreTemp = SPEKTRUM_TEMP_UNUSED;
#if defined(USE_ADC_INTERNAL)
coreTemp = getCoreTemperatureCelsius();
coreTemp = coreTemp * 9 / 5 + 32; // C -> F
#endif
UNUSED(currentTimeUs);
sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_RPM);
sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
sbufWriteU16BigEndian(dst, period_us); // pulse leading edges
if (telemetryConfig()->report_cell_voltage) {
sbufWriteU16BigEndian(dst, getBatteryAverageCellVoltage()); // Cell voltage is in units of 0.01V
} else {
sbufWriteU16BigEndian(dst, getBatteryVoltage()); // vbat is in units of 0.01V
}
sbufWriteU16BigEndian(dst, coreTemp); // temperature
sbufFill(dst, STRU_TELE_RPM_EMPTY_FIELDS_VALUE, STRU_TELE_RPM_EMPTY_FIELDS_COUNT);
// Mandatory frame, send it unconditionally.
return true;
}
void sendStatusPacket() {
packet.s.messageId = MESSAGE_STATUS;
packet.s.batteryVoltage = getBatteryVoltage();
packet.s.calibration = calibration;
packet.s.laserTripped = (0 != (PINB & _BV(PB1)));
radio.send(GATEWAY_ADDRESS, packet.buff, 9);
}
static void ltm_sframe(void)
{
uint8_t lt_flightmode;
uint8_t lt_statemode;
if (FLIGHT_MODE(PASSTHRU_MODE))
lt_flightmode = 0;
else if (FLIGHT_MODE(GPS_HOME_MODE))
lt_flightmode = 13;
else if (FLIGHT_MODE(GPS_HOLD_MODE))
lt_flightmode = 9;
else if (FLIGHT_MODE(HEADFREE_MODE))
lt_flightmode = 4;
else if (FLIGHT_MODE(BARO_MODE))
lt_flightmode = 8;
else if (FLIGHT_MODE(ANGLE_MODE))
lt_flightmode = 2;
else if (FLIGHT_MODE(HORIZON_MODE))
lt_flightmode = 3;
else
lt_flightmode = 1; // Rate mode
lt_statemode = (ARMING_FLAG(ARMED)) ? 1 : 0;
if (failsafeIsActive())
lt_statemode |= 2;
ltm_initialise_packet('S');
ltm_serialise_16(getBatteryVoltage() * 100); //vbat converted to mv
ltm_serialise_16(0); // current, not implemented
ltm_serialise_8((uint8_t)((getRssi() * 254) / 1023)); // scaled RSSI (uchar)
ltm_serialise_8(0); // no airspeed
ltm_serialise_8((lt_flightmode << 2) | lt_statemode);
ltm_finalise();
}
char processBatteryCommand(char commandCode, void* commandData, void* responseData) {
switch(commandCode) {
case GET_BATTERY_VOLTAGE:
getBatteryVoltage((char*) responseData);
break;
case GET_STEERING_VOLTAGE:
getSteeringVoltage((char*) responseData);
break;
}
}
static inline void hottEAMUpdateBattery(HOTT_EAM_MSG_t *hottEAMMessage)
{
uint8_t vbat_dcv = getBatteryVoltage() / 10; // vbat resolution is 10mV convert to 100mv (deciVolt)
hottEAMMessage->main_voltage_L = vbat_dcv & 0xFF;
hottEAMMessage->main_voltage_H = vbat_dcv >> 8;
hottEAMMessage->batt1_voltage_L = vbat_dcv & 0xFF;
hottEAMMessage->batt1_voltage_H = vbat_dcv >> 8;
updateAlarmBatteryStatus(hottEAMMessage);
}
/// \brief .
///
///
void radioSend(void)
{
MSG_FROM_THRM msg;
TIME time = getTime();
msg.info.temperatureActual = getNtcTemperature();
msg.info.valve = getMotorPosition();
msg.info.battery = getBatteryVoltage();
msg.info.temperatureNominal = targetTemperature;
msg.time.day = time.weekday;
msg.time.hour = time.hour;
msg.time.minute = time.minute;
nRF24L01_send((uint8_t *)&msg, sizeof(msg), 0);
}
void receiveGetBatteryVoltage(int connection, char* command){
unsigned char sendBuffer[5];
float batteryVoltage;
if(command[0] == RBS){
// syslog(LOG_DAEMON|LOG_INFO,"Getting battery voltage for GUI");
batteryVoltage = getBatteryVoltage();
syslog(LOG_DAEMON|LOG_INFO,"Retrieved battery voltage as: %f.",batteryVoltage);
sendBuffer[0] = RBS;
memcpy(sendBuffer+1,&batteryVoltage,4);
send(connection,sendBuffer,5,0);
} else {
syslog(LOG_DAEMON|LOG_ERR,"ERROR: Unrecognized command sent to receiveBatteryVoltage method.");
}
}
static void osdUpdateStats(void)
{
int16_t value = 0;
#ifdef USE_GPS
switch (osdConfig()->units) {
case OSD_UNIT_IMPERIAL:
value = CM_S_TO_MPH(gpsSol.groundSpeed);
break;
default:
value = CM_S_TO_KM_H(gpsSol.groundSpeed);
break;
}
#endif
if (stats.max_speed < value) {
stats.max_speed = value;
}
value = getBatteryVoltage();
if (stats.min_voltage > value) {
stats.min_voltage = value;
}
value = getAmperage() / 100;
if (stats.max_current < value) {
stats.max_current = value;
}
value = getRssiPercent();
if (stats.min_rssi > value) {
stats.min_rssi = value;
}
int altitude = getEstimatedAltitude();
if (stats.max_altitude < altitude) {
stats.max_altitude = altitude;
}
#ifdef USE_GPS
if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
value = GPS_distanceToHome;
if (stats.max_distance < GPS_distanceToHome) {
stats.max_distance = GPS_distanceToHome;
}
}
#endif
}
void setAdvData()
{
int scaledBatteryLevel = (int)(100.0*getBatteryVoltage()) - 100;
customAdvData.battery = (scaledBatteryLevel <= 255) ? scaledBatteryLevel : 255; // clip scaled level
customAdvData.statusFlags = 0;
customAdvData.statusFlags |= (dateReceived) ? 0x01 : 0x00; // set sync status bit
customAdvData.statusFlags |= (isCollecting) ? 0x02 : 0x00; // set collector status bit
customAdvData.statusFlags |= (scanner_enable) ? 0x04 : 0x00; // set collector status bit
customAdvData.group = badgeAssignment.group;
customAdvData.ID = badgeAssignment.ID;
// customAdvData.MAC is already set
//debug_log("custom data size: %d\r\n",sizeof(custom_data_array));
ble_advdata_manuf_data_t custom_manuf_data;
custom_manuf_data.company_identifier = 0xFF00; // unofficial manufacturer code
custom_manuf_data.data.p_data = (uint8_t*)(&customAdvData);
custom_manuf_data.data.size = CUSTOM_ADV_DATA_LEN;
// Build advertising data struct to pass into @ref ble_advdata_set.
ble_advdata_t advdata;
memset(&advdata, 0, sizeof(ble_advdata_t));
advdata.name_type = BLE_ADVDATA_FULL_NAME;
advdata.include_appearance = false;
advdata.flags = BLE_GAP_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE;
advdata.uuids_complete.uuid_cnt = sizeof(m_adv_uuids) / sizeof(m_adv_uuids[0]);
advdata.uuids_complete.p_uuids = m_adv_uuids;
advdata.p_manuf_specific_data = &custom_manuf_data;
uint32_t result = ble_advdata_set(&advdata, NULL); // set advertising data, don't set scan response data.
if(result != NRF_SUCCESS) {
debug_log("ERR: error setting advertising data, #%d.\r\n",(int)result);
}
else {
debug_log(" Updated adv. data.\r\n");
needAdvDataUpdate = false;
}
}
static uint8_t dispatchMeasurementReply(ibusAddress_t address)
{
int value;
switch (sensorAddressTypeLookup[address - ibusBaseAddress]) {
case IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE:
value = getBatteryVoltage() * 10;
if (telemetryConfig()->report_cell_voltage) {
value /= getBatteryCellCount();
}
return sendIbusMeasurement(address, value);
case IBUS_SENSOR_TYPE_TEMPERATURE:
value = gyroGetTemperature() * 10;
return sendIbusMeasurement(address, value + IBUS_TEMPERATURE_OFFSET);
case IBUS_SENSOR_TYPE_RPM:
return sendIbusMeasurement(address, (uint16_t) rcCommand[THROTTLE]);
}
return 0;
}
bool updateScanner()
{
bool scannerActive = true;
switch(scan_state)
{
case SCANNER_IDLE: // nothing to do, but check whether it's time to scan again.
if(scanner_enable) { // don't re-start scans if scanning is disabled
if(millis() - lastScanTime >= scanPeriod_ms) { // start scanning if it's time to scan
// don't start scans if BLE is inactive (paused)
if(BLEgetStatus() != BLE_INACTIVE) {
uint32_t result = startScan();
if(result == NRF_SUCCESS) {
scan_state = SCANNER_SCANNING;
debug_log("SCANNER: Started scan.\r\n");
}
else {
debug_log("ERR: error starting scan, #%u\r\n",(unsigned int)result);
}
lastScanTime = millis();
}
}
}
else {
scannerActive = false;
}
break;
case SCANNER_SCANNING:
// Scan timeout callback will exit scanning state.
break;
case SCANNER_SAVE:
debug_log("SCANNER: Saving scan results. %d devices seen\r\n",scan.num);
int numSaved = 0;
int chunksUsed = 0;
if (scan.num > SCAN_DEVICES_PER_CHUNK) {
// We have scanned more devices than we can store in a chunk, prune off the top SCAN_DEVICES_PER_CHUNK
// devices by RSSI values
// This requirement is in place because even though we can currently handle storing > SCAN_DEVICES_PER_CHUNK,
// these chunks break the sender.
sortScanByRSSIDescending();
debug_log("SCANNER: Pruned %d devices with low RSSI\r\n", scan.num - SCAN_DEVICES_PER_CHUNK);
scan.num = SCAN_DEVICES_PER_CHUNK;
}
do {
// Fill chunk header
scanBuffer[scan.to].timestamp = scan.timestamp;
scanBuffer[scan.to].num = scan.num;
int scaledBatteryLevel = (int)(100.0*getBatteryVoltage()) - 100;
scanBuffer[scan.to].batteryLevel = (scaledBatteryLevel <= 255) ? scaledBatteryLevel : 255; // clip scaled level
debug_log(" C:%d\r\n",scan.to);
// Fill chunk with results
int numLeft = scan.num - numSaved;
int numThisChunk = (numLeft <= SCAN_DEVICES_PER_CHUNK) ? numLeft : SCAN_DEVICES_PER_CHUNK;
for(int i = 0; i < numThisChunk; i++) {
scanBuffer[scan.to].devices[i].ID = scan.IDs[numSaved + i];
scanBuffer[scan.to].devices[i].rssi = scan.rssiSums[numSaved + i] / scan.counts[numSaved + i];
scanBuffer[scan.to].devices[i].count = scan.counts[numSaved + i];
debug_log(" bdg ID#%.4hX, rssi %d, count %d\r\n", scanBuffer[scan.to].devices[i].ID,
(int)scanBuffer[scan.to].devices[i].rssi,
(int)scanBuffer[scan.to].devices[i].count );
}
numSaved += numThisChunk;
// Terminate chunk
if(chunksUsed == 0) { // first chunk of saved results
if(numSaved >= scan.num) { // did all the results fit in the first chunk
scanBuffer[scan.to].check = scanBuffer[scan.to].timestamp; // mark as complete
}
else {
scanBuffer[scan.to].check = CHECK_TRUNC;
}
}
else { // else we're continuing results from a previous chunk
scanBuffer[scan.to].check = CHECK_CONTINUE;
}
//debug_log("--num: %d\r\n",scanBuffer[scan.to].num);
chunksUsed++;
scan.to = (scan.to < LAST_SCAN_CHUNK) ? scan.to+1 : 0;
} while(numSaved < scan.num);
debug_log("SCANNER: Done saving results. used %d chunks.\r\n",chunksUsed);
scan_state = SCANNER_IDLE;
break;
default:
break;
} // switch(scan_state)
return scannerActive;
}
inline static uint8_t isBatteryLow() {
return getBatteryVoltage() < 698;
}
static void parseCommand(uint8_t* line)
{
/* ======================== Action commands ======================== */
/**
<b>Action Commands</b> */
if (line[0] == 'a')
{
/**
<ul>
<li> <b>Snm</b> Manually set Switch. Follow by battery n (1-3, 0 = none) and
load m (0-1)/panel (2). Each two-bit field represents a load or panel, and the
setting is the battery to be connected (no two batteries can be connected to a
load/panel). */
switch (line[1])
{
case 'S':
{
uint8_t battery = line[2]-'0';
uint8_t setting = line[3]-'0'-1;
if ((battery < 4) && (setting < 4)) setSwitch(battery, setting);
if (setting == 2) setPanelSwitchSetting(battery);
break;
}
/**
<li> <b>Rn</b> Reset a tripped overcurrent circuit breaker.
Set a FreeRTOS timer to expire after 250ms at which time the reset line is
released. The command is followed by an interface number n=0-5 being batteries
1-3, loads 1-2 and module. */
case 'R':
{
portTickType resetTime = 250;
intf = line[2]-'0';
if (intf > NUM_IFS-1) break;
xTimerHandle resetHandle
= xTimerCreate("Reset",resetTime,pdFALSE,(void *)intf,resetCallback);
if(resetHandle == NULL) break;
if (xTimerStart(resetHandle,0) != pdPASS) break;
overCurrentReset(intf);
break;
}
/**
<li> <b>W</b> Write the current configuration block to FLASH */
case 'W':
{
writeConfigBlock();
break;
}
/**
<li> <b>E</b> Send an ident response */
case 'E':
{
char ident[35] = "Battery Management System,";
stringAppend(ident,FIRMWARE_VERSION);
stringAppend(ident,",");
char version[3];
intToAscii(VERSION,version);
stringAppend(ident,version);
sendStringLowPriority("dE",ident);
break;
}
/**
<li> <b>B</b> Set the battery SoC from the measured OCV */
case 'B':
{
uint8_t battery = line[2]-'1';
setBatterySoC(battery,computeSoC(getBatteryVoltage(battery),
getTemperature(),getBatteryType(battery)));
break;
}
}
}
/**
</ul> */
/* ======================== Data request commands ================ */
/**
<b>Data Request Commands</b> */
else if (line[0] == 'd')
{
switch (line[1])
{
/**
<ul>
<li> <b>S</b> Ask for all switch settings to be sent as well as control
settings. */
case 'S':
{
sendResponse("dS",(int)getSwitchControlBits());
uint8_t controlByte = getControls();
sendResponse("dD",controlByte);
break;
}
/**
<li> <b>Bn</b> Ask for battery n=1-3 parameters to be sent */
case 'B':
{
char id[] = "pR0";
id[2] = line[2];
uint8_t battery = line[2] - '1';
dataMessageSend(id,getBatteryResistanceAv(battery),0);
id[1] = 'T';
dataMessageSend(id,(int32_t)configData.config.batteryType[battery],
(int32_t)configData.config.batteryCapacity[battery]);
id[1] = 'F';
dataMessageSend(id,(int32_t)configData.config.
floatStageCurrentScale[battery],
(int32_t)configData.config.floatVoltage[battery]);
id[1] = 'A';
dataMessageSend(id,(int32_t)configData.config.
bulkCurrentLimitScale[battery],
(int32_t)configData.config.absorptionVoltage[battery]);
break;
}
/**
<li> <b>T</b> Ask for monitor strategy parameters to be sent. */
case 'T':
{
char id[] = "pts";
dataMessageSend(id,(int32_t)configData.config.monitorStrategy,0);
id[2] = 'V';
dataMessageSend(id,(int32_t)configData.config.lowVoltage,
(int32_t)configData.config.criticalVoltage);
id[2] = 'S';
dataMessageSend(id,(int32_t)configData.config.lowSoC,
(int32_t)configData.config.criticalSoC);
id[2] = 'F';
dataMessageSend(id,(int32_t)configData.config.floatBulkSoC,0);
break;
}
/**
<li> <b>C</b> Ask for charger strategy parameters to be sent. */
case 'C':
{
char id[] = "pcs";
dataMessageSend(id,(int32_t)configData.config.chargerStrategy,0);
id[2] = 'R';
dataMessageSend(id,(int32_t)configData.config.restTime,
(int32_t)configData.config.absorptionTime);
id[2] = 'D';
dataMessageSend(id,(int32_t)configData.config.minDutyCycle,0);
id[2] = 'F';
dataMessageSend(id,(int32_t)configData.config.floatTime,
(int32_t)configData.config.floatBulkSoC);
break;
}
}
}
/**
</ul> */
/* ================ Parameter Setting commands ================ */
/**
<b>Parameter Setting Commands</b> */
else if (line[0] == 'p')
{
uint8_t battery = line[2]-'1';
switch (line[1])
{
/**
<ul>
<li> <b>a-, a+</b> Turn autoTracking on or off */
case 'a':
{
if (line[2] == '-') configData.config.autoTrack = false;
else if (line[2] == '+') configData.config.autoTrack = true;
break;
}
/**
<li> <b>c-, c+</b> Turn communications sending on or off */
case 'c':
{
if (line[2] == '-') configData.config.enableSend = false;
else if (line[2] == '+') configData.config.enableSend = true;
break;
}
/**
<li> <b>C</b> Start a calibration sequence */
case 'C':
{
startCalibration();
break;
}
/**
<li> <b>d-, d+</b> Turn on debug messages */
case 'd':
{
if (line[2] == '+') configData.config.debugMessageSend = true;
if (line[2] == '-') configData.config.debugMessageSend = false;
break;
}
/**
<li> <b>Hxxxx</b> Set time from an ISO 8601 formatted string. */
case 'H':
{
setTimeFromString((char*)line+2);
break;
}
/**
<li> <b>M-, M+</b> Turn on/off data messaging (mainly for debug) */
case 'M':
{
if (line[2] == '-') configData.config.measurementSend = false;
else if (line[2] == '+') configData.config.measurementSend = true;
break;
}
/**
<li> <b>r-, r+</b> Turn recording on or off */
case 'r':
{
if (line[2] == '-') configData.config.recording = false;
else if ((line[2] == '+') && (writeFileHandle < 0x0FF))
configData.config.recording = true;
break;
}
/*--------------------*/
/* BATTERY parameters */
/**
<li> <b>Tntxx</b> Set battery type and capacity, n is battery, t is type,
xx is capacity */
case 'T':
{
if (battery < 3)
{
uint8_t type = line[3]-'0';
if (type < 3)
{
configData.config.batteryType[battery] =
(battery_Type)type;
configData.config.batteryCapacity[battery] =
asciiToInt((char*)line+4);
setBatteryChargeParameters(battery);
}
}
break;
}
/**
<li> <b>m-, m+</b> Turn on/off battery missing */
case 'm':
{
if (line[3] == '-') setBatteryMissing(battery,false);
else if (line[3] == '+') setBatteryMissing(battery,true);
break;
}
/**
<li> <b>Inxx</b> Set bulk current limit, n is battery, xx is limit */
case 'I':
{
if (battery < 3)
configData.config.bulkCurrentLimitScale[battery] =
asciiToInt((char*)line+3);
break;
}
/**
<li> <b>Anxx</b> Set battery gassing voltage limit, n is battery, xx is limit */
case 'A':
{
if (battery < 3)
configData.config.absorptionVoltage[battery] =
asciiToInt((char*)line+3);
break;
}
/**
<li> <b>fnxx</b> Set battery float current trigger, n is battery, xx is trigger */
case 'f':
{
if (battery < 3)
configData.config.floatStageCurrentScale[battery] =
asciiToInt((char*)line+3);
break;
}
/**
<li> <b>Fnxx</b> Set battery float voltage limit, n is battery, xx is limit */
case 'F':
{
if (battery < 3)
configData.config.floatVoltage[battery] =
asciiToInt((char*)line+3);
break;
}
/**
<li> <b>zn</b> zero current calibration by forcing current offset, n is battery */
case 'z':
{
if (battery < 3)
setCurrentOffset(battery,getCurrent(battery));
break;
}
/*--------------------*/
/* MONITOR parameters */
/**
<li> <b>sm</b> Set monitor strategy byte m for keeping isolation or avoiding
loading the battery under charge. */
case 's':
{
uint8_t monitorStrategy = line[2]-'0';
if (monitorStrategy <= 3)
configData.config.monitorStrategy = monitorStrategy;
break;
}
/**
<li> <b>vx</b> set low voltage threshold, x is voltage times 256. */
case 'v':
{
configData.config.lowVoltage = asciiToInt((char*)line+2);
break;
}
/**
<li> <b>Vx</b> set critical voltage threshold, x is voltage times 256. */
case 'V':
{
configData.config.criticalVoltage = asciiToInt((char*)line+2);
break;
}
/**
<li> <b>xx</b> set low SoC threshold, x is voltage times 256. */
case 'x':
{
configData.config.lowSoC = asciiToInt((char*)line+2);
break;
}
/**
<li> <b>Xx</b> set critical SoC threshold, x is voltage times 256. */
case 'X':
{
configData.config.criticalSoC = asciiToInt((char*)line+2);
break;
}
/*--------------------*/
/* CHARGER parameters */
/**
<li> <b>Sm</b> set charger strategy byte m. */
case 'S':
{
uint8_t chargerStrategy = line[2]-'0';
if (chargerStrategy < 2)
configData.config.chargerStrategy = chargerStrategy;
break;
}
/**
<li> <b>Rx</b> set charger algorithm minimum rest time x in seconds. */
case 'R':
{
configData.config.restTime = asciiToInt((char*)line+2);
break;
}
/**
<li> <b>Ax</b> set charger algorithm minimum gassing phase time x in seconds. */
case 'G':
{
configData.config.absorptionTime = asciiToInt((char*)line+2);
break;
}
/**
<li> <b>Dx</b> set charger minimum duty cycle x in seconds. */
case 'D':
{
configData.config.minDutyCycle = asciiToInt((char*)line+2);
break;
}
/**
<li> <b>Fx</b> set charger time to float x in seconds. */
case 'e':
{
configData.config.floatTime = asciiToInt((char*)line+2);
break;
}
/**
<li> <b>Bx</b> set charger SoC x to change from float to bulk phase. */
case 'B':
{
configData.config.floatBulkSoC = asciiToInt((char*)line+2);
break;
}
}
}
/**
</ul> */
/* ======================== File commands ================ */
/*
F - get free clusters
Wfilename - Open file for read/write. Filename is 8.3 string style. Returns handle.
Rfilename - Open file read only. Filename is 8.3 string style. Returns handle.
Xfilename - Delete the file. Filename is 8.3 string style.
Cxx - Close file. x is the file handle.
Gxx - Read a record from read or write file.
Ddirname - Get a directory listing. Directory name is 8.3 string style.
d[dirname] - Get the first (if dirname present) or next entry in directory.
s - Get status of open files and configData.config.recording flag
M - Mount the SD card.
All commands return an error status byte at the end.
Only one file for writing and a second for reading is possible.
Data is not written to the file externally. */
/**
<b>File Commands</b> */
else if (line[0] == 'f')
{
switch (line[1])
{
/**
<ul>
<li> <b>F</b> Return number of free clusters followed by the cluster size in bytes. */
case 'F':
{
uint8_t wordBuf;
uint32_t freeClusters = 0;
uint32_t sectorCluster = 0;
uint8_t fileStatus = FR_INT_ERR;
if (xSemaphoreTake(fileSendSemaphore,COMMS_FILE_TIMEOUT))
{
sendFileCommand('F',0,line+2);
uint8_t i;
for (i=0; i<4; i++)
{
wordBuf = 0;
xQueueReceive(fileReceiveQueue,&wordBuf,portMAX_DELAY);
freeClusters |= (wordBuf << 8*i);
}
for (i=0; i<4; i++)
{
wordBuf = 0;
xQueueReceive(fileReceiveQueue,&wordBuf,portMAX_DELAY);
sectorCluster |= (wordBuf << 8*i);
}
dataMessageSend("fF",freeClusters,sectorCluster);
xQueueReceive(fileReceiveQueue,&fileStatus,portMAX_DELAY);
xSemaphoreGive(fileSendSemaphore);
}
sendResponse("fE",(uint8_t)fileStatus);
break;
}
/**
<li> <b>Wf</b> Open a file f=filename for writing */
case 'W':
{
if (stringLength((char*)line+2) < 12)
{
uint8_t fileStatus = FR_INT_ERR;
if (xSemaphoreTake(fileSendSemaphore,COMMS_FILE_TIMEOUT))
{
stringCopy(writeFileName,(char*)line+2);
sendFileCommand('W',13,line+2);
xQueueReceive(fileReceiveQueue,&writeFileHandle,portMAX_DELAY);
sendResponse("fW",writeFileHandle);
xQueueReceive(fileReceiveQueue,&fileStatus,portMAX_DELAY);
xSemaphoreGive(fileSendSemaphore);
}
sendResponse("fE",(uint8_t)fileStatus);
}
break;
}
/**
<li> <b>Rf</b> Open a file f=filename for Reading */
case 'R':
{
if (stringLength((char*)line+2) < 12)
{
uint8_t fileStatus = FR_INT_ERR;
if (xSemaphoreTake(fileSendSemaphore,COMMS_FILE_TIMEOUT))
{
stringCopy(readFileName,(char*)line+2);
sendFileCommand('R',13,line+2);
xQueueReceive(fileReceiveQueue,&readFileHandle,portMAX_DELAY);
sendResponse("fR",readFileHandle);
xQueueReceive(fileReceiveQueue,&fileStatus,portMAX_DELAY);
xSemaphoreGive(fileSendSemaphore);
}
sendResponse("fE",(uint8_t)fileStatus);
}
break;
}
/**
<li> <b>Ghh</b> hh is the file handle. Close file for write or read file. The
file handle is a two character integer. */
case 'C':
{
uint8_t fileStatus = FR_INT_ERR;
if (xSemaphoreTake(fileSendSemaphore,COMMS_FILE_TIMEOUT))
{
uint8_t fileHandle = asciiToInt((char*)line+2);
sendFileCommand('C',1,&fileHandle);
xQueueReceive(fileReceiveQueue,&fileStatus,portMAX_DELAY);
if (fileStatus == FR_OK)
{
if (writeFileHandle == fileHandle)
{
writeFileHandle = 0xFF;
writeFileName[0] = 0;
}
else if (readFileHandle == fileHandle)
{
readFileHandle = 0xFF;
readFileName[0] = 0;
}
}
xSemaphoreGive(fileSendSemaphore);
}
sendResponse("fE",(uint8_t)fileStatus);
break;
}
/**
<li> <b>Ghh</b> hh is the file handle. Read a record of data from the read or
write file. The data starts from the end of the previous block that was read
(or the file start if just opened). The file handle is a two character integer.
A block of bytes is read from the file and stored in a circular buffer. A record
is taken from this block and sent, the rest remains in the buffer until the next
request. */
#define GET_RECORD_SIZE 80
case 'G':
{
uint8_t fileStatus = FR_INT_ERR;
if (xSemaphoreTake(fileSendSemaphore,COMMS_FILE_TIMEOUT))
{
int numberRecords = asciiToInt((char*)line+2);
if (numberRecords < 1) numberRecords = 1;
static FRESULT fileStatus = FR_OK;
static uint8_t buffer[GET_RECORD_SIZE];
static uint8_t readPointer = 0;
static uint8_t writePointer = 0;
char sendData[GET_RECORD_SIZE];
uint8_t sendPointer = 0;
uint8_t fileHandle = asciiToInt((char*)line+2);
uint8_t blockLength = GET_RECORD_SIZE-1;
uint8_t numRead;
uint8_t parameters[2] = {fileHandle, blockLength};
while (numberRecords > 0)
{
/* The buffer is empty, so fill up. */
if (readPointer == writePointer)
{
sendFileCommand('G',2,parameters);
numRead = 0;
xQueueReceive(fileReceiveQueue,&numRead,portMAX_DELAY);
/* As records are written in entirety, premature EOF should not happen. */
if (numRead != blockLength)
{
fileStatus = FR_DENIED;
break;
}
uint8_t i;
/* Read the entire block to the local buffer. */
for (i=0; i<numRead; i++)
{
uint8_t nextWritePointer = (writePointer+1)
% GET_RECORD_SIZE;
xQueueReceive(fileReceiveQueue,
buffer+writePointer,portMAX_DELAY);
writePointer = nextWritePointer;
}
/* Get status byte. */
xQueueReceive(fileReceiveQueue,&fileStatus,portMAX_DELAY);
}
/* Assemble the data message until EOL encountered, or block exhausted. */
while (sendPointer < GET_RECORD_SIZE-1)
{
sendData[sendPointer] = buffer[readPointer];
readPointer = (readPointer+1) % GET_RECORD_SIZE;
if (sendData[sendPointer] == '\n')
{
sendData[sendPointer+1] = 0;
sendString("fG",sendData);
sendPointer = 0;
numberRecords--;
break;
}
/* If the current block is exhausted, go get some more. */
if (readPointer == writePointer) break;
sendPointer++;
}
}
xSemaphoreGive(fileSendSemaphore);
}
/* Status sent is from the last time the file was read. */
sendResponse("fE",(uint8_t)fileStatus);
break;
}
/**
<li> <b>Dd</b> Get a directory listing d=dirname. Directory name is 8.3 string
style. Gets all items in the directory and sends the type,size and name, each
group preceded by a comma. The file command requests each entry in turn,
terminated by a null filename when the directory listing is exhausted. */
case 'D':
{
if (! xSemaphoreTake(commsSendSemaphore,COMMS_SEND_TIMEOUT))
break;
uint8_t fileStatus = FR_INT_ERR;
if (xSemaphoreTake(fileSendSemaphore,COMMS_FILE_TIMEOUT))
{
char firstCharacter;
sendFileCommand('D',13,line+2);
commsPrintString("fD");
do
{
char type = 0;
/* Single character entry type */
xQueueReceive(fileReceiveQueue,&type,portMAX_DELAY);
char character;
/* Four bytes of file size */
uint32_t fileSize = 0;
uint8_t i;
for (i=0; i<4; i++)
{
character = 0;
xQueueReceive(fileReceiveQueue,&character,portMAX_DELAY);
fileSize = (fileSize << 8) + character;
}
/* Filename. If the first character of name is zero then the listing is ended */
character = 0;
xQueueReceive(fileReceiveQueue,&character,portMAX_DELAY);
firstCharacter = character;
if (firstCharacter > 0)
{
commsPrintString(",");
commsPrintChar(&type);
commsPrintHex(fileSize >> 16);
commsPrintHex(fileSize & 0xFFFF);
while (character > 0)
{
commsPrintChar(&character);
character = 0;
xQueueReceive(fileReceiveQueue,&character,portMAX_DELAY);
}
/* End of directory entry. Discard the status byte */
xQueueReceive(fileReceiveQueue,&fileStatus,portMAX_DELAY);
uint8_t eol = 0;
/* Send a zero parameter to ask for the next entry */
sendFileCommand('D',1,&eol);
}
}
while (firstCharacter > 0);
commsPrintString("\r\n");
xQueueReceive(fileReceiveQueue,&fileStatus,portMAX_DELAY);
xSemaphoreGive(fileSendSemaphore);
}
xSemaphoreGive(commsSendSemaphore);
sendResponse("fE",(uint8_t)fileStatus);
break;
}
/**
<li> <b>d[d]</b> d is the d=directory name. Get the first (if d present) or next
entry in the directory. If the name has a zero in the first position, return the
next entry in the directory listing. Returns the type, size and name preceded by
a comma for compatibility with the full directory listing request. If there are
no further entries found in the directory, then an empty string is sent back. */
case 'd':
{
if (! xSemaphoreTake(commsSendSemaphore,COMMS_SEND_TIMEOUT))
break;
uint8_t fileStatus = FR_INT_ERR;
if (xSemaphoreTake(fileSendSemaphore,COMMS_FILE_TIMEOUT))
{
sendFileCommand('D',13,line+2);
commsPrintString("fd");
char type = 0;
/* Single character entry type */
xQueueReceive(fileReceiveQueue,&type,portMAX_DELAY);
char character;
/* Four bytes of file size */
uint32_t fileSize = 0;
uint8_t i;
for (i=0; i<4; i++)
{
character = 0;
xQueueReceive(fileReceiveQueue,&character,portMAX_DELAY);
fileSize = (fileSize << 8) + character;
}
/* Filename. If the first character of name is zero then the listing is ended */
character = 0;
xQueueReceive(fileReceiveQueue,&character,portMAX_DELAY);
if (character > 0) /* Pull in remaining characters of name */
{
commsPrintString(",");
commsPrintChar(&type);
commsPrintHex(fileSize >> 16);
commsPrintHex(fileSize & 0xFFFF);
while (character > 0)
{
commsPrintChar(&character);
character = 0;
xQueueReceive(fileReceiveQueue,&character,portMAX_DELAY);
}
}
commsPrintString("\r\n");
xQueueReceive(fileReceiveQueue,&fileStatus,portMAX_DELAY);
xSemaphoreGive(fileSendSemaphore);
}
xSemaphoreGive(commsSendSemaphore);
sendResponse("fE",(uint8_t)fileStatus);
break;
}
/**
<li> <b>M</b> Register (mount or remount) the SD card. */
case 'M':
{
uint8_t fileStatus = FR_INT_ERR;
if (xSemaphoreTake(fileSendSemaphore,COMMS_FILE_TIMEOUT))
{
sendFileCommand('M',0,line+2);
xQueueReceive(fileReceiveQueue,&fileStatus,portMAX_DELAY);
xSemaphoreGive(fileSendSemaphore);
}
sendResponse("fE",(uint8_t)fileStatus);
break;
}
/**
<li> <b>s</b> Send a status message containing: software switches
(configData.config.recording), names of open files, with open write filename
first followed by read filename, or blank if files are not open. */
case 's':
{
if (! xSemaphoreTake(commsSendSemaphore,COMMS_SEND_TIMEOUT))
break;;
commsPrintString("fs,");
commsPrintInt((int)getControls());
commsPrintString(",");
uint8_t writeStatus;
commsPrintInt(writeFileHandle);
commsPrintString(",");
if (writeFileHandle < 0xFF)
{
commsPrintString(writeFileName);
commsPrintString(",");
}
commsPrintInt(readFileHandle);
if (readFileHandle < 0xFF)
{
commsPrintString(",");
commsPrintString(readFileName);
}
commsPrintString("\r\n");
xSemaphoreGive(commsSendSemaphore);
break;
}
/**
<li> <b>Xf</b> Delete a designated file f=filename. The file must not be open at
the time. A status is returned to signal a directory refresh. */
case 'X':
{
uint8_t fileStatus = FR_INT_ERR;
if (xSemaphoreTake(fileSendSemaphore,COMMS_FILE_TIMEOUT))
{
sendFileCommand('X',13,line+2);
xQueueReceive(fileReceiveQueue,&fileStatus,portMAX_DELAY);
xSemaphoreGive(fileSendSemaphore);
}
sendResponse("fE",(uint8_t)fileStatus);
break;
}
/**
</ul> */
}
}
/**
* Gets average battery cell voltage in 0.01V units.
*/
static int osdGetBatteryAverageCellVoltage(void)
{
return (getBatteryVoltage() * 10) / getBatteryCellCount();
}
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;
uint16_t batteryVoltage = 0;
int16_t batteryAmperage = -1;
int8_t batteryRemaining = 100;
if (getBatteryState() < BATTERY_NOT_PRESENT) {
batteryVoltage = isBatteryVoltageConfigured() ? getBatteryVoltage() * 100 : batteryVoltage;
batteryAmperage = isAmperageConfigured() ? getAmperage() : batteryAmperage;
batteryRemaining = isBatteryVoltageConfigured() ? calculateBatteryPercentageRemaining() : batteryRemaining;
}
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)
batteryVoltage,
// current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
batteryAmperage,
// battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery
batteryRemaining,
// 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_UNIT_TESTED void osdRefresh(timeUs_t currentTimeUs)
{
static timeUs_t lastTimeUs = 0;
static bool osdStatsEnabled = false;
static bool osdStatsVisible = false;
static timeUs_t osdStatsRefreshTimeUs;
static uint16_t endBatteryVoltage;
// detect arm/disarm
if (armState != ARMING_FLAG(ARMED)) {
if (ARMING_FLAG(ARMED)) {
osdStatsEnabled = false;
osdStatsVisible = false;
osdResetStats();
osdShowArmed();
resumeRefreshAt = currentTimeUs + (REFRESH_1S / 2);
} else if (isSomeStatEnabled()
&& (!(getArmingDisableFlags() & ARMING_DISABLED_RUNAWAY_TAKEOFF)
|| !VISIBLE(osdConfig()->item_pos[OSD_WARNINGS]))) { // suppress stats if runaway takeoff triggered disarm and WARNINGS element is visible
osdStatsEnabled = true;
resumeRefreshAt = currentTimeUs + (60 * REFRESH_1S);
endBatteryVoltage = getBatteryVoltage();
}
armState = ARMING_FLAG(ARMED);
}
if (ARMING_FLAG(ARMED)) {
osdUpdateStats();
timeUs_t deltaT = currentTimeUs - lastTimeUs;
flyTime += deltaT;
stats.armed_time += deltaT;
} else if (osdStatsEnabled) { // handle showing/hiding stats based on OSD disable switch position
if (displayIsGrabbed(osdDisplayPort)) {
osdStatsEnabled = false;
resumeRefreshAt = 0;
stats.armed_time = 0;
} else {
if (IS_RC_MODE_ACTIVE(BOXOSD) && osdStatsVisible) {
osdStatsVisible = false;
displayClearScreen(osdDisplayPort);
} else if (!IS_RC_MODE_ACTIVE(BOXOSD)) {
if (!osdStatsVisible) {
osdStatsVisible = true;
osdStatsRefreshTimeUs = 0;
}
if (currentTimeUs >= osdStatsRefreshTimeUs) {
osdStatsRefreshTimeUs = currentTimeUs + REFRESH_1S;
osdShowStats(endBatteryVoltage);
}
}
}
}
lastTimeUs = currentTimeUs;
if (resumeRefreshAt) {
if (cmp32(currentTimeUs, resumeRefreshAt) < 0) {
// in timeout period, check sticks for activity to resume display.
if (IS_HI(THROTTLE) || IS_HI(PITCH)) {
resumeRefreshAt = currentTimeUs;
}
displayHeartbeat(osdDisplayPort);
return;
} else {
displayClearScreen(osdDisplayPort);
resumeRefreshAt = 0;
osdStatsEnabled = false;
stats.armed_time = 0;
}
}
blinkState = (currentTimeUs / 200000) % 2;
#ifdef USE_ESC_SENSOR
if (feature(FEATURE_ESC_SENSOR)) {
escDataCombined = getEscSensorData(ESC_SENSOR_COMBINED);
}
#endif
#ifdef USE_CMS
if (!displayIsGrabbed(osdDisplayPort)) {
osdUpdateAlarms();
osdDrawElements();
displayHeartbeat(osdDisplayPort);
#ifdef OSD_CALLS_CMS
} else {
cmsUpdate(currentTimeUs);
#endif
}
#endif
lastArmState = ARMING_FLAG(ARMED);
}
static void osdShowStats(uint16_t endBatteryVoltage)
{
uint8_t top = 2;
char buff[OSD_ELEMENT_BUFFER_LENGTH];
displayClearScreen(osdDisplayPort);
displayWrite(osdDisplayPort, 2, top++, " --- STATS ---");
if (osdStatGetState(OSD_STAT_RTC_DATE_TIME)) {
bool success = false;
#ifdef USE_RTC_TIME
success = osdFormatRtcDateTime(&buff[0]);
#endif
if (!success) {
tfp_sprintf(buff, "NO RTC");
}
displayWrite(osdDisplayPort, 2, top++, buff);
}
if (osdStatGetState(OSD_STAT_TIMER_1)) {
osdFormatTimer(buff, false, (OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_1]) == OSD_TIMER_SRC_ON ? false : true), OSD_TIMER_1);
osdDisplayStatisticLabel(top++, osdTimerSourceNames[OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_1])], buff);
}
if (osdStatGetState(OSD_STAT_TIMER_2)) {
osdFormatTimer(buff, false, (OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_2]) == OSD_TIMER_SRC_ON ? false : true), OSD_TIMER_2);
osdDisplayStatisticLabel(top++, osdTimerSourceNames[OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_2])], buff);
}
if (osdStatGetState(OSD_STAT_MAX_SPEED) && STATE(GPS_FIX)) {
itoa(stats.max_speed, buff, 10);
osdDisplayStatisticLabel(top++, "MAX SPEED", buff);
}
if (osdStatGetState(OSD_STAT_MAX_DISTANCE)) {
tfp_sprintf(buff, "%d%c", osdGetMetersToSelectedUnit(stats.max_distance), osdGetMetersToSelectedUnitSymbol());
osdDisplayStatisticLabel(top++, "MAX DISTANCE", buff);
}
if (osdStatGetState(OSD_STAT_MIN_BATTERY)) {
tfp_sprintf(buff, "%d.%1d%c", stats.min_voltage / 10, stats.min_voltage % 10, SYM_VOLT);
osdDisplayStatisticLabel(top++, "MIN BATTERY", buff);
}
if (osdStatGetState(OSD_STAT_END_BATTERY)) {
tfp_sprintf(buff, "%d.%1d%c", endBatteryVoltage / 10, endBatteryVoltage % 10, SYM_VOLT);
osdDisplayStatisticLabel(top++, "END BATTERY", buff);
}
if (osdStatGetState(OSD_STAT_BATTERY)) {
tfp_sprintf(buff, "%d.%1d%c", getBatteryVoltage() / 10, getBatteryVoltage() % 10, SYM_VOLT);
osdDisplayStatisticLabel(top++, "BATTERY", buff);
}
if (osdStatGetState(OSD_STAT_MIN_RSSI)) {
itoa(stats.min_rssi, buff, 10);
strcat(buff, "%");
osdDisplayStatisticLabel(top++, "MIN RSSI", buff);
}
if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) {
if (osdStatGetState(OSD_STAT_MAX_CURRENT)) {
itoa(stats.max_current, buff, 10);
strcat(buff, "A");
osdDisplayStatisticLabel(top++, "MAX CURRENT", buff);
}
if (osdStatGetState(OSD_STAT_USED_MAH)) {
tfp_sprintf(buff, "%d%c", getMAhDrawn(), SYM_MAH);
osdDisplayStatisticLabel(top++, "USED MAH", buff);
}
}
if (osdStatGetState(OSD_STAT_MAX_ALTITUDE)) {
osdFormatAltitudeString(buff, stats.max_altitude);
osdDisplayStatisticLabel(top++, "MAX ALTITUDE", buff);
}
#ifdef USE_BLACKBOX
if (osdStatGetState(OSD_STAT_BLACKBOX) && blackboxConfig()->device && blackboxConfig()->device != BLACKBOX_DEVICE_SERIAL) {
osdGetBlackboxStatusString(buff);
osdDisplayStatisticLabel(top++, "BLACKBOX", buff);
}
if (osdStatGetState(OSD_STAT_BLACKBOX_NUMBER) && blackboxConfig()->device && blackboxConfig()->device != BLACKBOX_DEVICE_SERIAL) {
itoa(blackboxGetLogNumber(), buff, 10);
osdDisplayStatisticLabel(top++, "BB LOG NUM", buff);
}
#endif
}
static bool osdDrawSingleElement(uint8_t item)
{
if (!VISIBLE(osdConfig()->item_pos[item]) || BLINK(item)) {
return false;
}
uint8_t elemPosX = OSD_X(osdConfig()->item_pos[item]);
uint8_t elemPosY = OSD_Y(osdConfig()->item_pos[item]);
char buff[OSD_ELEMENT_BUFFER_LENGTH] = "";
switch (item) {
case OSD_RSSI_VALUE:
{
uint16_t osdRssi = getRssi() * 100 / 1024; // change range
if (osdRssi >= 100)
osdRssi = 99;
tfp_sprintf(buff, "%c%2d", SYM_RSSI, osdRssi);
break;
}
case OSD_MAIN_BATT_VOLTAGE:
buff[0] = osdGetBatterySymbol(osdGetBatteryAverageCellVoltage());
tfp_sprintf(buff + 1, "%2d.%1d%c", getBatteryVoltage() / 10, getBatteryVoltage() % 10, SYM_VOLT);
break;
case OSD_CURRENT_DRAW:
{
const int32_t amperage = getAmperage();
tfp_sprintf(buff, "%3d.%02d%c", abs(amperage) / 100, abs(amperage) % 100, SYM_AMP);
break;
}
case OSD_MAH_DRAWN:
tfp_sprintf(buff, "%4d%c", getMAhDrawn(), SYM_MAH);
break;
#ifdef USE_GPS
case OSD_GPS_SATS:
tfp_sprintf(buff, "%c%c%2d", SYM_SAT_L, SYM_SAT_R, gpsSol.numSat);
break;
case OSD_GPS_SPEED:
// FIXME ideally we want to use SYM_KMH symbol but it's not in the font any more, so we use K (M for MPH)
switch (osdConfig()->units) {
case OSD_UNIT_IMPERIAL:
tfp_sprintf(buff, "%3dM", CM_S_TO_MPH(gpsSol.groundSpeed));
break;
default:
tfp_sprintf(buff, "%3dK", CM_S_TO_KM_H(gpsSol.groundSpeed));
break;
}
break;
case OSD_GPS_LAT:
// The SYM_LAT symbol in the actual font contains only blank, so we use the SYM_ARROW_NORTH
osdFormatCoordinate(buff, SYM_ARROW_NORTH, gpsSol.llh.lat);
break;
case OSD_GPS_LON:
// The SYM_LON symbol in the actual font contains only blank, so we use the SYM_ARROW_EAST
osdFormatCoordinate(buff, SYM_ARROW_EAST, gpsSol.llh.lon);
break;
case OSD_HOME_DIR:
if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
if (GPS_distanceToHome > 0) {
const int h = GPS_directionToHome - DECIDEGREES_TO_DEGREES(attitude.values.yaw);
buff[0] = osdGetDirectionSymbolFromHeading(h);
} else {
// We don't have a HOME symbol in the font, by now we use this
buff[0] = SYM_THR1;
}
} else {
// We use this symbol when we don't have a FIX
buff[0] = SYM_COLON;
}
buff[1] = 0;
break;
case OSD_HOME_DIST:
if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
const int32_t distance = osdGetMetersToSelectedUnit(GPS_distanceToHome);
tfp_sprintf(buff, "%d%c", distance, osdGetMetersToSelectedUnitSymbol());
} else {
// We use this symbol when we don't have a FIX
buff[0] = SYM_COLON;
// overwrite any previous distance with blanks
memset(buff + 1, SYM_BLANK, 6);
buff[7] = '\0';
}
break;
#endif // GPS
case OSD_COMPASS_BAR:
memcpy(buff, compassBar + osdGetHeadingIntoDiscreteDirections(DECIDEGREES_TO_DEGREES(attitude.values.yaw), 16), 9);
buff[9] = 0;
break;
case OSD_ALTITUDE:
osdFormatAltitudeString(buff, getEstimatedAltitude());
break;
case OSD_ITEM_TIMER_1:
case OSD_ITEM_TIMER_2:
osdFormatTimer(buff, true, true, item - OSD_ITEM_TIMER_1);
break;
case OSD_REMAINING_TIME_ESTIMATE:
{
const int mAhDrawn = getMAhDrawn();
const int remaining_time = (int)((osdConfig()->cap_alarm - mAhDrawn) * ((float)flyTime) / mAhDrawn);
if (mAhDrawn < 0.1 * osdConfig()->cap_alarm) {
tfp_sprintf(buff, "--:--");
} else if (mAhDrawn > osdConfig()->cap_alarm) {
tfp_sprintf(buff, "00:00");
} else {
osdFormatTime(buff, OSD_TIMER_PREC_SECOND, remaining_time);
}
break;
}
case OSD_FLYMODE:
{
if (FLIGHT_MODE(FAILSAFE_MODE)) {
strcpy(buff, "!FS!");
} else if (FLIGHT_MODE(ANGLE_MODE)) {
strcpy(buff, "STAB");
} else if (FLIGHT_MODE(HORIZON_MODE)) {
strcpy(buff, "HOR ");
} else if (FLIGHT_MODE(GPS_RESCUE_MODE)) {
strcpy(buff, "RESC");
} else if (isAirmodeActive()) {
strcpy(buff, "AIR ");
} else {
strcpy(buff, "ACRO");
}
break;
}
case OSD_ANTI_GRAVITY:
{
if (pidItermAccelerator() > 1.0f) {
strcpy(buff, "AG");
}
break;
}
case OSD_CRAFT_NAME:
// This does not strictly support iterative updating if the craft name changes at run time. But since the craft name is not supposed to be changing this should not matter, and blanking the entire length of the craft name string on update will make it impossible to configure elements to be displayed on the right hand side of the craft name.
//TODO: When iterative updating is implemented, change this so the craft name is only printed once whenever the OSD 'flight' screen is entered.
if (strlen(pilotConfig()->name) == 0) {
strcpy(buff, "CRAFT_NAME");
} else {
unsigned i;
for (i = 0; i < MAX_NAME_LENGTH; i++) {
if (pilotConfig()->name[i]) {
buff[i] = toupper((unsigned char)pilotConfig()->name[i]);
} else {
break;
}
}
buff[i] = '\0';
}
break;
case OSD_THROTTLE_POS:
buff[0] = SYM_THR;
buff[1] = SYM_THR1;
tfp_sprintf(buff + 2, "%3d", (constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN));
break;
#if defined(USE_VTX_COMMON)
case OSD_VTX_CHANNEL:
{
const char vtxBandLetter = vtx58BandLetter[vtxSettingsConfig()->band];
const char *vtxChannelName = vtx58ChannelNames[vtxSettingsConfig()->channel];
uint8_t vtxPower = vtxSettingsConfig()->power;
const vtxDevice_t *vtxDevice = vtxCommonDevice();
if (vtxDevice && vtxSettingsConfig()->lowPowerDisarm) {
vtxCommonGetPowerIndex(vtxDevice, &vtxPower);
}
tfp_sprintf(buff, "%c:%s:%1d", vtxBandLetter, vtxChannelName, vtxPower);
break;
}
#endif
case OSD_CROSSHAIRS:
buff[0] = SYM_AH_CENTER_LINE;
buff[1] = SYM_AH_CENTER;
buff[2] = SYM_AH_CENTER_LINE_RIGHT;
buff[3] = 0;
break;
case OSD_ARTIFICIAL_HORIZON:
{
// Get pitch and roll limits in tenths of degrees
const int maxPitch = osdConfig()->ahMaxPitch * 10;
const int maxRoll = osdConfig()->ahMaxRoll * 10;
const int rollAngle = constrain(attitude.values.roll, -maxRoll, maxRoll);
int pitchAngle = constrain(attitude.values.pitch, -maxPitch, maxPitch);
// Convert pitchAngle to y compensation value
// (maxPitch / 25) divisor matches previous settings of fixed divisor of 8 and fixed max AHI pitch angle of 20.0 degrees
pitchAngle = ((pitchAngle * 25) / maxPitch) - 41; // 41 = 4 * AH_SYMBOL_COUNT + 5
for (int x = -4; x <= 4; x++) {
const int y = ((-rollAngle * x) / 64) - pitchAngle;
if (y >= 0 && y <= 81) {
displayWriteChar(osdDisplayPort, elemPosX + x, elemPosY + (y / AH_SYMBOL_COUNT), (SYM_AH_BAR9_0 + (y % AH_SYMBOL_COUNT)));
}
}
return true;
}
case OSD_HORIZON_SIDEBARS:
{
// Draw AH sides
const int8_t hudwidth = AH_SIDEBAR_WIDTH_POS;
const int8_t hudheight = AH_SIDEBAR_HEIGHT_POS;
for (int y = -hudheight; y <= hudheight; y++) {
displayWriteChar(osdDisplayPort, elemPosX - hudwidth, elemPosY + y, SYM_AH_DECORATION);
displayWriteChar(osdDisplayPort, elemPosX + hudwidth, elemPosY + y, SYM_AH_DECORATION);
}
// AH level indicators
displayWriteChar(osdDisplayPort, elemPosX - hudwidth + 1, elemPosY, SYM_AH_LEFT);
displayWriteChar(osdDisplayPort, elemPosX + hudwidth - 1, elemPosY, SYM_AH_RIGHT);
return true;
}
case OSD_ROLL_PIDS:
osdFormatPID(buff, "ROL", ¤tPidProfile->pid[PID_ROLL]);
break;
case OSD_PITCH_PIDS:
osdFormatPID(buff, "PIT", ¤tPidProfile->pid[PID_PITCH]);
break;
case OSD_YAW_PIDS:
osdFormatPID(buff, "YAW", ¤tPidProfile->pid[PID_YAW]);
break;
case OSD_POWER:
tfp_sprintf(buff, "%4dW", getAmperage() * getBatteryVoltage() / 1000);
break;
case OSD_PIDRATE_PROFILE:
tfp_sprintf(buff, "%d-%d", getCurrentPidProfileIndex() + 1, getCurrentControlRateProfileIndex() + 1);
break;
case OSD_WARNINGS:
{
#define OSD_WARNINGS_MAX_SIZE 11
#define OSD_FORMAT_MESSAGE_BUFFER_SIZE (OSD_WARNINGS_MAX_SIZE + 1)
STATIC_ASSERT(OSD_FORMAT_MESSAGE_BUFFER_SIZE <= sizeof(buff), osd_warnings_size_exceeds_buffer_size);
const batteryState_e batteryState = getBatteryState();
if (osdWarnGetState(OSD_WARNING_BATTERY_CRITICAL) && batteryState == BATTERY_CRITICAL) {
osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, " LAND NOW");
break;
}
#ifdef USE_ADC_INTERNAL
uint8_t coreTemperature = getCoreTemperatureCelsius();
if (osdWarnGetState(OSD_WARNING_CORE_TEMPERATURE) && coreTemperature >= osdConfig()->core_temp_alarm) {
char coreTemperatureWarningMsg[OSD_FORMAT_MESSAGE_BUFFER_SIZE];
tfp_sprintf(coreTemperatureWarningMsg, "CORE: %3d%c", osdConvertTemperatureToSelectedUnit(getCoreTemperatureCelsius() * 10) / 10, osdGetTemperatureSymbolForSelectedUnit());
osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, coreTemperatureWarningMsg);
break;
}
#endif
#ifdef USE_ESC_SENSOR
// Show warning if we lose motor output, the ESC is overheating or excessive current draw
if (feature(FEATURE_ESC_SENSOR) && osdWarnGetState(OSD_WARNING_ESC_FAIL)) {
char escWarningMsg[OSD_FORMAT_MESSAGE_BUFFER_SIZE];
unsigned pos = 0;
const char *title = "ESC";
// center justify message
while (pos < (OSD_WARNINGS_MAX_SIZE - (strlen(title) + getMotorCount())) / 2) {
escWarningMsg[pos++] = ' ';
}
strcpy(escWarningMsg + pos, title);
pos += strlen(title);
unsigned i = 0;
unsigned escWarningCount = 0;
while (i < getMotorCount() && pos < OSD_FORMAT_MESSAGE_BUFFER_SIZE - 1) {
escSensorData_t *escData = getEscSensorData(i);
const char motorNumber = '1' + i;
// if everything is OK just display motor number else R, T or C
char warnFlag = motorNumber;
if (ARMING_FLAG(ARMED) && osdConfig()->esc_rpm_alarm != ESC_RPM_ALARM_OFF && calcEscRpm(escData->rpm) <= osdConfig()->esc_rpm_alarm) {
warnFlag = 'R';
}
if (osdConfig()->esc_temp_alarm != ESC_TEMP_ALARM_OFF && escData->temperature >= osdConfig()->esc_temp_alarm) {
warnFlag = 'T';
}
if (ARMING_FLAG(ARMED) && osdConfig()->esc_current_alarm != ESC_CURRENT_ALARM_OFF && escData->current >= osdConfig()->esc_current_alarm) {
warnFlag = 'C';
}
escWarningMsg[pos++] = warnFlag;
if (warnFlag != motorNumber) {
escWarningCount++;
}
i++;
}
escWarningMsg[pos] = '\0';
if (escWarningCount > 0) {
osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, escWarningMsg);
}
break;
}
#endif
// Warn when in flip over after crash mode
if (osdWarnGetState(OSD_WARNING_CRASH_FLIP) && isFlipOverAfterCrashMode()) {
osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "CRASH FLIP");
break;
}
// Show most severe reason for arming being disabled
if (osdWarnGetState(OSD_WARNING_ARMING_DISABLE) && IS_RC_MODE_ACTIVE(BOXARM) && isArmingDisabled()) {
const armingDisableFlags_e flags = getArmingDisableFlags();
for (int i = 0; i < ARMING_DISABLE_FLAGS_COUNT; i++) {
if (flags & (1 << i)) {
osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, armingDisableFlagNames[i]);
break;
}
}
break;
}
if (osdWarnGetState(OSD_WARNING_BATTERY_WARNING) && batteryState == BATTERY_WARNING) {
osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "LOW BATTERY");
break;
}
// Show warning if battery is not fresh
if (osdWarnGetState(OSD_WARNING_BATTERY_NOT_FULL) && !ARMING_FLAG(WAS_EVER_ARMED) && (getBatteryState() == BATTERY_OK)
&& getBatteryAverageCellVoltage() < batteryConfig()->vbatfullcellvoltage) {
osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "BATT < FULL");
break;
}
// Visual beeper
if (osdWarnGetState(OSD_WARNING_VISUAL_BEEPER) && showVisualBeeper) {
osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, " * * * *");
break;
}
osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, NULL);
break;
}
case OSD_AVG_CELL_VOLTAGE:
{
const int cellV = osdGetBatteryAverageCellVoltage();
buff[0] = osdGetBatterySymbol(cellV);
tfp_sprintf(buff + 1, "%d.%02d%c", cellV / 100, cellV % 100, SYM_VOLT);
break;
}
case OSD_DEBUG:
tfp_sprintf(buff, "DBG %5d %5d %5d %5d", debug[0], debug[1], debug[2], debug[3]);
break;
case OSD_PITCH_ANGLE:
case OSD_ROLL_ANGLE:
{
const int angle = (item == OSD_PITCH_ANGLE) ? attitude.values.pitch : attitude.values.roll;
tfp_sprintf(buff, "%c%02d.%01d", angle < 0 ? '-' : ' ', abs(angle / 10), abs(angle % 10));
break;
}
case OSD_MAIN_BATT_USAGE:
{
// Set length of indicator bar
#define MAIN_BATT_USAGE_STEPS 11 // Use an odd number so the bar can be centered.
// Calculate constrained value
const float value = constrain(batteryConfig()->batteryCapacity - getMAhDrawn(), 0, batteryConfig()->batteryCapacity);
// Calculate mAh used progress
const uint8_t mAhUsedProgress = ceilf((value / (batteryConfig()->batteryCapacity / MAIN_BATT_USAGE_STEPS)));
// Create empty battery indicator bar
buff[0] = SYM_PB_START;
for (int i = 1; i <= MAIN_BATT_USAGE_STEPS; i++) {
buff[i] = i <= mAhUsedProgress ? SYM_PB_FULL : SYM_PB_EMPTY;
}
buff[MAIN_BATT_USAGE_STEPS + 1] = SYM_PB_CLOSE;
if (mAhUsedProgress > 0 && mAhUsedProgress < MAIN_BATT_USAGE_STEPS) {
buff[1 + mAhUsedProgress] = SYM_PB_END;
}
buff[MAIN_BATT_USAGE_STEPS+2] = '\0';
break;
}
case OSD_DISARMED:
if (!ARMING_FLAG(ARMED)) {
tfp_sprintf(buff, "DISARMED");
} else {
if (!lastArmState) { // previously disarmed - blank out the message one time
tfp_sprintf(buff, " ");
}
}
break;
case OSD_NUMERICAL_HEADING:
{
const int heading = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
tfp_sprintf(buff, "%c%03d", osdGetDirectionSymbolFromHeading(heading), heading);
break;
}
case OSD_NUMERICAL_VARIO:
{
const int verticalSpeed = osdGetMetersToSelectedUnit(getEstimatedVario());
const char directionSymbol = verticalSpeed < 0 ? SYM_ARROW_SOUTH : SYM_ARROW_NORTH;
tfp_sprintf(buff, "%c%01d.%01d", directionSymbol, abs(verticalSpeed / 100), abs((verticalSpeed % 100) / 10));
break;
}
#ifdef USE_ESC_SENSOR
case OSD_ESC_TMP:
if (feature(FEATURE_ESC_SENSOR)) {
tfp_sprintf(buff, "%3d%c", osdConvertTemperatureToSelectedUnit(escDataCombined->temperature * 10) / 10, osdGetTemperatureSymbolForSelectedUnit());
}
break;
case OSD_ESC_RPM:
if (feature(FEATURE_ESC_SENSOR)) {
tfp_sprintf(buff, "%5d", escDataCombined == NULL ? 0 : calcEscRpm(escDataCombined->rpm));
}
break;
#endif
#ifdef USE_RTC_TIME
case OSD_RTC_DATETIME:
osdFormatRtcDateTime(&buff[0]);
break;
#endif
#ifdef USE_OSD_ADJUSTMENTS
case OSD_ADJUSTMENT_RANGE:
tfp_sprintf(buff, "%s: %3d", adjustmentRangeName, adjustmentRangeValue);
break;
#endif
#ifdef USE_ADC_INTERNAL
case OSD_CORE_TEMPERATURE:
tfp_sprintf(buff, "%3d%c", osdConvertTemperatureToSelectedUnit(getCoreTemperatureCelsius() * 10) / 10, osdGetTemperatureSymbolForSelectedUnit());
break;
#endif
default:
return false;
}
displayWrite(osdDisplayPort, elemPosX, elemPosY, buff);
return true;
}
