ardupilot/libraries/AP_OSD/AP_OSD.cpp

389 lines
12 KiB
C++
Raw Normal View History

2018-06-23 04:11:05 -03:00
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* AP_OSD partially based on betaflight and inav osd.c implemention.
* clarity.mcm font is taken from inav configurator.
* Many thanks to their authors.
*/
2018-06-24 07:00:23 -03:00
#include "AP_OSD.h"
#include "AP_OSD_MAX7456.h"
2018-07-01 07:09:51 -03:00
#ifdef WITH_SITL_OSD
#include "AP_OSD_SITL.h"
#endif
2018-06-23 04:11:05 -03:00
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/Util.h>
#include <RC_Channel/RC_Channel.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_BattMonitor/AP_BattMonitor.h>
2018-06-23 04:11:05 -03:00
#include <utility>
#include <AP_Notify/AP_Notify.h>
2018-06-23 04:11:05 -03:00
const AP_Param::GroupInfo AP_OSD::var_info[] = {
2018-06-24 07:00:23 -03:00
// @Param: _TYPE
// @DisplayName: OSD type
// @Description: OSD type
// @Values: 0:None,1:MAX7456
2018-06-23 04:11:05 -03:00
// @User: Standard
2018-06-25 04:39:48 -03:00
// @RebootRequired: True
2018-06-24 07:00:23 -03:00
AP_GROUPINFO_FLAGS("_TYPE", 1, AP_OSD, osd_type, 0, AP_PARAM_FLAG_ENABLE),
2018-06-23 04:11:05 -03:00
// @Param: _CHAN
// @DisplayName: Screen switch transmitter channel
// @Description: This sets the channel used to switch different OSD screens.
// @Values: 0:Disable,5:Chan5,6:Chan6,7:Chan7,8:Chan8,9:Chan9,10:Chan10,11:Chan11,12:Chan12,13:Chan13,14:Chan14,15:Chan15,16:Chan16
2018-06-23 04:11:05 -03:00
// @User: Standard
AP_GROUPINFO("_CHAN", 2, AP_OSD, rc_channel, 0),
2018-06-23 04:11:05 -03:00
2018-06-24 07:00:23 -03:00
// @Group: 1_
// @Path: AP_OSD_Screen.cpp
AP_SUBGROUPINFO(screen[0], "1_", 3, AP_OSD, AP_OSD_Screen),
2018-06-23 04:11:05 -03:00
2018-06-24 07:00:23 -03:00
// @Group: 2_
// @Path: AP_OSD_Screen.cpp
AP_SUBGROUPINFO(screen[1], "2_", 4, AP_OSD, AP_OSD_Screen),
2018-06-23 04:11:05 -03:00
2018-06-25 04:39:48 -03:00
// @Group: 3_
2018-06-24 07:00:23 -03:00
// @Path: AP_OSD_Screen.cpp
AP_SUBGROUPINFO(screen[2], "3_", 5, AP_OSD, AP_OSD_Screen),
2018-06-23 04:11:05 -03:00
2018-06-25 04:39:48 -03:00
// @Group: 4_
2018-06-24 07:00:23 -03:00
// @Path: AP_OSD_Screen.cpp
AP_SUBGROUPINFO(screen[3], "4_", 6, AP_OSD, AP_OSD_Screen),
// @Param: _SW_METHOD
// @DisplayName: Screen switch method
// @Description: This sets the method used to switch different OSD screens.
// @Values: 0: switch to next screen if channel value was changed,
// 1: select screen based on pwm ranges specified for each screen,
// 2: switch to next screen after low to high transition and every 1s while channel value is high
// @User: Standard
AP_GROUPINFO("_SW_METHOD", 7, AP_OSD, sw_method, AP_OSD::TOGGLE),
// @Param: _OPTIONS
// @DisplayName: OSD Options
// @Description: This sets options that change the display
// @Bitmask: 0:UseDecimalPack, 1:InvertedWindPointer, 2:InvertedAHRoll
// @User: Standard
AP_GROUPINFO("_OPTIONS", 8, AP_OSD, options, OPTION_DECIMAL_PACK),
// @Param: _FONT
// @DisplayName: OSD Font
// @Description: This sets which OSD font to use. It is an integer from 0 to the number of fonts available
// @User: Standard
// @RebootRequired: True
AP_GROUPINFO("_FONT", 9, AP_OSD, font_num, 0),
// @Param: _V_OFFSET
// @DisplayName: OSD vertical offset
// @Description: Sets vertical offset of the osd inside image
// @Range: 0 31
// @User: Standard
// @RebootRequired: True
AP_GROUPINFO("_V_OFFSET", 10, AP_OSD, v_offset, 16),
// @Param: _H_OFFSET
// @DisplayName: OSD horizontal offset
// @Description: Sets horizontal offset of the osd inside image
// @Range: 0 63
// @User: Standard
// @RebootRequired: True
AP_GROUPINFO("_H_OFFSET", 11, AP_OSD, h_offset, 32),
// @Param: _W_RSSI
// @DisplayName: RSSI warn level (in %)
// @Description: Set level at which RSSI item will flash
// @Range: 0 99
// @User: Standard
AP_GROUPINFO("_W_RSSI", 12, AP_OSD, warn_rssi, 30),
// @Param: _W_NSAT
// @DisplayName: NSAT warn level
// @Description: Set level at which NSAT item will flash
// @Range: 1 30
// @User: Standard
AP_GROUPINFO("_W_NSAT", 13, AP_OSD, warn_nsat, 9),
// @Param: _W_BATVOLT
// @DisplayName: BAT_VOLT warn level
// @Description: Set level at which BAT_VOLT item will flash
// @Range: 0 100
// @User: Standard
AP_GROUPINFO("_W_BATVOLT", 14, AP_OSD, warn_batvolt, 10.0f),
// @Param: _UNITS
// @DisplayName: Display Units
// @Description: Sets the units to use in displaying items
2018-07-09 19:54:21 -03:00
// @Values: 0:Metric,1:Imperial,2:SI,3:Aviation
// @User: Standard
AP_GROUPINFO("_UNITS", 15, AP_OSD, units, 0),
2018-07-12 19:30:56 -03:00
// @Param: _MSG_TIME
// @DisplayName: Message display duration in seconds
// @Description: Sets message duration seconds
// @Range: 1 20
// @User: Standard
AP_GROUPINFO("_MSG_TIME", 16, AP_OSD, msgtime_s, 10),
2019-09-16 18:35:45 -03:00
// @Param: _ARM_SCR
// @DisplayName: Arm screen
// @Description: Screen to be shown on Arm event. Zero to disable the feature.
// @Range: 0 4
// @User: Standard
AP_GROUPINFO("_ARM_SCR", 17, AP_OSD, arm_scr, 0),
2019-09-16 18:35:45 -03:00
// @Param: _DSARM_SCR
// @DisplayName: Disarm screen
// @Description: Screen to be shown on disarm event. Zero to disable the feature.
// @Range: 0 4
// @User: Standard
AP_GROUPINFO("_DSARM_SCR", 18, AP_OSD, disarm_scr, 0),
2018-07-12 19:30:56 -03:00
// @Param: _FS_SCR
// @DisplayName: Failsafe screen
// @Description: Screen to be shown on failsafe event. Zero to disable the feature.
// @Range: 0 4
// @User: Standard
AP_GROUPINFO("_FS_SCR", 19, AP_OSD, failsafe_scr, 0),
2018-07-12 19:30:56 -03:00
2018-06-23 04:11:05 -03:00
AP_GROUPEND
};
extern const AP_HAL::HAL& hal;
2018-06-24 07:45:58 -03:00
AP_OSD::AP_OSD()
2018-06-23 04:11:05 -03:00
{
AP_Param::setup_object_defaults(this, var_info);
2018-06-24 07:45:58 -03:00
// default first screen enabled
screen[0].enabled = 1;
#ifdef WITH_SITL_OSD
osd_type.set_default(2);
#endif
2019-09-16 18:35:45 -03:00
#ifdef HAL_OSD_TYPE_DEFAULT
osd_type.set_default(HAL_OSD_TYPE_DEFAULT);
#endif
previous_pwm_screen = -1;
2018-06-23 04:11:05 -03:00
}
void AP_OSD::init()
{
2018-06-24 07:00:23 -03:00
switch ((enum osd_types)osd_type.get()) {
case OSD_NONE:
2018-06-25 04:39:48 -03:00
default:
2018-06-24 07:00:23 -03:00
break;
2018-07-01 07:09:51 -03:00
case OSD_MAX7456: {
2018-06-24 07:00:23 -03:00
AP_HAL::OwnPtr<AP_HAL::Device> spi_dev = std::move(hal.spi->get_device("osd"));
if (!spi_dev) {
break;
}
backend = AP_OSD_MAX7456::probe(*this, std::move(spi_dev));
if (backend == nullptr) {
break;
}
hal.console->printf("Started MAX7456 OSD\n");
break;
2018-06-23 04:11:05 -03:00
}
2018-07-01 07:09:51 -03:00
#ifdef WITH_SITL_OSD
case OSD_SITL: {
backend = AP_OSD_SITL::probe(*this);
if (backend == nullptr) {
break;
}
hal.console->printf("Started SITL OSD\n");
break;
}
#endif
}
if (backend != nullptr) {
// create thread as higher priority than IO
hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_OSD::osd_thread, void), "OSD", 1024, AP_HAL::Scheduler::PRIORITY_IO, 1);
}
2018-06-23 04:11:05 -03:00
}
void AP_OSD::osd_thread()
2018-06-23 04:11:05 -03:00
{
while (true) {
hal.scheduler->delay(100);
2018-06-23 04:11:05 -03:00
update_osd();
}
}
void AP_OSD::update_osd()
{
backend->clear();
stats();
update_current_screen();
2018-06-24 07:00:23 -03:00
screen[current_screen].set_backend(backend);
screen[current_screen].draw();
2018-06-23 04:11:05 -03:00
backend->flush();
}
//update maximums and totals
void AP_OSD::stats()
{
uint32_t now = AP_HAL::millis();
if (!AP_Notify::flags.armed) {
last_update_ms = now;
return;
}
2019-09-16 18:35:45 -03:00
// flight distance
uint32_t delta_ms = now - last_update_ms;
last_update_ms = now;
2019-09-16 18:35:45 -03:00
AP_AHRS &ahrs = AP::ahrs();
Vector2f v = ahrs.groundspeed_vector();
float speed = v.length();
if (speed < 0.178) {
speed = 0.0;
}
float dist_m = (speed * delta_ms)*0.001;
last_distance_m += dist_m;
2019-09-16 18:35:45 -03:00
// maximum ground speed
max_speed_mps = fmaxf(max_speed_mps,speed);
2019-09-16 18:35:45 -03:00
// maximum distance
Location loc;
if (ahrs.get_position(loc) && ahrs.home_is_set()) {
const Location &home_loc = ahrs.get_home();
float distance = home_loc.get_distance(loc);
max_dist_m = fmaxf(max_dist_m, distance);
}
2019-09-16 18:35:45 -03:00
// maximum altitude
float alt;
AP::ahrs().get_relative_position_D_home(alt);
alt = -alt;
max_alt_m = fmaxf(max_alt_m, alt);
// maximum current
AP_BattMonitor &battery = AP::battery();
float amps;
if (battery.current_amps(amps)) {
max_current_a = fmaxf(max_current_a, amps);
}
}
//Thanks to minimosd authors for the multiple osd screen idea
void AP_OSD::update_current_screen()
{
// Switch on ARM/DISARM event
2019-09-16 18:35:45 -03:00
if (AP_Notify::flags.armed) {
if (!was_armed && arm_scr > 0 && arm_scr <= AP_OSD_NUM_SCREENS && screen[arm_scr-1].enabled) {
current_screen = arm_scr-1;
}
was_armed = true;
} else if (was_armed) {
2019-09-16 18:35:45 -03:00
if (disarm_scr > 0 && disarm_scr <= AP_OSD_NUM_SCREENS && screen[disarm_scr-1].enabled) {
current_screen = disarm_scr-1;
2019-09-16 18:35:45 -03:00
}
was_armed = false;
}
2019-09-16 18:35:45 -03:00
// Switch on failsafe event
if (AP_Notify::flags.failsafe_radio || AP_Notify::flags.failsafe_battery) {
2019-09-16 18:35:45 -03:00
if (!was_failsafe && failsafe_scr > 0 && failsafe_scr <= AP_OSD_NUM_SCREENS && screen[failsafe_scr-1].enabled) {
pre_fs_screen = current_screen;
current_screen = failsafe_scr-1;
}
was_failsafe = true;
} else if (was_failsafe) {
2019-09-16 18:35:45 -03:00
if (screen[pre_fs_screen].enabled) {
current_screen = pre_fs_screen;
2019-09-16 18:35:45 -03:00
}
was_failsafe = false;
}
2019-09-16 18:35:45 -03:00
if (rc_channel == 0) {
return;
}
RC_Channel *channel = RC_Channels::rc_channel(rc_channel-1);
if (channel == nullptr) {
return;
}
int16_t channel_value = channel->get_radio_in();
switch (sw_method) {
//switch to next screen if channel value was changed
default:
case TOGGLE:
if (previous_channel_value == 0) {
//do not switch to the next screen just after initialization
previous_channel_value = channel_value;
}
if (abs(channel_value-previous_channel_value) > 200) {
if (switch_debouncer) {
next_screen();
previous_channel_value = channel_value;
} else {
switch_debouncer = true;
return;
}
}
break;
//select screen based on pwm ranges specified
case PWM_RANGE:
for (int i=0; i<AP_OSD_NUM_SCREENS; i++) {
if (screen[i].enabled && screen[i].channel_min <= channel_value && screen[i].channel_max > channel_value && previous_pwm_screen != i) {
current_screen = previous_pwm_screen = i;
break;
}
}
break;
//switch to next screen after low to high transition and every 1s while channel value is high
case AUTO_SWITCH:
if (channel_value > channel->get_radio_trim()) {
if (switch_debouncer) {
uint32_t now = AP_HAL::millis();
if (now - last_switch_ms > 1000) {
next_screen();
last_switch_ms = now;
}
} else {
switch_debouncer = true;
return;
}
} else {
last_switch_ms = 0;
}
break;
}
switch_debouncer = false;
}
//select next avaliable screen, do nothing if all screens disabled
void AP_OSD::next_screen()
{
uint8_t t = current_screen;
do {
t = (t + 1)%AP_OSD_NUM_SCREENS;
} while (t != current_screen && !screen[t].enabled);
current_screen = t;
}
// set navigation information for display
void AP_OSD::set_nav_info(NavInfo &navinfo)
{
// do this without a lock for now
nav_info = navinfo;
}