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"
2020-03-11 22:57:43 -03:00
2020-10-19 17:30:08 -03:00
# if OSD_ENABLED || OSD_PARAM_ENABLED
2020-03-11 22:57:43 -03:00
2018-06-24 07:00:23 -03:00
# include "AP_OSD_MAX7456.h"
2018-07-01 07:09:51 -03:00
# ifdef WITH_SITL_OSD
# include "AP_OSD_SITL.h"
# endif
2020-08-04 17:41:02 -03:00
# include "AP_OSD_MSP.h"
2021-08-06 12:54:02 -03:00
# include "AP_OSD_MSP_DisplayPort.h"
2018-06-23 04:11:05 -03:00
# include <AP_HAL/AP_HAL.h>
# include <AP_HAL/Util.h>
2018-06-25 18:25:42 -03:00
# include <RC_Channel/RC_Channel.h>
2018-08-11 14:07:31 -03:00
# include <AP_AHRS/AP_AHRS.h>
2019-06-13 19:57:56 -03:00
# include <AP_BattMonitor/AP_BattMonitor.h>
2018-06-23 04:11:05 -03:00
# include <utility>
2018-08-11 14:07:31 -03:00
# include <AP_Notify/AP_Notify.h>
2021-03-05 13:12:43 -04:00
# include <AP_Terrain/AP_Terrain.h>
2021-09-06 11:07:53 -03:00
# include <AP_RSSI/AP_RSSI.h>
2022-11-08 00:23:10 -04:00
# include <GCS_MAVLink/GCS.h>
2018-06-23 04:11:05 -03:00
2021-07-15 21:59:29 -03:00
// macro for easy use of var_info2
2023-01-03 21:39:14 -04:00
# define AP_SUBGROUPINFO2(element, name, idx, thisclazz, elclazz) { name, AP_VAROFFSET(thisclazz, element), { group_info : elclazz::var_info2 }, AP_PARAM_FLAG_NESTED_OFFSET, idx, AP_PARAM_GROUP }
2021-07-15 21:59:29 -03:00
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
2020-10-19 17:30:08 -03:00
// @Description: OSD type. TXONLY makes the OSD parameter selection available to other modules even if there is no native OSD support on the board, for instance CRSF.
2022-08-31 16:57:07 -03:00
// @Values: 0:None,1:MAX7456,2:SITL,3:MSP,4:TXONLY,5:MSP_DISPLAYPORT
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
2020-10-19 17:30:08 -03:00
# if OSD_ENABLED
2018-06-25 18:25:42 -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
2018-06-25 18:25:42 -03:00
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 ) ,
2018-06-25 18:26:21 -03:00
2018-06-29 17:14:09 -03:00
// @Param: _SW_METHOD
// @DisplayName: Screen switch method
// @Description: This sets the method used to switch different OSD screens.
2020-09-12 19:56:54 -03:00
// @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
2018-06-29 17:14:09 -03:00
// @User: Standard
AP_GROUPINFO ( " _SW_METHOD " , 7 , AP_OSD , sw_method , AP_OSD : : TOGGLE ) ,
2018-07-04 05:07:20 -03:00
// @Param: _OPTIONS
// @DisplayName: OSD Options
// @Description: This sets options that change the display
2023-09-10 12:00:51 -03:00
// @Bitmask: 0:UseDecimalPack, 1:InvertedWindArrow, 2:InvertedAHRoll, 3:Convert feet to miles at 5280ft instead of 10000ft, 4:DisableCrosshair, 5:TranslateArrows, 6:AviationStyleAH
2018-07-04 05:07:20 -03:00
// @User: Standard
2018-07-04 18:42:15 -03:00
AP_GROUPINFO ( " _OPTIONS " , 8 , AP_OSD , options , OPTION_DECIMAL_PACK ) ,
2018-07-09 05:08:47 -03:00
// @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 ) ,
2018-07-11 19:13:44 -03:00
2018-07-05 17:31:16 -03:00
// @Param: _V_OFFSET
// @DisplayName: OSD vertical offset
// @Description: Sets vertical offset of the osd inside image
// @Range: 0 31
// @User: Standard
2018-07-08 19:32:29 -03:00
// @RebootRequired: True
2018-07-05 17:31:16 -03:00
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
2018-07-08 19:32:29 -03:00
// @RebootRequired: True
2018-07-05 17:31:16 -03:00
AP_GROUPINFO ( " _H_OFFSET " , 11 , AP_OSD , h_offset , 32 ) ,
2018-07-05 19:31:01 -03:00
// @Param: _W_RSSI
// @DisplayName: RSSI warn level (in %)
// @Description: Set level at which RSSI item will flash
// @Range: 0 99
// @User: Standard
2018-07-08 19:32:29 -03:00
AP_GROUPINFO ( " _W_RSSI " , 12 , AP_OSD , warn_rssi , 30 ) ,
2018-07-05 19:31:01 -03:00
// @Param: _W_NSAT
2018-07-08 19:32:29 -03:00
// @DisplayName: NSAT warn level
2018-07-05 19:31:01 -03:00
// @Description: Set level at which NSAT item will flash
// @Range: 1 30
// @User: Standard
2018-07-08 19:32:29 -03:00
AP_GROUPINFO ( " _W_NSAT " , 13 , AP_OSD , warn_nsat , 9 ) ,
2018-07-05 19:31:01 -03:00
// @Param: _W_BATVOLT
// @DisplayName: BAT_VOLT warn level
// @Description: Set level at which BAT_VOLT item will flash
// @Range: 0 100
// @User: Standard
2018-07-08 19:32:29 -03:00
AP_GROUPINFO ( " _W_BATVOLT " , 14 , AP_OSD , warn_batvolt , 10.0f ) ,
2018-07-05 19:31:01 -03:00
2018-07-09 19:01:17 -03:00
// @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
2018-07-09 19:01:17 -03:00
// @User: Standard
AP_GROUPINFO ( " _UNITS " , 15 , AP_OSD , units , 0 ) ,
2018-07-11 19:13:44 -03:00
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
2019-03-01 08:54:36 -04: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
2019-03-01 08:54:36 -04: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
2019-03-01 08:54:36 -04: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
2020-04-18 17:16:02 -03:00
# if OSD_PARAM_ENABLED
// @Param: _BTN_DELAY
// @DisplayName: Button delay
// @Description: Debounce time in ms for stick commanded parameter navigation.
// @Range: 0 3000
// @User: Advanced
AP_GROUPINFO ( " _BTN_DELAY " , 20 , AP_OSD , button_delay_ms , 300 ) ,
2020-10-19 17:30:08 -03:00
# endif
2021-03-05 13:12:43 -04:00
# if AP_TERRAIN_AVAILABLE
// @Param: _W_TERR
// @DisplayName: Terrain warn level
2021-04-29 09:58:21 -03:00
// @Description: Set level below which TER_HGT item will flash. -1 disables.
// @Range: -1 3000
2021-03-05 13:12:43 -04:00
// @Units: m
// @User: Standard
AP_GROUPINFO ( " _W_TERR " , 23 , AP_OSD , warn_terr , - 1 ) ,
2020-10-19 17:30:08 -03:00
# endif
2021-03-05 13:12:43 -04:00
2021-03-22 12:03:42 -03:00
// @Param: _W_AVGCELLV
// @DisplayName: AVGCELLV warn level
// @Description: Set level at which AVGCELLV item will flash
// @Range: 0 100
// @User: Standard
AP_GROUPINFO ( " _W_AVGCELLV " , 24 , AP_OSD , warn_avgcellvolt , 3.6f ) ,
// @Param: _CELL_COUNT
// @DisplayName: Battery cell count
// @Description: Used for average cell voltage display. -1 disables, 0 uses cell count autodetection for well charged LIPO/LIION batteries at connection, other values manually select cell count used.
// @Increment: 1
// @User: Advanced
AP_GROUPINFO ( " _CELL_COUNT " , 25 , AP_OSD , cell_count , - 1 ) ,
// @Param: _W_RESTVOLT
// @DisplayName: RESTVOLT warn level
// @Description: Set level at which RESTVOLT item will flash
// @Range: 0 100
// @User: Standard
AP_GROUPINFO ( " _W_RESTVOLT " , 26 , AP_OSD , warn_restvolt , 10.0f ) ,
2023-02-06 09:22:48 -04:00
// @Param: _W_ACRVOLT
// @DisplayName: Avg Cell Resting Volt warn level
// @Description: Set level at which ACRVOLT item will flash
// @Range: 0 100
// @User: Standard
AP_GROUPINFO ( " _W_ACRVOLT " , 31 , AP_OSD , warn_avgcellrestvolt , 3.6f ) ,
2021-03-22 12:03:42 -03:00
2021-03-05 13:12:43 -04:00
# endif //osd enabled
2020-10-19 17:30:08 -03:00
# if OSD_PARAM_ENABLED
2020-04-18 17:16:02 -03:00
// @Group: 5_
// @Path: AP_OSD_ParamScreen.cpp
AP_SUBGROUPINFO ( param_screen [ 0 ] , " 5_ " , 21 , AP_OSD , AP_OSD_ParamScreen ) ,
// @Group: 6_
// @Path: AP_OSD_ParamScreen.cpp
AP_SUBGROUPINFO ( param_screen [ 1 ] , " 6_ " , 22 , AP_OSD , AP_OSD_ParamScreen ) ,
# endif
2021-07-15 21:59:29 -03:00
2021-11-03 15:31:18 -03:00
# if OSD_ENABLED
2021-07-15 21:59:29 -03:00
// additional tables to go beyond 63 limit
AP_SUBGROUPINFO2 ( screen [ 0 ] , " 1_ " , 27 , AP_OSD , AP_OSD_Screen ) ,
AP_SUBGROUPINFO2 ( screen [ 1 ] , " 2_ " , 28 , AP_OSD , AP_OSD_Screen ) ,
AP_SUBGROUPINFO2 ( screen [ 2 ] , " 3_ " , 29 , AP_OSD , AP_OSD_Screen ) ,
AP_SUBGROUPINFO2 ( screen [ 3 ] , " 4_ " , 30 , AP_OSD , AP_OSD_Screen ) ,
2021-11-03 15:31:18 -03:00
# endif
2023-07-07 15:30:54 -03:00
// @Param: _TYPE2
// @DisplayName: OSD type 2
// @Description: OSD type 2. TXONLY makes the OSD parameter selection available to other modules even if there is no native OSD support on the board, for instance CRSF.
// @Values: 0:None,1:MAX7456,2:SITL,3:MSP,4:TXONLY,5:MSP_DISPLAYPORT
// @User: Standard
// @RebootRequired: True
AP_GROUPINFO ( " _TYPE2 " , 32 , AP_OSD , osd_type2 , 0 ) ,
2018-06-23 04:11:05 -03:00
AP_GROUPEND
} ;
extern const AP_HAL : : HAL & hal ;
2019-11-22 17:52:11 -04:00
// singleton instance
AP_OSD * AP_OSD : : _singleton ;
2018-06-24 07:45:58 -03:00
AP_OSD : : AP_OSD ( )
2018-06-23 04:11:05 -03:00
{
2019-11-22 17:52:11 -04:00
if ( _singleton ! = nullptr ) {
AP_HAL : : panic ( " AP_OSD must be singleton " ) ;
}
2018-06-23 04:11:05 -03:00
AP_Param : : setup_object_defaults ( this , var_info ) ;
2020-10-19 17:30:08 -03:00
# if OSD_ENABLED
2018-06-24 07:45:58 -03:00
// default first screen enabled
2022-07-05 00:08:56 -03:00
screen [ 0 ] . enabled . set ( 1 ) ;
2020-10-19 17:30:08 -03:00
previous_pwm_screen = - 1 ;
# endif
2018-07-07 18:52:46 -03:00
# ifdef WITH_SITL_OSD
2020-10-19 17:30:08 -03:00
osd_type . set_default ( OSD_SITL ) ;
2018-07-07 18:52:46 -03:00
# endif
2019-09-16 18:35:45 -03:00
2018-10-31 21:10:21 -03:00
# ifdef HAL_OSD_TYPE_DEFAULT
osd_type . set_default ( HAL_OSD_TYPE_DEFAULT ) ;
# endif
2019-11-22 17:52:11 -04:00
_singleton = this ;
2018-06-23 04:11:05 -03:00
}
void AP_OSD : : init ( )
{
2023-07-07 15:30:54 -03:00
const AP_OSD : : osd_types types [ OSD_MAX_INSTANCES ] = {
osd_types ( osd_type . get ( ) ) ,
osd_types ( osd_type2 . get ( ) )
} ;
for ( uint8_t instance = 0 ; instance < OSD_MAX_INSTANCES ; instance + + ) {
if ( init_backend ( types [ instance ] , instance ) ) {
_backend_count + + ;
}
}
if ( _backend_count > 0 ) {
hal . scheduler - > thread_create ( FUNCTOR_BIND_MEMBER ( & AP_OSD : : osd_thread , void ) , " OSD " , 1280 , AP_HAL : : Scheduler : : PRIORITY_IO , 1 ) ;
}
}
bool AP_OSD : : init_backend ( const AP_OSD : : osd_types type , const uint8_t instance )
{
// check if we can run this backend instance in parallel with backend instance 0
if ( instance > 0 ) {
if ( _backends [ 0 ] & & ! _backends [ 0 ] - > is_compatible_with_backend_type ( type ) ) {
return false ;
}
}
switch ( type ) {
2018-06-24 07:00:23 -03:00
case OSD_NONE :
2020-10-19 17:30:08 -03:00
case OSD_TXONLY :
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 : {
2020-08-31 02:31:39 -03:00
# ifdef HAL_WITH_SPI_OSD
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 ;
}
2022-05-12 10:01:27 -03:00
# if HAL_WITH_OSD_BITMAP
2023-07-07 15:30:54 -03:00
_backends [ instance ] = AP_OSD_MAX7456 : : probe ( * this , std : : move ( spi_dev ) ) ;
2022-05-12 10:01:27 -03:00
# endif
2023-07-07 15:30:54 -03:00
if ( _backends [ instance ] = = nullptr ) {
2018-06-24 07:00:23 -03:00
break ;
}
2022-03-21 06:36:41 -03:00
DEV_PRINTF ( " Started MAX7456 OSD \n " ) ;
2020-08-31 02:31:39 -03:00
# endif
2018-06-24 07:00:23 -03:00
break ;
2018-06-23 04:11:05 -03:00
}
2018-07-01 07:09:51 -03:00
# ifdef WITH_SITL_OSD
case OSD_SITL : {
2023-07-07 15:30:54 -03:00
_backends [ instance ] = AP_OSD_SITL : : probe ( * this ) ;
if ( _backends [ instance ] = = nullptr ) {
2018-07-01 07:09:51 -03:00
break ;
}
2022-03-21 06:36:41 -03:00
DEV_PRINTF ( " Started SITL OSD \n " ) ;
2018-07-01 07:09:51 -03:00
break ;
}
# endif
2020-08-04 17:41:02 -03:00
case OSD_MSP : {
2023-07-07 15:30:54 -03:00
_backends [ instance ] = AP_OSD_MSP : : probe ( * this ) ;
if ( _backends [ instance ] = = nullptr ) {
2020-08-04 17:41:02 -03:00
break ;
}
2022-03-21 06:36:41 -03:00
DEV_PRINTF ( " Started MSP OSD \n " ) ;
2020-08-04 17:41:02 -03:00
break ;
}
2021-08-06 12:54:02 -03:00
# if HAL_WITH_MSP_DISPLAYPORT
case OSD_MSP_DISPLAYPORT : {
2023-07-07 15:30:54 -03:00
_backends [ instance ] = AP_OSD_MSP_DisplayPort : : probe ( * this ) ;
if ( _backends [ instance ] = = nullptr ) {
2021-08-06 12:54:02 -03:00
break ;
}
2022-03-21 06:36:41 -03:00
DEV_PRINTF ( " Started MSP DisplayPort OSD \n " ) ;
2021-08-06 12:54:02 -03:00
break ;
}
# endif
2018-07-01 07:09:51 -03:00
}
2020-10-19 17:30:08 -03:00
# if OSD_ENABLED
2023-07-07 15:30:54 -03:00
if ( _backends [ instance ] ! = nullptr ) {
2021-08-06 12:54:02 -03:00
// populate the fonts lookup table
2023-07-07 15:30:54 -03:00
_backends [ instance ] - > init_symbol_set ( AP_OSD_AbstractScreen : : symbols_lookup_table , AP_OSD_NUM_SYMBOLS ) ;
return true ;
2018-07-06 20:30:53 -03:00
}
2020-10-19 17:30:08 -03:00
# endif
2023-09-16 03:34:29 -03:00
return false ;
2018-06-23 04:11:05 -03:00
}
2020-10-19 17:30:08 -03:00
# if OSD_ENABLED
2018-07-06 20:30:53 -03:00
void AP_OSD : : osd_thread ( )
2018-06-23 04:11:05 -03:00
{
2022-07-26 07:06:42 -03:00
// initialize thread specific code once
2023-07-07 15:30:54 -03:00
for ( uint8_t instance = 0 ; instance < _backend_count ; instance + + ) {
_backends [ instance ] - > osd_thread_run_once ( ) ;
}
2022-07-26 07:06:42 -03:00
2018-07-06 20:30:53 -03:00
while ( true ) {
hal . scheduler - > delay ( 100 ) ;
2023-07-07 15:30:54 -03:00
if ( ! _disable ) {
update_stats ( ) ;
update_current_screen ( ) ;
}
2018-06-23 04:11:05 -03:00
update_osd ( ) ;
}
}
void AP_OSD : : update_osd ( )
{
2023-07-07 15:30:54 -03:00
for ( uint8_t instance = 0 ; instance < _backend_count ; instance + + ) {
_backends [ instance ] - > clear ( ) ;
if ( ! _disable ) {
get_screen ( current_screen ) . set_backend ( _backends [ instance ] ) ;
// skip drawing for MSP OSD backends to save some resources
if ( _backends [ instance ] - > get_backend_type ( ) ! = OSD_MSP ) {
get_screen ( current_screen ) . draw ( ) ;
}
2021-09-06 11:07:53 -03:00
}
2018-06-23 04:11:05 -03:00
2023-07-07 15:30:54 -03:00
_backends [ instance ] - > flush ( ) ;
}
2018-06-23 04:11:05 -03:00
}
2018-08-11 14:07:31 -03:00
//update maximums and totals
2021-09-06 11:07:53 -03:00
void AP_OSD : : update_stats ( )
2018-08-11 14:07:31 -03:00
{
2021-09-06 11:07:53 -03:00
// allow other threads to consume stats info
WITH_SEMAPHORE ( _sem ) ;
2018-08-11 14:07:31 -03:00
uint32_t now = AP_HAL : : millis ( ) ;
if ( ! AP_Notify : : flags . armed ) {
2021-09-06 11:07:53 -03:00
_stats . last_update_ms = now ;
2018-08-11 14:07:31 -03:00
return ;
}
2019-09-16 18:35:45 -03:00
// flight distance
2021-09-06 11:07:53 -03:00
uint32_t delta_ms = now - _stats . last_update_ms ;
_stats . last_update_ms = now ;
Vector2f v ;
Location loc { } ;
Location home_loc ;
bool home_is_set ;
bool have_airspeed_estimate ;
float alt ;
float aspd_mps = 0.0f ;
{
// minimize semaphore scope
AP_AHRS & ahrs = AP : : ahrs ( ) ;
WITH_SEMAPHORE ( ahrs . get_semaphore ( ) ) ;
v = ahrs . groundspeed_vector ( ) ;
2022-01-20 19:42:41 -04:00
home_is_set = ahrs . get_location ( loc ) & & ahrs . home_is_set ( ) ;
2021-09-06 11:07:53 -03:00
if ( home_is_set ) {
home_loc = ahrs . get_home ( ) ;
}
ahrs . get_relative_position_D_home ( alt ) ;
have_airspeed_estimate = ahrs . airspeed_estimate ( aspd_mps ) ;
}
2018-08-11 14:07:31 -03:00
float speed = v . length ( ) ;
2019-09-26 13:59:41 -03:00
if ( speed < 0.178 ) {
2018-08-11 14:07:31 -03:00
speed = 0.0 ;
}
float dist_m = ( speed * delta_ms ) * 0.001 ;
2021-09-06 11:07:53 -03:00
_stats . last_distance_m + = dist_m ;
2019-09-16 18:35:45 -03:00
2018-08-11 14:07:31 -03:00
// maximum ground speed
2021-09-06 11:07:53 -03:00
_stats . max_speed_mps = fmaxf ( _stats . max_speed_mps , speed ) ;
2019-09-16 18:35:45 -03:00
2018-08-11 14:07:31 -03:00
// maximum distance
2021-09-06 11:07:53 -03:00
if ( home_is_set ) {
2019-02-24 20:16:25 -04:00
float distance = home_loc . get_distance ( loc ) ;
2021-09-06 11:07:53 -03:00
_stats . max_dist_m = fmaxf ( _stats . max_dist_m , distance ) ;
2018-08-11 14:07:31 -03:00
}
2019-09-16 18:35:45 -03:00
2018-08-11 14:07:31 -03:00
// maximum altitude
alt = - alt ;
2021-09-06 11:07:53 -03:00
_stats . max_alt_m = fmaxf ( _stats . max_alt_m , alt ) ;
2018-08-11 14:07:31 -03:00
// maximum current
2019-02-10 14:56:43 -04:00
AP_BattMonitor & battery = AP : : battery ( ) ;
2019-07-07 11:39:31 -03:00
float amps ;
if ( battery . current_amps ( amps ) ) {
2021-09-06 11:07:53 -03:00
_stats . max_current_a = fmaxf ( _stats . max_current_a , amps ) ;
}
// minimum voltage
float voltage = battery . voltage ( ) ;
if ( voltage > 0 ) {
_stats . min_voltage_v = fminf ( _stats . min_voltage_v , voltage ) ;
}
2023-12-07 22:02:40 -04:00
# if AP_RSSI_ENABLED
2021-09-06 11:07:53 -03:00
// minimum rssi
AP_RSSI * ap_rssi = AP_RSSI : : get_singleton ( ) ;
if ( ap_rssi ) {
_stats . min_rssi = fminf ( _stats . min_rssi , ap_rssi - > read_receiver_rssi ( ) ) ;
2019-07-07 11:39:31 -03:00
}
2023-12-07 22:02:40 -04:00
# endif
2021-09-06 11:07:53 -03:00
// max airspeed either true or synthetic
if ( have_airspeed_estimate ) {
_stats . max_airspeed_mps = fmaxf ( _stats . max_airspeed_mps , aspd_mps ) ;
}
# if HAL_WITH_ESC_TELEM
// max esc temp
AP_ESC_Telem & telem = AP : : esc_telem ( ) ;
int16_t highest_temperature = 0 ;
telem . get_highest_motor_temperature ( highest_temperature ) ;
_stats . max_esc_temp = MAX ( _stats . max_esc_temp , highest_temperature ) ;
# endif
2018-08-11 14:07:31 -03:00
}
2018-07-01 16:51:21 -03:00
//Thanks to minimosd authors for the multiple osd screen idea
2018-06-25 18:25:42 -03:00
void AP_OSD : : update_current_screen ( )
{
2019-03-01 08:54:36 -04:00
// Switch on ARM/DISARM event
2019-09-16 18:35:45 -03:00
if ( AP_Notify : : flags . armed ) {
2020-04-18 17:16:02 -03:00
if ( ! was_armed & & arm_scr > 0 & & arm_scr < = AP_OSD_NUM_DISPLAY_SCREENS & & get_screen ( arm_scr - 1 ) . enabled ) {
2019-03-01 08:54:36 -04:00
current_screen = arm_scr - 1 ;
}
was_armed = true ;
} else if ( was_armed ) {
2020-04-18 17:16:02 -03:00
if ( disarm_scr > 0 & & disarm_scr < = AP_OSD_NUM_DISPLAY_SCREENS & & get_screen ( disarm_scr - 1 ) . enabled ) {
2019-03-01 08:54:36 -04:00
current_screen = disarm_scr - 1 ;
2019-09-16 18:35:45 -03:00
}
2019-03-01 08:54:36 -04:00
was_armed = false ;
}
2019-09-16 18:35:45 -03:00
2019-03-01 08:54:36 -04:00
// Switch on failsafe event
if ( AP_Notify : : flags . failsafe_radio | | AP_Notify : : flags . failsafe_battery ) {
2020-04-18 17:16:02 -03:00
if ( ! was_failsafe & & failsafe_scr > 0 & & failsafe_scr < = AP_OSD_NUM_DISPLAY_SCREENS & & get_screen ( failsafe_scr - 1 ) . enabled ) {
2019-03-01 08:54:36 -04:00
pre_fs_screen = current_screen ;
current_screen = failsafe_scr - 1 ;
}
was_failsafe = true ;
} else if ( was_failsafe ) {
2020-04-18 17:16:02 -03:00
if ( get_screen ( pre_fs_screen ) . enabled ) {
2019-03-01 08:54:36 -04:00
current_screen = pre_fs_screen ;
2019-09-16 18:35:45 -03:00
}
2019-03-01 08:54:36 -04:00
was_failsafe = false ;
}
2019-09-16 18:35:45 -03:00
2018-06-25 18:26:21 -03:00
if ( rc_channel = = 0 ) {
return ;
2018-06-25 18:25:42 -03:00
}
RC_Channel * channel = RC_Channels : : rc_channel ( rc_channel - 1 ) ;
2018-06-25 18:26:21 -03:00
if ( channel = = nullptr ) {
return ;
2018-06-25 18:25:42 -03:00
}
2018-06-25 18:26:21 -03:00
int16_t channel_value = channel - > get_radio_in ( ) ;
2018-06-29 17:14:09 -03:00
switch ( sw_method ) {
//switch to next screen if channel value was changed
default :
case TOGGLE :
2018-06-30 17:57:38 -03:00
if ( previous_channel_value = = 0 ) {
//do not switch to the next screen just after initialization
previous_channel_value = channel_value ;
}
2018-06-29 17:14:09 -03:00
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 + + ) {
2021-02-02 00:18:34 -04:00
if ( get_screen ( i ) . enabled & & get_screen ( i ) . channel_min < = channel_value & & get_screen ( i ) . channel_max > channel_value ) {
2021-08-10 20:51:35 -03:00
if ( previous_pwm_screen = = i ) {
break ;
} else {
2019-03-01 08:54:36 -04:00
current_screen = previous_pwm_screen = i ;
2021-08-10 20:51:35 -03:00
}
2018-06-29 17:14:09 -03:00
}
2018-06-25 18:26:21 -03:00
}
2018-06-29 17:14:09 -03:00
break ;
2018-06-30 17:57:38 -03:00
//switch to next screen after low to high transition and every 1s while channel value is high
2018-06-29 17:14:09 -03:00
case AUTO_SWITCH :
if ( channel_value > channel - > get_radio_trim ( ) ) {
if ( switch_debouncer ) {
uint32_t now = AP_HAL : : millis ( ) ;
2018-06-30 17:57:38 -03:00
if ( now - last_switch_ms > 1000 ) {
2018-06-29 17:14:09 -03:00
next_screen ( ) ;
last_switch_ms = now ;
}
} else {
switch_debouncer = true ;
return ;
}
2018-06-30 17:57:38 -03:00
} else {
last_switch_ms = 0 ;
2018-06-29 17:14:09 -03:00
}
break ;
2018-06-25 18:25:42 -03:00
}
2018-06-29 17:14:09 -03:00
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 ;
2020-04-18 17:16:02 -03:00
} while ( t ! = current_screen & & ! get_screen ( t ) . enabled ) ;
2018-06-29 17:14:09 -03:00
current_screen = t ;
2018-06-25 18:25:42 -03:00
}
2018-07-25 20:47:06 -03:00
// set navigation information for display
void AP_OSD : : set_nav_info ( NavInfo & navinfo )
{
// do this without a lock for now
nav_info = navinfo ;
}
2023-07-07 15:30:54 -03:00
// pre_arm_check - returns true if all pre-takeoff checks have completed successfully
bool AP_OSD : : pre_arm_check ( char * failure_msg , const uint8_t failure_msg_len ) const
{
# if OSD_PARAM_ENABLED
// currently in the OSD menu, do not allow arming
if ( ! is_readonly_screen ( ) ) {
hal . util - > snprintf ( failure_msg , failure_msg_len , " In OSD menu " ) ;
return false ;
}
# endif
//check if second backend was requested by user but not instantiated
if ( osd_type . get ( ) ! = OSD_NONE & & _backend_count = = 1 & & osd_type2 . get ( ) ! = OSD_NONE ) {
hal . util - > snprintf ( failure_msg , failure_msg_len , " OSD_TYPE2 not compatible with first OSD " ) ;
return false ;
}
// if we got this far everything must be ok
return true ;
}
2020-10-19 17:30:08 -03:00
# endif // OSD_ENABLED
2019-11-22 17:52:11 -04:00
2020-04-18 17:16:02 -03:00
// handle OSD parameter configuration
2021-09-17 10:37:20 -03:00
# if HAL_GCS_ENABLED
2020-04-18 17:16:02 -03:00
void AP_OSD : : handle_msg ( const mavlink_message_t & msg , const GCS_MAVLINK & link )
{
bool found = false ;
switch ( msg . msgid ) {
case MAVLINK_MSG_ID_OSD_PARAM_CONFIG : {
mavlink_osd_param_config_t packet ;
mavlink_msg_osd_param_config_decode ( & msg , & packet ) ;
# if OSD_PARAM_ENABLED
for ( uint8_t i = 0 ; i < AP_OSD_NUM_PARAM_SCREENS ; i + + ) {
if ( packet . osd_screen = = i + AP_OSD_NUM_DISPLAY_SCREENS + 1 ) {
param_screen [ i ] . handle_write_msg ( packet , link ) ;
found = true ;
}
}
# endif
// send back an error
if ( ! found ) {
mavlink_msg_osd_param_config_reply_send ( link . get_chan ( ) , packet . request_id , OSD_PARAM_INVALID_SCREEN ) ;
}
}
break ;
case MAVLINK_MSG_ID_OSD_PARAM_SHOW_CONFIG : {
mavlink_osd_param_show_config_t packet ;
mavlink_msg_osd_param_show_config_decode ( & msg , & packet ) ;
# if OSD_PARAM_ENABLED
for ( uint8_t i = 0 ; i < AP_OSD_NUM_PARAM_SCREENS ; i + + ) {
if ( packet . osd_screen = = i + AP_OSD_NUM_DISPLAY_SCREENS + 1 ) {
param_screen [ i ] . handle_read_msg ( packet , link ) ;
found = true ;
}
}
# endif
// send back an error
if ( ! found ) {
mavlink_msg_osd_param_show_config_reply_send ( link . get_chan ( ) , packet . request_id , OSD_PARAM_INVALID_SCREEN ,
nullptr , OSD_PARAM_NONE , 0 , 0 , 0 ) ;
}
}
break ;
default :
break ;
}
}
2021-09-17 10:37:20 -03:00
# endif
2020-04-18 17:16:02 -03:00
2019-11-22 17:52:11 -04:00
AP_OSD * AP : : osd ( ) {
return AP_OSD : : get_singleton ( ) ;
}
2020-03-11 22:57:43 -03:00
2020-10-19 17:30:08 -03:00
# endif // OSD_ENABLED || OSD_PARAM_ENABLED