Skip to content

Commit

Permalink
Add WFB telemetry
Browse files Browse the repository at this point in the history
  • Loading branch information
@svpcom
svpcom committed Sep 26, 2018
1 parent 8a21f05 commit fa5149b
Show file tree
Hide file tree
Showing 7 changed files with 174 additions and 74 deletions.
20 changes: 13 additions & 7 deletions osdconfig.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,19 +26,19 @@ osd_params_t osd_params = {
.BattVolt_en=1,
.BattVolt_panel=1,
.BattVolt_posX=10,
.BattVolt_posY=240,
.BattVolt_posY=230,
.BattVolt_fontsize=0,
.BattVolt_align=0,
.BattCurrent_en=1,
.BattCurrent_panel=1,
.BattCurrent_posX=65,
.BattCurrent_posX=2,
.BattCurrent_posY=240,
.BattCurrent_fontsize=0,
.BattCurrent_align=0,
.BattRemaining_en=1,
.BattRemaining_panel=1,
.BattRemaining_posX=10,
.BattRemaining_posY=230,
.BattRemaining_posX=18,
.BattRemaining_posY=220,
.BattRemaining_fontsize=0,
.BattRemaining_align=0,
.FlightMode_en=1,
Expand Down Expand Up @@ -188,8 +188,8 @@ osd_params_t osd_params = {
.RSSI_en=1,
.RSSI_type=0,
.RSSI_panel=1,
.RSSI_posX=200,
.RSSI_posY=220,
.RSSI_posX=72,
.RSSI_posY=230,
.RSSI_fontsize=0,
.RSSI_align=0,
.RSSI_min=0,
Expand Down Expand Up @@ -287,5 +287,11 @@ osd_params_t osd_params = {
.HomeLongitude_posX=264,
.HomeLongitude_posY=240,
.HomeLongitude_fontsize=0,
.HomeLongitude_align=0
.HomeLongitude_align=0,
.WFBState_en=1,
.WFBState_panel=1,
.WFBState_posX=72,
.WFBState_posY=240,
.WFBState_fontsize=0,
.WFBState_align=0,
};
7 changes: 7 additions & 0 deletions osdconfig.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -334,6 +334,13 @@ typedef struct
uint16_t HomeLongitude_fontsize;
uint16_t HomeLongitude_align;

uint16_t WFBState_en;
uint16_t WFBState_panel;
uint16_t WFBState_posX;
uint16_t WFBState_posY;
uint16_t WFBState_fontsize;
uint16_t WFBState_align;

} osd_params_t;

extern osd_params_t osd_params;
Expand Down
13 changes: 13 additions & 0 deletions osdmavlink.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -206,6 +206,19 @@ void parse_mavlink_packet(uint8_t *buf, int buflen)
}
break;

case MAVLINK_MSG_ID_RADIO_STATUS:
{
if ((msg.sysid != 1) || (msg.compid != 242)) {
break;
}

wfb_rssi = (int8_t)mavlink_msg_radio_status_get_rssi(&msg);
wfb_errors = mavlink_msg_radio_status_get_rxerrors(&msg);
wfb_fec_fixed = mavlink_msg_radio_status_get_fixed(&msg);
wfb_flags = mavlink_msg_radio_status_get_txbuf(&msg);
}
break;

default:
//Do nothing
break;
Expand Down
192 changes: 125 additions & 67 deletions osdrender.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@ uint64_t new_panel_start_time = 0;

uint8_t last_warn_type = 0;
uint64_t last_warn_time = 0;
char* warn_str = "";
char warn_str[256];

