@ -194,6 +194,18 @@ const AP_Param::GroupInfo AP_OSD_Screen::var_info[] = {
@@ -194,6 +194,18 @@ const AP_Param::GroupInfo AP_OSD_Screen::var_info[] = {
// @Path: AP_OSD_Setting.cpp
AP_SUBGROUPINFO ( btemp , " BTEMP " , 37 , AP_OSD_Screen , AP_OSD_Setting ) ,
// @Group: ATEMP
// @Path: AP_OSD_Setting.cpp
AP_SUBGROUPINFO ( atemp , " ATEMP " , 38 , AP_OSD_Screen , AP_OSD_Setting ) ,
// @Group: BAT2VLT
// @Path: AP_OSD_Setting.cpp
AP_SUBGROUPINFO ( bat2_vlt , " BAT2_VLT " , 39 , AP_OSD_Screen , AP_OSD_Setting ) ,
// @Group: BAT2USED
// @Path: AP_OSD_Setting.cpp
AP_SUBGROUPINFO ( bat2used , " BAT2USED " , 40 , AP_OSD_Screen , AP_OSD_Setting ) ,
AP_GROUPEND
} ;
@ -287,6 +299,7 @@ AP_OSD_Screen::AP_OSD_Screen()
@@ -287,6 +299,7 @@ AP_OSD_Screen::AP_OSD_Screen()
# define SYM_DIST 0x22
# define SYM_FLY 0x9C
# define SYM_EFF 0xF2
# define SYM_AH 0xF3
void AP_OSD_Screen : : set_backend ( AP_OSD_Backend * _backend )
{
@ -919,6 +932,38 @@ void AP_OSD_Screen::draw_btemp(uint8_t x, uint8_t y)
@@ -919,6 +932,38 @@ void AP_OSD_Screen::draw_btemp(uint8_t x, uint8_t y)
backend - > write ( x , y , false , " %3d%c " , ( int ) u_scale ( TEMPERATURE , btmp ) , u_icon ( TEMPERATURE ) ) ;
}
void AP_OSD_Screen : : draw_atemp ( uint8_t x , uint8_t y )
{
AP_Airspeed * airspeed = AP_Airspeed : : get_singleton ( ) ;
float temperature ;
airspeed - > get_temperature ( temperature ) ;
if ( airspeed - > healthy ( ) ) {
backend - > write ( x , y , false , " %3d%c " , ( int ) u_scale ( TEMPERATURE , temperature ) , u_icon ( TEMPERATURE ) ) ;
} else {
backend - > write ( x , y , false , " --%c " , u_icon ( TEMPERATURE ) ) ;
}
}
void AP_OSD_Screen : : draw_bat2_vlt ( uint8_t x , uint8_t y )
{
AP_BattMonitor & battery = AP_BattMonitor : : battery ( ) ;
uint8_t pct2 = battery . capacity_remaining_pct ( 1 ) ;
uint8_t p2 = ( 100 - pct2 ) / 16.6 ;
float v2 = battery . voltage ( 1 ) ;
backend - > write ( x , y , v2 < osd - > warn_bat2volt , " %c%2.1f%c " , SYM_BATT_FULL + p2 , v2 , SYM_VOLT ) ;
}
void AP_OSD_Screen : : draw_bat2used ( uint8_t x , uint8_t y )
{
AP_BattMonitor & battery = AP_BattMonitor : : battery ( ) ;
float ah = battery . consumed_mah ( 1 ) / 1000 ;
if ( battery . consumed_mah ( 1 ) < = 9999 ) {
backend - > write ( x , y , false , " %4d%c " , ( int ) battery . consumed_mah ( 1 ) , SYM_MAH ) ;
} else {
backend - > write ( x , y , false , " %2.2f%c " , ah , SYM_AH ) ;
}
}
# define DRAW_SETTING(n) if (n.enabled) draw_ ## n(n.xpos, n.ypos)
void AP_OSD_Screen : : draw ( void )
@ -937,9 +982,11 @@ void AP_OSD_Screen::draw(void)
@@ -937,9 +982,11 @@ void AP_OSD_Screen::draw(void)
DRAW_SETTING ( waypoint ) ;
DRAW_SETTING ( xtrack_error ) ;
DRAW_SETTING ( bat_volt ) ;
DRAW_SETTING ( bat2_vlt ) ;
DRAW_SETTING ( rssi ) ;
DRAW_SETTING ( current ) ;
DRAW_SETTING ( batused ) ;
DRAW_SETTING ( bat2used ) ;
DRAW_SETTING ( sats ) ;
DRAW_SETTING ( fltmode ) ;
DRAW_SETTING ( gspeed ) ;
@ -953,6 +1000,7 @@ void AP_OSD_Screen::draw(void)
@@ -953,6 +1000,7 @@ void AP_OSD_Screen::draw(void)
DRAW_SETTING ( pitch_angle ) ;
DRAW_SETTING ( temp ) ;
DRAW_SETTING ( btemp ) ;
DRAW_SETTING ( atemp ) ;
DRAW_SETTING ( hdop ) ;
DRAW_SETTING ( flightime ) ;