void AP_OSD_Screen::draw_altitude(uint8_t x, uint8_t y)
{
float alt;
AP::ahrs().get_relative_position_D_home(alt);
alt = -alt;
backend->write(x, y, false, "%4d%c", (int)u_scale(ALTITUDE, alt), u_icon(ALTITUDE));
}
void AP_OSD_Screen::draw_aspeed(uint8_t x, uint8_t y)
{
float aspd = 0.0f;
bool have_estimate = AP::ahrs().airspeed_estimate(&aspd);
if (have_estimate) {
backend->write(x, y, false, "%c%4d%c", SYM_ASPD, (int)u_scale(SPEED, aspd), u_icon(SPEED));
} else {
backend->write(x, y, false, "%c ---%c", SYM_ASPD, u_icon(SPEED));
}
}
void AP_OSD_Screen::draw_eff(uint8_t x, uint8_t y)
{
AP_BattMonitor &battery = AP::battery();
AP_AHRS &ahrs = AP::ahrs();
Vector2f v = ahrs.groundspeed_vector();
float speed = u_scale(SPEED,v.length());
if (speed > 2.0){
backend->write(x, y, false, "%c%3d%c", SYM_EFF,int(1000*battery.current_amps()/speed),SYM_MAH);
} else {
backend->write(x, y, false, "%c---%c", SYM_EFF,SYM_MAH);
}
}
void AP_OSD_Screen::draw_distance(uint8_t x, uint8_t y, float distance)
{
char unit_icon = u_icon(DISTANCE);
float distance_scaled = u_scale(DISTANCE, distance);
const char *fmt = "%4.0f%c";
if (distance_scaled > 9999.0f) {
distance_scaled = u_scale(DISTANCE_LONG, distance);
unit_icon= u_icon(DISTANCE_LONG);
//try to pack as many useful info as possible
if (distance_scaled<9.0f) {
fmt = "%1.3f%c";
} else if (distance_scaled < 99.0f) {
fmt = "%2.2f%c";
} else if (distance_scaled < 999.0f) {
fmt = "%3.1f%c";
} else {
fmt = "%4.0f%c";
}
}
backend->write(x, y, false, fmt, (double)distance_scaled, unit_icon);
}
void AP_OSD_Screen::draw_speed_vector(uint8_t x, uint8_t y,Vector2f v, int32_t yaw)
{
float v_length = v.length();
char arrow = SYM_ARROW_START;
if (v_length > 1.0f) {
int32_t angle = wrap_360_cd(DEGX100 * atan2f(v.y, v.x) - yaw);
int32_t interval = 36000 / SYM_ARROW_COUNT;
arrow = SYM_ARROW_START + ((angle + interval / 2) / interval) % SYM_ARROW_COUNT;
}
backend->write(x, y, false, "%c%3d%c", arrow, (int)u_scale(SPEED, v_length), u_icon(SPEED));
}
void AP_OSD_Screen::draw_aspd2(uint8_t x, uint8_t y)
{
AP_Airspeed *airspeed = AP_Airspeed::get_singleton();
if (!airspeed) {
return;
}
float asp2 = airspeed->get_airspeed(1);
if (airspeed != nullptr && airspeed->healthy(1)) {
backend->write(x, y, false, "%c%4d%c", SYM_ASPD, (int)u_scale(SPEED, asp2), u_icon(SPEED));
} else {
backend->write(x, y, false, "%c ---%c", SYM_ASPD, u_icon(SPEED));
}
}
void AP_OSD_Screen::draw_atemp(uint8_t x, uint8_t y)
{
AP_Airspeed *airspeed = AP_Airspeed::get_singleton();
if (!airspeed) {
return;
}
float temperature = 0;
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_blh_temp(uint8_t x, uint8_t y)
{
AP_BLHeli *blheli = AP_BLHeli::get_singleton();
if (blheli) {
AP_BLHeli::telem_data td;
// first parameter is index into array of ESC's. Hardwire to zero (first) for now.
if (!blheli->get_telem_data(0, td)) {
return;
}
// AP_BLHeli & blh = AP_BLHeli::AP_BLHeli();
uint8_t esc_temp = td.temperature;
backend->write(x, y, false, "%3d%c", (int)u_scale(TEMPERATURE, esc_temp), u_icon(TEMPERATURE));
}
}
void AP_OSD_Screen::draw_climbeff(uint8_t x, uint8_t y)
{
char unit_icon = u_icon(DISTANCE);
Vector3f v;
float vspd;
if (AP::ahrs().get_velocity_NED(v)) {
vspd = -v.z;
} else {
vspd = AP::baro().get_climb_rate();
}
if (vspd < 0.0) vspd = 0.0;
AP_BattMonitor &battery = AP::battery();
float amps = battery.current_amps();
if (amps > 0.0) {
backend->write(x, y, false,"%c%c%3.1f%c",SYM_PTCHUP,SYM_EFF,(double)(3.6f * u_scale(VSPEED,vspd)/amps),unit_icon);
} else {
backend->write(x, y, false,"%c%c---%c",SYM_PTCHUP,SYM_EFF,unit_icon);
}
}
void AP_OSD_Screen::draw_vspeed(uint8_t x, uint8_t y)
{
Vector3f v;
float vspd;
if (AP::ahrs().get_velocity_NED(v)) {
vspd = -v.z;
} else {
vspd = AP::baro().get_climb_rate();
}
char sym;
if (vspd > 3.0f) {
sym = SYM_UP_UP;
} else if (vspd >=0.0f) {
sym = SYM_UP;
} else if (vspd >= -3.0f) {
sym = SYM_DOWN;
} else {
sym = SYM_DOWN_DOWN;
}
vspd = fabsf(vspd);
backend->write(x, y, false, "%c%2d%c", sym, (int)u_scale(VSPEED, vspd), u_icon(VSPEED));
}
void AP_OSD_Screen::draw_btemp(uint8_t x, uint8_t y)
{
AP_Baro &barometer = AP::baro();
float btmp = barometer.get_temperature(1);
backend->write(x, y, false, "%3d%c", (int)u_scale(TEMPERATURE, btmp), u_icon(TEMPERATURE));
}