const char METRIC_SPEED[] = "KM/H"; //kilometer per hour
const char METRIC_DIST_SHORT[] = "M"; //meter
Expand Down Expand Up @@ -171,6 +171,7 @@ void RenderScreen(void) {
draw_CWH();
draw_climb_rate();
draw_rssi();
draw_wfb_state();
draw_link_quality();
draw_efficiency();
draw_wind();
Expand Down Expand Up @@ -310,10 +311,10 @@ void draw_uav2d() {
}

void draw_throttle(void) {
/*if (enabledAndShownOnPanel(osd_params.Throt_en,
if (!enabledAndShownOnPanel(osd_params.Throt_en,
osd_params.Throt_panel)) {
return;
}*/
return;
}

int16_t pos_th_y, pos_th_x;
int posX, posY;
Expand Down Expand Up @@ -619,12 +620,12 @@ void draw_CWH(void) {

//direction - map-like mode
if (osd_params.CWH_Nmode_en == 1 && shownAtPanel(osd_params.CWH_Nmode_panel)) {
draw_head_wp_home();
draw_head_wp_home();
}

//direction - scale mode
if (osd_params.CWH_Tmode_en == 1 && shownAtPanel(osd_params.CWH_Tmode_panel)) {
draw_linear_compass(osd_heading, 0, 120, 180, GRAPHICS_X_MIDDLE, osd_params.CWH_Tmode_posY, 15, 30, 5, 8, 0);
draw_linear_compass(osd_heading, 0, 120, 180, GRAPHICS_X_MIDDLE, osd_params.CWH_Tmode_posY, 15, 30, 5, 8, 0);
}
}

Expand All @@ -637,9 +638,11 @@ void draw_climb_rate() {
float average_climb = 0.0f;
osd_climb_ma[osd_climb_ma_index] = osd_climb;
osd_climb_ma_index = (osd_climb_ma_index + 1) % 10;

for (int i = 0; i < 10; i++) {
average_climb = average_climb + osd_climb_ma[i];
}

average_climb = roundf(10 * (average_climb / 10)) / 10.0f;

int x = osd_params.ClimbRate_posX;
Expand Down Expand Up @@ -705,15 +708,18 @@ void draw_rssi() {
if ((rssiMax - rssiMin) > 0)
rssi = (int) ((float) (rssi - rssiMin) / (float) (rssiMax - rssiMin) * 100.0f);

if (rssi < 0)
rssi = 0;
snprintf(tmp_str, sizeof(tmp_str), "RSSI %d%%", rssi);
} else {
snprintf(tmp_str, sizeof(tmp_str), "RSSI %d", rssi);
if (rssi < 0) rssi = 0;
snprintf(tmp_str, sizeof(tmp_str), "RC: %d%%", rssi);
rc_lost = (rssi < 5) ? true : false;
}
else
{
snprintf(tmp_str, sizeof(tmp_str), "RC: %d", rssi);
rc_lost = false;
}

write_string(tmp_str, osd_params.RSSI_posX,
osd_params.RSSI_posY, 0, 0, TEXT_VA_MIDDLE,
osd_params.RSSI_posY, 0, 0, TEXT_VA_TOP,
osd_params.RSSI_align, 0,
SIZE_TO_FONT[osd_params.RSSI_fontsize]);
}
Expand Down Expand Up @@ -830,8 +836,9 @@ void draw_linear_compass(int v, int home_dir, int range, int width, int x, int y
int range_2 = range / 2;
bool home_drawn = false;

// home_dir = 30;
// int wp_dir = 60;
// v = 30;
// home_dir = 30;
// int wp_dir = 60;

for (r = -range_2; r <= +range_2; r++) {
style = 0;
Expand Down Expand Up @@ -1192,17 +1199,11 @@ void draw_wind(void) {
}

void draw_warning(void) {
write_string(warn_str, osd_params.Alarm_posX, osd_params.Alarm_posY, 0, 0, TEXT_VA_TOP, osd_params.Alarm_align, 0, SIZE_TO_FONT[osd_params.Alarm_fontsize]);

if ((GetSystimeMS() - last_warn_time) < 1000)
{
return;
}


bool haswarn = false;
const static int warn_cnt = 6;
uint8_t warning[] = { 0, 0, 0, 0, 0, 0, 0 };
uint8_t warning[] = { 0, 0, 0,
0, 0, 0,
0, 0, 0,
0 };

//no GPS fix!
if (osd_params.Alarm_GPS_status_en == 1 && (osd_fix_type < GPS_OK_FIX_3D)) {
Expand Down Expand Up @@ -1256,52 +1257,82 @@ void draw_warning(void) {
warning[6] = 1;
}

if (haswarn) {
if (osd_params.Alarm_rc_status_en == 1 && rc_lost) {
haswarn = true;
warning[7] = 1;
}

if (osd_params.Alarm_wfb_status_en == 1 && (wfb_flags & WFB_LINK_LOST)) {
haswarn = true;
warning[8] = 1;
}

if (osd_params.Alarm_wfb_status_en == 1 && (wfb_flags & WFB_LINK_JAMMED)) {
haswarn = true;
warning[9] = 1;
}

if (haswarn && (GetSystimeMS() - last_warn_time) >= 1000) {
last_warn_time = GetSystimeMS();
if (last_warn_type > (warn_cnt - 1)) last_warn_type = 0;

if (last_warn_type == 0 && (warning[0] == 1)) {
warn_str = "NO GPS FIX";
last_warn_type++;
return;
}
do
{
last_warn_type = (last_warn_type + 1) % (sizeof(warning) / sizeof(uint8_t));
}while(!warning[last_warn_type]);

if (last_warn_type == 1 && (warning[1] == 1)) {
warn_str = "LOW BATTERY";
last_warn_type++;
return;
}
switch(last_warn_type)
{
case 0:
strncpy(warn_str, "NO GPS FIX", sizeof(warn_str));
break;

case 1:
strncpy(warn_str, "LOW BATTERY", sizeof(warn_str));
break;

case 2:
strncpy(warn_str, "SPEED LOW", sizeof(warn_str));
break;

case 3:
strncpy(warn_str, "OVER SPEED", sizeof(warn_str));
break;

case 4:
strncpy(warn_str, "LOW ALT", sizeof(warn_str));
break;

case 5:
strncpy(warn_str, "HIGH ALT", sizeof(warn_str));
break;

case 6:
strncpy(warn_str, "NO HOME POSITION SET", sizeof(warn_str));
break;

case 7:
strncpy(warn_str, "RC LOST", sizeof(warn_str));
break;

case 8:
strncpy(warn_str, "WFB LOST", sizeof(warn_str));
break;

case 9:
strncpy(warn_str, "WFB JAMMED", sizeof(warn_str));
break;

if (last_warn_type == 2 && (warning[2] == 1)) {
warn_str = "SPEED LOW";
last_warn_type++;
return;
}
if (last_warn_type == 3 && (warning[3] == 1)) {
warn_str = "OVER SPEED";
last_warn_type++;
return;
}
if (last_warn_type == 4 && (warning[4] == 1)) {
warn_str = "LOW ALT";
last_warn_type++;
return;
}
if (last_warn_type == 5 && (warning[5] == 1)) {
warn_str = "HIGH ALT";
last_warn_type++;
return;
}
if (last_warn_type == 6 && warning[6] == 1) {
warn_str = "NO HOME POSITION SET";
last_warn_type++;
return;
}
last_warn_type++;
}
else{
warn_str = "";

if(!haswarn)
{
//Show a new warning immediately
last_warn_time = 0;
strcpy(warn_str, "");
}

write_string(warn_str, osd_params.Alarm_posX, osd_params.Alarm_posY, 0, 0, TEXT_VA_TOP, osd_params.Alarm_align, 0, SIZE_TO_FONT[osd_params.Alarm_fontsize]);
}

void debug_wps(void) {
Expand Down Expand Up @@ -1448,7 +1479,7 @@ void draw_flight_mode() {

default:
{
mode_str = "UNSUPPORTED";
mode_str = "----";
}
}

Expand Down Expand Up @@ -1476,7 +1507,7 @@ void draw_battery_voltage() {
return;
}

snprintf(tmp_str, sizeof(tmp_str), "%0.1fV", (double) osd_vbat_A);
snprintf(tmp_str, sizeof(tmp_str), "%4.1fV", (double) osd_vbat_A);
write_string(tmp_str, osd_params.BattVolt_posX,
osd_params.BattVolt_posY, 0, 0, TEXT_VA_TOP,
osd_params.BattVolt_align, 0,
Expand All @@ -1489,7 +1520,7 @@ void draw_battery_current() {
return;
}

snprintf(tmp_str, sizeof(tmp_str), "%0.1fA", (double) (osd_curr_A * 0.01));
snprintf(tmp_str, sizeof(tmp_str), "%5.1fA", (double) (osd_curr_A * 0.01));
write_string(tmp_str, osd_params.BattCurrent_posX,
osd_params.BattCurrent_posY, 0, 0, TEXT_VA_TOP,
osd_params.BattCurrent_align, 0,
Expand All @@ -1502,7 +1533,7 @@ void draw_battery_remaining() {
return;
}

snprintf(tmp_str, sizeof(tmp_str), "%d%%", osd_battery_remaining_A);
snprintf(tmp_str, sizeof(tmp_str), "%3d%%", osd_battery_remaining_A);
write_string(tmp_str, osd_params.BattRemaining_posX,
osd_params.BattRemaining_posY, 0, 0, TEXT_VA_TOP,
osd_params.BattRemaining_align, 0,
Expand All @@ -1522,6 +1553,33 @@ void draw_battery_consumed() {
SIZE_TO_FONT[osd_params.BattConsumed_fontsize]);
}

void draw_wfb_state() {
if (!enabledAndShownOnPanel(osd_params.WFBState_en,
osd_params.WFBState_panel)) {
return;
}

if (wfb_flags & WFB_LINK_LOST)
{
snprintf(tmp_str, sizeof(tmp_str), "WFB LOST");
}
else if (wfb_flags & WFB_LINK_JAMMED)
{
snprintf(tmp_str, sizeof(tmp_str), "WFB %d/JAM", wfb_rssi);
}
else
{
snprintf(tmp_str, sizeof(tmp_str), "WFB %d/%d/%d", wfb_rssi, wfb_fec_fixed, wfb_errors);
}

write_string(tmp_str,
osd_params.WFBState_posX,
osd_params.WFBState_posY, 0, 0, TEXT_VA_TOP,
osd_params.WFBState_align, 0,
SIZE_TO_FONT[osd_params.WFBState_fontsize]);
}


void draw_altitude_scale() {
if (!enabledAndShownOnPanel(osd_params.Alt_Scale_en,
osd_params.Alt_Scale_panel)) {
Expand Down
1 change: 1 addition & 0 deletions osdrender.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -75,6 +75,7 @@ void draw_total_trip(void);
void draw_time(void);
void draw_climb_rate(void);
void draw_rssi(void);
void draw_wfb_state(void);
void draw_link_quality(void);
void draw_efficiency(void);
void draw_wind(void);
Expand Down
Loading

0 comments on commit fa5149b

Please sign in to comment.