diff --git a/Makefile b/Makefile index 9f9d1a0c..7d0fc725 100644 --- a/Makefile +++ b/Makefile @@ -173,7 +173,7 @@ CENTRAL_MENU_ITEMS_ALL := $(FOLDER_CENTRAL_MENU)/menu_items.o $(FOLDER_CENTRAL_M CENTRAL_MENU_ALL1 := $(FOLDER_CENTRAL_MENU)/menu.o $(FOLDER_CENTRAL_MENU)/menu_objects.o $(FOLDER_CENTRAL_MENU)/menu_preferences_buttons.o $(FOLDER_CENTRAL_MENU)/menu_root.o $(FOLDER_CENTRAL_MENU)/menu_search.o $(FOLDER_CENTRAL_MENU)/menu_spectator.o $(FOLDER_CENTRAL_MENU)/menu_vehicles.o $(FOLDER_CENTRAL_MENU)/menu_vehicle.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_general.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_expert.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_video.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_video_bidir.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_camera.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_osd.o $(FOLDER_CENTRAL_MENU)/menu_confirmation.o $(FOLDER_CENTRAL_MENU)/menu_storage.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_telemetry.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_video_encodings.o $(FOLDER_CENTRAL_MENU)/menu_text.o $(FOLDER_CENTRAL_MENU)/menu_tx_power.o $(FOLDER_CENTRAL_MENU)/menu_tx_power_8812eu.o $(FOLDER_CENTRAL_MENU)/menu_calibrate_hdmi.o CENTRAL_MENU_ALL2 := $(FOLDER_CENTRAL_MENU)/menu_vehicle_relay.o $(FOLDER_CENTRAL_MENU)/menu_controller.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_alarms.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_camera_gains.o $(FOLDER_CENTRAL_MENU)/menu_controller_peripherals.o $(FOLDER_CENTRAL_MENU)/menu_controller_expert.o $(FOLDER_CENTRAL_MENU)/menu_system.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_osd_instruments.o $(FOLDER_CENTRAL_MENU)/menu_preferences_ui.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_video_profile.o $(FOLDER_CENTRAL_MENU)/menu_confirmation_import.o $(FOLDER_CENTRAL_MENU)/menu_system_alarms.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_selector.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_osd_widgets.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_osd_widget.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_dev.o $(FOLDER_CENTRAL_MENU)/menu_controller_dev.o $(FOLDER_CENTRAL_MENU)/menu_controller_dev_stats.o CENTRAL_MENU_ALL3 := $(FOLDER_CENTRAL_MENU)/menu_vehicle_management.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_import.o $(FOLDER_CENTRAL_MENU)/menu_switch_vehicle.o $(FOLDER_CENTRAL_MENU)/menu_controller_joystick.o $(FOLDER_CENTRAL_MENU)/menu_system_all_params.o $(FOLDER_CENTRAL_MENU)/menu_color_picker.o $(FOLDER_CENTRAL_MENU)/menu_controller_video.o $(FOLDER_CENTRAL_MENU)/menu_controller_telemetry.o $(FOLDER_CENTRAL_MENU)/menu_update_vehicle.o $(FOLDER_CENTRAL_MENU)/menu_device_i2c.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_osd_stats.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_audio.o $(FOLDER_CENTRAL_MENU)/menu_channels_select.o $(FOLDER_CENTRAL_MENU)/menu_tx_power_max.o $(FOLDER_CENTRAL_MENU)/menu_system_dev_logs.o $(FOLDER_CENTRAL_MENU)/menu_item_vehicle.o $(FOLDER_CENTRAL_MENU)/menu_radio_config.o $(FOLDER_CENTRAL_MENU)/menu_preferences.o -CENTRAL_MENU_ALL4 := $(FOLDER_CENTRAL_MENU)/menu_vehicle_data_link.o $(FOLDER_CENTRAL_MENU)/menu_controller_network.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_osd_plugins.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_instruments_general.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_osd_elements.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_osd_plugin.o $(FOLDER_CENTRAL_MENU)/menu_controller_plugins.o $(FOLDER_CENTRAL_MENU)/menu_controller_encryption.o $(FOLDER_CENTRAL_MENU)/menu_search_connect.o $(FOLDER_CENTRAL_MENU)/menu_system_hardware.o $(FOLDER_CENTRAL_MENU)/menu_confirmation_hdmi.o $(FOLDER_CENTRAL_MENU)/menu_controller_recording.o $(FOLDER_CENTRAL_MENU)/menu_system_video_profiles.o $(FOLDER_CENTRAL_MENU)/menu_info_booster.o $(FOLDER_CENTRAL_MENU)/menu_controller_radio_interface.o +CENTRAL_MENU_ALL4 := $(FOLDER_CENTRAL_MENU)/menu_vehicle_data_link.o $(FOLDER_CENTRAL_MENU)/menu_controller_network.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_osd_plugins.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_instruments_general.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_osd_elements.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_osd_plugin.o $(FOLDER_CENTRAL_MENU)/menu_controller_plugins.o $(FOLDER_CENTRAL_MENU)/menu_controller_encryption.o $(FOLDER_CENTRAL_MENU)/menu_search_connect.o $(FOLDER_CENTRAL_MENU)/menu_system_hardware.o $(FOLDER_CENTRAL_MENU)/menu_confirmation_hdmi.o $(FOLDER_CENTRAL_MENU)/menu_controller_recording.o $(FOLDER_CENTRAL_MENU)/menu_system_video_profiles.o $(FOLDER_CENTRAL_MENU)/menu_info_booster.o $(FOLDER_CENTRAL_MENU)/menu_controller_radio_interface.o $(FOLDER_CENTRAL_MENU)/menu_negociate_radio.o CENTRAL_MENU_ALL5 := $(FOLDER_CENTRAL_MENU)/menu_system_dev_stats.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_radio_link.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_radio_interface.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_management_plugins.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_peripherals.o $(FOLDER_CENTRAL_MENU)/menu_confirmation_delete_logs.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_radio.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_cpu_oipc.o $(FOLDER_CENTRAL_MENU)/menu_confirmation_vehicle_board.o CENTRAL_MENU_RC := $(FOLDER_CENTRAL_MENU)/menu_vehicle_rc.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_rc_failsafe.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_rc_channels.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_rc_expo.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_rc_camera.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_rc_input.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_functions.o CENTRAL_MENU_RADIO := $(FOLDER_CENTRAL_MENU)/menu_controller_radio_interface_sik.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_radio_link_sik.o $(FOLDER_CENTRAL_MENU)/menu_diagnose_radio_link.o $(FOLDER_CENTRAL_MENU)/menu_vehicle_radio_link_elrs.o diff --git a/code/base/base.h b/code/base/base.h index c8537b97..1fd17517 100644 --- a/code/base/base.h +++ b/code/base/base.h @@ -30,9 +30,9 @@ typedef u32 __le32; #define SYSTEM_NAME "Ruby" // dword: BB.BB.MM.mm (BB.BB: build number, MM: major ver, mm: minor ver) -#define SYSTEM_SW_VERSION_MAJOR 9 -#define SYSTEM_SW_VERSION_MINOR 80 -#define SYSTEM_SW_BUILD_NUMBER 243 +#define SYSTEM_SW_VERSION_MAJOR 10 +#define SYSTEM_SW_VERSION_MINOR 0 +#define SYSTEM_SW_BUILD_NUMBER 245 #if __BYTE_ORDER == __LITTLE_ENDIAN #define le16_to_cpu(x) (x) diff --git a/code/base/commands.cpp b/code/base/commands.cpp index ec255ad8..a0c254e8 100644 --- a/code/base/commands.cpp +++ b/code/base/commands.cpp @@ -57,6 +57,7 @@ const char* commands_get_description(u8 command_type) case COMMAND_ID_SET_CAMERA_PROFILE: strcpy(szCommandDesc, "Set_Camera_Profile"); break; case COMMAND_ID_SET_CAMERA_PARAMETERS: strcpy(szCommandDesc, "Set_Camera_Params"); break; case COMMAND_ID_SET_CURRENT_CAMERA: strcpy(szCommandDesc, "Set_Current_Camera"); break; + case COMMAND_ID_SET_OVERCLOCKING_PARAMS: strcpy(szCommandDesc, "SetOverclockingParams"); break; case COMMAND_ID_SET_VIDEO_H264_QUANTIZATION: strcpy(szCommandDesc, "Set_VideoH264_Quantization"); break; case COMMAND_ID_FORCE_CAMERA_TYPE: strcpy(szCommandDesc, "Set_Force_Camera_Type"); break; case COMMAND_ID_SET_OSD_CURRENT_LAYOUT: strcpy(szCommandDesc, "Set_Current_OSD_Layout"); break; diff --git a/code/base/commands.h b/code/base/commands.h index cffb043f..8b9943ce 100644 --- a/code/base/commands.h +++ b/code/base/commands.h @@ -198,6 +198,7 @@ typedef struct int overvoltage; // 0 or negative - disabled, negative - default value int freq_arm; // 0 - disabled; in mhz int freq_gpu; // 0 - disabled; in mhz; (0/1 for OIPC boost) + u32 uProcessesFlags; } __attribute__((packed)) command_packet_overclocking_params; diff --git a/code/base/config.h b/code/base/config.h index 9a79badb..fe5a264f 100644 --- a/code/base/config.h +++ b/code/base/config.h @@ -9,6 +9,8 @@ #include "config_video.h" #include "config_timers.h" +#define ALIGN_STRUCT_SPEC_INFO __attribute__((aligned(4))) + //#define DISABLE_ALL_LOGS 1 #define FEATURE_ENABLE_RC 1 #define FEATURE_RELAYING 1 @@ -50,8 +52,8 @@ #define MAX_MCS_INDEX 9 -#define SYSTEM_RT_INFO_UPDATE_INTERVAL_MS 10 -#define SYSTEM_RT_INFO_INTERVALS 200 +#define SYSTEM_RT_INFO_UPDATE_INTERVAL_MS 5 +#define SYSTEM_RT_INFO_INTERVALS 180 #define DEFAULT_TX_TIME_OVERLOAD 500 // milisec diff --git a/code/base/config_obj_names.h b/code/base/config_obj_names.h index c2931c70..727d226e 100644 --- a/code/base/config_obj_names.h +++ b/code/base/config_obj_names.h @@ -12,3 +12,7 @@ #define SEMAPHORE_STOP_VIDEO_RECORD "RUBY_SEM_STOP_VIDEO_REC" #define SEMAPHORE_STOP_RX_RC "RUBY_SEM_STOP_RX_RC" + +#define RUBY_SEM_RX_RADIO_HIGH_PRIORITY "RUBY_SEM_RX_RADIO_HIGH_PRIORITY" +#define RUBY_SEM_RX_RADIO_REG_PRIORITY "RUBY_SEM_RX_RADIO_REG_PRIORITY" + diff --git a/code/base/config_video.h b/code/base/config_video.h index 9d6094db..aef421ec 100644 --- a/code/base/config_video.h +++ b/code/base/config_video.h @@ -5,7 +5,7 @@ // Maximum percent of radio datarate to use for video #define DEFAULT_VIDEO_LINK_MAX_LOAD_PERCENT 90 #define DEFAULT_VIDEO_LINK_LOAD_PERCENT 80 -#define DEFAULT_LOWER_VIDEO_RADIO_DATARATE_AFTER_MS 80 +#define DEFAULT_LOWER_VIDEO_RADIO_DATARATE_AFTER_MS 50 #define DEFAULT_MINIMUM_OK_INTERVAL_MS_TO_SWITCH_VIDEO_PROFILE_UP 1500 #define DEFAULT_VIDEO_PARAMS_ADJUSTMENT_STRENGTH 5 @@ -18,6 +18,7 @@ #define MAX_VIDEO_PACKET_DATA_SIZE 1150 #define DEFAULT_VIDEO_KEYFRAME 200 // milisec +#define DEFAULT_VIDEO_KEYFRAME_OIPC_SIGMASTAR 3000 #define DEFAULT_VIDEO_KEYFRAME_OIPC_GOKE 330 #define DEFAULT_VIDEO_MAX_AUTO_KEYFRAME_INTERVAL 6000 // in miliseconds #define DEFAULT_VIDEO_MIN_AUTO_KEYFRAME_INTERVAL 100 // in miliseconds @@ -39,14 +40,16 @@ //#define DEFAULT_VIDEO_HEIGHT 576 #define DEFAULT_VIDEO_FPS 30 #define DEFAULT_VIDEO_FPS_OIPC 60 +#define DEFAULT_VIDEO_FPS_OIPC_SIGMASTAR 120 #define DEFAULT_LOWEST_ALLOWED_ADAPTIVE_VIDEO_BITRATE 1000000 -#define DEFAULT_VIDEO_BITRATE 7000000 // in bps -#define DEFAULT_VIDEO_BITRATE_PI_ZERO 4500000 // in bps +#define DEFAULT_VIDEO_BITRATE 6000000 // in bps +#define DEFAULT_VIDEO_BITRATE_PI_ZERO 4000000 // in bps +#define DEFAULT_VIDEO_BITRATE_OPIC_SIGMASTAR 7000000 #define DEFAULT_VIDEO_BLOCK_PACKETS_HP 9 -#define DEFAULT_VIDEO_BLOCK_FECS_HP 3 -#define DEFAULT_VIDEO_DATA_LENGTH_HP 1150 +#define DEFAULT_VIDEO_BLOCK_FECS_HP 2 +#define DEFAULT_VIDEO_DATA_LENGTH_HP 1180 #define DEFAULT_VIDEO_BLOCK_PACKETS_HQ 9 #define DEFAULT_VIDEO_BLOCK_FECS_HQ 3 @@ -54,19 +57,17 @@ //#define DEFAULT_HP_VIDEO_RADIO_DATARATE 12000000 #define DEFAULT_HP_VIDEO_RADIO_DATARATE 0 // Auto -#define DEFAULT_HP_VIDEO_BITRATE 7000000 +#define DEFAULT_HP_VIDEO_BITRATE 5000000 #define DEFAULT_MQ_VIDEO_BITRATE 4000000 // in bps #define DEFAULT_MQ_VIDEO_BLOCK_PACKETS 9 #define DEFAULT_MQ_VIDEO_BLOCK_FECS 3 #define DEFAULT_MQ_VIDEO_DATA_LENGTH 1150 -#define DEFAULT_MQ_VIDEO_FPS 30 #define DEFAULT_LQ_VIDEO_BITRATE 2500000 // in bps #define DEFAULT_LQ_VIDEO_BLOCK_PACKETS 6 #define DEFAULT_LQ_VIDEO_BLOCK_FECS 3 #define DEFAULT_LQ_VIDEO_DATA_LENGTH 1150 -#define DEFAULT_LQ_VIDEO_FPS 30 #define MAX_BUFFERED_AUDIO_PACKETS 32 diff --git a/code/base/controller_rt_info.cpp b/code/base/controller_rt_info.cpp index 6b71285c..41224384 100644 --- a/code/base/controller_rt_info.cpp +++ b/code/base/controller_rt_info.cpp @@ -127,7 +127,7 @@ controller_runtime_info_vehicle* controller_rt_info_get_vehicle_info(controller_ return NULL; } -void controller_rt_info_update_ack_rt_time(controller_runtime_info* pRTInfo, u32 uVehicleId, u32 uRoundTripTime) +void controller_rt_info_update_ack_rt_time(controller_runtime_info* pRTInfo, u32 uVehicleId, int iRadioLink, u32 uRoundTripTime) { if ( (NULL == pRTInfo) || (0 == uVehicleId) || (MAX_U32 == uVehicleId) ) return; @@ -136,14 +136,22 @@ void controller_rt_info_update_ack_rt_time(controller_runtime_info* pRTInfo, u32 if ( NULL == pRTInfoVehicle ) return; - if ( 0 == pRTInfoVehicle->uMinAckTime[pRTInfo->iCurrentIndex] ) - pRTInfoVehicle->uMinAckTime[pRTInfo->iCurrentIndex] = (u8)uRoundTripTime; - else if ( (u8)uRoundTripTime < pRTInfoVehicle->uMinAckTime[pRTInfo->iCurrentIndex] ) - pRTInfoVehicle->uMinAckTime[pRTInfo->iCurrentIndex] = (u8)uRoundTripTime; - if ( 0 == pRTInfoVehicle->uMaxAckTime[pRTInfo->iCurrentIndex] ) - pRTInfoVehicle->uMaxAckTime[pRTInfo->iCurrentIndex] = (u8)uRoundTripTime; - else if ( (u8)uRoundTripTime > pRTInfoVehicle->uMaxAckTime[pRTInfo->iCurrentIndex] ) - pRTInfoVehicle->uMaxAckTime[pRTInfo->iCurrentIndex] = (u8)uRoundTripTime; + if ( (iRadioLink >= 0) && (iRadioLink < MAX_RADIO_INTERFACES) ) + { + pRTInfoVehicle->iAckTimeIndex[iRadioLink]++; + if ( pRTInfoVehicle->iAckTimeIndex[iRadioLink] >= SYSTEM_RT_INFO_INTERVALS/4 ) + pRTInfoVehicle->iAckTimeIndex[iRadioLink] = 0; + pRTInfoVehicle->uAckTimes[pRTInfoVehicle->iAckTimeIndex[iRadioLink]][iRadioLink] = uRoundTripTime; + } + + if ( 0 == pRTInfoVehicle->uMinAckTime[pRTInfo->iCurrentIndex][iRadioLink] ) + pRTInfoVehicle->uMinAckTime[pRTInfo->iCurrentIndex][iRadioLink] = (u8)uRoundTripTime; + else if ( (u8)uRoundTripTime < pRTInfoVehicle->uMinAckTime[pRTInfo->iCurrentIndex][iRadioLink] ) + pRTInfoVehicle->uMinAckTime[pRTInfo->iCurrentIndex][iRadioLink] = (u8)uRoundTripTime; + if ( 0 == pRTInfoVehicle->uMaxAckTime[pRTInfo->iCurrentIndex][iRadioLink] ) + pRTInfoVehicle->uMaxAckTime[pRTInfo->iCurrentIndex][iRadioLink] = (u8)uRoundTripTime; + else if ( (u8)uRoundTripTime > pRTInfoVehicle->uMaxAckTime[pRTInfo->iCurrentIndex][iRadioLink] ) + pRTInfoVehicle->uMaxAckTime[pRTInfo->iCurrentIndex][iRadioLink] = (u8)uRoundTripTime; } int controller_rt_info_will_advance_index(controller_runtime_info* pRTInfo, u32 uTimeNowMs) @@ -193,6 +201,8 @@ int controller_rt_info_check_advance_index(controller_runtime_info* pRTInfo, u32 for( int i=0; iuRxVideoPackets[iIndex][i] = 0; + pRTInfo->uRxVideoECPackets[iIndex][i] = 0; + pRTInfo->uRxVideoRetrPackets[iIndex][i] = 0; pRTInfo->uRxDataPackets[iIndex][i] = 0; pRTInfo->uRxMissingPackets[iIndex][i] = 0; pRTInfo->uRxMissingPacketsMaxGap[iIndex][i] = 0; @@ -205,8 +215,11 @@ int controller_rt_info_check_advance_index(controller_runtime_info* pRTInfo, u32 for( int i=0; ivehicles[i].uMinAckTime[iIndex] = 0; - pRTInfo->vehicles[i].uMaxAckTime[iIndex] = 0; + for( int k=0; kvehicles[i].uMinAckTime[iIndex][k] = 0; + pRTInfo->vehicles[i].uMaxAckTime[iIndex][k] = 0; + } pRTInfo->vehicles[i].uCountReqRetransmissions[iIndex] = 0; pRTInfo->vehicles[i].uCountAckRetransmissions[iIndex] = 0; } diff --git a/code/base/controller_rt_info.h b/code/base/controller_rt_info.h index 0a8955d0..0966860d 100644 --- a/code/base/controller_rt_info.h +++ b/code/base/controller_rt_info.h @@ -29,17 +29,19 @@ typedef struct int iDbmNoiseMin[MAX_RADIO_ANTENNAS]; int iDbmNoiseMax[MAX_RADIO_ANTENNAS]; int iDbmNoiseAvg[MAX_RADIO_ANTENNAS]; -} __attribute__((packed)) controller_runtime_info_radio_interface_rx_signal; +} ALIGN_STRUCT_SPEC_INFO controller_runtime_info_radio_interface_rx_signal; typedef struct { u32 uVehicleId; - u8 uMinAckTime[SYSTEM_RT_INFO_INTERVALS]; - u8 uMaxAckTime[SYSTEM_RT_INFO_INTERVALS]; + u8 uMinAckTime[SYSTEM_RT_INFO_INTERVALS][MAX_RADIO_INTERFACES]; + u8 uMaxAckTime[SYSTEM_RT_INFO_INTERVALS][MAX_RADIO_INTERFACES]; u8 uCountReqRetransmissions[SYSTEM_RT_INFO_INTERVALS]; u8 uCountAckRetransmissions[SYSTEM_RT_INFO_INTERVALS]; -} __attribute__((packed)) controller_runtime_info_vehicle; + u8 uAckTimes[SYSTEM_RT_INFO_INTERVALS][MAX_RADIO_INTERFACES]; + int iAckTimeIndex[MAX_RADIO_INTERFACES]; +} ALIGN_STRUCT_SPEC_INFO controller_runtime_info_vehicle; typedef struct { @@ -49,6 +51,8 @@ typedef struct int iDeltaIndexFromVehicle; u32 uSliceUpdateTime[SYSTEM_RT_INFO_INTERVALS]; u8 uRxVideoPackets[SYSTEM_RT_INFO_INTERVALS][MAX_RADIO_INTERFACES]; + u8 uRxVideoECPackets[SYSTEM_RT_INFO_INTERVALS][MAX_RADIO_INTERFACES]; + u8 uRxVideoRetrPackets[SYSTEM_RT_INFO_INTERVALS][MAX_RADIO_INTERFACES]; u8 uRxDataPackets[SYSTEM_RT_INFO_INTERVALS][MAX_RADIO_INTERFACES]; u8 uRxMissingPackets[SYSTEM_RT_INFO_INTERVALS][MAX_RADIO_INTERFACES]; u8 uRxMissingPacketsMaxGap[SYSTEM_RT_INFO_INTERVALS][MAX_RADIO_INTERFACES]; @@ -72,7 +76,7 @@ typedef struct u32 uFlagsAdaptiveVideo[SYSTEM_RT_INFO_INTERVALS]; controller_runtime_info_vehicle vehicles[MAX_CONCURENT_VEHICLES]; -} __attribute__((packed)) controller_runtime_info; +} ALIGN_STRUCT_SPEC_INFO controller_runtime_info; controller_runtime_info* controller_rt_info_open_for_read(); @@ -80,7 +84,7 @@ controller_runtime_info* controller_rt_info_open_for_write(); void controller_rt_info_close(controller_runtime_info* pAddress); void controller_rt_info_init(controller_runtime_info* pRTInfo); controller_runtime_info_vehicle* controller_rt_info_get_vehicle_info(controller_runtime_info* pRTInfo, u32 uVehicleId); -void controller_rt_info_update_ack_rt_time(controller_runtime_info* pRTInfo, u32 uVehicleId, u32 uRoundTripTime); +void controller_rt_info_update_ack_rt_time(controller_runtime_info* pRTInfo, u32 uVehicleId, int iRadioLink, u32 uRoundTripTime); int controller_rt_info_will_advance_index(controller_runtime_info* pRTInfo, u32 uTimeNowMs); int controller_rt_info_check_advance_index(controller_runtime_info* pRTInfo, u32 uTimeNowMs); #ifdef __cplusplus diff --git a/code/base/ctrl_preferences.c b/code/base/ctrl_preferences.c index 4d5a1b01..149fcbf2 100644 --- a/code/base/ctrl_preferences.c +++ b/code/base/ctrl_preferences.c @@ -64,9 +64,6 @@ void reset_Preferences() s_Preferences.iShowControllerCPUInfo = 1; s_Preferences.iShowBigRecordButton = 0; s_Preferences.iSwapUpDownButtons = 0; - #ifdef HW_PLATFORM_RASPBERRY - s_Preferences.iSwapUpDownButtons = 1; - #endif s_Preferences.iSwapUpDownButtonsValues = 0; s_Preferences.iAHIToSides = 0; @@ -79,10 +76,10 @@ void reset_Preferences() s_Preferences.iColorOSD[1] = 250; s_Preferences.iColorOSD[2] = 220; s_Preferences.iColorOSD[3] = 100; // 100% - s_Preferences.iColorOSDOutline[0] = 185; - s_Preferences.iColorOSDOutline[1] = 185; - s_Preferences.iColorOSDOutline[2] = 155; - s_Preferences.iColorOSDOutline[3] = 70; // 70% + s_Preferences.iColorOSDOutline[0] = 10; + s_Preferences.iColorOSDOutline[1] = 10; + s_Preferences.iColorOSDOutline[2] = 10; + s_Preferences.iColorOSDOutline[3] = 90; // 90% s_Preferences.iColorAHI[0] = 208; s_Preferences.iColorAHI[1] = 238; s_Preferences.iColorAHI[2] = 214; @@ -119,7 +116,8 @@ void reset_Preferences() CTRL_RT_DEBUG_INFO_FLAG_SHOW_RX_H264265_FRAMES | CTRL_RT_DEBUG_INFO_FLAG_SHOW_RX_DBM | CTRL_RT_DEBUG_INFO_FLAG_SHOW_RX_MISSING_PACKETS_MAX_GAP | - CTRL_RT_DEBUG_INFO_FLAG_SHOW_MIN_MAX_ACK_TIME | + //CTRL_RT_DEBUG_INFO_FLAG_SHOW_MIN_MAX_ACK_TIME | + CTRL_RT_DEBUG_INFO_FLAG_SHOW_ACK_TIME_HISTORY | CTRL_RT_DEBUG_INFO_FLAG_SHOW_RX_VIDEO_MAX_EC_USED | CTRL_RT_DEBUG_INFO_FLAG_SHOW_RX_VIDEO_UNRECOVERABLE_BLOCKS | CTRL_RT_DEBUG_INFO_FLAG_SHOW_VIDEO_PROFILE_CHANGES; @@ -359,7 +357,8 @@ int load_Preferences() s_Preferences.iDebugStatsQAButton = 0; s_Preferences.uDebugStatsFlags = CTRL_RT_DEBUG_INFO_FLAG_SHOW_RX_VIDEO_DATA_PACKETS | CTRL_RT_DEBUG_INFO_FLAG_SHOW_RX_MISSING_PACKETS_MAX_GAP | - CTRL_RT_DEBUG_INFO_FLAG_SHOW_MIN_MAX_ACK_TIME | + //CTRL_RT_DEBUG_INFO_FLAG_SHOW_MIN_MAX_ACK_TIME | + CTRL_RT_DEBUG_INFO_FLAG_SHOW_ACK_TIME_HISTORY | CTRL_RT_DEBUG_INFO_FLAG_SHOW_RX_VIDEO_MAX_EC_USED; } diff --git a/code/base/ctrl_preferences.h b/code/base/ctrl_preferences.h index 6429793e..7fa38ebd 100644 --- a/code/base/ctrl_preferences.h +++ b/code/base/ctrl_preferences.h @@ -17,6 +17,7 @@ extern "C" { #define CTRL_RT_DEBUG_INFO_FLAG_SHOW_RX_VIDEO_UNRECOVERABLE_BLOCKS ((u32)(((u32)0x01)<<10)) #define CTRL_RT_DEBUG_INFO_FLAG_SHOW_VIDEO_PROFILE_CHANGES ((u32)(((u32)0x01)<<11)) #define CTRL_RT_DEBUG_INFO_FLAG_SHOW_VIDEO_RETRANSMISSIONS ((u32)(((u32)0x01)<<12)) +#define CTRL_RT_DEBUG_INFO_FLAG_SHOW_ACK_TIME_HISTORY ((u32)(((u32)0x01)<<13)) typedef enum diff --git a/code/base/flags.h b/code/base/flags.h index 3c4ba127..48710f8c 100644 --- a/code/base/flags.h +++ b/code/base/flags.h @@ -120,6 +120,7 @@ // Used on radioLinksParams.uGlobalRadioLinksFlags : #define MODEL_RADIOLINKS_FLAGS_DOWNLINK_ONLY ((u32)(((u32)0x01))) #define MODEL_RADIOLINKS_FLAGS_BYPASS_SOCKETS_BUFFERS ((u32)(((u32)0x02))) +#define MODEL_RADIOLINKS_FLAGS_HAS_NEGOCIATED_LINKS ((u32)(((u32)0x04))) // Used on uDeveloperFlags : #define DEVELOPER_FLAGS_BIT_LIVE_LOG ((u32)(((u32)0x01))) diff --git a/code/base/flags_osd.h b/code/base/flags_osd.h index 1ee0111f..863c23ba 100644 --- a/code/base/flags_osd.h +++ b/code/base/flags_osd.h @@ -31,6 +31,7 @@ #define OSD_FLAG_SHOW_FLIGHT_MODE ((u32)(((u32)0x01)<<28)) #define OSD_FLAG_SHOW_TIME ((u32)(((u32)0x01)<<29)) #define OSD_FLAG_SHOW_RADIO_INTERFACES_INFO ((u32)(((u32)0x01)<<30)) +#define OSD_FLAG_SHOW_FLIGHT_MODE_CHANGE ((u32)(((u32)0x01)<<31)) #define OSD_FLAG2_SHOW_BGBARS ((u32)(((u32)0x01))) #define OSD_FLAG2_SHOW_BATTERY_CELLS ((u32)(((u32)0x01)<<1)) @@ -83,6 +84,8 @@ #define OSD_FLAG3_RENDER_MSP_OSD ((u32)(((u32)0x01)<<17)) +#define OSD_PREFERENCES_OSD_TRANSPARENCY_BITMASK ((u32)0x0000FF00) +#define OSD_PREFERENCES_OSD_TRANSPARENCY_SHIFT 8 #define OSD_PREFERENCES_BIT_FLAG_SHOW_CONTROLLER_LINK_LOST_ALARM ((u32)(((u32)0x01)<<24)) #define OSD_PREFERENCES_BIT_FLAG_ARANGE_STATS_WINDOWS_TOP ((u32)(((u32)0x01)<<25)) #define OSD_PREFERENCES_BIT_FLAG_ARANGE_STATS_WINDOWS_BOTTOM ((u32)(((u32)0x01)<<26)) diff --git a/code/base/gpio.c b/code/base/gpio.c index e481cabf..59d8f883 100644 --- a/code/base/gpio.c +++ b/code/base/gpio.c @@ -77,6 +77,7 @@ int _gpio_reverse_find_pin_mapping(int iGPIOPin) log_line("[GPIO] Mapped PIN_%d to Radxa pin %d", iGPIOPin, iNewPin); return iNewPin; #endif + return -1; } void _gpio_load_custom_mapping() diff --git a/code/base/hardware.cpp b/code/base/hardware.cpp index e5af3864..53545bfb 100644 --- a/code/base/hardware.cpp +++ b/code/base/hardware.cpp @@ -422,11 +422,21 @@ void hardware_reboot() { hardware_sleep_ms(200); hw_execute_bash_command("sync", NULL); - hardware_sleep_sec(2); - hw_execute_bash_command("reboot -f", NULL); + hardware_sleep_sec(1); + hw_execute_bash_command("reboot", NULL); + int iCounter = 0; + // reboot -f --no-wall + // reboot --reboot --no-wall while (1) { - hardware_sleep_ms(100); + hardware_sleep_ms(500); + iCounter++; + + if ( iCounter > 10 ) + { + hw_execute_bash_command("reboot -f", NULL); + iCounter = 0; + } } } @@ -511,6 +521,10 @@ u32 hardware_getOnlyBoardType() #ifdef HW_PLATFORM_RADXA_ZERO3 s_uHardwareBoardType = BOARD_TYPE_RADXA_ZERO3; + char szOutput[1024]; + hw_execute_bash_command_raw("uname -a", szOutput); + if ( NULL != strstr(szOutput, "radxa3c") ) + s_uHardwareBoardType = BOARD_TYPE_RADXA_3C; #endif #ifdef HW_PLATFORM_OPENIPC_CAMERA @@ -1695,6 +1709,7 @@ int hardware_has_eth() int nHasETH = 1; char szOutput[1024]; + szOutput[0] = 0; #if defined(HW_PLATFORM_RASPBERRY) hw_execute_bash_command_raw("ip link | grep eth0 2>&1", szOutput); #endif @@ -1705,7 +1720,7 @@ int hardware_has_eth() hw_execute_bash_command_raw("ip link | grep eth0 2>&1", szOutput); #endif - if ( 0 == szOutput[0] || NULL != strstr(szOutput, "not found") ) + if ( (0 == szOutput[0]) || (NULL != strstr(szOutput, "not found")) ) { nHasETH = 0; szOutput[1023] = 0; @@ -1881,16 +1896,104 @@ void hardware_set_oipc_gpu_boost(int iGPUBoost) #if defined (HW_PLATFORM_OPENIPC_CAMERA) if ( hardware_board_is_sigmastar(hardware_getOnlyBoardType() & BOARD_TYPE_MASK) ) { - if ( 1 == iGPUBoost ) + if ( 0 == iGPUBoost ) { hw_execute_bash_command_raw("echo 384000000 > /sys/venc/ven_clock", NULL); hw_execute_bash_command_raw("echo 320000000 > /sys/venc/ven_clock_2nd", NULL); } - else + else if ( 1 == iGPUBoost ) + { + hw_execute_bash_command_raw("echo 432000000 > /sys/venc/ven_clock", NULL); + hw_execute_bash_command_raw("echo 336000000 > /sys/venc/ven_clock_2nd", NULL); + } + else if ( 2 == iGPUBoost ) { hw_execute_bash_command_raw("echo 480000000 > /sys/venc/ven_clock", NULL); - hw_execute_bash_command_raw("echo 384000000 > /sys/venc/ven_clock_2nd", NULL); + hw_execute_bash_command_raw("echo 348000000 > /sys/venc/ven_clock_2nd", NULL); } + else + { + int iCore1 = (iGPUBoost/10)%10; + int iCore2 = (iGPUBoost/100)%10; + int iFreq1 = 0; + int iFreq2 = 0; + switch(iCore1) + { + case 0: iFreq1 = 384; break; + case 1: iFreq1 = 432; break; + case 2: iFreq1 = 480; break; + default: break; + } + switch(iCore2) + { + case 0: iFreq2 = 320; break; + case 1: iFreq2 = 336; break; + case 2: iFreq2 = 348; break; + case 3: iFreq2 = 384; break; + default: break; + } + if ( iFreq1 > 0 ) + { + char szComm[256]; + sprintf(szComm, "echo %d > /sys/venc/ven_clock", iFreq1*1000*1000); + hw_execute_bash_command_raw(szComm, NULL); + } + if ( iFreq2 > 0 ) + { + char szComm[256]; + sprintf(szComm, "echo %d > /sys/venc/ven_clock_2nd", iFreq2*1000*1000); + hw_execute_bash_command_raw(szComm, NULL); + } + } + } + #endif +} + +void _hardware_set_int_affinity_core(char* szIntName, int iCoreIndex) +{ + if ( (NULL == szIntName) || (0 == szIntName[0]) || (iCoreIndex < 0) ) + return; + + char szComm[256]; + char szOutput[256]; + szOutput[0] = 0; + sprintf(szComm, "cat /proc/interrupts | grep %s", szIntName); + hw_execute_bash_command(szComm, szOutput); + for( int i=0; i<(int)strlen(szOutput); i++ ) + { + if ( ! isdigit(szOutput[i]) ) + szOutput[i] = ' '; } + int iIntNumber = -1; + if ( 1 != sscanf(szOutput, "%d", &iIntNumber) ) + iIntNumber = -1; + + if ( -1 == iIntNumber ) + { + log_softerror_and_alarm("Can't find interrupt number for interrupt: (%s)", szIntName); + return; + } + + sprintf(szComm, "echo %d > /proc/irq/%d/smp_affinity", iCoreIndex+1, iIntNumber); + hw_execute_bash_command(szComm, NULL); +} + +void hardware_balance_interupts() +{ + #if defined (HW_PLATFORM_OPENIPC_CAMERA) + char szOutput[128]; + hw_execute_bash_command_raw("nproc", szOutput); + int iCPUCoresCount = 1; + if ( 1 != sscanf(szOutput, "%d", &iCPUCoresCount) ) + iCPUCoresCount = 1; + if ( iCPUCoresCount > 1 ) + { + log_line("Balancing interrupts..."); + _hardware_set_int_affinity_core(":usb", 1); + _hardware_set_int_affinity_core("ms_serial", 1); + _hardware_set_int_affinity_core("ms_serial_dma", 1); + } + else + log_line("Can't balance interrupts: single core CPU."); #endif } \ No newline at end of file diff --git a/code/base/hardware.h b/code/base/hardware.h index 38392adc..19f37080 100644 --- a/code/base/hardware.h +++ b/code/base/hardware.h @@ -41,6 +41,7 @@ #define BOARD_TYPE_RADXA_ZERO3 60 +#define BOARD_TYPE_RADXA_3C 61 #define CAMERA_TYPE_NONE 0 @@ -162,7 +163,7 @@ int hardware_set_audio_output(int iAudioDevice, int iAudioVolume); void hardware_set_oipc_freq_boost(int iFreqCPUMhz, int iGPUBoost); void hardware_set_oipc_gpu_boost(int iGPUBoost); - +void hardware_balance_interupts(); #ifdef __cplusplus } #endif \ No newline at end of file diff --git a/code/base/hardware_camera.cpp b/code/base/hardware_camera.cpp index 6a8c5f96..e0acb9db 100644 --- a/code/base/hardware_camera.cpp +++ b/code/base/hardware_camera.cpp @@ -341,7 +341,7 @@ int hardware_isCameraHDMI() } -void hardware_camera_apply_all_majestic_camera_settings(camera_profile_parameters_t* pCameraParams, bool bForceUpdate) +void hardware_camera_apply_all_majestic_camera_settings(camera_profile_parameters_t* pCameraParams) { if ( NULL == pCameraParams ) { @@ -362,6 +362,14 @@ void hardware_camera_apply_all_majestic_camera_settings(camera_profile_parameter sprintf(szComm, "cli -s .image.hue %d", pCameraParams->hue); hw_execute_bash_command_raw(szComm, NULL); + if ( pCameraParams->uFlags & CAMERA_FLAG_OPENIPC_3A_SIGMASTAR ) + hw_execute_bash_command_raw("cli -s .fpv.enabled true", NULL); + else + { + hw_execute_bash_command_raw("cli -s .fpv.enabled false", NULL); + hw_execute_bash_command_raw("cli -d .fpv.enabled", NULL); + } + if ( pCameraParams->flip_image ) { hw_execute_bash_command_raw("cli -s .image.flip true", NULL); @@ -391,9 +399,6 @@ void hardware_camera_apply_all_majestic_camera_settings(camera_profile_parameter hardware_camera_set_irfilter_off(pCameraParams->uFlags & CAMERA_FLAG_IR_FILTER_OFF); hardware_camera_set_daylight_off(pCameraParams->uFlags & CAMERA_FLAG_OPENIPC_DAYLIGHT_OFF); - - if ( bForceUpdate ) - hw_execute_bash_command_raw("killall -1 majestic", NULL); } void hardware_camera_apply_all_majestic_settings(Model* pModel, camera_profile_parameters_t* pCameraParams, int iVideoProfile, video_parameters_t* pVideoParams) @@ -437,7 +442,7 @@ void hardware_camera_apply_all_majestic_settings(Model* pModel, camera_profile_p sprintf(szComm, "cli -s .video0.gopSize %.1f", fGOP); hw_execute_bash_command_raw(szComm, NULL); - hardware_camera_apply_all_majestic_camera_settings(pCameraParams, true); + hardware_camera_apply_all_majestic_camera_settings(pCameraParams); } void hardware_camera_set_irfilter_off(int iOff) diff --git a/code/base/hardware_camera.h b/code/base/hardware_camera.h index 465055cf..08d6b1fd 100644 --- a/code/base/hardware_camera.h +++ b/code/base/hardware_camera.h @@ -12,7 +12,7 @@ int hardware_isCameraVeye(); int hardware_isCameraVeye307(); int hardware_isCameraHDMI(); -void hardware_camera_apply_all_majestic_camera_settings(camera_profile_parameters_t* pCameraParams, bool bForceUpdate); +void hardware_camera_apply_all_majestic_camera_settings(camera_profile_parameters_t* pCameraParams); void hardware_camera_apply_all_majestic_settings(Model* pModel, camera_profile_parameters_t* pCameraParams, int iVideoProfile, video_parameters_t* pVideoParams); void hardware_camera_set_irfilter_off(int iOff); void hardware_camera_set_daylight_off(int iDLOff); diff --git a/code/base/hardware_i2c.c b/code/base/hardware_i2c.c index db26676d..a3c3565f 100644 --- a/code/base/hardware_i2c.c +++ b/code/base/hardware_i2c.c @@ -69,6 +69,7 @@ void hardware_i2c_log_devices() if ( ! s_iI2CDeviceSettingsLoaded ) hardware_i2c_load_device_settings(); + log_line("[Hardware] -----------------------------------------"); log_line("[Hardware] I2C Buses: %d", s_iHardwareI2CBusCount); char szBuff[256]; @@ -98,6 +99,7 @@ void hardware_i2c_log_devices() log_line("[Hardware] %d I2C devices on bus %d (%d %s): %s", iCount, i, s_HardwareI2CBusInfo[i].nBusNumber, s_HardwareI2CBusInfo[i].szName, szBuff); } + log_line("[Hardware] -----------------------------------------"); log_line("[Hardware] Settings stored for %d I2C devices:", s_iCountI2CDevicesSettings); szBuff[0] = 0; for( int i=0; i 0 ) if ( s_listI2CDevicesSettings[i].nI2CAddress > 0 ) { + szBuff[0] = 0; char szTmp[32]; if ( 0 == szBuff[0] ) sprintf(szTmp, "%d-%d", s_listI2CDevicesSettings[i].nI2CAddress, s_listI2CDevicesSettings[i].nDeviceType); else sprintf(szTmp, ", %d-%d", s_listI2CDevicesSettings[i].nI2CAddress, s_listI2CDevicesSettings[i].nDeviceType); + strcat(szBuff, szTmp); + log_line("[Hardware] Dev I2CAddress/Type: %s, Custom params: %d %d %d", szBuff, s_listI2CDevicesSettings[i].uParams[0], s_listI2CDevicesSettings[i].uParams[1], s_listI2CDevicesSettings[i].uParams[2]); } } - if ( 0 < s_iCountI2CDevicesSettings ) - log_line("[Hardware] Dev I2CAddress/Type: %s", szBuff); + log_line("[Hardware] -----------------------------------------"); } void hardware_enumerate_i2c_busses() diff --git a/code/base/hardware_i2c.h b/code/base/hardware_i2c.h index 9b13960c..d0274dab 100644 --- a/code/base/hardware_i2c.h +++ b/code/base/hardware_i2c.h @@ -1,5 +1,6 @@ #pragma once #include "../public/i2c_protocols.h" +#include "config.h" #define MAX_I2C_BUS_COUNT 10 #define MAX_I2C_BUS_NAME 32 @@ -60,7 +61,7 @@ typedef struct char szName[MAX_I2C_BUS_NAME]; u8 devices[128]; // 0 - none, 1 - detected u8 picoExtenderVersion; -} __attribute__((packed)) hw_i2c_bus_info_t; +} ALIGN_STRUCT_SPEC_INFO hw_i2c_bus_info_t; typedef struct @@ -73,7 +74,7 @@ typedef struct int bEnabled; u32 uCapabilitiesFlags; u32 uParams[MAX_I2C_DEVICE_SETTINGS]; -} t_i2c_device_settings; +} ALIGN_STRUCT_SPEC_INFO t_i2c_device_settings; void hardware_i2c_reset_enumerated_flag(); diff --git a/code/base/hardware_radio.c b/code/base/hardware_radio.c index 16aa0cb4..813b3dd3 100644 --- a/code/base/hardware_radio.c +++ b/code/base/hardware_radio.c @@ -64,6 +64,8 @@ static int s_iHwRadiosCount = 0; static int s_iHwRadiosSupportedCount = 0; static int s_HardwareRadiosEnumeratedOnce = 0; +int g_ArrayTestRadioRates[] = {18000000, 24000000, 36000000, 48000000, -1, -2, -3, -4, -5, -6, -7, -8}; +int g_ArrayTestRadioRatesCount = 12; void reset_runtime_radio_rx_info(type_runtime_radio_rx_info* pRuntimeRadioRxInfo) { @@ -1401,6 +1403,13 @@ int hardware_radio_is_wifi_radio(radio_hw_info_t* pRadioInfo) return 0; } +int hardware_radio_is_index_wifi_radio(int iRadioIndex) +{ + if ( (iRadioIndex <0) || (iRadioIndex >= s_iHwRadiosCount) ) + return 0; + return hardware_radio_is_wifi_radio(&sRadioInfo[iRadioIndex]); +} + int hardware_radio_is_serial_radio(radio_hw_info_t* pRadioInfo) { if ( NULL == pRadioInfo ) diff --git a/code/base/hardware_radio.h b/code/base/hardware_radio.h index 9deeee77..0edd290e 100644 --- a/code/base/hardware_radio.h +++ b/code/base/hardware_radio.h @@ -70,6 +70,9 @@ #define RADIO_HW_EXTRA_FLAG_FIRMWARE_OLD ((u32)(((u32)0x01))) +extern int g_ArrayTestRadioRates[]; +extern int g_ArrayTestRadioRatesCount; + #ifdef __cplusplus extern "C" { #endif @@ -91,7 +94,7 @@ typedef struct int nDbmNoiseAvg[MAX_RADIO_ANTENNAS]; int nDbmNoiseMin[MAX_RADIO_ANTENNAS]; int nDbmNoiseMax[MAX_RADIO_ANTENNAS]; -} __attribute__((packed)) type_runtime_radio_rx_info; +} ALIGN_STRUCT_SPEC_INFO type_runtime_radio_rx_info; void reset_runtime_radio_rx_info(type_runtime_radio_rx_info* pRuntimeRadioRxInfo); @@ -106,7 +109,7 @@ typedef struct int nPort; int iErrorCount; type_runtime_radio_rx_info radioHwRxInfo; -} type_runtime_radio_interface_info; +} ALIGN_STRUCT_SPEC_INFO type_runtime_radio_interface_info; typedef struct @@ -138,7 +141,7 @@ typedef struct int openedForWrite; type_runtime_radio_interface_info runtimeInterfaceInfoRx; type_runtime_radio_interface_info runtimeInterfaceInfoTx; -} radio_hw_info_t; +} ALIGN_STRUCT_SPEC_INFO radio_hw_info_t; typedef struct @@ -150,7 +153,7 @@ typedef struct u8 slot_time; u8 thresh62; u32 extraInfo; // not used, for future use; -} __attribute__((packed)) radio_info_wifi_t; +} ALIGN_STRUCT_SPEC_INFO radio_info_wifi_t; typedef struct @@ -161,7 +164,7 @@ typedef struct char szProductId[32]; char szName[64]; int iDriver; -} usb_radio_interface_info_t; +} ALIGN_STRUCT_SPEC_INFO usb_radio_interface_info_t; void hardware_save_radio_info(); @@ -189,6 +192,7 @@ const char* hardware_get_radio_name(int iRadioIndex); const char* hardware_get_radio_description(int iRadioIndex); int hardware_radio_is_wifi_radio(radio_hw_info_t* pRadioInfo); +int hardware_radio_is_index_wifi_radio(int iRadioIndex); int hardware_radio_is_serial_radio(radio_hw_info_t* pRadioInfo); int hardware_radio_is_elrs_radio(radio_hw_info_t* pRadioInfo); int hardware_radio_is_sik_radio(radio_hw_info_t* pRadioInfo); diff --git a/code/base/hardware_serial.h b/code/base/hardware_serial.h index 6d841884..c43d995b 100644 --- a/code/base/hardware_serial.h +++ b/code/base/hardware_serial.h @@ -1,4 +1,5 @@ #pragma once +#include "config.h" #ifdef __cplusplus extern "C" { @@ -26,7 +27,7 @@ typedef struct long lPortSpeed; int iPortUsage; int iSupported; -} __attribute__((packed)) hw_serial_port_info_t; +} ALIGN_STRUCT_SPEC_INFO hw_serial_port_info_t; int* hardware_get_serial_baud_rates(); diff --git a/code/base/hdmi.c b/code/base/hdmi.cpp similarity index 99% rename from code/base/hdmi.c rename to code/base/hdmi.cpp index 260f998b..2c269071 100644 --- a/code/base/hdmi.c +++ b/code/base/hdmi.cpp @@ -2,6 +2,7 @@ #include "base.h" #include "config.h" #include "hardware.h" +#include "hardware_files.h" #include "hw_procs.h" #include #include @@ -18,6 +19,7 @@ #define HDMI_ASPECT_MODE_16_10 2 #define HDMI_ASPECT_MODE_21_9 3 +void hardware_mount_root(); int s_nHDMI_CurrentResolutionIndex = -1; int s_nHDMI_CurrentResolutionRefreshIndex = -1; diff --git a/code/base/hw_procs.c b/code/base/hw_procs.c index 5ff54197..01f8536b 100644 --- a/code/base/hw_procs.c +++ b/code/base/hw_procs.c @@ -14,13 +14,18 @@ int hw_process_exists(const char* szProcName) char szComm[128]; char szPids[1024]; - if ( NULL == szProcName || 0 == szProcName[0] ) + if ( (NULL == szProcName) || (0 == szProcName[0]) ) return 0; sprintf(szComm, "pidof %s", szProcName); hw_execute_bash_command_silent(szComm, szPids); - if ( strlen(szPids) > 2 ) + if ( (strlen(szPids) > 2) && isdigit(szPids[0]) ) { + if ( (szPids[0] == 10) || (szPids[0] == 13) ) + szPids[0] = ' '; + if ( (szPids[1] == 10) || (szPids[1] == 13) ) + szPids[1] = ' '; + // get only first pid for( int i=0; i/dev/null", szProcName); + sprintf(szComm, "kill %d $(pidof %s) 2>/dev/null", iSignal, szProcName); hw_execute_bash_command(szComm, NULL); hardware_sleep_ms(20); sprintf(szComm, "pidof %s", szProcName); hw_execute_bash_command(szComm, szPids); - if ( strlen(szPids) > 2 ) + if ( strlen(szPids) < 3 ) + return 1; + + log_line("Process %s pid is: %s", szProcName, szPids); + + int retryCount = 10; + while ( retryCount > 0 ) { + hardware_sleep_ms(10); + szPids[0] = 0; + hw_execute_bash_command(szComm, szPids); + if ( strlen(szPids) < 3 ) + return 1; log_line("Process %s pid is: %s", szProcName, szPids); - - int retryCount = 10; - sprintf(szComm, "pidof %s", szProcName); - while ( retryCount > 0 ) - { - hardware_sleep_ms(10); - szPids[0] = 0; - hw_execute_bash_command(szComm, szPids); - if ( strlen(szPids) < 2 ) - return; - log_line("Process %s pid is: %s", szProcName, szPids); - retryCount--; - } + retryCount--; } + return 0; } diff --git a/code/base/hw_procs.h b/code/base/hw_procs.h index 97d7c6fa..339b0613 100644 --- a/code/base/hw_procs.h +++ b/code/base/hw_procs.h @@ -13,7 +13,7 @@ int hw_process_exists(const char* szProcName); char* hw_process_get_pid(const char* szProcName); void hw_stop_process(const char* szProcName); -void hw_kill_process(const char* szProcName); +int hw_kill_process(const char* szProcName, int iSignal); void hw_set_priority_current_proc(int nice); void hw_set_proc_priority(const char* szProgName, int nice, int ionice, int waitForProcess); diff --git a/code/base/models.cpp b/code/base/models.cpp index 2e633b5f..2a871d6c 100644 --- a/code/base/models.cpp +++ b/code/base/models.cpp @@ -2133,6 +2133,13 @@ bool Model::loadVersion10(FILE* fd) bOk = false; } + if ( bOk && (1 != fscanf(fd, "%u", &processesPriorities.uProcessesFlags)) ) + { + log_softerror_and_alarm("Failed to read processes flags."); + processesPriorities.uProcessesFlags = PROCESSES_FLAGS_BALANCE_INT_CORES; + bOk = false; + } + //-------------------------------------------------- // End reading file; //---------------------------------------- @@ -2624,6 +2631,9 @@ bool Model::saveVersion10(FILE* fd, bool isOnController) sprintf(szSetting, "%u\n", osd_params.uFlags); strcat(szModel, szSetting); + sprintf(szSetting, "%u\n", processesPriorities.uProcessesFlags); + strcat(szModel, szSetting); + // End writing values to file // --------------------------------------------------- @@ -2636,7 +2646,7 @@ void Model::resetVideoParamsToDefaults() { memset(&video_params, 0, sizeof(video_params)); - video_params.user_selected_video_link_profile = VIDEO_PROFILE_BEST_PERF; + video_params.user_selected_video_link_profile = VIDEO_PROFILE_HIGH_QUALITY; video_params.iH264Slices = DEFAULT_VIDEO_H264_SLICES; video_params.videoAdjustmentStrength = DEFAULT_VIDEO_PARAMS_ADJUSTMENT_STRENGTH; video_params.lowestAllowedAdaptiveVideoBitrate = DEFAULT_LOWEST_ALLOWED_ADAPTIVE_VIDEO_BITRATE; @@ -2670,18 +2680,24 @@ void Model::resetVideoLinkProfiles(int iProfile) video_link_profiles[i].video_data_length = DEFAULT_VIDEO_DATA_LENGTH_HP; video_link_profiles[i].fps = DEFAULT_VIDEO_FPS; - if ( hardware_board_is_sigmastar(hardware_getOnlyBoardType()) ) + if ( hardware_board_is_openipc(hardware_getOnlyBoardType()) ) video_link_profiles[i].fps = DEFAULT_VIDEO_FPS_OIPC; + if ( hardware_board_is_sigmastar(hardware_getOnlyBoardType()) ) + video_link_profiles[i].fps = DEFAULT_VIDEO_FPS_OIPC_SIGMASTAR; video_link_profiles[i].keyframe_ms = DEFAULT_VIDEO_KEYFRAME; if ( hardware_board_is_goke(hardware_getOnlyBoardType()) ) video_link_profiles[i].keyframe_ms = DEFAULT_VIDEO_KEYFRAME_OIPC_GOKE; + if ( hardware_board_is_sigmastar(hardware_getOnlyBoardType()) ) + video_link_profiles[i].keyframe_ms = DEFAULT_VIDEO_KEYFRAME_OIPC_SIGMASTAR; video_link_profiles[i].bitrate_fixed_bps = DEFAULT_VIDEO_BITRATE; if ( (hardware_getOnlyBoardType() == BOARD_TYPE_PIZERO) || (hardware_getOnlyBoardType() == BOARD_TYPE_PIZEROW) || hardware_board_is_goke(hardware_getOnlyBoardType()) ) - video_link_profiles[i].bitrate_fixed_bps = DEFAULT_VIDEO_BITRATE_PI_ZERO; + video_link_profiles[i].bitrate_fixed_bps = DEFAULT_VIDEO_BITRATE_PI_ZERO; + if ( hardware_board_is_sigmastar(hardware_getOnlyBoardType()) ) + video_link_profiles[i].bitrate_fixed_bps = DEFAULT_VIDEO_BITRATE_OPIC_SIGMASTAR; if ( hardware_isCameraVeye() || hardware_isCameraHDMI() ) { @@ -2697,102 +2713,91 @@ void Model::resetVideoLinkProfiles(int iProfile) // Best Perf - if ( iProfile == -1 || iProfile == VIDEO_PROFILE_BEST_PERF ) - { - video_link_profiles[VIDEO_PROFILE_BEST_PERF].flags = 0; - video_link_profiles[VIDEO_PROFILE_BEST_PERF].uProfileEncodingFlags = VIDEO_PROFILE_ENCODING_FLAG_ENABLE_RETRANSMISSIONS | VIDEO_PROFILE_ENCODING_FLAG_ENABLE_ADAPTIVE_VIDEO_LINK; - video_link_profiles[VIDEO_PROFILE_BEST_PERF].uProfileEncodingFlags |= (DEFAULT_VIDEO_RETRANS_MS5_HP<<8); - video_link_profiles[VIDEO_PROFILE_BEST_PERF].uProfileEncodingFlags |= VIDEO_PROFILE_ENCODING_FLAG_RETRANSMISSIONS_DUPLICATION_PERCENT_AUTO; - video_link_profiles[VIDEO_PROFILE_BEST_PERF].uProfileEncodingFlags |= VIDEO_PROFILE_ENCODING_FLAG_USE_MEDIUM_ADAPTIVE_VIDEO; - video_link_profiles[VIDEO_PROFILE_BEST_PERF].uProfileEncodingFlags |= VIDEO_PROFILE_ENCODING_FLAG_EC_SCHEME_SPREAD_FACTOR_HIGHBIT; - video_link_profiles[VIDEO_PROFILE_BEST_PERF].bitrate_fixed_bps = DEFAULT_HP_VIDEO_BITRATE; - if ( (hardware_getOnlyBoardType() == BOARD_TYPE_PIZERO) || - (hardware_getOnlyBoardType() == BOARD_TYPE_PIZEROW) || - hardware_board_is_goke(hardware_getOnlyBoardType()) ) - video_link_profiles[VIDEO_PROFILE_BEST_PERF].bitrate_fixed_bps = DEFAULT_VIDEO_BITRATE_PI_ZERO; + if ( (iProfile == -1) || (iProfile == VIDEO_PROFILE_BEST_PERF) ) + { + video_link_profiles[VIDEO_PROFILE_BEST_PERF].flags = 0; + video_link_profiles[VIDEO_PROFILE_BEST_PERF].uProfileEncodingFlags = VIDEO_PROFILE_ENCODING_FLAG_ENABLE_RETRANSMISSIONS | VIDEO_PROFILE_ENCODING_FLAG_ENABLE_ADAPTIVE_VIDEO_LINK; + video_link_profiles[VIDEO_PROFILE_BEST_PERF].uProfileEncodingFlags |= (DEFAULT_VIDEO_RETRANS_MS5_HP<<8); + video_link_profiles[VIDEO_PROFILE_BEST_PERF].uProfileEncodingFlags |= VIDEO_PROFILE_ENCODING_FLAG_RETRANSMISSIONS_DUPLICATION_PERCENT_AUTO; + video_link_profiles[VIDEO_PROFILE_BEST_PERF].uProfileEncodingFlags |= VIDEO_PROFILE_ENCODING_FLAG_USE_MEDIUM_ADAPTIVE_VIDEO; + video_link_profiles[VIDEO_PROFILE_BEST_PERF].uProfileEncodingFlags |= VIDEO_PROFILE_ENCODING_FLAG_EC_SCHEME_SPREAD_FACTOR_HIGHBIT; + video_link_profiles[VIDEO_PROFILE_BEST_PERF].bitrate_fixed_bps = DEFAULT_HP_VIDEO_BITRATE; + if ( (hardware_getOnlyBoardType() == BOARD_TYPE_PIZERO) || + (hardware_getOnlyBoardType() == BOARD_TYPE_PIZEROW) || + hardware_board_is_goke(hardware_getOnlyBoardType()) ) + video_link_profiles[VIDEO_PROFILE_BEST_PERF].bitrate_fixed_bps = DEFAULT_VIDEO_BITRATE_PI_ZERO; - video_link_profiles[VIDEO_PROFILE_BEST_PERF].block_packets = DEFAULT_VIDEO_BLOCK_PACKETS_HP; - video_link_profiles[VIDEO_PROFILE_BEST_PERF].block_fecs = DEFAULT_VIDEO_BLOCK_FECS_HP; - video_link_profiles[VIDEO_PROFILE_BEST_PERF].video_data_length = DEFAULT_VIDEO_DATA_LENGTH_HP; - video_link_profiles[VIDEO_PROFILE_BEST_PERF].keyframe_ms = DEFAULT_VIDEO_KEYFRAME; - if ( hardware_board_is_goke(hardware_getOnlyBoardType()) ) - video_link_profiles[VIDEO_PROFILE_BEST_PERF].keyframe_ms = DEFAULT_VIDEO_KEYFRAME_OIPC_GOKE; - video_link_profiles[VIDEO_PROFILE_BEST_PERF].radio_datarate_video_bps = DEFAULT_HP_VIDEO_RADIO_DATARATE; + video_link_profiles[VIDEO_PROFILE_BEST_PERF].block_packets = DEFAULT_VIDEO_BLOCK_PACKETS_HP; + video_link_profiles[VIDEO_PROFILE_BEST_PERF].block_fecs = DEFAULT_VIDEO_BLOCK_FECS_HP; + video_link_profiles[VIDEO_PROFILE_BEST_PERF].video_data_length = DEFAULT_VIDEO_DATA_LENGTH_HP; + video_link_profiles[VIDEO_PROFILE_BEST_PERF].radio_datarate_video_bps = DEFAULT_HP_VIDEO_RADIO_DATARATE; } // High Quality - if ( iProfile == -1 || iProfile == VIDEO_PROFILE_HIGH_QUALITY ) + if ( (iProfile == -1) || (iProfile == VIDEO_PROFILE_HIGH_QUALITY) ) { - video_link_profiles[VIDEO_PROFILE_HIGH_QUALITY].flags = 0; - video_link_profiles[VIDEO_PROFILE_HIGH_QUALITY].uProfileEncodingFlags = VIDEO_PROFILE_ENCODING_FLAG_ENABLE_RETRANSMISSIONS | VIDEO_PROFILE_ENCODING_FLAG_ENABLE_ADAPTIVE_VIDEO_LINK; - video_link_profiles[VIDEO_PROFILE_HIGH_QUALITY].uProfileEncodingFlags |= (DEFAULT_VIDEO_RETRANS_MS5_HQ<<8); - video_link_profiles[VIDEO_PROFILE_HIGH_QUALITY].uProfileEncodingFlags |= VIDEO_PROFILE_ENCODING_FLAG_RETRANSMISSIONS_DUPLICATION_PERCENT_AUTO; - video_link_profiles[VIDEO_PROFILE_HIGH_QUALITY].uProfileEncodingFlags |= VIDEO_PROFILE_ENCODING_FLAG_EC_SCHEME_SPREAD_FACTOR_HIGHBIT; - video_link_profiles[VIDEO_PROFILE_HIGH_QUALITY].block_packets = DEFAULT_VIDEO_BLOCK_PACKETS_HQ; - video_link_profiles[VIDEO_PROFILE_HIGH_QUALITY].block_fecs = DEFAULT_VIDEO_BLOCK_FECS_HQ; - video_link_profiles[VIDEO_PROFILE_HIGH_QUALITY].video_data_length = DEFAULT_VIDEO_DATA_LENGTH_HQ; + video_link_profiles[VIDEO_PROFILE_HIGH_QUALITY].flags = 0; + video_link_profiles[VIDEO_PROFILE_HIGH_QUALITY].uProfileEncodingFlags = VIDEO_PROFILE_ENCODING_FLAG_ENABLE_RETRANSMISSIONS | VIDEO_PROFILE_ENCODING_FLAG_ENABLE_ADAPTIVE_VIDEO_LINK; + video_link_profiles[VIDEO_PROFILE_HIGH_QUALITY].uProfileEncodingFlags |= (DEFAULT_VIDEO_RETRANS_MS5_HQ<<8); + video_link_profiles[VIDEO_PROFILE_HIGH_QUALITY].uProfileEncodingFlags |= VIDEO_PROFILE_ENCODING_FLAG_RETRANSMISSIONS_DUPLICATION_PERCENT_AUTO; + video_link_profiles[VIDEO_PROFILE_HIGH_QUALITY].uProfileEncodingFlags |= VIDEO_PROFILE_ENCODING_FLAG_EC_SCHEME_SPREAD_FACTOR_HIGHBIT; + video_link_profiles[VIDEO_PROFILE_HIGH_QUALITY].block_packets = DEFAULT_VIDEO_BLOCK_PACKETS_HQ; + video_link_profiles[VIDEO_PROFILE_HIGH_QUALITY].block_fecs = DEFAULT_VIDEO_BLOCK_FECS_HQ; + video_link_profiles[VIDEO_PROFILE_HIGH_QUALITY].video_data_length = DEFAULT_VIDEO_DATA_LENGTH_HQ; } // User - if ( iProfile == -1 || iProfile == VIDEO_PROFILE_USER ) + if ( (iProfile == -1) || (iProfile == VIDEO_PROFILE_USER) ) { - video_link_profiles[VIDEO_PROFILE_USER].flags = 0; - video_link_profiles[VIDEO_PROFILE_USER].uProfileEncodingFlags = VIDEO_PROFILE_ENCODING_FLAG_ENABLE_RETRANSMISSIONS | VIDEO_PROFILE_ENCODING_FLAG_ENABLE_ADAPTIVE_VIDEO_LINK; - video_link_profiles[VIDEO_PROFILE_USER].uProfileEncodingFlags |= (DEFAULT_VIDEO_RETRANS_MS5_HP<<8); - video_link_profiles[VIDEO_PROFILE_USER].uProfileEncodingFlags |= VIDEO_PROFILE_ENCODING_FLAG_RETRANSMISSIONS_DUPLICATION_PERCENT_AUTO; - video_link_profiles[VIDEO_PROFILE_USER].uProfileEncodingFlags |= VIDEO_PROFILE_ENCODING_FLAG_EC_SCHEME_SPREAD_FACTOR_HIGHBIT; - video_link_profiles[VIDEO_PROFILE_USER].block_packets = DEFAULT_VIDEO_BLOCK_PACKETS_HP; - video_link_profiles[VIDEO_PROFILE_USER].block_fecs = DEFAULT_VIDEO_BLOCK_FECS_HP; - video_link_profiles[VIDEO_PROFILE_USER].video_data_length = DEFAULT_VIDEO_DATA_LENGTH_HP; + video_link_profiles[VIDEO_PROFILE_USER].flags = 0; + video_link_profiles[VIDEO_PROFILE_USER].uProfileEncodingFlags = VIDEO_PROFILE_ENCODING_FLAG_ENABLE_RETRANSMISSIONS | VIDEO_PROFILE_ENCODING_FLAG_ENABLE_ADAPTIVE_VIDEO_LINK; + video_link_profiles[VIDEO_PROFILE_USER].uProfileEncodingFlags |= (DEFAULT_VIDEO_RETRANS_MS5_HP<<8); + video_link_profiles[VIDEO_PROFILE_USER].uProfileEncodingFlags |= VIDEO_PROFILE_ENCODING_FLAG_RETRANSMISSIONS_DUPLICATION_PERCENT_AUTO; + video_link_profiles[VIDEO_PROFILE_USER].uProfileEncodingFlags |= VIDEO_PROFILE_ENCODING_FLAG_EC_SCHEME_SPREAD_FACTOR_HIGHBIT; + video_link_profiles[VIDEO_PROFILE_USER].block_packets = DEFAULT_VIDEO_BLOCK_PACKETS_HP; + video_link_profiles[VIDEO_PROFILE_USER].block_fecs = DEFAULT_VIDEO_BLOCK_FECS_HP; + video_link_profiles[VIDEO_PROFILE_USER].video_data_length = DEFAULT_VIDEO_DATA_LENGTH_HP; } // MQ - if ( iProfile == -1 || iProfile == VIDEO_PROFILE_MQ ) - { - video_link_profiles[VIDEO_PROFILE_MQ].flags = 0; - video_link_profiles[VIDEO_PROFILE_MQ].uProfileEncodingFlags = VIDEO_PROFILE_ENCODING_FLAG_ENABLE_RETRANSMISSIONS | VIDEO_PROFILE_ENCODING_FLAG_ENABLE_ADAPTIVE_VIDEO_LINK | VIDEO_PROFILE_ENCODING_FLAG_RETRANSMISSIONS_DUPLICATION_PERCENT_AUTO; - video_link_profiles[VIDEO_PROFILE_MQ].uProfileEncodingFlags |= (DEFAULT_VIDEO_RETRANS_MS5_MQ<<8); - video_link_profiles[VIDEO_PROFILE_MQ].uProfileEncodingFlags |= VIDEO_PROFILE_ENCODING_FLAG_AUTO_EC_SCHEME; - video_link_profiles[VIDEO_PROFILE_MQ].uProfileEncodingFlags |= VIDEO_PROFILE_ENCODING_FLAG_EC_SCHEME_SPREAD_FACTOR_HIGHBIT; - video_link_profiles[VIDEO_PROFILE_MQ].radio_datarate_video_bps = 0; - video_link_profiles[VIDEO_PROFILE_MQ].radio_datarate_data_bps = 0; - video_link_profiles[VIDEO_PROFILE_MQ].h264profile = 2; // high - video_link_profiles[VIDEO_PROFILE_MQ].h264level = 2; - video_link_profiles[VIDEO_PROFILE_MQ].h264refresh = 2; // both - video_link_profiles[VIDEO_PROFILE_MQ].h264quantization = DEFAULT_VIDEO_H264_QUANTIZATION; // auto - video_link_profiles[VIDEO_PROFILE_MQ].block_packets = DEFAULT_MQ_VIDEO_BLOCK_PACKETS; - video_link_profiles[VIDEO_PROFILE_MQ].block_fecs = DEFAULT_MQ_VIDEO_BLOCK_FECS; - video_link_profiles[VIDEO_PROFILE_MQ].video_data_length = DEFAULT_MQ_VIDEO_DATA_LENGTH; - video_link_profiles[VIDEO_PROFILE_MQ].keyframe_ms = DEFAULT_VIDEO_KEYFRAME; - if ( hardware_board_is_goke(hardware_getOnlyBoardType()) ) - video_link_profiles[VIDEO_PROFILE_MQ].keyframe_ms = DEFAULT_VIDEO_KEYFRAME_OIPC_GOKE; - video_link_profiles[VIDEO_PROFILE_MQ].fps = DEFAULT_MQ_VIDEO_FPS; - video_link_profiles[VIDEO_PROFILE_MQ].bitrate_fixed_bps = DEFAULT_MQ_VIDEO_BITRATE; + if ( (iProfile == -1) || (iProfile == VIDEO_PROFILE_MQ) ) + { + video_link_profiles[VIDEO_PROFILE_MQ].flags = 0; + video_link_profiles[VIDEO_PROFILE_MQ].uProfileEncodingFlags = VIDEO_PROFILE_ENCODING_FLAG_ENABLE_RETRANSMISSIONS | VIDEO_PROFILE_ENCODING_FLAG_ENABLE_ADAPTIVE_VIDEO_LINK | VIDEO_PROFILE_ENCODING_FLAG_RETRANSMISSIONS_DUPLICATION_PERCENT_AUTO; + video_link_profiles[VIDEO_PROFILE_MQ].uProfileEncodingFlags |= (DEFAULT_VIDEO_RETRANS_MS5_MQ<<8); + video_link_profiles[VIDEO_PROFILE_MQ].uProfileEncodingFlags |= VIDEO_PROFILE_ENCODING_FLAG_AUTO_EC_SCHEME; + video_link_profiles[VIDEO_PROFILE_MQ].uProfileEncodingFlags |= VIDEO_PROFILE_ENCODING_FLAG_EC_SCHEME_SPREAD_FACTOR_HIGHBIT; + video_link_profiles[VIDEO_PROFILE_MQ].radio_datarate_video_bps = 0; + video_link_profiles[VIDEO_PROFILE_MQ].radio_datarate_data_bps = 0; + video_link_profiles[VIDEO_PROFILE_MQ].h264profile = 2; // high + video_link_profiles[VIDEO_PROFILE_MQ].h264level = 2; + video_link_profiles[VIDEO_PROFILE_MQ].h264refresh = 2; // both + video_link_profiles[VIDEO_PROFILE_MQ].h264quantization = DEFAULT_VIDEO_H264_QUANTIZATION; // auto + video_link_profiles[VIDEO_PROFILE_MQ].block_packets = DEFAULT_MQ_VIDEO_BLOCK_PACKETS; + video_link_profiles[VIDEO_PROFILE_MQ].block_fecs = DEFAULT_MQ_VIDEO_BLOCK_FECS; + video_link_profiles[VIDEO_PROFILE_MQ].video_data_length = DEFAULT_MQ_VIDEO_DATA_LENGTH; + video_link_profiles[VIDEO_PROFILE_MQ].bitrate_fixed_bps = DEFAULT_MQ_VIDEO_BITRATE; } // LQ - if ( iProfile == -1 || iProfile == VIDEO_PROFILE_LQ ) - { - video_link_profiles[VIDEO_PROFILE_LQ].flags = 0; - video_link_profiles[VIDEO_PROFILE_LQ].uProfileEncodingFlags = VIDEO_PROFILE_ENCODING_FLAG_ENABLE_RETRANSMISSIONS | VIDEO_PROFILE_ENCODING_FLAG_ENABLE_ADAPTIVE_VIDEO_LINK | VIDEO_PROFILE_ENCODING_FLAG_RETRANSMISSIONS_DUPLICATION_PERCENT_AUTO; - video_link_profiles[VIDEO_PROFILE_LQ].uProfileEncodingFlags |= (DEFAULT_VIDEO_RETRANS_MS5_LQ<<8); - video_link_profiles[VIDEO_PROFILE_LQ].uProfileEncodingFlags |= VIDEO_PROFILE_ENCODING_FLAG_AUTO_EC_SCHEME; - video_link_profiles[VIDEO_PROFILE_LQ].uProfileEncodingFlags |= VIDEO_PROFILE_ENCODING_FLAG_EC_SCHEME_SPREAD_FACTOR_HIGHBIT; - video_link_profiles[VIDEO_PROFILE_LQ].radio_datarate_video_bps = 0; - video_link_profiles[VIDEO_PROFILE_LQ].radio_datarate_data_bps = 0; - video_link_profiles[VIDEO_PROFILE_LQ].radio_flags = 0; - video_link_profiles[VIDEO_PROFILE_LQ].h264profile = 2; // high - video_link_profiles[VIDEO_PROFILE_LQ].h264level = 2; - video_link_profiles[VIDEO_PROFILE_LQ].h264refresh = 2; // both - video_link_profiles[VIDEO_PROFILE_LQ].h264quantization = DEFAULT_VIDEO_H264_QUANTIZATION; // auto - video_link_profiles[VIDEO_PROFILE_LQ].block_packets = DEFAULT_LQ_VIDEO_BLOCK_PACKETS; - video_link_profiles[VIDEO_PROFILE_LQ].block_fecs = DEFAULT_LQ_VIDEO_BLOCK_FECS; - video_link_profiles[VIDEO_PROFILE_LQ].video_data_length = DEFAULT_LQ_VIDEO_DATA_LENGTH; - video_link_profiles[VIDEO_PROFILE_LQ].keyframe_ms = DEFAULT_VIDEO_KEYFRAME; - if ( hardware_board_is_goke(hardware_getOnlyBoardType()) ) - video_link_profiles[VIDEO_PROFILE_LQ].keyframe_ms = DEFAULT_VIDEO_KEYFRAME_OIPC_GOKE; - video_link_profiles[VIDEO_PROFILE_LQ].fps = DEFAULT_LQ_VIDEO_FPS; - video_link_profiles[VIDEO_PROFILE_LQ].bitrate_fixed_bps = DEFAULT_LQ_VIDEO_BITRATE; + if ( (iProfile == -1) || (iProfile == VIDEO_PROFILE_LQ) ) + { + video_link_profiles[VIDEO_PROFILE_LQ].flags = 0; + video_link_profiles[VIDEO_PROFILE_LQ].uProfileEncodingFlags = VIDEO_PROFILE_ENCODING_FLAG_ENABLE_RETRANSMISSIONS | VIDEO_PROFILE_ENCODING_FLAG_ENABLE_ADAPTIVE_VIDEO_LINK | VIDEO_PROFILE_ENCODING_FLAG_RETRANSMISSIONS_DUPLICATION_PERCENT_AUTO; + video_link_profiles[VIDEO_PROFILE_LQ].uProfileEncodingFlags |= (DEFAULT_VIDEO_RETRANS_MS5_LQ<<8); + video_link_profiles[VIDEO_PROFILE_LQ].uProfileEncodingFlags |= VIDEO_PROFILE_ENCODING_FLAG_AUTO_EC_SCHEME; + video_link_profiles[VIDEO_PROFILE_LQ].uProfileEncodingFlags |= VIDEO_PROFILE_ENCODING_FLAG_EC_SCHEME_SPREAD_FACTOR_HIGHBIT; + video_link_profiles[VIDEO_PROFILE_LQ].radio_datarate_video_bps = 0; + video_link_profiles[VIDEO_PROFILE_LQ].radio_datarate_data_bps = 0; + video_link_profiles[VIDEO_PROFILE_LQ].radio_flags = 0; + video_link_profiles[VIDEO_PROFILE_LQ].h264profile = 2; // high + video_link_profiles[VIDEO_PROFILE_LQ].h264level = 2; + video_link_profiles[VIDEO_PROFILE_LQ].h264refresh = 2; // both + video_link_profiles[VIDEO_PROFILE_LQ].h264quantization = DEFAULT_VIDEO_H264_QUANTIZATION; // auto + video_link_profiles[VIDEO_PROFILE_LQ].block_packets = DEFAULT_LQ_VIDEO_BLOCK_PACKETS; + video_link_profiles[VIDEO_PROFILE_LQ].block_fecs = DEFAULT_LQ_VIDEO_BLOCK_FECS; + video_link_profiles[VIDEO_PROFILE_LQ].video_data_length = DEFAULT_LQ_VIDEO_DATA_LENGTH; + video_link_profiles[VIDEO_PROFILE_LQ].bitrate_fixed_bps = DEFAULT_LQ_VIDEO_BITRATE; } for( int i=0; i= 0 ) + if ( radioInterfacesParams.interface_link_id[i] < radioLinksParams.links_count ) + { + int iRadioLink = radioInterfacesParams.interface_link_id[i]; + radioLinksParams.link_datarate_video_bps[iRadioLink] = DEFAULT_RADIO_DATARATE_VIDEO_ATHEROS; + } + } + } +} void Model::copy_video_link_profile(int from, int to) { @@ -3562,7 +3594,7 @@ int Model::logVehicleRadioLinkDifferences(type_radio_links_parameters* pData1, t iDifferences++; } - if ( pData1->uGlobalRadioLinksFlags != pData2->uGlobalRadioLinksFlags ) + if ( (pData1->uGlobalRadioLinksFlags & (~MODEL_RADIOLINKS_FLAGS_HAS_NEGOCIATED_LINKS)) != (pData2->uGlobalRadioLinksFlags & (~MODEL_RADIOLINKS_FLAGS_HAS_NEGOCIATED_LINKS)) ) { log_line("* Radio Links global radio links flags changed from %u to %u", pData1->uGlobalRadioLinksFlags, pData2->uGlobalRadioLinksFlags); iDifferences++; @@ -3816,18 +3848,19 @@ bool Model::validate_settings() { if ( ! radioLinkIsSiKRadio(i) ) { + log_softerror_and_alarm("Invalid radio video data rates (%d). Reseting to default (%d).", radioLinksParams.link_datarate_video_bps[i], DEFAULT_RADIO_DATARATE_VIDEO); radioLinksParams.link_datarate_video_bps[i] = DEFAULT_RADIO_DATARATE_VIDEO; - log_softerror_and_alarm("Invalid radio video data rates. Reseting to default (%d).", radioLinksParams.link_datarate_video_bps[i]); } } if ( getRealDataRateFromRadioDataRate(radioLinksParams.link_datarate_data_bps[i], 0) < 500 ) if ( radioLinksParams.link_capabilities_flags[i] & RADIO_HW_CAPABILITY_FLAG_CAN_USE_FOR_DATA ) { + int iTmp = radioLinksParams.link_datarate_data_bps[i]; if ( radioLinkIsSiKRadio(i) ) radioLinksParams.link_datarate_data_bps[i] = DEFAULT_RADIO_DATARATE_SIK_AIR; else radioLinksParams.link_datarate_data_bps[i] = DEFAULT_RADIO_DATARATE_DATA; - log_softerror_and_alarm("Invalid radio data data rates. Reseting to default (%d).", radioLinksParams.link_datarate_data_bps[i]); + log_softerror_and_alarm("Invalid radio data data rates (%d). Reseting to default (%d).", iTmp, radioLinksParams.link_datarate_data_bps[i]); } } @@ -3862,7 +3895,7 @@ bool Model::validate_settings() telemetry_params.flags = TELEMETRY_FLAGS_RXTX | TELEMETRY_FLAGS_REQUEST_DATA_STREAMS | TELEMETRY_FLAGS_SPECTATOR_ENABLE; if ( rxtx_sync_type < 0 || rxtx_sync_type >= RXTX_SYNC_TYPE_LAST ) - rxtx_sync_type = RXTX_SYNC_TYPE_NONE; + rxtx_sync_type = RXTX_SYNC_TYPE_BASIC; if ( processesPriorities.iNiceRouter < -18 || processesPriorities.iNiceRouter > 0 ) processesPriorities.iNiceRouter = DEFAULT_PRIORITY_PROCESS_ROUTER; @@ -4075,8 +4108,9 @@ void Model::resetToDefaults(bool generateId) resetRelayParamsToDefaults(&relay_params); - rxtx_sync_type = RXTX_SYNC_TYPE_NONE; + rxtx_sync_type = RXTX_SYNC_TYPE_BASIC; + processesPriorities.uProcessesFlags = PROCESSES_FLAGS_BALANCE_INT_CORES; processesPriorities.iNiceTelemetry = DEFAULT_PRIORITY_PROCESS_TELEMETRY; processesPriorities.iNiceRC = DEFAULT_PRIORITY_PROCESS_RC; processesPriorities.iNiceRouter = DEFAULT_PRIORITY_PROCESS_ROUTER; @@ -4094,7 +4128,7 @@ void Model::resetToDefaults(bool generateId) if ( hardware_board_is_sigmastar(hwCapabilities.uBoardType & BOARD_TYPE_MASK) ) { processesPriorities.iFreqARM = DEFAULT_FREQ_OPENIPC_SIGMASTAR; - processesPriorities.iFreqGPU = 1; + processesPriorities.iFreqGPU = 0; } processesPriorities.iThreadPriorityRadioRx = DEFAULT_PRIORITY_THREAD_RADIO_RX; @@ -4212,7 +4246,7 @@ void Model::resetOSDFlags() osd_params.osd_flags[0] = 0; // horizontal layout for stats panels; osd_params.osd_flags[1] = OSD_FLAG_SHOW_DISTANCE | OSD_FLAG_SHOW_ALTITUDE | OSD_FLAG_SHOW_BATTERY; - osd_params.osd_flags[2] = OSD_FLAG_SHOW_DISTANCE | OSD_FLAG_SHOW_ALTITUDE | OSD_FLAG_SHOW_BATTERY | OSD_FLAG_SHOW_HOME | OSD_FLAG_SHOW_VIDEO_MODE | OSD_FLAG_SHOW_CPU_INFO | OSD_FLAG_SHOW_FLIGHT_MODE | OSD_FLAG_SHOW_TIME | OSD_FLAG_SHOW_TIME_LOWER | OSD_FLAG_SHOW_RADIO_INTERFACES_INFO; + osd_params.osd_flags[2] = OSD_FLAG_SHOW_DISTANCE | OSD_FLAG_SHOW_ALTITUDE | OSD_FLAG_SHOW_BATTERY | OSD_FLAG_SHOW_HOME | OSD_FLAG_SHOW_VIDEO_MODE | OSD_FLAG_SHOW_CPU_INFO | OSD_FLAG_SHOW_FLIGHT_MODE | OSD_FLAG_SHOW_FLIGHT_MODE_CHANGE | OSD_FLAG_SHOW_TIME | OSD_FLAG_SHOW_TIME_LOWER | OSD_FLAG_SHOW_RADIO_INTERFACES_INFO; osd_params.osd_flags[3] = 0; osd_params.osd_flags[4] = 0; osd_params.osd_flags2[0] = OSD_FLAG2_LAYOUT_ENABLED | OSD_FLAG2_SHOW_BGBARS | OSD_FLAG2_SHOW_STATS_VIDEO | OSD_FLAG2_SHOW_STATS_RADIO_INTERFACES | OSD_FLAG2_SHOW_RC_RSSI; @@ -4225,7 +4259,7 @@ void Model::resetOSDFlags() for( int i=0; iuFlags = 0; + pCamParams->uFlags = CAMERA_FLAG_OPENIPC_3A_SIGMASTAR; pCamParams->flip_image = 0; pCamParams->brightness = 47; pCamParams->contrast = 50; @@ -5182,6 +5216,11 @@ void Model::setDefaultVideoBitrate() video_link_profiles[VIDEO_PROFILE_BEST_PERF].bitrate_fixed_bps = DEFAULT_VIDEO_BITRATE_PI_ZERO; video_link_profiles[VIDEO_PROFILE_USER].bitrate_fixed_bps = DEFAULT_VIDEO_BITRATE_PI_ZERO; } + if ( hardware_board_is_sigmastar(hardware_getOnlyBoardType()) ) + { + video_link_profiles[VIDEO_PROFILE_HIGH_QUALITY].bitrate_fixed_bps = DEFAULT_VIDEO_BITRATE_OPIC_SIGMASTAR; + video_link_profiles[VIDEO_PROFILE_USER].bitrate_fixed_bps = DEFAULT_VIDEO_BITRATE_OPIC_SIGMASTAR; + } // Lower video bitrate on all video profiles if running on a single core CPU for( int i=0; iradio_interfaces[i].rxBytesPerSec = 0; + pSMRS->radio_interfaces[i].totalRxBytes = 0; + pSMRS->radio_interfaces[i].totalRxPackets = 0; + pSMRS->radio_interfaces[i].totalRxPacketsBad = 0; + pSMRS->radio_interfaces[i].totalRxPacketsLost = 0; + pSMRS->radio_interfaces[i].rxPacketsPerSec = 0; + pSMRS->radio_interfaces[i].timeLastRxPacket = 0; + + pSMRS->radio_interfaces[i].tmpRxBytes = 0; + pSMRS->radio_interfaces[i].tmpRxPackets = 0; + + for( int k=0; kradio_interfaces[i].hist_rxPacketsCount[k] = 0; + pSMRS->radio_interfaces[i].hist_rxPacketsBadCount[k] = 0; + pSMRS->radio_interfaces[i].hist_rxPacketsLostCount[k] = 0; + pSMRS->radio_interfaces[i].hist_rxGapMiliseconds[k] = 0xFF; + } + pSMRS->radio_interfaces[i].hist_tmp_rxPacketsCount = 0; + pSMRS->radio_interfaces[i].hist_tmp_rxPacketsBadCount = 0; + pSMRS->radio_interfaces[i].hist_tmp_rxPacketsLostCount = 0; + } +} + void radio_stats_set_graph_refresh_interval(shared_mem_radio_stats* pSMRS, int graphRefreshInterval) { if ( NULL == pSMRS ) diff --git a/code/common/radio_stats.h b/code/common/radio_stats.h index ddd72ec0..e968d2ef 100644 --- a/code/common/radio_stats.h +++ b/code/common/radio_stats.h @@ -14,6 +14,7 @@ void radio_stats_interfaces_rx_graph_reset(shared_mem_radio_stats_interfaces_rx_ void radio_stats_reset(shared_mem_radio_stats* pSMRS, int graphRefreshInterval); void radio_stats_reset_received_info(shared_mem_radio_stats* pSMRS); void radio_stats_reset_signal_info_for_card(shared_mem_radio_stats* pSMRS, int iInterfaceIndex); +void radio_stats_reset_interfaces_rx_info(shared_mem_radio_stats* pSMRS); void radio_stats_set_graph_refresh_interval(shared_mem_radio_stats* pSMRS, int graphRefreshInterval); void radio_stats_enable_history_monitor(int iEnable); diff --git a/code/common/string_utils.c b/code/common/string_utils.c index eb03550f..50407aa4 100644 --- a/code/common/string_utils.c +++ b/code/common/string_utils.c @@ -292,7 +292,7 @@ char* str_get_packet_type(int iPacketType) case PACKET_TYPE_RUBY_TELEMETRY_RADIO_RX_HISTORY: strcpy(s_szPacketType, "PACKET_TYPE_RUBY_TELEMETRY_RADIO_RX_HISTORY"); break; case PACKET_TYPE_VEHICLE_RECORDING: strcpy(s_szPacketType, "PACKET_TYPE_VEHICLE_RECORDING"); break; - + case PACKET_TYPE_NEGOCIATE_RADIO_LINKS: strcpy(s_szPacketType, "PACKET_TYPE_NEGOCIATE_RADIO_LINKS"); break; // Local packets case PACKET_TYPE_LOCAL_CONTROL_PAUSE_VIDEO: strcpy(s_szPacketType, "PACKET_TYPE_LOCAL_CONTROL_PAUSE_VIDEO"); break; @@ -643,6 +643,7 @@ const char* str_get_hardware_board_name(u32 board_type) #endif #ifdef HW_PLATFORM_RADXA_ZERO3 static const char* s_szBoardTypeRadxaZero3 = "Radxa Zero 3"; + static const char* s_szBoardTypeRadxa3C = "Radxa 3C"; #endif static const char* s_szBoardTypeOpenIPCGoke200 = "OpenIPC Goke200"; static const char* s_szBoardTypeOpenIPCGoke210 = "OpenIPC Goke210"; @@ -679,6 +680,8 @@ const char* str_get_hardware_board_name(u32 board_type) #if defined (HW_PLATFORM_RADXA_ZERO3) if ( (board_type & BOARD_TYPE_MASK) == BOARD_TYPE_RADXA_ZERO3 ) return s_szBoardTypeRadxaZero3; + if ( (board_type & BOARD_TYPE_MASK) == BOARD_TYPE_RADXA_3C ) + return s_szBoardTypeRadxa3C; #endif if ( (board_type & BOARD_TYPE_MASK) == BOARD_TYPE_OPENIPC_GOKE200 ) @@ -721,6 +724,7 @@ const char* str_get_hardware_board_name_short(u32 board_type) #ifdef HW_PLATFORM_RADXA_ZERO3 static const char* s_szBoardSTypeRadxaZero3 = "Radxa Zero3"; + static const char* s_szBoardSTypeRadxa3C = "Radxa 3C"; #endif static const char* s_szBoardSTypeOpenIPCGoke200 = "Goke200"; @@ -758,6 +762,8 @@ const char* str_get_hardware_board_name_short(u32 board_type) #if defined(HW_PLATFORM_RADXA_ZERO3) if ( (board_type & BOARD_TYPE_MASK) == BOARD_TYPE_RADXA_ZERO3 ) return s_szBoardSTypeRadxaZero3; + if ( (board_type & BOARD_TYPE_MASK) == BOARD_TYPE_RADXA_3C ) + return s_szBoardSTypeRadxa3C; #endif if ( (board_type & BOARD_TYPE_MASK) == BOARD_TYPE_OPENIPC_GOKE200 ) diff --git a/code/public/settings_info.h b/code/public/settings_info.h index 454c4946..10ecb9fa 100644 --- a/code/public/settings_info.h +++ b/code/public/settings_info.h @@ -3,6 +3,7 @@ #include #define PLUGIN_VAR __attribute__ ((visibility("hidden"))) +#define ALIGN_STRUCT_SPEC_INFO __attribute__((aligned(4))) #define MAX_PLUGIN_SETTINGS 10 @@ -26,7 +27,7 @@ typedef struct float fOutlineThicknessPx; float fBackgroundAlpha; void* pExtraInfo; // pointer to extra info structure below -} __attribute__((packed)) plugin_settings_info_t2; +} ALIGN_STRUCT_SPEC_INFO plugin_settings_info_t2; typedef struct @@ -36,7 +37,7 @@ typedef struct // 1 - imperial, mi // 2 - metric, meters // 3 - imperial, ft -} __attribute__((packed)) plugin_settings_info_t2_extra; +} ALIGN_STRUCT_SPEC_INFO plugin_settings_info_t2_extra; #ifdef __cplusplus } diff --git a/code/public/telemetry_info.h b/code/public/telemetry_info.h index 2552087a..9bd5d7e9 100644 --- a/code/public/telemetry_info.h +++ b/code/public/telemetry_info.h @@ -3,6 +3,8 @@ #include +#define ALIGN_STRUCT_SPEC_INFO __attribute__((aligned(4))) + #define MAX_U32 0xFFFFFFFF #define MAX_VEHICLE_NAME_LENGTH 16 @@ -136,7 +138,7 @@ typedef struct void* pExtraInfo; // Extended info, see the structure below: vehicle_and_telemetry_info2_t, valid starting with version 6.8 -} __attribute__((packed)) vehicle_and_telemetry_info_t; +} ALIGN_STRUCT_SPEC_INFO vehicle_and_telemetry_info_t; typedef struct @@ -161,7 +163,7 @@ typedef struct u16 uThrottleInput; u16 uThrottleOutput; -} __attribute__((packed)) vehicle_and_telemetry_info2_t; +} ALIGN_STRUCT_SPEC_INFO vehicle_and_telemetry_info2_t; #ifdef __cplusplus } diff --git a/code/r_central/events.cpp b/code/r_central/events.cpp index 6c40e548..f753375c 100644 --- a/code/r_central/events.cpp +++ b/code/r_central/events.cpp @@ -57,6 +57,7 @@ #include "menu_update_vehicle.h" #include "menu_confirmation.h" #include "menu_confirmation_vehicle_board.h" +#include "menu_negociate_radio.h" #include "process_router_messages.h" #include "fonts.h" @@ -517,6 +518,23 @@ bool onEventReceivedModelSettings(u32 uVehicleId, u8* pBuffer, int length, bool bool bRadioChanged = false; bool bCameraChanged = false; + bool bMustNegociateRadioLinks = false; + + if ( (NULL != g_pCurrentModel) && (g_pCurrentModel->uVehicleId == pModel->uVehicleId) ) + if ( !(pModel->radioLinksParams.uGlobalRadioLinksFlags & MODEL_RADIOLINKS_FLAGS_HAS_NEGOCIATED_LINKS) ) + bMustNegociateRadioLinks = true; + + if ( pModel->radioInterfacesParams.interfaces_count != modelTemp.radioInterfacesParams.interfaces_count ) + bMustNegociateRadioLinks = true; + for( int i=0; iradioInterfacesParams.interfaces_count; i++ ) + { + if ( pModel->radioInterfacesParams.interface_card_model[i] != modelTemp.radioInterfacesParams.interface_card_model[i] ) + bMustNegociateRadioLinks = true; + if ( pModel->radioInterfacesParams.interface_radiotype_and_driver[i] != modelTemp.radioInterfacesParams.interface_radiotype_and_driver[i] ) + bMustNegociateRadioLinks = true; + if ( 0 != strcmp(pModel->radioInterfacesParams.interface_szMAC[i], modelTemp.radioInterfacesParams.interface_szMAC[i]) ) + bMustNegociateRadioLinks = true; + } if ( pModel->uVehicleId == modelTemp.uVehicleId ) { @@ -528,6 +546,7 @@ bool onEventReceivedModelSettings(u32 uVehicleId, u8* pBuffer, int length, bool (modelTemp.camera_params[modelTemp.iCurrentCamera].iForcedCameraType != pModel->camera_params[pModel->iCurrentCamera].iForcedCameraType ) ) bCameraChanged = true; } + bRadioChanged = IsModelRadioConfigChanged(&(pModel->radioLinksParams), &(pModel->radioInterfacesParams), &(modelTemp.radioLinksParams), &(modelTemp.radioInterfacesParams)); log_line("Received model has different radio config? %s", bRadioChanged?"yes":"no"); @@ -600,6 +619,7 @@ bool onEventReceivedModelSettings(u32 uVehicleId, u8* pBuffer, int length, bool log_line("The vehicle has Ruby version %d.%d (b%d) (%u) and the controller %d.%d (b%d) (%u)", ((pModel->sw_version)>>8) & 0xFF, (pModel->sw_version) & 0xFF, ((pModel->sw_version)>>16), pModel->sw_version, SYSTEM_SW_VERSION_MAJOR, SYSTEM_SW_VERSION_MINOR, SYSTEM_SW_BUILD_NUMBER, (SYSTEM_SW_VERSION_MAJOR*256+SYSTEM_SW_VERSION_MINOR) | (SYSTEM_SW_BUILD_NUMBER<<16) ); + bool bMustUpdate = false; if ( ((u32)SYSTEM_SW_VERSION_MAJOR)*(int)256 + (u32)SYSTEM_SW_VERSION_MINOR > (pModel->sw_version & 0xFFFF) ) bMustUpdate = true; @@ -624,9 +644,11 @@ bool onEventReceivedModelSettings(u32 uVehicleId, u8* pBuffer, int length, bool char szBuff[256]; char szBuff2[32]; char szBuff3[32]; + char szBuff4[64]; getSystemVersionString(szBuff2, pModel->sw_version); getSystemVersionString(szBuff3, (SYSTEM_SW_VERSION_MAJOR<<8) | SYSTEM_SW_VERSION_MINOR); - sprintf(szBuff, "Vehicle has Ruby version %s (b%d) and your controller %s (b%d). You should update your vehicle.", szBuff2, pModel->sw_version>>16, szBuff3, SYSTEM_SW_BUILD_NUMBER); + strcpy(szBuff4, pModel->getVehicleTypeString()); + snprintf(szBuff, sizeof(szBuff)/sizeof(szBuff[0]), "%s has Ruby version %s (b%d) and your controller %s (b%d). You should update your %s.", szBuff4, szBuff2, pModel->sw_version>>16, szBuff3, SYSTEM_SW_BUILD_NUMBER, szBuff4); warnings_add(pModel->uVehicleId, szBuff, 0, NULL, 12); bool bArmed = false; if ( g_VehiclesRuntimeInfo[iRuntimeIndex].bGotFCTelemetry ) @@ -639,6 +661,7 @@ bool onEventReceivedModelSettings(u32 uVehicleId, u8* pBuffer, int length, bool { add_menu_to_stack( new MenuUpdateVehiclePopup(-1) ); g_bMenuPopupUpdateVehicleShown = true; + bMustNegociateRadioLinks = false; } } @@ -649,6 +672,7 @@ bool onEventReceivedModelSettings(u32 uVehicleId, u8* pBuffer, int length, bool if ( ! menu_has_menu(MENU_ID_VEHICLE_BOARD) ) { add_menu_to_stack( new MenuConfirmationVehicleBoard() ); + bMustNegociateRadioLinks = false; } pModel->is_spectator = is_spectator; @@ -671,6 +695,8 @@ bool onEventReceivedModelSettings(u32 uVehicleId, u8* pBuffer, int length, bool pModel->logVehicleRadioInfo(); + char szTextW[256]; + // Check supported cards if ( pModel->uVehicleId == g_pCurrentModel->uVehicleId ) @@ -682,21 +708,25 @@ bool onEventReceivedModelSettings(u32 uVehicleId, u8* pBuffer, int length, bool if ( countUnsuported == pModel->radioInterfacesParams.interfaces_count ) { - Popup* p = new Popup("No radio interface on your vehicle is fully supported.", 0.3,0.4, 0.5, 6); + sprintf(szTextW, "No radio interface on your %s is fully supported.", pModel->getVehicleTypeString()); + Popup* p = new Popup(szTextW, 0.3,0.4, 0.5, 6); p->setIconId(g_idIconError, get_Color_IconError()); popups_add_topmost(p); } else if ( countUnsuported > 0 ) { - Popup* p = new Popup("Some radio interfaces on your vehicle are not fully supported.", 0.3,0.4, 0.5, 6); + sprintf(szTextW, "Some radio interfaces on your %s are not fully supported.", pModel->getVehicleTypeString()); + Popup* p = new Popup(szTextW, 0.3,0.4, 0.5, 6); p->setIconId(g_idIconWarning, get_Color_IconWarning()); popups_add_topmost(p); } } if ( pModel->alarms & ALARM_ID_UNSUPORTED_USB_SERIAL ) - warnings_add(pModel->uVehicleId, "Your vehicle has an unsupported USB to Serial adapter. Use brand name serial adapters or ones with CP2102 chipset. The ones with 340 chipset are not compatible.", g_idIconError); - + { + sprintf(szTextW, "Your %s has an unsupported USB to Serial adapter. Use brand name serial adapters or ones with CP2102 chipset. The ones with 340 chipset are not compatible.", pModel->getVehicleTypeString()); + warnings_add(pModel->uVehicleId, szTextW, g_idIconError); + } if ( pModel->audio_params.enabled ) { if ( ! pModel->audio_params.has_audio_device ) @@ -752,7 +782,8 @@ bool onEventReceivedModelSettings(u32 uVehicleId, u8* pBuffer, int length, bool if ( bMustPair ) { - Popup* p = new Popup("Radio links configuration changed on the vehicle. Updating local radio configuration...", 0.15,0.5, 0.7, 5); + sprintf(szTextW, "Radio links configuration changed on your %s. Updating local radio configuration...", pModel->getVehicleTypeString()); + Popup* p = new Popup(szTextW, 0.15,0.5, 0.7, 5); p->setIconId(g_idIconRadio, get_Color_IconWarning()); popups_add_topmost(p); @@ -761,6 +792,7 @@ bool onEventReceivedModelSettings(u32 uVehicleId, u8* pBuffer, int length, bool hardware_sleep_ms(100); pairing_start_normal(); log_line("[Events] Handing of event OnReceivedModelSettings complete."); + bMustNegociateRadioLinks = false; return true; } @@ -777,12 +809,26 @@ bool onEventReceivedModelSettings(u32 uVehicleId, u8* pBuffer, int length, bool #if defined(HW_PLATFORM_RASPBERRY) if ( pModel->video_params.uVideoExtraFlags & VIDEO_FLAG_GENERATE_H265 ) { - warnings_add(pModel->uVehicleId, "Your vehicle generates H265 video but your controller supports only H264. Change vehicle video encoder to H264 encoder", g_idIconCamera, get_Color_IconWarning() ); - MenuConfirmation* pMC = new MenuConfirmation("Unsuppoerted video codec","Your vehicle generates H265 video but your controller supports only H264. Change vehicle video encoder to H264 encoder (from Menu->Vehicle Settings->Video)", 0, true); + char szVehicleType[64]; + strcpy(szVehicleType, pModel->getVehicleTypeString()); + sprintf(szTextW, "Your %s generates H265 video but your controller supports only H264. Change the %s video encoder to H264 encoder", szVehicleType, szVehicleType); + warnings_add(pModel->uVehicleId, szTextW, g_idIconCamera, get_Color_IconWarning() ); + snprintf(szTextW, sizeof(szTextW)/sizeof(szTextW[0]), "Your %s generates H265 video but your controller supports only H264. Change teh %s video encoder to H264 encoder (from Menu->Vehicle Settings->Video)", szVehicleType, szVehicleType); + MenuConfirmation* pMC = new MenuConfirmation("Unsuppoerted video codec", szTextW, 0, true); pMC->m_yPos = 0.3; add_menu_to_stack(pMC); + bMustNegociateRadioLinks = false; } #endif + + if ( pModel->hasCamera() ) + if ( bMustNegociateRadioLinks ) + if ( ! g_bDidAnUpdate ) + if ( ! menu_has_menu(MENU_ID_NEGOCIATE_RADIO) ) + { + add_menu_to_stack(new MenuNegociateRadio()); + } + log_line("[Event] Handled of event OnReceivedModelSettings complete."); return true; } diff --git a/code/r_central/fonts.cpp b/code/r_central/fonts.cpp index ba485ab7..1a84b25a 100644 --- a/code/r_central/fonts.cpp +++ b/code/r_central/fonts.cpp @@ -167,6 +167,9 @@ bool loadAllFonts(bool bReloadMenuFonts) log_line("Loading OSD fonts..."); _loadFontFamily(szFont, s_ListOSDFontSizes, &s_iListOSDFontSizesCount ); + for( int i=0; isetFontOutlineColor(s_ListOSDFontSizes[i], p->iColorOSDOutline[0], p->iColorOSDOutline[1], p->iColorOSDOutline[2], p->iColorOSDOutline[3]); + applyFontScaleChanges(); log_line("Done loading fonts."); diff --git a/code/r_central/handle_commands.cpp b/code/r_central/handle_commands.cpp index 8310ea09..9e24efdf 100644 --- a/code/r_central/handle_commands.cpp +++ b/code/r_central/handle_commands.cpp @@ -826,8 +826,10 @@ bool handle_last_command_result() { log_line("[Commands] Received confirmation from vehicle that logs have been deleted. Parameter: %d", (int)s_CommandParam); if ( 1 == s_CommandParam ) - { - MenuConfirmation* pMC = new MenuConfirmation("Confirmation","Your vehicle logs have been deleted.", -1, true); + { + char szTextW[256]; + sprintf(szTextW, "Your %s logs have been deleted.", g_pCurrentModel->getVehicleTypeString()); + MenuConfirmation* pMC = new MenuConfirmation("Confirmation", szTextW, -1, true); pMC->m_yPos = 0.3; add_menu_to_stack(pMC); } @@ -909,7 +911,7 @@ bool handle_last_command_result() pm->m_xPos = 0.4; pm->m_yPos = 0.4; pm->m_Width = 0.36; pm->addTopLine("Your vehicle was reset to default settings (including name, id, frequencies) and the full configuration is as on a fresh instalation. It will reboot now."); - pm->addTopLine("You need to search for and pair with the vehicle as with a new vehicle."); + pm->addTopLine("You need to search for it again and pair with the vehicle as with a new vehicle."); add_menu_to_stack(pm); log_line("[Commands] Command response factory reset: Deleted model 2/3."); } @@ -936,6 +938,7 @@ bool handle_last_command_result() char szName[128]; snprintf(szName, sizeof(szName)/sizeof(szName[0]), "Did set vehicle type to: %s", str_get_hardware_board_name(s_CommandParam)); warnings_add(g_pCurrentModel->uVehicleId, szName, g_idIconCPU); + g_pCurrentModel->b_mustSyncFromVehicle = true; } break; @@ -1310,7 +1313,9 @@ bool handle_last_command_result() g_pCurrentModel->processesPriorities.iFreqARM = params.freq_arm; g_pCurrentModel->processesPriorities.iFreqGPU = params.freq_gpu; g_pCurrentModel->processesPriorities.iOverVoltage = params.overvoltage; + g_pCurrentModel->processesPriorities.uProcessesFlags = params.uProcessesFlags; saveControllerModel(g_pCurrentModel); + send_model_changed_message_to_router(MODEL_CHANGED_GENERIC, 0); } break; @@ -1506,7 +1511,7 @@ bool handle_last_command_result() tmp1 = (s_CommandParam & 0xFF); tmp2 = ((int)((s_CommandParam >> 8) & 0xFF)) - 128; - if ( tmp1 >= 0 && tmp1 < g_pCurrentModel->radioInterfacesParams.interfaces_count ) + if ( (tmp1 >= 0) && (tmp1 < g_pCurrentModel->radioInterfacesParams.interfaces_count) ) { g_pCurrentModel->radioInterfacesParams.interface_card_model[tmp1] = tmp2; saveControllerModel(g_pCurrentModel); @@ -1785,6 +1790,29 @@ bool handle_last_command_result() case COMMAND_ID_SET_TX_POWERS: { u8* pData = &s_CommandBuffer[0]; + + if ( s_CommandBufferLength >= 11 ) + { + if ( ( s_CommandBuffer[8] == 0x81 ) && ( s_CommandBuffer[10] == 0x81 ) ) + { + int iSiKPower = s_CommandBuffer[9]; + log_line("[Commands] Received message confirmation for SiK radio power level: %d", iSiKPower); + + if ( iSiKPower > 0 && iSiKPower < 30 ) + if ( g_pCurrentModel->radioInterfacesParams.txPowerSiK != iSiKPower ) + { + log_line("[Commands] Updated current model's SiK radio power level to %d", iSiKPower); + g_pCurrentModel->radioInterfacesParams.txPowerSiK = iSiKPower; + } + } + else + log_softerror_and_alarm("[Commands] Received invalid power levels message response (for SiK radio interfaces). Ignoring it."); + saveControllerModel(g_pCurrentModel); + send_model_changed_message_to_router(MODEL_CHANGED_GENERIC, 0); + menu_refresh_all_menus(); + break; + } + u8 txPowerRTL8812AU = *pData; pData++; u8 txPowerRTL8812EU = *pData; @@ -1806,6 +1834,9 @@ bool handle_last_command_result() txPowerRTL8812AU, txPowerRTL8812EU, txPowerAtheros, txMaxPowerRTL8812AU, txMaxPowerRTL8812EU, txMaxPowerAtheros, cardIndex, cardPower); + log_line("[Commands] Current model radio power levels: 8812AU: %d, 8812EU: %d, Atheros: %d, Max: %d, %d, %d", g_pCurrentModel->radioInterfacesParams.txPowerRTL8812AU, g_pCurrentModel->radioInterfacesParams.txPowerRTL8812EU, g_pCurrentModel->radioInterfacesParams.txPowerAtheros, + g_pCurrentModel->radioInterfacesParams.txMaxPowerRTL8812AU, g_pCurrentModel->radioInterfacesParams.txMaxPowerRTL8812EU, g_pCurrentModel->radioInterfacesParams.txMaxPowerAtheros); + if ( (txMaxPowerRTL8812AU > 0) && (txMaxPowerRTL8812AU != 0xFF) ) g_pCurrentModel->radioInterfacesParams.txMaxPowerRTL8812AU = txMaxPowerRTL8812AU; if ( (txMaxPowerRTL8812EU > 0) && (txMaxPowerRTL8812EU != 0xFF) ) @@ -1824,24 +1855,8 @@ bool handle_last_command_result() if ( cardIndex < g_pCurrentModel->radioInterfacesParams.interfaces_count ) g_pCurrentModel->radioInterfacesParams.interface_power[cardIndex] = cardPower; - if ( s_CommandBufferLength >= 11 ) - { - if ( ( s_CommandBuffer[8] == 0x81 ) && ( s_CommandBuffer[10] == 0x81 ) ) - { - int iSiKPower = s_CommandBuffer[9]; - log_line("[Commands] Received message confirmation for SiK radio power level: %d", iSiKPower); - - if ( iSiKPower > 0 && iSiKPower < 30 ) - if ( g_pCurrentModel->radioInterfacesParams.txPowerSiK != iSiKPower ) - { - log_line("[Commands] Updated current model's SiK radio power level to %d", iSiKPower); - g_pCurrentModel->radioInterfacesParams.txPowerSiK = iSiKPower; - } - } - else - log_softerror_and_alarm("[Commands] Received invalid power levels message response (for SiK radio interfaces). Ignoring it."); - } - saveControllerModel(g_pCurrentModel); + saveControllerModel(g_pCurrentModel); + send_model_changed_message_to_router(MODEL_CHANGED_GENERIC, 0); menu_refresh_all_menus(); break; } @@ -2164,7 +2179,9 @@ void _handle_commands_on_command_timeout() warnings_remove_configuring_radio_link(false); link_reset_reconfiguring_radiolink(); - MenuConfirmation* pMC = new MenuConfirmation("Unsupported Parameter","Your vehicle radio link does not support this combination of radio params.", -1, true); + char szTextW[256]; + sprintf(szTextW, "Your %s radio link does not support this combination of radio params.", g_pCurrentModel->getVehicleTypeString()); + MenuConfirmation* pMC = new MenuConfirmation("Unsupported Parameter", szTextW, -1, true); pMC->m_yPos = 0.3; add_menu_to_stack(pMC); menu_invalidate_all(); diff --git a/code/r_central/keyboard.cpp b/code/r_central/keyboard.cpp index 63247ff0..287dd457 100644 --- a/code/r_central/keyboard.cpp +++ b/code/r_central/keyboard.cpp @@ -339,9 +339,9 @@ int _read_keyboard_input_events() else if ( (events[k].code == 14) || (events[k].code == 1) ) uEvent = INPUT_EVENT_PRESS_BACK; else if ( (events[k].code == 103) || (events[k].code == 22) || (events[k].code == 72) || (events[k].code == 75) ) - uEvent = INPUT_EVENT_PRESS_MINUS; - else if ( (events[k].code == 108) || (events[k].code == 32) || (events[k].code == 80) || (events[k].code == 77) ) uEvent = INPUT_EVENT_PRESS_PLUS; + else if ( (events[k].code == 108) || (events[k].code == 32) || (events[k].code == 80) || (events[k].code == 77) ) + uEvent = INPUT_EVENT_PRESS_MINUS; else if ( (events[k].code == 2) || (events[k].code == 79) ) uEvent = INPUT_EVENT_PRESS_QA1; else if ( (events[k].code == 3) ) diff --git a/code/r_central/menu/menu.cpp b/code/r_central/menu/menu.cpp index 61c47bde..a8b506de 100644 --- a/code/r_central/menu/menu.cpp +++ b/code/r_central/menu/menu.cpp @@ -616,11 +616,15 @@ void menu_loop_parse_input_events() g_pPopupCameraParams->handleRotaryEvents(false, false, false, false, false, true); return; } + bool bHasModalMenu = false; for( int i=0; iisModal(); if ( NULL != g_pMenuStack[i] ) g_pMenuStack[i]->onBack(); - - menu_discard_all(); + } + if ( ! bHasModalMenu ) + menu_discard_all(); return; } diff --git a/code/r_central/menu/menu_color_picker.cpp b/code/r_central/menu/menu_color_picker.cpp index d9fe1722..5d66dfc2 100644 --- a/code/r_central/menu/menu_color_picker.cpp +++ b/code/r_central/menu/menu_color_picker.cpp @@ -34,6 +34,7 @@ #include "../../base/config.h" #include "../../base/commands.h" #include "../colors.h" +#include "../fonts.h" #include "../osd/osd_common.h" #include "../../renderer/render_engine.h" #include "menu.h" @@ -168,6 +169,9 @@ void MenuColorPicker::onSelectItem() } if ( bChanged ) - if ( m_ColorType == COLORPICKER_TYPE_OSD ) + if ( (m_ColorType == COLORPICKER_TYPE_OSD) || (m_ColorType == COLORPICKER_TYPE_OSD_OUTLINE) ) + { osd_reload_msp_resources(); + loadAllFonts(true); + } } \ No newline at end of file diff --git a/code/r_central/menu/menu_confirmation_vehicle_board.cpp b/code/r_central/menu/menu_confirmation_vehicle_board.cpp index b3da3957..5f3b16db 100644 --- a/code/r_central/menu/menu_confirmation_vehicle_board.cpp +++ b/code/r_central/menu/menu_confirmation_vehicle_board.cpp @@ -45,8 +45,13 @@ MenuConfirmationVehicleBoard::MenuConfirmationVehicleBoard() { m_xPos = 0.35; m_yPos = 0.35; m_Width = 0.30; - addTopLine("The hardware detected on the vehicle has multiple variants."); - addTopLine("Select the type of your vehicle board:"); + char szText[256]; + sprintf(szText, "Select your %s board type", g_pCurrentModel->getVehicleTypeString()); + setTitle(szText); + sprintf(szText, "The hardware detected on the %s has multiple variants.", g_pCurrentModel->getVehicleTypeString()); + addTopLine(szText); + sprintf(szText, "Select the type of your %s board:", g_pCurrentModel->getVehicleTypeString()); + addTopLine(szText); addTopLine("(You can later change it from vehicle menu)"); addMenuItem(new MenuItem("Generic Sigmastar SSC338Q board")); addMenuItem(new MenuItem("Ultrasight AIO")); diff --git a/code/r_central/menu/menu_controller.cpp b/code/r_central/menu/menu_controller.cpp index dad432c8..53845f28 100644 --- a/code/r_central/menu/menu_controller.cpp +++ b/code/r_central/menu/menu_controller.cpp @@ -338,7 +338,12 @@ void MenuController::onSelectItem() if ( g_VehiclesRuntimeInfo[g_iCurrentActiveVehicleRuntimeInfoIndex].bGotFCTelemetry ) if ( g_VehiclesRuntimeInfo[g_iCurrentActiveVehicleRuntimeInfoIndex].headerFCTelemetry.flags & FC_TELE_FLAGS_ARMED ) { - MenuConfirmation* pMC = new MenuConfirmation("Warning! Reboot Confirmation","Your vehicle is armed. Are you sure you want to reboot the controller?", 10); + char szText[256]; + if ( NULL != g_VehiclesRuntimeInfo[g_iCurrentActiveVehicleRuntimeInfoIndex].pModel ) + sprintf(szText, "Your %s is armed. Are you sure you want to reboot the controller?", g_VehiclesRuntimeInfo[g_iCurrentActiveVehicleRuntimeInfoIndex].pModel->getVehicleTypeString()); + else + strcpy(szText, "Your vehicle is armed. Are you sure you want to reboot the controller?"); + MenuConfirmation* pMC = new MenuConfirmation("Warning! Reboot Confirmation", szText, 10); if ( g_pCurrentModel->rc_params.rc_enabled ) { pMC->addTopLine(" "); diff --git a/code/r_central/menu/menu_controller_dev.cpp b/code/r_central/menu/menu_controller_dev.cpp index d888e741..e37671f8 100644 --- a/code/r_central/menu/menu_controller_dev.cpp +++ b/code/r_central/menu/menu_controller_dev.cpp @@ -145,8 +145,8 @@ void MenuControllerDev::addItems() addMenuItem(new MenuItemSection("Other Settings")); m_IndexVersion = addMenuItem(new MenuItem("Modules versions", "Get all modules versions.")); - m_IndexResetDev = addMenuItem(new MenuItem("Reset Developer Settings", "Resets all the developer settings to the factory default values.")); + m_IndexExit = addMenuItem(new MenuItem("Exit to shell", "Closes Ruby and exits to linux shell.")); for( int i=0; isetTextColor(get_Color_Dev()); @@ -434,5 +434,11 @@ void MenuControllerDev::onSelectItem() add_menu_to_stack(new MenuControllerDevStatsConfig()); return; } + + if ( m_IndexExit == m_SelectedIndex ) + { + pairing_stop(); + g_bQuit = true; + } } diff --git a/code/r_central/menu/menu_controller_dev.h b/code/r_central/menu/menu_controller_dev.h index 2d2d697f..701a1d02 100644 --- a/code/r_central/menu/menu_controller_dev.h +++ b/code/r_central/menu/menu_controller_dev.h @@ -31,4 +31,5 @@ class MenuControllerDev: public Menu int m_IndexFreezeOSD; int m_IndexVersion; int m_IndexResetDev; + int m_IndexExit; }; \ No newline at end of file diff --git a/code/r_central/menu/menu_controller_dev_stats.cpp b/code/r_central/menu/menu_controller_dev_stats.cpp index 25afc315..e4e7a7d9 100644 --- a/code/r_central/menu/menu_controller_dev_stats.cpp +++ b/code/r_central/menu/menu_controller_dev_stats.cpp @@ -121,6 +121,14 @@ void MenuControllerDevStatsConfig::addItems() m_pItemsSelect[7]->setSelectedIndex( (pP->uDebugStatsFlags & CTRL_RT_DEBUG_INFO_FLAG_SHOW_MIN_MAX_ACK_TIME)?1:0); m_IndexShowMinMaxAckTime = addMenuItem(m_pItemsSelect[7]); + m_pItemsSelect[12] = new MenuItemSelect("Show Ack time History", ""); + m_pItemsSelect[12]->addSelection("No"); + m_pItemsSelect[12]->addSelection("Yes"); + m_pItemsSelect[12]->setUseMultiViewLayout(); + m_pItemsSelect[12]->setSelectedIndex( (pP->uDebugStatsFlags & CTRL_RT_DEBUG_INFO_FLAG_SHOW_ACK_TIME_HISTORY)?1:0); + m_IndexShowAckTimeHist = addMenuItem(m_pItemsSelect[12]); + + m_pItemsSelect[8] = new MenuItemSelect("Show RX Max EC used", ""); m_pItemsSelect[8]->addSelection("No"); m_pItemsSelect[8]->addSelection("Yes"); @@ -252,6 +260,14 @@ void MenuControllerDevStatsConfig::onSelectItem() pP->uDebugStatsFlags &= ~CTRL_RT_DEBUG_INFO_FLAG_SHOW_MIN_MAX_ACK_TIME; } + if ( m_IndexShowAckTimeHist == m_SelectedIndex ) + { + if ( m_pItemsSelect[12]->getSelectedIndex() != 0 ) + pP->uDebugStatsFlags |= CTRL_RT_DEBUG_INFO_FLAG_SHOW_ACK_TIME_HISTORY; + else + pP->uDebugStatsFlags &= ~CTRL_RT_DEBUG_INFO_FLAG_SHOW_ACK_TIME_HISTORY; + } + if ( m_IndexShowRxMaxECUsed == m_SelectedIndex ) { if ( m_pItemsSelect[8]->getSelectedIndex() != 0 ) diff --git a/code/r_central/menu/menu_controller_dev_stats.h b/code/r_central/menu/menu_controller_dev_stats.h index bb5bd767..a66083af 100644 --- a/code/r_central/menu/menu_controller_dev_stats.h +++ b/code/r_central/menu/menu_controller_dev_stats.h @@ -26,6 +26,7 @@ class MenuControllerDevStatsConfig: public Menu int m_IndexShowRxMissingPacketsMaxGap; int m_IndexShowRxConsumedPackets; int m_IndexShowMinMaxAckTime; + int m_IndexShowAckTimeHist; int m_IndexShowRxMaxECUsed; int m_IndexShowUnrecoverableVideoBlocks; int m_IndexShowVideoProfileChanges; diff --git a/code/r_central/menu/menu_controller_expert.cpp b/code/r_central/menu/menu_controller_expert.cpp index 4d235389..44b91866 100644 --- a/code/r_central/menu/menu_controller_expert.cpp +++ b/code/r_central/menu/menu_controller_expert.cpp @@ -630,7 +630,12 @@ void MenuControllerExpert::onSelectItem() if ( g_VehiclesRuntimeInfo[g_iCurrentActiveVehicleRuntimeInfoIndex].bGotFCTelemetry ) if ( g_VehiclesRuntimeInfo[g_iCurrentActiveVehicleRuntimeInfoIndex].headerFCTelemetry.flags & FC_TELE_FLAGS_ARMED ) { - MenuConfirmation* pMC = new MenuConfirmation("Warning! Reboot Confirmation","Your vehicle is armed. Are you sure you want to reboot the controller?", 1); + char szText[256]; + if ( NULL != g_VehiclesRuntimeInfo[g_iCurrentActiveVehicleRuntimeInfoIndex].pModel ) + sprintf(szText, "Your %s is armed. Are you sure you want to reboot the controller?", g_VehiclesRuntimeInfo[g_iCurrentActiveVehicleRuntimeInfoIndex].pModel->getVehicleTypeString()); + else + strcpy(szText, "Your vehicle is armed. Are you sure you want to reboot the controller?"); + MenuConfirmation* pMC = new MenuConfirmation("Warning! Reboot Confirmation", szText, 1); if ( g_pCurrentModel->rc_params.rc_enabled ) { pMC->addTopLine(" "); diff --git a/code/r_central/menu/menu_controller_network.cpp b/code/r_central/menu/menu_controller_network.cpp index 2bf45e21..3dda8d52 100644 --- a/code/r_central/menu/menu_controller_network.cpp +++ b/code/r_central/menu/menu_controller_network.cpp @@ -183,10 +183,27 @@ void MenuControllerNetwork::onSelectItem() if ( m_IndexSSH == m_SelectedIndex ) { + #if defined(HW_PLATFORM_RASPBERRY) hw_execute_bash_command("touch /boot/ssh", NULL); + #endif + + #if defined(HW_PLATFORM_RADXA_ZERO3) + hw_execute_bash_command("sudo systemctl enable ssh", NULL); + #endif + + menu_discard_all(); + popups_remove_all(); addMessage("SSH enabled. Controller will reboot now."); + for( int i=0; i<5; i++ ) - hardware_sleep_ms(400); + { + ruby_processing_loop(true); + g_TimeNow = get_current_timestamp_ms(); + render_all(g_TimeNow); + ruby_signal_alive(); + hardware_sleep_ms(200); + } + onEventReboot(); hardware_reboot(); } } diff --git a/code/r_central/menu/menu_negociate_radio.cpp b/code/r_central/menu/menu_negociate_radio.cpp new file mode 100644 index 00000000..8f0eecb0 --- /dev/null +++ b/code/r_central/menu/menu_negociate_radio.cpp @@ -0,0 +1,401 @@ +/* + Ruby Licence + Copyright (c) 2024 Petru Soroaga petrusoroaga@yahoo.com + All rights reserved. + + Redistribution and use in source and/or binary forms, with or without + modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Copyright info and developer info must be preserved as is in the user + interface, additions could be made to that info. + * Neither the name of the organization nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + * Military use is not permited. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + DISCLAIMED. IN NO EVENT SHALL THE AUTHOR (PETRU SOROAGA) BE LIABLE FOR ANY + DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include "menu.h" +#include "menu_negociate_radio.h" +#include "menu_item_select.h" +#include "menu_item_section.h" +#include "menu_item_text.h" +#include "menu_item_legend.h" +#include "../process_router_messages.h" + +MenuNegociateRadio::MenuNegociateRadio(void) +:Menu(MENU_ID_NEGOCIATE_RADIO, "Initial Auto Radio Link Adjustment", NULL) +{ + m_Width = 0.6; + m_xPos = 0.18; m_yPos = 0.26; + float height_text = g_pRenderEngine->textHeight(g_idFontMenu); + addExtraHeightAtEnd(2.0*height_text + height_text * 1.5 * hardware_get_radio_interfaces_count()); + m_uShowTime = g_TimeNow; + m_MenuIndexCancel = -1; + m_iCounter = 0; + addTopLine("Doing the initial radio link parameters adjustment for best performance..."); + addTopLine("(This is done on first installation and on first pairing with a vehicle or when hardware has changed on the vehicle)"); + + strcpy(m_szStatusMessage, "Please wait, it will take a minute"); + addTopLine(m_szStatusMessage); + free(m_szTopLines[2]); + m_szTopLines[2] = (char*)malloc(256); + strcpy(m_szTopLines[2], m_szStatusMessage); + + for( int i=0; i<20; i++ ) + for( int k=0; ktextHeight(g_idFontMenu); + float wPixel = g_pRenderEngine->getPixelWidth(); + float hPixel = g_pRenderEngine->getPixelHeight(); + y += height_text*0.5; + g_pRenderEngine->setColors(get_Color_MenuText()); + float fTextWidth = g_pRenderEngine->textWidth(g_idFontMenu, "Computing Link Quality (you can press [Cancel] to cancel)"); + g_pRenderEngine->drawText(m_RenderXPos+m_sfMenuPaddingX + 0.5 * (m_RenderWidth-2.0*m_sfMenuPaddingX - fTextWidth), y, g_idFontMenu, "Computing Link Quality (you can press [Cancel] to cancel)"); + y += height_text*1.5; + + float hBar = height_text*1.5; + float wBar = height_text*0.5; + float x = m_RenderXPos+m_sfMenuPaddingX; + + x += 0.5*(m_RenderWidth - 2.0*m_sfMenuPaddingX - (wBar+4.0*wPixel)*3*g_ArrayTestRadioRatesCount); + + _computeQualities(); + + int iIndexBestQuality = -1; + int iRunBestQuality = -1; + float fBestQ = 0; + + for( int iTest=0; iTest 0 ) + fQuality = (float)m_iRXPackets[iTest][iInt][iRun]/(float)(m_iRXPackets[iTest][iInt][iRun] + m_iRxLostPackets[iTest][iInt][iRun]); + if ( m_iRXPackets[iTest][iInt][iRun] < 100 ) + if ( (iTest < m_iDataRateIndex) || ((iTest == m_iDataRateIndex) && (iRun <= m_iDataRateTestCount)) ) + fQuality = 0.1; + + if ( fQuality > fBestQ ) + { + fBestQ = fQuality; + iIndexBestQuality = iTest; + iRunBestQuality = iRun; + } + g_pRenderEngine->setFill(0,0,0,0); + g_pRenderEngine->drawRect(x + (iTest*3+iRun)*(wBar+4.0*wPixel), y + iInt*(hBar+hPixel*2.0), wBar, hBar-2.0*hPixel); + + if ( fQuality > 0.99 ) + g_pRenderEngine->setFill(0,200,0,1.0); + else if ( fQuality > 0.95 ) + g_pRenderEngine->setColors(get_Color_MenuText()); + else if ( fQuality > 0.9 ) + g_pRenderEngine->setFill(200,200,0,1.0); + else if ( fQuality > 0.0001 ) + { + fQuality = 0.3; + g_pRenderEngine->setFill(200,0,0,1.0); + } + else + g_pRenderEngine->setFill(0,0,0,0); + g_pRenderEngine->drawRect(x + (iTest*3+iRun)*(wBar+4.0*wPixel) + wPixel, y + iInt*(hBar+hPixel*2.0) + hBar - fQuality*hBar - hPixel, wBar - 2.0*wPixel, fQuality*(hBar-2.0*hPixel)); + } + + if ( iIndexBestQuality >= 0 ) + { + g_pRenderEngine->setFill(0,0,0,0); + g_pRenderEngine->drawRect(x + (iIndexBestQuality*3+iRunBestQuality)*(wBar+4.0*wPixel) - 3.0*wPixel, + y - hPixel*4.0, wBar+5.0*wPixel, (hBar+2.0*hPixel)*hardware_get_radio_interfaces_count() + 6.0*hPixel); + } + + RenderEnd(yTop); +} + +int MenuNegociateRadio::onBack() +{ + if ( -1 != m_MenuIndexCancel ) + { + _onCancel(); + } + return 0; +} + +void MenuNegociateRadio::_computeQualities() +{ + for( int iTest=0; iTest 0 ) + fQuality = (float)m_iRXPackets[iTest][iInt][iRun]/(float)(m_iRXPackets[iTest][iInt][iRun] + m_iRxLostPackets[iTest][iInt][iRun]); + if ( m_iRXPackets[iTest][iInt][iRun] < 100 ) + fQuality = 0.1; + + if ( fQuality > m_fQualities[iTest] ) + m_fQualities[iTest] = fQuality; + } + } +} + +void MenuNegociateRadio::_send_command_to_vehicle() +{ + t_packet_header PH; + radio_packet_init(&PH, PACKET_COMPONENT_RUBY, PACKET_TYPE_NEGOCIATE_RADIO_LINKS, STREAM_ID_DATA); + PH.vehicle_id_src = g_uControllerId; + PH.vehicle_id_dest = g_pCurrentModel->uVehicleId; + PH.total_length = sizeof(t_packet_header) + 2*sizeof(u8) + sizeof(u32); + + u8 buffer[1024]; + + memcpy(buffer, (u8*)&PH, sizeof(t_packet_header)); + u8 uType = 0; + u32 uParam = 0; + int iParam = 0; + + if ( m_iStep == NEGOCIATE_RADIO_STEP_DATA_RATE ) + { + iParam = g_ArrayTestRadioRates[m_iDataRateIndex]; + memcpy(&uParam, &iParam, sizeof(int)); + } + + if ( m_iStep == NEGOCIATE_RADIO_STEP_END ) + { + iParam = m_iDataRateToApply; + memcpy(&uParam, &iParam, sizeof(int)); + log_line("Sending to vehicle final video radio datarate: %d (int size: %d, u32 size: %d)", m_iDataRateToApply, (int)sizeof(int), (int)sizeof(u32)); + } + + buffer[sizeof(t_packet_header)] = uType; + buffer[sizeof(t_packet_header)+sizeof(u8)] = (u8)m_iStep; + memcpy(&(buffer[0]) + sizeof(t_packet_header) + 2*sizeof(u8), &uParam, sizeof(u32)); + + radio_packet_compute_crc(buffer, PH.total_length); + send_packet_to_router(buffer, PH.total_length); + + m_uLastTimeSendCommandToVehicle = g_TimeNow; +} + +void MenuNegociateRadio::_switch_to_step(int iStep) +{ + m_iStep = iStep; + + if ( m_iStep == NEGOCIATE_RADIO_STEP_END ) + { + strcpy(m_szStatusMessage, "Done. Saving parameters."); + + m_iDataRateToApply = 0; + + _computeQualities(); + float fQuality18 = m_fQualities[0]; + float fQuality24 = m_fQualities[1]; + float fQuality48 = m_fQualities[3]; + float fQualityMCS2 = m_fQualities[6]; + float fQualityMCS3 = m_fQualities[7]; + float fQualityMCS4 = m_fQualities[8]; + + if ( fQualityMCS4 > 0.99 ) + m_iDataRateToApply = -5; + else if ( fQualityMCS3 > 0.99 ) + m_iDataRateToApply = -4; + else if ( fQualityMCS2 > fQuality18 ) + m_iDataRateToApply = -3; + else if ( fQuality48 > 0.99 ) + m_iDataRateToApply = 48000000; + else if ( fQuality24 > 0.99 ) + m_iDataRateToApply = 24000000; + else + m_iDataRateToApply = 18000000; + + log_line("Computed Q: 18: %.3f, 24: %.3f, 48: %.3f, MCS2: %.3f, MCS3: %.3f, MCS4: %.3f ", + fQuality18, fQuality24, fQuality48, fQualityMCS2, fQualityMCS3, fQualityMCS4); + log_line("Appling datarate: %d", m_iDataRateToApply); + } + if ( m_iStep == NEGOCIATE_RADIO_STEP_CANCEL ) + strcpy(m_szStatusMessage, "Canceling, please wait."); + + m_bWaitingVehicleConfirmation = true; + m_uStepStartTime = g_TimeNow; + _send_command_to_vehicle(); + +} + + +bool MenuNegociateRadio::periodicLoop() +{ + m_iCounter++; + if ( -1 == m_MenuIndexCancel ) + if ( g_TimeNow > m_uShowTime + 4000 ) + { + m_MenuIndexCancel = addMenuItem(new MenuItem("Cancel", "Aborts the autoadjustment procedure without making any changes.")); + invalidate(); + } + + strcpy(m_szTopLines[2], m_szStatusMessage); + for(int i=0; i<(m_iCounter%4); i++ ) + strcat(m_szTopLines[2], "."); + + if ( m_bWaitingVehicleConfirmation ) + { + if ( g_TimeNow > m_uLastTimeSendCommandToVehicle + 100 ) + { + _send_command_to_vehicle(); + } + } + else + { + if ( m_iStep == NEGOCIATE_RADIO_STEP_DATA_RATE ) + { + for(int i=0; i m_uStepStartTime + 2000 ) + { + if ( m_iDataRateIndex < g_ArrayTestRadioRatesCount ) + { + m_iDataRateTestCount++; + if ( m_iDataRateTestCount > 2 ) + { + m_iDataRateIndex++; + m_iDataRateTestCount = 0; + } + if ( m_iDataRateIndex >= g_ArrayTestRadioRatesCount ) + m_iStep = NEGOCIATE_RADIO_STEP_END; + _switch_to_step(m_iStep); + } + } + } + return false; + } + + if ( m_iStep == NEGOCIATE_RADIO_STEP_CANCEL ) + if ( g_TimeNow > m_uStepStartTime + 5000 ) + { + setModal(false); + menu_stack_pop(0); + } + return false; +} + +void MenuNegociateRadio::onReceivedVehicleResponse(u8* pPacketData, int iPacketLength) +{ + if ( NULL == pPacketData ) + return; + + t_packet_header* pPH = (t_packet_header*)pPacketData; + + if ( pPH->packet_type != PACKET_TYPE_NEGOCIATE_RADIO_LINKS ) + return; + + u8 uCommand = pPacketData[sizeof(t_packet_header) + sizeof(u8)]; + u32 uParam = 0; + int iParam = 0; + memcpy(&uParam, pPacketData + sizeof(t_packet_header) + 2*sizeof(u8), sizeof(u32)); + memcpy(&iParam, &uParam, sizeof(int)); + log_line("Received negociate radio link conf, command %d, param: %d", uCommand, iParam); + + + if ( uCommand != m_iStep ) + return; + if ( (uCommand == NEGOCIATE_RADIO_STEP_END) || (uCommand == NEGOCIATE_RADIO_STEP_CANCEL ) ) + { + if ( uCommand == NEGOCIATE_RADIO_STEP_END ) + { + log_line("Saving new video radio datarate on current model: %d", m_iDataRateToApply); + g_pCurrentModel->resetVideoLinkProfilesToDataRates(m_iDataRateToApply, m_iDataRateToApply); + g_pCurrentModel->radioLinksParams.uGlobalRadioLinksFlags |= MODEL_RADIOLINKS_FLAGS_HAS_NEGOCIATED_LINKS; + saveControllerModel(g_pCurrentModel); + send_model_changed_message_to_router(MODEL_CHANGED_GENERIC, 0); + } + setModal(false); + menu_stack_pop(0); + } + m_bWaitingVehicleConfirmation = false; +} + +void MenuNegociateRadio::onReturnFromChild(int iChildMenuId, int returnValue) +{ + Menu::onReturnFromChild(iChildMenuId, returnValue); +} + +void MenuNegociateRadio::_onCancel() +{ + if ( -1 != m_MenuIndexCancel ) + { + removeMenuItem(m_pMenuItems[0]); + m_MenuIndexCancel = -1; + } + _switch_to_step(NEGOCIATE_RADIO_STEP_CANCEL); +} + +void MenuNegociateRadio::onSelectItem() +{ + Menu::onSelectItem(); + if ( -1 == m_SelectedIndex ) + return; + + if ( handle_commands_is_command_in_progress() ) + { + handle_commands_show_popup_progress(); + return; + } + + if ( (-1 != m_MenuIndexCancel) && (m_MenuIndexCancel == m_SelectedIndex) ) + { + _onCancel(); + } +} diff --git a/code/r_central/menu/menu_negociate_radio.h b/code/r_central/menu/menu_negociate_radio.h new file mode 100644 index 00000000..da6ccebc --- /dev/null +++ b/code/r_central/menu/menu_negociate_radio.h @@ -0,0 +1,40 @@ +#pragma once +#include "menu_objects.h" +#include "menu_item_select.h" + +class MenuNegociateRadio: public Menu +{ + public: + MenuNegociateRadio(); + virtual ~MenuNegociateRadio(); + virtual void Render(); + virtual void valuesToUI(); + virtual int onBack(); + virtual bool periodicLoop(); + virtual void onReturnFromChild(int iChildMenuId, int returnValue); + virtual void onSelectItem(); + + void onReceivedVehicleResponse(u8* pPacketData, int iPacketLength); + + private: + void _computeQualities(); + void _send_command_to_vehicle(); + void _switch_to_step(int iStep); + void _onCancel(); + MenuItemSelect* m_pItemsSelect[10]; + int m_MenuIndexCancel; + char m_szStatusMessage[256]; + int m_iCounter; + u32 m_uShowTime; + u32 m_uStepStartTime; + u32 m_uLastTimeSendCommandToVehicle; + bool m_bWaitingVehicleConfirmation; + int m_iStep; + int m_iDataRateIndex; + int m_iDataRateTestCount; + int m_iDataRateToApply; + + int m_iRXPackets[20][MAX_RADIO_INTERFACES][3]; + int m_iRxLostPackets[20][MAX_RADIO_INTERFACES][3]; + float m_fQualities[20]; +}; \ No newline at end of file diff --git a/code/r_central/menu/menu_objects.cpp b/code/r_central/menu/menu_objects.cpp index 79f6e338..b9415e72 100644 --- a/code/r_central/menu/menu_objects.cpp +++ b/code/r_central/menu/menu_objects.cpp @@ -83,6 +83,7 @@ Menu::Menu(int id, const char* title, const char* subTitle) m_MenuId = id; m_MenuDepth = 0; m_iColumnsCount = 1; + m_bIsModal = false; m_bEnableColumnSelection = true; m_bIsSelectingInsideColumn = false; m_bFullWidthSelection = false; @@ -198,6 +199,16 @@ void Menu::invalidate() m_pMenuItems[i]->invalidate(); } +void Menu::setModal(bool bModal) +{ + m_bIsModal = bModal; +} + +bool Menu::isModal() +{ + return m_bIsModal; +} + void Menu::disableScrolling() { m_bEnableScrolling = false; @@ -1370,7 +1381,8 @@ int Menu::onBack() return 1; } - menu_stack_pop(0); + if ( ! m_bIsModal ) + menu_stack_pop(0); return 0; } @@ -1820,7 +1832,16 @@ bool Menu::checkIsArmed() if ( g_VehiclesRuntimeInfo[g_iCurrentActiveVehicleRuntimeInfoIndex].bGotFCTelemetry ) if ( g_VehiclesRuntimeInfo[g_iCurrentActiveVehicleRuntimeInfoIndex].headerFCTelemetry.flags & FC_TELE_FLAGS_ARMED ) { - Popup* p = new Popup("Your vehicle is armed, you can't perform this operation now. Please stop/disarm your vehicle first.", 0.3, 0.3, 0.5, 6 ); + char szText[256]; + if ( NULL != g_VehiclesRuntimeInfo[g_iCurrentActiveVehicleRuntimeInfoIndex].pModel ) + { + char szModelType[64]; + strcpy(szModelType, g_VehiclesRuntimeInfo[g_iCurrentActiveVehicleRuntimeInfoIndex].pModel->getVehicleTypeString()); + sprintf(szText, "Your %s is armed, you can't perform this operation now. Please stop/disarm your %s first.", szModelType, szModelType); + } + else + strcpy(szText, "Your vehicle is armed, you can't perform this operation now. Please stop/disarm your vehicle first."); + Popup* p = new Popup(szText, 0.3, 0.3, 0.5, 6 ); p->setCentered(); p->setIconId(g_idIconError, get_Color_IconError()); popups_add_topmost(p); @@ -2157,6 +2178,8 @@ bool Menu::uploadSoftware() log_line("Successfully sent software package to vehicle."); bool bProcessingFailed = false; + char szProcessingError[256]; + szProcessingError[0] = 0; if ( get_sw_version_build(g_pCurrentModel) < 242 ) { @@ -2210,7 +2233,19 @@ bool Menu::uploadSoftware() render_commands_set_custom_status("Post update"); if ( s_uOTAStatus == OTA_UPDATE_STATUS_COMPLETED ) render_commands_set_custom_status("Finishing up"); - + if ( s_uOTAStatus == OTA_UPDATE_STATUS_FAILED ) + { + strcpy(szProcessingError, "Vehicle failed to process the update. Disk error."); + render_commands_set_custom_status(szProcessingError); + bProcessingFailed = true; + break; + } + if ( s_uOTAStatus == OTA_UPDATE_STATUS_FAILED_DISK_SPACE ) + { + strcpy(szProcessingError, "Vehicle failed to process the update. Not enough space on device."); + render_commands_set_custom_status(szProcessingError); + break; + } if ( g_TimeNow > (uTimeLastRender+100) ) { uTimeLastRender = g_TimeNow; @@ -2234,11 +2269,14 @@ bool Menu::uploadSoftware() ruby_resume_watchdog(); g_bUpdateInProgress = false; send_control_message_to_router(PACKET_TYPE_LOCAL_CONTROL_UPDATE_STOPED,0); + if ( 0 != szProcessingError[0] ) + addMessage(szProcessingError); return false; } g_nSucceededOTAUpdates++; - + g_bDidAnUpdate = true; + if ( NULL != g_pCurrentModel ) { for( int i=0; i<4; i++ ) diff --git a/code/r_central/menu/menu_objects.h b/code/r_central/menu/menu_objects.h index fb5b4407..c3dc0267 100644 --- a/code/r_central/menu/menu_objects.h +++ b/code/r_central/menu/menu_objects.h @@ -106,6 +106,7 @@ #define MENU_ID_CONTROLLER_DEV 119 #define MENU_ID_VEHICLE_BOARD 120 #define MENU_ID_CONTROLLER_DEV_STATS 121 +#define MENU_ID_NEGOCIATE_RADIO 122 #define MAX_MENU_ITEMS 150 @@ -154,9 +155,12 @@ class Menu float m_fAlfaWhenInBackground; bool m_bDisableBackgroundAlpha; bool m_bDisableStacking; + bool m_bIsModal; void setParent(Menu* pParent); void invalidate(); + void setModal(bool bModal); + bool isModal(); void disableScrolling(); void setTitle(const char* szTitle); void setSubTitle(const char* szSubTitle); diff --git a/code/r_central/menu/menu_preferences_ui.cpp b/code/r_central/menu/menu_preferences_ui.cpp index 4e597d06..e34da1e1 100644 --- a/code/r_central/menu/menu_preferences_ui.cpp +++ b/code/r_central/menu/menu_preferences_ui.cpp @@ -87,7 +87,7 @@ MenuPreferencesUI::MenuPreferencesUI(bool bShowOnlyOSD) m_IndexInvertColors = addMenuItem(m_pItemsSelect[2]); m_IndexColorPickerOSD = addMenuItem(new MenuItem("OSD Text Color","Change color of the text in the OSD.")); - m_IndexColorPickerOSDOutline = addMenuItem(new MenuItem("OSD Text Outline Color","Change color of the text outline in the OSD.")); + m_IndexColorPickerOSDOutline = addMenuItem(new MenuItem("OSD Outline Color","Change color of the outline in the OSD.")); m_pItemsSelect[3] = new MenuItemSelect("OSD Outline Thickness", "Increase/decrease OSD outline thickness."); m_pItemsSelect[3]->addSelection("None"); @@ -98,7 +98,7 @@ MenuPreferencesUI::MenuPreferencesUI(bool bShowOnlyOSD) m_pItemsSelect[3]->addSelection("Larger"); m_pItemsSelect[3]->addSelection("Largest"); //m_IndexOSDOutlineThickness = addMenuItem(m_pItemsSelect[3]); - m_IndexColorPickerOSDOutline = -1; + m_IndexOSDOutlineThickness = -1; m_pItemsSelect[6] = new MenuItemSelect("OSD Screen Size", "Change how big is the OSD relative to the screen."); diff --git a/code/r_central/menu/menu_radio_config.cpp b/code/r_central/menu/menu_radio_config.cpp index 6ec3e1b8..6c390579 100644 --- a/code/r_central/menu/menu_radio_config.cpp +++ b/code/r_central/menu/menu_radio_config.cpp @@ -84,6 +84,7 @@ MenuRadioConfig::MenuRadioConfig(void) m_fHeaderHeight = 0.0; m_fTotalHeightRadioConfig = 0.0; m_bComputedHeights = false; + m_bGoToFirstRadioLinkOnShow = false; m_bHasSwapInterfacesCommand = false; m_bHasRotateRadioLinksOrderCommand = false; for( int i=0; i&1 1>/dev/null", FOLDER_MEDIA); + hw_execute_bash_command(szComm, NULL); + sprintf(szComm, "chmod 777 %s* 2>&1 1>/dev/null", FOLDER_MEDIA); hw_execute_bash_command(szComm, NULL); media_scan_files(); diff --git a/code/r_central/menu/menu_system_expert.cpp b/code/r_central/menu/menu_system_expert.cpp deleted file mode 100644 index d3ebbc43..00000000 --- a/code/r_central/menu/menu_system_expert.cpp +++ /dev/null @@ -1,676 +0,0 @@ -/* - Ruby Licence - Copyright (c) 2024 Petru Soroaga petrusoroaga@yahoo.com - All rights reserved. - - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Copyright info and developer info must be preserved as is in the user - interface, additions could be made to that info. - * Neither the name of the organization nor the - names of its contributors may be used to endorse or promote products - derived from this software without specific prior written permission. - * Military use is not permited. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - DISCLAIMED. IN NO EVENT SHALL Julien Verneuil BE LIABLE FOR ANY - DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*/ - -#include "menu.h" -#include "menu_objects.h" -#include "menu_controller.h" -#include "menu_text.h" -#include "menu_system_expert.h" -#include "menu_system_dev_logs.h" -#include "menu_system_video_profiles.h" -#include "menu_item_section.h" -#include "menu_confirmation.h" -#include "menu_system_dev_stats.h" -#include "../../radio/radiolink.h" -#include "../../base/utils.h" -#include "../process_router_messages.h" -#include "../rx_scope.h" - -#include -#include -#include - - -MenuSystemExpert::MenuSystemExpert(void) -:Menu(MENU_ID_SYSTEM_EXPERT, "Developer Settings", NULL) -{ - m_Width = 0.34; - m_xPos = menu_get_XStartPos(m_Width); m_yPos = 0.16; - float fSliderWidth = 0.14 * m_sfScaleFactor; - - m_IndexLogs = -1; - //m_IndexLogs = addMenuItem( new MenuItem("Logs Settings") ); - //m_pMenuItems[m_IndexLogs]->showArrow(); - - m_IndexDevStats = addMenuItem( new MenuItem("Developer Stats Windows") ); - m_pMenuItems[m_IndexDevStats]->showArrow(); - - addMenuItem(new MenuItemSection("Video and Radio Link")); - - m_IndexVideoProfiles = addMenuItem(new MenuItem("Video Link Profiles", "Change video link profiles and params.")); - m_pMenuItems[m_IndexVideoProfiles]->showArrow(); - - m_pItemsSlider[0] = new MenuItemSlider("Max Radio Packet Size", "Maximum size in bytes that can be set for a radio packet in the user interface.", 100,1500,1250, fSliderWidth); - m_pItemsSlider[0]->setStep(10); - m_IndexPacket = addMenuItem(m_pItemsSlider[0]); - - char szTitle[128]; - if ( NULL == g_pCurrentModel ) - strcpy(szTitle, "Vehicle Radio Tx Type"); - else - { - if ( g_pCurrentModel->uDeveloperFlags & DEVELOPER_FLAGS_USE_PCAP_RADIO_TX ) - strcpy(szTitle, "Vehicle Radio Tx Type (now PPCAP)"); - else - strcpy(szTitle, "Vehicle Radio Tx Type (now socket)"); - } - m_pItemsSelect[9] = new MenuItemSelect(szTitle, "What method the vehicle uses for transmitting radio packets. Requires a vehicle reboot on change."); - m_pItemsSelect[9]->addSelection("Sockets"); - m_pItemsSelect[9]->addSelection("PPCAP"); - m_pItemsSelect[9]->setIsEditable(); - m_iIndexPCAPRadioTx = addMenuItem(m_pItemsSelect[9]); - - m_pItemsSelect[0] = new MenuItemSelect("RxTx Sync Type", "How the Rx/Tx time slots between vehicle and controller are synchronized."); - m_pItemsSelect[0]->addSelection("None"); - m_pItemsSelect[0]->addSelection("Basic"); - m_pItemsSelect[0]->addSelection("Advanced"); - m_pItemsSelect[0]->setIsEditable(); - m_IndexClockSync = addMenuItem(m_pItemsSelect[0]); - - m_pItemsSlider[7] = new MenuItemSlider("Ping/Clock Sync Frequency", "How often is the clock sync done with the vehicle.", 1,50,10, fSliderWidth); - m_pItemsSlider[7]->setStep(1); - m_IndexPingClockSpeed = addMenuItem(m_pItemsSlider[7]); - - m_pItemsSelect[5] = new MenuItemSelect("Video Link Overload Check", "Continously monitor the video link and if the video link is overloaded (takes too much time to transmit or has too big video bitrate for current radio datarate) will temporarly reduce the video bitrate."); - m_pItemsSelect[5]->addSelection("Off"); - m_pItemsSelect[5]->addSelection("On"); - m_pItemsSelect[5]->setUseMultiViewLayout(); - m_IndexVideoOverload = addMenuItem(m_pItemsSelect[5]); - - m_pItemsSlider[8] = new MenuItemSlider("Radio Reconfiguration Delay (ms)", "When 2.4/5.8Ghz radio interfaces need to be reconfigured, allow a delay for reconfiguration. Important for Atheros chipset cards mostly (in miliseconds).", 1,100,10, fSliderWidth); - m_pItemsSlider[8]->setStep(1); - m_IndexWiFiChangeDelay = addMenuItem(m_pItemsSlider[8]); - - m_pItemsSelect[1] = new MenuItemSelect("Radio Silence Failsafe", "Restarts the vehicle if there is radio silence for more than 1 minute."); - m_pItemsSelect[1]->addSelection("Disabled"); - m_pItemsSelect[1]->addSelection("Enabled"); - m_IndexRadioSilence = addMenuItem(m_pItemsSelect[1]); - - m_pItemsSlider[9] = new MenuItemSlider("Radio Rx Loop Check Max Time (ms)", "The threshold for generating an alarm when radio Rx loop takes too much time (in miliseconds).", 1,100,10, fSliderWidth); - m_pItemsSlider[9]->setStep(1); - m_IndexRxLoopTimeout = addMenuItem(m_pItemsSlider[9]); - - /* - m_pItemsSelect[4] = new MenuItemSelect("Radio CTS protection", "Enable CTS protection on the radio frames."); - m_pItemsSelect[4]->addSelection("No"); - m_pItemsSelect[4]->addSelection("Yes"); - m_pItemsSelect[4]->setUseMultiViewLayout(); - m_IndexFrameType = addMenuItem(m_pItemsSelect[4]); - - m_pItemsSlider[3] = new MenuItemSlider("Radio SlotTime", "Sets the 'slottime' parameter on the radio stack (only for Atheros chipsets).", 1,60,0, fSliderWidth); - m_IndexSlotTime = addMenuItem(m_pItemsSlider[3]); - - m_pItemsSlider[4] = new MenuItemSlider("Radio Thresh62", "Sets the 'thresh62' parameter on the radio stack (only for Atheros chipsets).", 1,60,0, fSliderWidth); - m_IndexThresh62 = addMenuItem(m_pItemsSlider[4]); - */ - - m_IndexFrameType = -1; - m_IndexSlotTime = -1; - m_IndexThresh62 = -1; - - addMenuItem(new MenuItemSection("Dev Tools")); - - m_pItemsSelect[3] = new MenuItemSelect("OSD Render FPS", "How often should the OSD be drawn."); - m_pItemsSelect[3]->addSelection("10"); - m_pItemsSelect[3]->addSelection("15"); - m_pItemsSelect[3]->addSelection("20"); - m_pItemsSelect[3]->addSelection("25"); - m_pItemsSelect[3]->setIsEditable(); - m_IndexRenderFSP = addMenuItem(m_pItemsSelect[3]); - - m_pItemsSelect[13] = new MenuItemSelect("Show UI/OSD CPU Usage", "Shows the CPU resources used by the UI and OSD interface."); - m_pItemsSelect[13]->addSelection("No"); - m_pItemsSelect[13]->addSelection("Yes"); - m_pItemsSelect[13]->setUseMultiViewLayout(); - m_IndexCPULoad = addMenuItem(m_pItemsSelect[13]); - - m_pItemsSelect[14] = new MenuItemSelect("Pause OSD", "Pause/Resume OSD using the [Cancel]/[Back] button."); - m_pItemsSelect[14]->addSelection("No"); - m_pItemsSelect[14]->addSelection("Yes"); - m_pItemsSelect[14]->setUseMultiViewLayout(); - m_IndexFreezeOSD = addMenuItem(m_pItemsSelect[14]); - - m_pItemsSelect[15] = new MenuItemSelect("Inject recoverable video tx faults", "Inject video tx faults: packets that can't be transmitted but could be retransmitted."); - m_pItemsSelect[15]->addSelection("No"); - m_pItemsSelect[15]->addSelection("Yes"); - m_pItemsSelect[15]->setUseMultiViewLayout(); - m_IndexInjectMinorFaults = addMenuItem(m_pItemsSelect[15]); - - m_pItemsSelect[12] = new MenuItemSelect("Inject unrecoverable video tx faults", "Inject video tx faults: packets that can't be transmitted and packets that can't be retransmitted too."); - m_pItemsSelect[12]->addSelection("No"); - m_pItemsSelect[12]->addSelection("Yes"); - m_pItemsSelect[12]->setUseMultiViewLayout(); - m_IndexInjectFaults = addMenuItem(m_pItemsSelect[12]); - - - m_IndexVersion = addMenuItem(new MenuItem("Modules versions", "Get all modules versions.")); - - m_IndexResetDev = addMenuItem(new MenuItem("Reset Developer Settings", "Resets all the developer settings to the factory default values.")); - - //m_IndexDetailedPackets = addMenuItem( new MenuItem("RX/TX Scope") ); - //m_IndexRXScope = addMenuItem( new MenuItem("RX Scope") ); - m_IndexDetailedPackets = -1; - m_IndexRXScope = -1; - - for( int i=0; isetTextColor(get_Color_Dev()); -} - -void MenuSystemExpert::valuesToUI() -{ - - Preferences* pP = get_Preferences(); - ControllerSettings* pCS = get_ControllerSettings(); - - m_pItemsSlider[0]->setCurrentValue(pP->iDebugMaxPacketSize); - m_pItemsSlider[7]->setCurrentValue(pCS->nPingClockSyncFrequency); - m_pItemsSlider[8]->setCurrentValue(pP->iDebugWiFiChangeDelay); - m_pItemsSlider[9]->setCurrentValue(pCS->iDevRxLoopTimeout); - - if ( NULL == g_pCurrentModel ) - { - m_pItemsSelect[0]->setSelection(0); - m_pItemsSelect[0]->setEnabled(false); - m_pItemsSelect[1]->setSelection(0); - m_pItemsSelect[1]->setEnabled(false); - - m_pItemsSelect[5]->setEnabled(false); - - //m_pItemsSelect[4]->setSelection(1); - //m_pItemsSlider[3]->setCurrentValue(1); - //m_pItemsSlider[4]->setCurrentValue(1); - - m_pItemsSelect[9]->setEnabled(false); - m_pItemsSelect[12]->setEnabled(false); - m_pItemsSelect[15]->setEnabled(false); - m_pMenuItems[m_IndexResetDev]->setEnabled(false); - } - else - { - m_pItemsSelect[0]->setEnabled(true); - m_pItemsSelect[0]->setSelection( g_pCurrentModel->rxtx_sync_type ); - m_pItemsSelect[1]->setEnabled(true); - m_pItemsSelect[1]->setSelection( (g_pCurrentModel->uDeveloperFlags & DEVELOPER_FLAGS_BIT_RADIO_SILENCE_FAILSAFE)?1:0 ); - - m_pItemsSelect[5]->setEnabled(true); - m_pItemsSelect[5]->setSelection( (g_pCurrentModel->uDeveloperFlags & DEVELOPER_FLAGS_BIT_DISABLE_VIDEO_OVERLOAD_CHECK)?0:1 ); - - //if ( g_pCurrentModel->nic_radio_flags[0] & RADIO_FLAGS_FRAME_TYPE_RTS ) - // m_pItemsSelect[4]->setSelection(0); - //else - // m_pItemsSelect[4]->setSelection(1); - //m_pItemsSlider[3]->setCurrentValue(g_pCurrentModel->slotTime); - //m_pItemsSlider[4]->setCurrentValue(g_pCurrentModel->thresh62); - - m_pItemsSelect[9]->setEnabled(true); - m_pItemsSelect[9]->setSelectedIndex(0); - if ( g_pCurrentModel->uDeveloperFlags & DEVELOPER_FLAGS_USE_PCAP_RADIO_TX ) - m_pItemsSelect[9]->setSelectedIndex(1); - - m_pItemsSelect[12]->setEnabled(true); - m_pItemsSelect[12]->setSelectedIndex(0); - if ( g_pCurrentModel->uDeveloperFlags & DEVELOPER_FLAGS_BIT_INJECT_VIDEO_FAULTS ) - m_pItemsSelect[12]->setSelectedIndex(1); - m_pItemsSelect[15]->setEnabled(true); - m_pItemsSelect[15]->setSelectedIndex(0); - if ( g_pCurrentModel->uDeveloperFlags & DEVELOPER_FLAGS_BIT_INJECT_RECOVERABLE_VIDEO_FAULTS ) - m_pItemsSelect[15]->setSelectedIndex(1); - - } - - m_pItemsSelect[3]->setSelection( (pCS->iRenderFPS-10)/5 ); - m_pItemsSelect[13]->setSelectedIndex(pP->iShowCPULoad); - m_pItemsSelect[14]->setSelectedIndex(pCS->iFreezeOSD); -} - -void MenuSystemExpert::onShow() -{ - Menu::onShow(); -} - - -void MenuSystemExpert::Render() -{ - RenderPrepare(); - float yTop = RenderFrameAndTitle(); - float y = yTop; - for( int i=0; iiDisableRetransmissionsAfterControllerLinkLostMiliseconds = DEFAULT_CONTROLLER_LINK_MILISECONDS_TIMEOUT_TO_DISABLE_RETRANSMISSIONS; - pCS->nRetryRetransmissionAfterTimeoutMS = DEFAULT_VIDEO_RETRANS_MINIMUM_RETRY_INTERVAL; - pCS->nRequestRetransmissionsOnVideoSilenceMs = DEFAULT_VIDEO_RETRANS_REQUEST_ON_VIDEO_SILENCE_MS; - save_ControllerSettings(); - - Menu* pm = new MenuConfirmation("Developer Settings Reset", "Vehicle and controller will reboot now.", 3, true); - pm->m_yPos = 0.4; - add_menu_to_stack(pm); - return; - } - - if ( 3 == iChildMenuId/1000 ) - { - hardware_reboot(); - } -} - - -void MenuSystemExpert::onSelectItem() -{ - Menu::onSelectItem(); - if ( m_pMenuItems[m_SelectedIndex]->isEditing() ) - return; - - Preferences* pP = get_Preferences(); - ControllerSettings* pCS = get_ControllerSettings(); - - if ( m_IndexDevStats == m_SelectedIndex ) - { - add_menu_to_stack(new MenuSystemDevStats()); - return; - } - - if ( m_IndexPacket == m_SelectedIndex ) - { - int val = m_pItemsSlider[0]->getCurrentValue(); - pP->iDebugMaxPacketSize = val; - save_Preferences(); - } - - if ( m_IndexClockSync == m_SelectedIndex ) - { - if ( NULL == g_pCurrentModel ) - return; - int rxtx = m_pItemsSelect[0]->getSelectedIndex(); - if ( rxtx != g_pCurrentModel->rxtx_sync_type ) - if ( ! handle_commands_send_to_vehicle(COMMAND_ID_SET_RXTX_SYNC_TYPE, rxtx, NULL, 0) ) - valuesToUI(); - } - - if ( m_IndexPingClockSpeed == m_SelectedIndex ) - { - pCS->nPingClockSyncFrequency = m_pItemsSlider[7]->getCurrentValue(); - save_ControllerSettings(); - send_control_message_to_router(PACKET_TYPE_LOCAL_CONTROL_CONTROLLER_CHANGED, PACKET_COMPONENT_LOCAL_CONTROL); - valuesToUI(); - } - - if ( m_IndexRenderFSP == m_SelectedIndex ) - { - pCS->iRenderFPS = 10 + m_pItemsSelect[3]->getSelectedIndex()*5; - save_ControllerSettings(); - valuesToUI(); - } - - if ( m_IndexWiFiChangeDelay == m_SelectedIndex ) - { - pP->iDebugWiFiChangeDelay = m_pItemsSlider[8]->getCurrentValue(); - save_Preferences(); - valuesToUI(); - return; - } - - if ( m_IndexRxLoopTimeout == m_SelectedIndex ) - { - pCS->iDevRxLoopTimeout = m_pItemsSlider[9]->getCurrentValue(); - save_ControllerSettings(); - valuesToUI(); - send_control_message_to_router(PACKET_TYPE_LOCAL_CONTROL_CONTROLLER_CHANGED, PACKET_COMPONENT_LOCAL_CONTROL); - - if ( ! handle_commands_send_developer_flags() ) - valuesToUI(); - return; - } - - if ( m_iIndexPCAPRadioTx == m_SelectedIndex ) - { - if ( NULL == g_pCurrentModel ) - return; - if ( 0 == m_pItemsSelect[9]->getSelectedIndex() ) - g_pCurrentModel->uDeveloperFlags &= (~DEVELOPER_FLAGS_USE_PCAP_RADIO_TX); - else - g_pCurrentModel->uDeveloperFlags |= DEVELOPER_FLAGS_USE_PCAP_RADIO_TX; - if ( ! handle_commands_send_developer_flags() ) - valuesToUI(); - return; - } - - if ( m_IndexRadioSilence == m_SelectedIndex ) - { - if ( NULL == g_pCurrentModel ) - return; - u32 enable = m_pItemsSelect[1]->getSelectedIndex(); - if ( 0 == enable ) - g_pCurrentModel->uDeveloperFlags &= (~DEVELOPER_FLAGS_BIT_RADIO_SILENCE_FAILSAFE); - else - g_pCurrentModel->uDeveloperFlags |= DEVELOPER_FLAGS_BIT_RADIO_SILENCE_FAILSAFE; - if ( ! handle_commands_send_developer_flags() ) - valuesToUI(); - return; - } - - if ( m_IndexFrameType == m_SelectedIndex && menu_check_current_model_ok_for_edit() ) - { - if ( NULL == g_pCurrentModel ) - { - addMessage("Radio CTS protection can't be changed. Connect to a vehicle to change it."); - return; - } - - return; - } - - - if ( m_IndexSlotTime == m_SelectedIndex && menu_check_current_model_ok_for_edit() ) - { - if ( NULL == g_pCurrentModel ) - { - addMessage("Radio SlotTime can't be changed. Connect to a vehicle to change it."); - return; - } - int val = m_pItemsSlider[3]->getCurrentValue(); - if ( ! handle_commands_send_to_vehicle(COMMAND_ID_SET_RADIO_SLOTTIME, val , NULL, 0) ) - valuesToUI(); - return; - } - - if ( m_IndexThresh62 == m_SelectedIndex && menu_check_current_model_ok_for_edit() ) - { - if ( NULL == g_pCurrentModel ) - { - addMessage("Radio Thresh62 can't be changed. Connect to a vehicle to change it."); - return; - } - int val = m_pItemsSlider[4]->getCurrentValue(); - if ( ! handle_commands_send_to_vehicle(COMMAND_ID_SET_RADIO_THRESH62, val , NULL, 0) ) - valuesToUI(); - return; - } - - if ( m_IndexVideoProfiles == m_SelectedIndex ) - { - add_menu_to_stack(new MenuSystemVideoProfiles()); - return; - } - - if ( -1 != m_IndexLogs ) - if ( m_IndexLogs == m_SelectedIndex ) - { - add_menu_to_stack(new MenuSystemDevLogs()); - return; - } - - if ( m_IndexDetailedPackets == m_SelectedIndex ) - { - t_packet_header PH; - radio_packet_init(&PH, PACKET_COMPONENT_LOCAL_CONTROL, PACKET_TYPE_LOCAL_CONTROL_DEBUG_SCOPE_STOP, STREAM_ID_DATA); - if ( g_bIsRouterPacketsHistoryGraphOn ) - { - g_bIsRouterPacketsHistoryGraphOn = false; - g_bIsRouterPacketsHistoryGraphPaused = false; - handle_commands_send_ruby_message(&PH, NULL, 0); - menu_discard_all(); - shared_mem_router_packets_stats_history_close(g_pDebugSMRPST); - return; - } - else - { - g_bIsRouterPacketsHistoryGraphOn = true; - g_bIsRouterPacketsHistoryGraphPaused = false; - PH.packet_type = PACKET_TYPE_LOCAL_CONTROL_DEBUG_SCOPE_START; - handle_commands_send_ruby_message(&PH, NULL, 0); - hardware_sleep_ms(500); - g_pDebugSMRPST = shared_mem_router_packets_stats_history_open_read(); - if ( NULL == g_pDebugSMRPST ) - { - log_softerror_and_alarm("Failed to open shared mem for read for router packets stats history."); - return; - } - menu_discard_all(); - return; - } - } - - if ( m_IndexCPULoad == m_SelectedIndex ) - { - pP->iShowCPULoad = m_pItemsSelect[13]->getSelectedIndex(); - save_Preferences(); - valuesToUI(); - return; - } - - if ( m_IndexVideoOverload == m_SelectedIndex ) - { - if ( NULL == g_pCurrentModel ) - return; - u32 enable = m_pItemsSelect[5]->getSelectedIndex(); - if ( 1 == enable ) - g_pCurrentModel->uDeveloperFlags &= (~DEVELOPER_FLAGS_BIT_DISABLE_VIDEO_OVERLOAD_CHECK); - else - g_pCurrentModel->uDeveloperFlags |= DEVELOPER_FLAGS_BIT_DISABLE_VIDEO_OVERLOAD_CHECK; - - if ( ! handle_commands_send_developer_flags() ) - valuesToUI(); - return; - } - - if ( m_IndexFreezeOSD == m_SelectedIndex ) - { - pCS->iFreezeOSD = m_pItemsSelect[14]->getSelectedIndex(); - save_ControllerSettings(); - valuesToUI(); - return; - } - - if ( m_IndexInjectFaults == m_SelectedIndex ) - { - if ( NULL == g_pCurrentModel ) - return; - int val = m_pItemsSelect[12]->getSelectedIndex(); - if ( 0 == val ) - g_pCurrentModel->uDeveloperFlags &= (~DEVELOPER_FLAGS_BIT_INJECT_VIDEO_FAULTS); - else - g_pCurrentModel->uDeveloperFlags |= DEVELOPER_FLAGS_BIT_INJECT_VIDEO_FAULTS; - if ( ! handle_commands_send_developer_flags() ) - valuesToUI(); - } - - if ( m_IndexInjectMinorFaults == m_SelectedIndex ) - { - if ( NULL == g_pCurrentModel ) - return; - int val = m_pItemsSelect[15]->getSelectedIndex(); - if ( 0 == val ) - g_pCurrentModel->uDeveloperFlags &= (~DEVELOPER_FLAGS_BIT_INJECT_RECOVERABLE_VIDEO_FAULTS); - else - g_pCurrentModel->uDeveloperFlags |= DEVELOPER_FLAGS_BIT_INJECT_RECOVERABLE_VIDEO_FAULTS; - if ( ! handle_commands_send_developer_flags() ) - valuesToUI(); - } - - if ( m_IndexVersion == m_SelectedIndex ) - { - char szBuff[1024]; - char szOutput[1024]; - - Menu* pMenu = new Menu(0,"All Modules Versions",NULL); - pMenu->m_xPos = 0.32; - pMenu->m_yPos = 0.17; - pMenu->m_Width = 0.6; - - hw_execute_bash_command_raw_silent("./ruby_start -ver", szOutput); - if ( strlen(szOutput)> 0 ) - if ( szOutput[strlen(szOutput)-1] == 10 || szOutput[strlen(szOutput)-1] == 13 ) - szOutput[strlen(szOutput)-1] = 0; - if ( strlen(szOutput)> 0 ) - if ( szOutput[strlen(szOutput)-1] == 10 || szOutput[strlen(szOutput)-1] == 13 ) - szOutput[strlen(szOutput)-1] = 0; - snprintf(szBuff, sizeof(szBuff)/sizeof(szBuff[0]), "ruby_start: %s", szOutput); - pMenu->addTopLine(szBuff); - - hw_execute_bash_command_raw_silent("./ruby_controller -ver", szOutput); - if ( strlen(szOutput)> 0 ) - if ( szOutput[strlen(szOutput)-1] == 10 || szOutput[strlen(szOutput)-1] == 13 ) - szOutput[strlen(szOutput)-1] = 0; - if ( strlen(szOutput)> 0 ) - if ( szOutput[strlen(szOutput)-1] == 10 || szOutput[strlen(szOutput)-1] == 13 ) - szOutput[strlen(szOutput)-1] = 0; - snprintf(szBuff, sizeof(szBuff)/sizeof(szBuff[0]), "ruby_controller: %s", szOutput); - pMenu->addTopLine(szBuff); - - hw_execute_bash_command_raw_silent("./ruby_central -ver", szOutput); - if ( strlen(szOutput)> 0 ) - if ( szOutput[strlen(szOutput)-1] == 10 || szOutput[strlen(szOutput)-1] == 13 ) - szOutput[strlen(szOutput)-1] = 0; - if ( strlen(szOutput)> 0 ) - if ( szOutput[strlen(szOutput)-1] == 10 || szOutput[strlen(szOutput)-1] == 13 ) - szOutput[strlen(szOutput)-1] = 0; - snprintf(szBuff, sizeof(szBuff)/sizeof(szBuff[0]), "ruby_central: %s", szOutput); - pMenu->addTopLine(szBuff); - - hw_execute_bash_command_raw_silent("./ruby_rt_station -ver", szOutput); - if ( strlen(szOutput)> 0 ) - if ( szOutput[strlen(szOutput)-1] == 10 || szOutput[strlen(szOutput)-1] == 13 ) - szOutput[strlen(szOutput)-1] = 0; - if ( strlen(szOutput)> 0 ) - if ( szOutput[strlen(szOutput)-1] == 10 || szOutput[strlen(szOutput)-1] == 13 ) - szOutput[strlen(szOutput)-1] = 0; - snprintf(szBuff, sizeof(szBuff)/sizeof(szBuff[0]), "ruby_rt_station: %s", szOutput); - pMenu->addTopLine(szBuff); - - hw_execute_bash_command_raw_silent("./ruby_rx_telemetry -ver", szOutput); - if ( strlen(szOutput)> 0 ) - if ( szOutput[strlen(szOutput)-1] == 10 || szOutput[strlen(szOutput)-1] == 13 ) - szOutput[strlen(szOutput)-1] = 0; - if ( strlen(szOutput)> 0 ) - if ( szOutput[strlen(szOutput)-1] == 10 || szOutput[strlen(szOutput)-1] == 13 ) - szOutput[strlen(szOutput)-1] = 0; - snprintf(szBuff, sizeof(szBuff)/sizeof(szBuff[0]), "ruby_rx_telemetry: %s", szOutput); - pMenu->addTopLine(szBuff); - - hw_execute_bash_command_raw_silent("./ruby_tx_rc -ver", szOutput); - if ( strlen(szOutput)> 0 ) - if ( szOutput[strlen(szOutput)-1] == 10 || szOutput[strlen(szOutput)-1] == 13 ) - szOutput[strlen(szOutput)-1] = 0; - if ( strlen(szOutput)> 0 ) - if ( szOutput[strlen(szOutput)-1] == 10 || szOutput[strlen(szOutput)-1] == 13 ) - szOutput[strlen(szOutput)-1] = 0; - snprintf(szBuff, sizeof(szBuff)/sizeof(szBuff[0]), "ruby_tx_rc: %s", szOutput); - pMenu->addTopLine(szBuff); - - hw_execute_bash_command_raw_silent("./ruby_i2c -ver", szOutput); - if ( strlen(szOutput)> 0 ) - if ( szOutput[strlen(szOutput)-1] == 10 || szOutput[strlen(szOutput)-1] == 13 ) - szOutput[strlen(szOutput)-1] = 0; - if ( strlen(szOutput)> 0 ) - if ( szOutput[strlen(szOutput)-1] == 10 || szOutput[strlen(szOutput)-1] == 13 ) - szOutput[strlen(szOutput)-1] = 0; - snprintf(szBuff, sizeof(szBuff)/sizeof(szBuff[0]), "ruby_i2c: %s", szOutput); - pMenu->addTopLine(szBuff); - - hw_execute_bash_command_raw_silent("./ruby_player_p -ver", szOutput); - if ( strlen(szOutput)> 0 ) - if ( szOutput[strlen(szOutput)-1] == 10 || szOutput[strlen(szOutput)-1] == 13 ) - szOutput[strlen(szOutput)-1] = 0; - if ( strlen(szOutput)> 0 ) - if ( szOutput[strlen(szOutput)-1] == 10 || szOutput[strlen(szOutput)-1] == 13 ) - szOutput[strlen(szOutput)-1] = 0; - snprintf(szBuff, sizeof(szBuff)/sizeof(szBuff[0]), "ruby_player_p: %s", szOutput); - pMenu->addTopLine(szBuff); - - hw_execute_bash_command_raw_silent("./ruby_update_worker -ver", szOutput); - if ( strlen(szOutput)> 0 ) - if ( szOutput[strlen(szOutput)-1] == 10 || szOutput[strlen(szOutput)-1] == 13 ) - szOutput[strlen(szOutput)-1] = 0; - if ( strlen(szOutput)> 0 ) - if ( szOutput[strlen(szOutput)-1] == 10 || szOutput[strlen(szOutput)-1] == 13 ) - szOutput[strlen(szOutput)-1] = 0; - snprintf(szBuff, sizeof(szBuff)/sizeof(szBuff[0]), "ruby_update_worker: %s", szOutput); - pMenu->addTopLine(szBuff); - - hw_execute_bash_command_raw_silent("./ruby_rt_vehicle -ver", szOutput); - if ( strlen(szOutput)> 0 ) - if ( szOutput[strlen(szOutput)-1] == 10 || szOutput[strlen(szOutput)-1] == 13 ) - szOutput[strlen(szOutput)-1] = 0; - if ( strlen(szOutput)> 0 ) - if ( szOutput[strlen(szOutput)-1] == 10 || szOutput[strlen(szOutput)-1] == 13 ) - szOutput[strlen(szOutput)-1] = 0; - snprintf(szBuff, sizeof(szBuff)/sizeof(szBuff[0]), "ruby_rt_vehicle: %s", szOutput); - pMenu->addTopLine(szBuff); - - hw_execute_bash_command_raw_silent("./ruby_tx_telemetry -ver", szOutput); - if ( strlen(szOutput)> 0 ) - if ( szOutput[strlen(szOutput)-1] == 10 || szOutput[strlen(szOutput)-1] == 13 ) - szOutput[strlen(szOutput)-1] = 0; - if ( strlen(szOutput)> 0 ) - if ( szOutput[strlen(szOutput)-1] == 10 || szOutput[strlen(szOutput)-1] == 13 ) - szOutput[strlen(szOutput)-1] = 0; - snprintf(szBuff, sizeof(szBuff)/sizeof(szBuff[0]), "ruby_tx_telemetry: %s", szOutput); - pMenu->addTopLine(szBuff); - - add_menu_to_stack(pMenu); - return; - } - - if ( m_IndexResetDev == m_SelectedIndex ) - { - Menu* pm = new MenuConfirmation("Developer Settings Reset", "All developer settings where reset.", 2); - pm->m_yPos = 0.4; - add_menu_to_stack(pm); - } - - if ( m_IndexRXScope == m_SelectedIndex ) - { - menu_discard_all(); - rx_scope_start(); - } -} - diff --git a/code/r_central/menu/menu_system_expert.h b/code/r_central/menu/menu_system_expert.h deleted file mode 100644 index 1fa1090b..00000000 --- a/code/r_central/menu/menu_system_expert.h +++ /dev/null @@ -1,42 +0,0 @@ -#pragma once -#include "menu_objects.h" -#include "menu_item_select.h" -#include "menu_item_slider.h" - -class MenuSystemExpert: public Menu -{ - public: - MenuSystemExpert(); - virtual void onShow(); - virtual void Render(); - virtual void valuesToUI(); - virtual void onReturnFromChild(int iChildMenuId, int returnValue); - virtual void onSelectItem(); - - private: - MenuItemSelect* m_pItemsSelect[20]; - MenuItemSlider* m_pItemsSlider[15]; - - int m_IndexDevStats; - int m_IndexPacket; - int m_IndexClockSync; - int m_IndexPingClockSpeed; - int m_IndexRadioSilence; - int m_IndexWiFiChangeDelay; - int m_IndexVideoProfiles; - int m_IndexVideoOverload; - int m_IndexFrameType; - int m_IndexSlotTime, m_IndexThresh62; - int m_IndexLogs; - int m_IndexInjectMinorFaults; - int m_IndexInjectFaults; - int m_IndexRenderFSP; - int m_IndexCPULoad; - int m_IndexFreezeOSD; - int m_IndexResetDev; - int m_IndexDetailedPackets; - int m_iIndexPCAPRadioTx; - int m_IndexRXScope; - int m_IndexVersion; - int m_IndexRxLoopTimeout; -}; \ No newline at end of file diff --git a/code/r_central/menu/menu_system_video_profiles.cpp b/code/r_central/menu/menu_system_video_profiles.cpp index 398bea2c..2ba14d90 100644 --- a/code/r_central/menu/menu_system_video_profiles.cpp +++ b/code/r_central/menu/menu_system_video_profiles.cpp @@ -574,6 +574,8 @@ void MenuSystemVideoProfiles::sendVideoLinkProfiles() log_line("Sending new video link profiles to vehicle."); if ( ! handle_commands_send_to_vehicle(COMMAND_ID_UPDATE_VIDEO_LINK_PROFILES, 0, buffer, MAX_VIDEO_LINK_PROFILES*sizeof(type_video_link_profile)) ) valuesToUI(); + else + send_control_message_to_router(PACEKT_TYPE_LOCAL_CONTROLLER_ADAPTIVE_VIDEO_PAUSE, 10000); } diff --git a/code/r_central/menu/menu_tx_power.cpp b/code/r_central/menu/menu_tx_power.cpp index 99f124ee..42c20796 100644 --- a/code/r_central/menu/menu_tx_power.cpp +++ b/code/r_central/menu/menu_tx_power.cpp @@ -612,7 +612,9 @@ int MenuTXPower::onBack() if ( m_bValuesChangedVehicle ) if ( (g_pCurrentModel != NULL) && (! g_pCurrentModel->isRunningOnOpenIPCHardware() ) ) { - MenuConfirmation* pMC = new MenuConfirmation("Restart Required","You need to restart the vehicle for the power changes to take effect.", 3); + char szTextW[256]; + sprintf(szTextW, "You need to restart the %s for the power changes to take effect.", g_pCurrentModel->getVehicleTypeString()); + MenuConfirmation* pMC = new MenuConfirmation("Restart Required",szTextW, 3); pMC->m_yPos = 0.3; pMC->addTopLine(""); @@ -620,7 +622,8 @@ int MenuTXPower::onBack() { pMC->addTopLine("Warning! You have RC link enabled. RC link will be lost while the vehicle restarts."); } - pMC->addTopLine("Do you want to restart your vehicle now?"); + sprintf(szTextW, "Do you want to restart your %s now?", g_pCurrentModel->getVehicleTypeString()); + pMC->addTopLine(szTextW); add_menu_to_stack(pMC); return 1; } diff --git a/code/r_central/menu/menu_txinfo.cpp b/code/r_central/menu/menu_txinfo.cpp index 8bd107ef..4659906a 100644 --- a/code/r_central/menu/menu_txinfo.cpp +++ b/code/r_central/menu/menu_txinfo.cpp @@ -662,7 +662,9 @@ int MenuTXInfo::onBack() if ( m_bValuesChangedVehicle ) if ( (g_pCurrentModel != NULL) && (! g_pCurrentModel->isRunningOnOpenIPCHardware() ) ) { - MenuConfirmation* pMC = new MenuConfirmation("Restart Required","You need to restart the vehicle for the power changes to take effect.", 3); + char szTextW[256]; + sprintf(szTextW, "You need to restart the %s for the power changes to take effect.", g_pCurrentModel->getVehicleTypeString()); + MenuConfirmation* pMC = new MenuConfirmation("Restart Required", szTextW, 3); pMC->m_yPos = 0.3; pMC->addTopLine(""); @@ -670,7 +672,8 @@ int MenuTXInfo::onBack() { pMC->addTopLine("Warning! You have RC link enabled. RC link will be lost while the vehicle restarts."); } - pMC->addTopLine("Do you want to restart your vehicle now?"); + sprintf(szTextW, "Do you want to restart your %s now?", g_pCurrentModel->getVehicleTypeString()); + pMC->addTopLine(szTextW); add_menu_to_stack(pMC); return 1; } diff --git a/code/r_central/menu/menu_update_vehicle.cpp b/code/r_central/menu/menu_update_vehicle.cpp index 7d650f4d..dead1ec4 100644 --- a/code/r_central/menu/menu_update_vehicle.cpp +++ b/code/r_central/menu/menu_update_vehicle.cpp @@ -47,13 +47,14 @@ MenuUpdateVehiclePopup::MenuUpdateVehiclePopup(int vehicleIndex) char szBuff3[32]; getSystemVersionString(szBuff2, g_pCurrentModel->sw_version); getSystemVersionString(szBuff3, (SYSTEM_SW_VERSION_MAJOR<<8) | SYSTEM_SW_VERSION_MINOR); - sprintf(szBuff, "Your vehicle %s has Ruby version %s (b.%d) and your controller has Ruby version %s (b.%d). You should update your vehicle.", g_pCurrentModel->getLongName(), szBuff2, g_pCurrentModel->sw_version >> 16, szBuff3, SYSTEM_SW_BUILD_NUMBER); + sprintf(szBuff, "Your %s has Ruby version %s (b.%d) and your controller has Ruby version %s (b.%d). You should update your %s.", g_pCurrentModel->getLongName(), szBuff2, g_pCurrentModel->sw_version >> 16, szBuff3, SYSTEM_SW_BUILD_NUMBER, g_pCurrentModel->getVehicleTypeString()); addTopLine(szBuff); if ( get_sw_version_build(g_pCurrentModel) < 242 ) addTopLine("Video protocols have changed from the version running on vehicle. It is recommended to update vehicle otherways you will not get a video feed."); - addTopLine("Do you want to update the vehicle?"); + sprintf(szBuff, "Do you want to update now?"); + addTopLine(szBuff); addMenuItem(new MenuItem("Yes")); addMenuItem(new MenuItem("No")); } @@ -86,7 +87,9 @@ void MenuUpdateVehiclePopup::onSelectItem() { if ( uploadSoftware() ) { - Menu* pm = new MenuConfirmation("Upload Succeeded", "Your vehicle was updated. It will reboot now.", 3, true); + char szBuff[256]; + sprintf(szBuff, "Your %s was updated. It will reboot now.", g_pCurrentModel->getVehicleTypeString()); + Menu* pm = new MenuConfirmation("Upload Succeeded", szBuff, 3, true); pm->m_xPos = 0.4; pm->m_yPos = 0.4; pm->m_Width = 0.36; pm->m_bDisableStacking = true; diff --git a/code/r_central/menu/menu_vehicle.cpp b/code/r_central/menu/menu_vehicle.cpp index 24124f54..4a58435b 100644 --- a/code/r_central/menu/menu_vehicle.cpp +++ b/code/r_central/menu/menu_vehicle.cpp @@ -88,7 +88,7 @@ void MenuVehicle::onShow() //if ( NULL != g_pCurrentModel && g_pCurrentModel->is_spectator ) // m_pMenuItems[m_IndexGeneral]->setEnabled(false); - m_IndexRadio = addMenuItem(new MenuItem("Radio Configuration", "Change the radio configuration of this vehicle.")); + m_IndexRadio = addMenuItem(new MenuItem("Radio Links", "Change the radio links configuration of this vehicle.")); if ( NULL != g_pCurrentModel && g_pCurrentModel->is_spectator ) m_pMenuItems[m_IndexRadio]->setEnabled(false); diff --git a/code/r_central/menu/menu_vehicle_camera.cpp b/code/r_central/menu/menu_vehicle_camera.cpp index 67c7495d..f9a0ba54 100644 --- a/code/r_central/menu/menu_vehicle_camera.cpp +++ b/code/r_central/menu/menu_vehicle_camera.cpp @@ -93,6 +93,7 @@ void MenuVehicleCamera::resetIndexes() m_IndexVideoStab = m_IndexFlip = m_IndexReset = -1; m_IndexIRCut = -1; m_IndexOpenIPCDayNight = -1; + m_IndexOpenIPC3A = -1; } void MenuVehicleCamera::addItems() @@ -288,6 +289,13 @@ void MenuVehicleCamera::addItems() } else if ( hardware_board_is_sigmastar(g_pCurrentModel->hwCapabilities.uBoardType) ) { + m_pItemsSelect[22] = new MenuItemSelect("3A Algorithms", "Sets 3A algorithms (autoexposure, autowhitebalance, autofocus) used by the camera ISP processor."); + m_pItemsSelect[22]->addSelection("Default"); + m_pItemsSelect[22]->addSelection("Sigmastar"); + m_pItemsSelect[22]->setIsEditable(); + m_pItemsSelect[22]->setMargin(fMargin); + m_IndexOpenIPC3A = addMenuItem(m_pItemsSelect[22]); + m_pItemsSelect[7] = new MenuItemSelect("Shutter Speed", "Sets the shutter speed to be auto controllerd by camera or manula set by user."); m_pItemsSelect[7]->addSelection("Auto"); m_pItemsSelect[7]->addSelection("Manual"); @@ -500,6 +508,15 @@ void MenuVehicleCamera::updateUIValues(int iCameraProfileIndex) m_pItemsSelect[15]->setEnabled(false); } + if ( hardware_board_is_sigmastar(g_pCurrentModel->hwCapabilities.uBoardType) ) + if ( -1 != m_IndexOpenIPC3A ) + if ( NULL != m_pItemsSelect[22] ) + { + if ( g_pCurrentModel->camera_params[g_pCurrentModel->iCurrentCamera].profiles[iCameraProfileIndex].uFlags & CAMERA_FLAG_OPENIPC_3A_SIGMASTAR ) + m_pItemsSelect[22]->setSelectedIndex(1); + else + m_pItemsSelect[22]->setSelectedIndex(0); + } if ( (-1 != m_IndexWhiteBalance) && (NULL != m_pItemsSelect[3]) ) m_pItemsSelect[3]->setSelection(g_pCurrentModel->camera_params[g_pCurrentModel->iCurrentCamera].profiles[iCameraProfileIndex].whitebalance); @@ -704,6 +721,14 @@ void MenuVehicleCamera::sendCameraParams(int itemIndex, bool bQuick) bSendToVehicle = true; } + if ( -1 != m_IndexOpenIPC3A ) + if ( NULL != m_pItemsSelect[22] ) + { + if ( 0 == m_pItemsSelect[22]->getSelectedIndex() ) + cparams.profiles[iProfile].uFlags &= ~CAMERA_FLAG_OPENIPC_3A_SIGMASTAR; + else + cparams.profiles[iProfile].uFlags |= CAMERA_FLAG_OPENIPC_3A_SIGMASTAR; + } if (-1 != m_IndexOpenIPCDayNight) if ((m_IndexOpenIPCDayNight == itemIndex) || (itemIndex == -1)) { @@ -1022,6 +1047,10 @@ void MenuVehicleCamera::onSelectItem() if ( m_IndexIRCut == m_SelectedIndex ) sendCameraParams(-1, false); + if ( m_IndexOpenIPC3A != -1 ) + if ( m_IndexOpenIPC3A == m_SelectedIndex ) + sendCameraParams(-1, false); + if (m_IndexOpenIPCDayNight != -1) if (m_IndexOpenIPCDayNight == m_SelectedIndex) sendCameraParams(-1, false); diff --git a/code/r_central/menu/menu_vehicle_camera.h b/code/r_central/menu/menu_vehicle_camera.h index 76b6b1ee..4355f82c 100644 --- a/code/r_central/menu/menu_vehicle_camera.h +++ b/code/r_central/menu/menu_vehicle_camera.h @@ -43,6 +43,7 @@ class MenuVehicleCamera: public Menu int m_IndexDayNight; int m_IndexVideoStab, m_IndexFlip, m_IndexReset; int m_IndexIRCut, m_IndexOpenIPCDayNight; + int m_IndexOpenIPC3A; int m_IndexCalibrateHDMI; bool m_bDidAnyLiveUpdates; diff --git a/code/r_central/menu/menu_vehicle_cpu_oipc.cpp b/code/r_central/menu/menu_vehicle_cpu_oipc.cpp index f24a95b3..f1dbe2c4 100644 --- a/code/r_central/menu/menu_vehicle_cpu_oipc.cpp +++ b/code/r_central/menu/menu_vehicle_cpu_oipc.cpp @@ -43,16 +43,35 @@ MenuVehicleCPU_OIPC::MenuVehicleCPU_OIPC(void) float fSliderWidth = 0.10; setSubTitle("Change vehicle processes priorities, for expert users."); + m_IndexBalanceIntCores = -1; + m_pItemsSlider[5] = new MenuItemSlider("CPU Speed (Mhz)", "Sets the main CPU frequency.", 700, 1200, 900, fSliderWidth); m_pItemsSlider[5]->setStep(25); m_IndexCPUSpeed = addMenuItem(m_pItemsSlider[5]); m_pItemsSelect[5] = new MenuItemSelect("GPU Boost", "Increases the video encoder clock speed."); m_pItemsSelect[5]->addSelection("Off"); - m_pItemsSelect[5]->addSelection("On"); + m_pItemsSelect[5]->addSelection("Medium"); + m_pItemsSelect[5]->addSelection("High"); + m_pItemsSelect[5]->addSelection("Custom"); m_pItemsSelect[5]->setIsEditable(); m_IndexGPUBoost = addMenuItem(m_pItemsSelect[5]); + m_pItemsSelect[7] = new MenuItemSelect("GPU Freq Core 1", "Set custom freq for GPU core 1"); + m_pItemsSelect[7]->addSelection("384 Mhz"); + m_pItemsSelect[7]->addSelection("432 Mhz"); + m_pItemsSelect[7]->addSelection("480 Mhz"); + m_pItemsSelect[7]->setIsEditable(); + m_IndexGPUFreqCore1 = addMenuItem(m_pItemsSelect[7]); + + m_pItemsSelect[8] = new MenuItemSelect("GPU Freq Core 2", "Set custom freq for GPU core 2"); + m_pItemsSelect[8]->addSelection("320 Mhz"); + m_pItemsSelect[8]->addSelection("336 Mhz"); + m_pItemsSelect[8]->addSelection("348 Mhz"); + m_pItemsSelect[8]->addSelection("384 Mhz"); + m_pItemsSelect[8]->setIsEditable(); + m_IndexGPUFreqCore2 = addMenuItem(m_pItemsSelect[8]); + addMenuItem(new MenuItemSection("Priorities")); m_pItemsSelect[0] = new MenuItemSelect("Core Priority Adjustment", "Change the way the priority of Ruby processes is adjusted."); @@ -93,6 +112,15 @@ MenuVehicleCPU_OIPC::MenuVehicleCPU_OIPC(void) m_pItemsSlider[4] = new MenuItemSlider(" Video Priority", "Sets the priority for the video processes. Higher values means higher priority.", 1,18,11, fSliderWidth); m_IndexNiceVideo = addMenuItem(m_pItemsSlider[4]); + + if ( hardware_board_is_sigmastar(g_pCurrentModel->hwCapabilities.uBoardType) ) + { + m_pItemsSelect[6] = new MenuItemSelect("Balance CPU Cores", "Tries to balance the load on the CPU cores."); + m_pItemsSelect[6]->addSelection("Off"); + m_pItemsSelect[6]->addSelection("On"); + m_pItemsSelect[6]->setIsEditable(); + m_IndexBalanceIntCores = addMenuItem(m_pItemsSelect[6]); + } } void MenuVehicleCPU_OIPC::valuesToUI() @@ -101,8 +129,39 @@ void MenuVehicleCPU_OIPC::valuesToUI() m_pItemsSlider[5]->setCurrentValue(g_pCurrentModel->processesPriorities.iFreqARM); m_pItemsSelect[5]->setSelectedIndex(0); - if ( g_pCurrentModel->processesPriorities.iFreqGPU == 1 ) - m_pItemsSelect[5]->setSelectedIndex(1); + if ( g_pCurrentModel->processesPriorities.iFreqGPU < 3 ) + { + m_pItemsSelect[5]->setSelectedIndex(g_pCurrentModel->processesPriorities.iFreqGPU); + m_pItemsSelect[7]->setEnabled(false); + m_pItemsSelect[8]->setEnabled(false); + + if ( 0 == g_pCurrentModel->processesPriorities.iFreqGPU ) + { + m_pItemsSelect[7]->setSelectedIndex(0); + m_pItemsSelect[8]->setSelectedIndex(0); + } + if ( 1 == g_pCurrentModel->processesPriorities.iFreqGPU ) + { + m_pItemsSelect[7]->setSelectedIndex(1); + m_pItemsSelect[8]->setSelectedIndex(1); + } + if ( 2 == g_pCurrentModel->processesPriorities.iFreqGPU ) + { + m_pItemsSelect[7]->setSelectedIndex(2); + m_pItemsSelect[8]->setSelectedIndex(2); + } + } + else + { + m_pItemsSelect[5]->setSelectedIndex(3); + m_pItemsSelect[7]->setEnabled(true); + m_pItemsSelect[8]->setEnabled(true); + + int iCore1 = (g_pCurrentModel->processesPriorities.iFreqGPU/10)%10; + int iCore2 = (g_pCurrentModel->processesPriorities.iFreqGPU/100)%10; + m_pItemsSelect[7]->setSelectedIndex(iCore1); + m_pItemsSelect[8]->setSelectedIndex(iCore2); + } if ( g_pCurrentModel->processesPriorities.iNiceRouter == 0 ) { @@ -159,6 +218,15 @@ void MenuVehicleCPU_OIPC::valuesToUI() m_pItemsSlider[4]->setCurrentValue(-g_pCurrentModel->processesPriorities.iNiceVideo); m_pItemsSlider[4]->setEnabled(true); } + + if ( -1 != m_IndexBalanceIntCores ) + if ( hardware_board_is_sigmastar(g_pCurrentModel->hwCapabilities.uBoardType) ) + { + if ( g_pCurrentModel->processesPriorities.uProcessesFlags & PROCESSES_FLAGS_BALANCE_INT_CORES ) + m_pItemsSelect[6]->setSelectedIndex(1); + else + m_pItemsSelect[6]->setSelectedIndex(0); + } } void MenuVehicleCPU_OIPC::onShow() @@ -274,7 +342,8 @@ void MenuVehicleCPU_OIPC::onSelectItem() params.freq_arm = g_pCurrentModel->processesPriorities.iFreqARM; params.freq_gpu = g_pCurrentModel->processesPriorities.iFreqGPU; params.overvoltage = g_pCurrentModel->processesPriorities.iOverVoltage; - + params.uProcessesFlags = g_pCurrentModel->processesPriorities.uProcessesFlags; + if ( m_IndexCPUSpeed == m_SelectedIndex ) { params.freq_arm = m_pItemsSlider[5]->getCurrentValue(); @@ -286,6 +355,25 @@ void MenuVehicleCPU_OIPC::onSelectItem() sendUpdate = true; } + if ( (m_IndexGPUFreqCore1 == m_SelectedIndex) || (m_IndexGPUFreqCore2 == m_SelectedIndex) ) + { + params.freq_gpu = 3; + params.freq_gpu += m_pItemsSelect[7]->getSelectedIndex()*10; + params.freq_gpu += m_pItemsSelect[8]->getSelectedIndex()*100; + sendUpdate = true; + } + + if ( -1 != m_IndexBalanceIntCores ) + if ( m_IndexBalanceIntCores == m_SelectedIndex ) + { + if ( 1 == m_pItemsSelect[6]->getSelectedIndex() ) + params.uProcessesFlags |= PROCESSES_FLAGS_BALANCE_INT_CORES; + else + params.uProcessesFlags &= ~PROCESSES_FLAGS_BALANCE_INT_CORES; + + sendUpdate = true; + } + if ( sendUpdate ) { if ( ! handle_commands_send_to_vehicle(COMMAND_ID_SET_OVERCLOCKING_PARAMS, 0, (u8*)(¶ms), sizeof(command_packet_overclocking_params)) ) diff --git a/code/r_central/menu/menu_vehicle_cpu_oipc.h b/code/r_central/menu/menu_vehicle_cpu_oipc.h index ba2e8d67..cf2be4ee 100644 --- a/code/r_central/menu/menu_vehicle_cpu_oipc.h +++ b/code/r_central/menu/menu_vehicle_cpu_oipc.h @@ -25,6 +25,9 @@ class MenuVehicleCPU_OIPC: public Menu int m_IndexRadioTx; int m_IndexCPUSpeed; int m_IndexGPUBoost; + int m_IndexGPUFreqCore1; + int m_IndexGPUFreqCore2; + int m_IndexBalanceIntCores; MenuItemSlider* m_pItemsSlider[10]; MenuItemSelect* m_pItemsSelect[10]; diff --git a/code/r_central/menu/menu_vehicle_expert.cpp b/code/r_central/menu/menu_vehicle_expert.cpp index e161048a..68e19e95 100644 --- a/code/r_central/menu/menu_vehicle_expert.cpp +++ b/code/r_central/menu/menu_vehicle_expert.cpp @@ -450,7 +450,10 @@ void MenuVehicleExpert::onSelectItem() if ( g_VehiclesRuntimeInfo[g_iCurrentActiveVehicleRuntimeInfoIndex].bGotFCTelemetry ) if ( g_VehiclesRuntimeInfo[g_iCurrentActiveVehicleRuntimeInfoIndex].headerFCTelemetry.flags & FC_TELE_FLAGS_ARMED ) { - MenuConfirmation* pMC = new MenuConfirmation("Warning! Reboot Confirmation","Your vehicle is armed. Are you sure you want to reboot the controller?", 10); + char szTextW[256]; + if ( NULL != g_VehiclesRuntimeInfo[g_iCurrentActiveVehicleRuntimeInfoIndex].pModel ) + sprintf(szTextW, "Your %s is armed. Are you sure you want to reboot the controller?", g_VehiclesRuntimeInfo[g_iCurrentActiveVehicleRuntimeInfoIndex].pModel->getVehicleTypeString()); + MenuConfirmation* pMC = new MenuConfirmation("Warning! Reboot Confirmation", szTextW, 10); if ( g_pCurrentModel->rc_params.rc_enabled ) { pMC->addTopLine(" "); @@ -476,6 +479,7 @@ void MenuVehicleExpert::onSelectItem() params.freq_arm = g_pCurrentModel->processesPriorities.iFreqARM; params.freq_gpu = g_pCurrentModel->processesPriorities.iFreqGPU; params.overvoltage = g_pCurrentModel->processesPriorities.iOverVoltage; + params.uProcessesFlags = g_pCurrentModel->processesPriorities.uProcessesFlags; if ( m_IndexCPUEnabled == m_SelectedIndex ) { diff --git a/code/r_central/menu/menu_vehicle_management.cpp b/code/r_central/menu/menu_vehicle_management.cpp index 616012c5..095c40c6 100644 --- a/code/r_central/menu/menu_vehicle_management.cpp +++ b/code/r_central/menu/menu_vehicle_management.cpp @@ -158,7 +158,9 @@ void MenuVehicleManagement::onReturnFromChild(int iChildMenuId, int returnValue) //Menu* pm = new Menu(MENU_ID_SIMPLE_MESSAGE+3*1000,"Upload Succeeded",NULL); //pm->addTopLine("Your vehicle was updated. It will reboot now."); - Menu* pm = new MenuConfirmation("Upload Succeeded", "Your vehicle was updated. It will reboot now.", 3, true); + char szTextW[256]; + sprintf(szTextW, "Your %s was updated. It will reboot now.", g_pCurrentModel->getVehicleTypeString()); + Menu* pm = new MenuConfirmation("Upload Succeeded", szTextW, 3, true); pm->m_xPos = 0.4; pm->m_yPos = 0.4; pm->m_Width = 0.36; pm->m_bDisableStacking = true; @@ -366,7 +368,7 @@ void MenuVehicleManagement::onSelectItem() if ( m_IndexUpdate == m_SelectedIndex ) { bool bSupportsOTA = false; - + char szTextW[256]; if ( hardware_board_is_raspberry(g_pCurrentModel->hwCapabilities.uBoardType) ) bSupportsOTA = true; if ( g_pCurrentModel->hwCapabilities.uFlags & MODEL_HW_CAP_FLAG_OTA ) @@ -387,7 +389,8 @@ void MenuVehicleManagement::onSelectItem() return; if ( (! pairing_isStarted()) || (NULL == g_pCurrentModel) || (! link_is_vehicle_online_now(g_pCurrentModel->uVehicleId)) ) { - addMessage("Please connect to your vehicle first, if you want to update the sowftware on the vehicle."); + sprintf(szTextW, "Please connect to your %s first, if you want to update the sowftware on the vehicle.", g_pCurrentModel->getVehicleTypeString()); + addMessage(szTextW); return; } @@ -398,8 +401,7 @@ void MenuVehicleManagement::onSelectItem() if ( (g_pCurrentModel->sw_version >> 16) >= SYSTEM_SW_BUILD_NUMBER ) { char szBuff[256]; - - sprintf(szBuff, "Your vehicle already has the latest version of the software (version %s). Do you still want to upgrade vehicle?", szBuff2); + sprintf(szBuff, "Your %s already has the latest version of the software (version %s). Do you still want to upgrade?", g_pCurrentModel->getVehicleTypeString(), szBuff2); MenuConfirmation* pMC = new MenuConfirmation("Upgrade Confirmation",szBuff, 4); add_menu_to_stack(pMC); //pMC->addTopLine(" "); @@ -409,7 +411,7 @@ void MenuVehicleManagement::onSelectItem() char szBuff[512]; char szBuff3[64]; getSystemVersionString(szBuff3, (SYSTEM_SW_VERSION_MAJOR<<8) | SYSTEM_SW_VERSION_MINOR); - sprintf(szBuff, "Your vehicle has software version %s (b%d) and software version %s (b%d) is available on the controller. Do you want to upgrade vehicle?", szBuff2, (int)(g_pCurrentModel->sw_version >> 16), szBuff3, SYSTEM_SW_BUILD_NUMBER); + sprintf(szBuff, "Your %s has software version %s (b%d) and software version %s (b%d) is available on the controller. Do you want to upgrade?", g_pCurrentModel->getVehicleTypeString(), szBuff2, (int)(g_pCurrentModel->sw_version >> 16), szBuff3, SYSTEM_SW_BUILD_NUMBER); MenuConfirmation* pMC = new MenuConfirmation("Upgrade Confirmation",szBuff, 2); add_menu_to_stack(pMC); } @@ -429,9 +431,12 @@ void MenuVehicleManagement::onSelectItem() if ( checkIsArmed() ) return; char szBuff[256]; - sprintf(szBuff, "Are you sure you want to factory reset %s?", g_pCurrentModel->getLongName()); + sprintf(szBuff, "Factory reset %s", g_pCurrentModel->getLongName()); MenuConfirmation* pMC = new MenuConfirmation("Confirmation",szBuff, 21); pMC->addTopLine("All parameters (including vehicle name, radio frequency, etc) and state will be reset to default values as after a fresh instalation."); + pMC->addTopLine("You will need to search and pair with the vehicle again after that."); + sprintf(szBuff, "Are you sure you want to factory reset %s?", g_pCurrentModel->getLongName()); + pMC->addTopLine(szBuff); add_menu_to_stack(pMC); } @@ -449,7 +454,9 @@ void MenuVehicleManagement::onSelectItem() if ( g_VehiclesRuntimeInfo[g_iCurrentActiveVehicleRuntimeInfoIndex].bGotFCTelemetry ) if ( g_VehiclesRuntimeInfo[g_iCurrentActiveVehicleRuntimeInfoIndex].headerFCTelemetry.flags & FC_TELE_FLAGS_ARMED ) { - MenuConfirmation* pMC = new MenuConfirmation("Warning! Reboot Confirmation","Your vehicle is armed. Are you sure you want to reboot the vehicle?", 10); + char szTextW[256]; + sprintf(szTextW, "Your %s is armed. Are you sure you want to reboot it?", g_pCurrentModel->getVehicleTypeString()); + MenuConfirmation* pMC = new MenuConfirmation("Warning! Reboot Confirmation", szTextW, 10); if ( g_pCurrentModel->rc_params.rc_enabled ) { pMC->addTopLine(" "); diff --git a/code/r_central/menu/menu_vehicle_osd.cpp b/code/r_central/menu/menu_vehicle_osd.cpp index fc3a8c8e..6927de16 100644 --- a/code/r_central/menu/menu_vehicle_osd.cpp +++ b/code/r_central/menu/menu_vehicle_osd.cpp @@ -106,6 +106,7 @@ MenuVehicleOSD::MenuVehicleOSD(void) m_pItemsSelect[3]->addSelection("Medium"); m_pItemsSelect[3]->addSelection("Normal"); m_pItemsSelect[3]->addSelection("Minimum"); + m_pItemsSelect[3]->addSelection("None"); m_pItemsSelect[3]->setIsEditable(); m_IndexOSDTransparency = addMenuItem(m_pItemsSelect[3]); @@ -154,7 +155,7 @@ void MenuVehicleOSD::valuesToUI() m_pItemsSelect[0]->setSelectedIndex(layoutIndex); m_pItemsSelect[2]->setSelectedIndex(g_pCurrentModel->osd_params.osd_preferences[layoutIndex] & 0xFF); - m_pItemsSelect[3]->setSelectedIndex(((g_pCurrentModel->osd_params.osd_preferences[layoutIndex])>>8) & 0xFF); + m_pItemsSelect[3]->setSelectedIndex(((g_pCurrentModel->osd_params.osd_preferences[layoutIndex]) & OSD_PREFERENCES_OSD_TRANSPARENCY_BITMASK) >> OSD_PREFERENCES_OSD_TRANSPARENCY_SHIFT); m_pItemsSelect[4]->setSelectedIndex(0); if ( g_pCurrentModel->osd_params.osd_flags2[layoutIndex] & OSD_FLAG2_SHOW_BACKGROUND_ON_TEXTS_ONLY ) @@ -350,7 +351,7 @@ void MenuVehicleOSD::onSelectItem() if ( m_IndexOSDTransparency == m_SelectedIndex ) { params.osd_preferences[layoutIndex] &= 0xFFFF00FF; - params.osd_preferences[layoutIndex] |= ((u32)m_pItemsSelect[3]->getSelectedIndex())<<8; + params.osd_preferences[layoutIndex] |= ((((u32)m_pItemsSelect[3]->getSelectedIndex()) << OSD_PREFERENCES_OSD_TRANSPARENCY_SHIFT) & OSD_PREFERENCES_OSD_TRANSPARENCY_BITMASK); sendToVehicle = true; } diff --git a/code/r_central/menu/menu_vehicle_osd_elements.cpp b/code/r_central/menu/menu_vehicle_osd_elements.cpp index dc24292e..678750fb 100644 --- a/code/r_central/menu/menu_vehicle_osd_elements.cpp +++ b/code/r_central/menu/menu_vehicle_osd_elements.cpp @@ -162,8 +162,11 @@ MenuVehicleOSDElements::MenuVehicleOSDElements(void) m_pItemsSelect[11] = new MenuItemSelect("Flight mode", "Shows flight mode on the OSD"); m_pItemsSelect[11]->addSelection("No"); m_pItemsSelect[11]->addSelection("Yes"); - if ( bUseMultiSelection ) - m_pItemsSelect[11]->setUseMultiViewLayout(); + m_pItemsSelect[11]->addSelection("Only when changing"); + m_pItemsSelect[11]->addSelection("Both"); + m_pItemsSelect[11]->setIsEditable(); + //if ( bUseMultiSelection ) + // m_pItemsSelect[11]->setUseMultiViewLayout(); m_IndexMode = addMenuItem(m_pItemsSelect[11]); m_pItemsSelect[12] = new MenuItemSelect("Time", "Shows the arm/flight time on the OSD"); @@ -368,8 +371,15 @@ void MenuVehicleOSDElements::valuesToUI() if ( g_pCurrentModel->osd_params.osd_flags[layoutIndex] & OSD_FLAG_SCRAMBLE_GPS ) m_pItemsSelect[9]->setSelection(2); - m_pItemsSelect[11]->setSelection((g_pCurrentModel->osd_params.osd_flags[layoutIndex] & OSD_FLAG_SHOW_FLIGHT_MODE)?1:0); - + m_pItemsSelect[11]->setSelection(0); + if ( (g_pCurrentModel->osd_params.osd_flags[layoutIndex] & OSD_FLAG_SHOW_FLIGHT_MODE) && + (g_pCurrentModel->osd_params.osd_flags[layoutIndex] & OSD_FLAG_SHOW_FLIGHT_MODE_CHANGE) ) + m_pItemsSelect[11]->setSelection(3); + else if ( g_pCurrentModel->osd_params.osd_flags[layoutIndex] & OSD_FLAG_SHOW_FLIGHT_MODE ) + m_pItemsSelect[11]->setSelection(1); + else if ( g_pCurrentModel->osd_params.osd_flags[layoutIndex] & OSD_FLAG_SHOW_FLIGHT_MODE_CHANGE ) + m_pItemsSelect[11]->setSelection(2); + if ( g_pCurrentModel->osd_params.osd_flags[layoutIndex] & OSD_FLAG_SHOW_TIME ) { if ( g_pCurrentModel->osd_params.osd_flags[layoutIndex] & OSD_FLAG_SHOW_TIME_LOWER ) @@ -738,9 +748,25 @@ void MenuVehicleOSDElements::onSelectItem() if ( m_IndexMode == m_SelectedIndex ) { if ( 0 == m_pItemsSelect[11]->getSelectedIndex() ) + { + params.osd_flags[layoutIndex] &= ~OSD_FLAG_SHOW_FLIGHT_MODE; + params.osd_flags[layoutIndex] &= ~OSD_FLAG_SHOW_FLIGHT_MODE_CHANGE; + } + else if ( 1 == m_pItemsSelect[11]->getSelectedIndex() ) + { + params.osd_flags[layoutIndex] |= OSD_FLAG_SHOW_FLIGHT_MODE; + params.osd_flags[layoutIndex] &= ~OSD_FLAG_SHOW_FLIGHT_MODE_CHANGE; + } + else if ( 2 == m_pItemsSelect[11]->getSelectedIndex() ) + { params.osd_flags[layoutIndex] &= ~OSD_FLAG_SHOW_FLIGHT_MODE; + params.osd_flags[layoutIndex] |= OSD_FLAG_SHOW_FLIGHT_MODE_CHANGE; + } else + { params.osd_flags[layoutIndex] |= OSD_FLAG_SHOW_FLIGHT_MODE; + params.osd_flags[layoutIndex] |= OSD_FLAG_SHOW_FLIGHT_MODE_CHANGE; + } sendToVehicle = true; } diff --git a/code/r_central/menu/menu_vehicle_radio.cpp b/code/r_central/menu/menu_vehicle_radio.cpp index a74da244..9940d83a 100644 --- a/code/r_central/menu/menu_vehicle_radio.cpp +++ b/code/r_central/menu/menu_vehicle_radio.cpp @@ -40,6 +40,7 @@ #include "menu_radio_config.h" #include "menu_tx_power.h" #include "menu_tx_power_8812eu.h" +#include "menu_negociate_radio.h" #include "../link_watch.h" #include "../launchers_controller.h" @@ -263,21 +264,21 @@ void MenuVehicleRadioConfig::populate() if ( g_pCurrentModel->hasRadioCardsRTL8812AU() > 0 ) { - m_IndexTxPowerRTL8812AU = addMenuItem(new MenuItem("Tx Power Level (RTL8812AU)", "Change the transmit power levels for the vehicle's radio interfaces that use RTL8812AU chipset.")); + m_IndexTxPowerRTL8812AU = addMenuItem(new MenuItem("Power Level (RTL8812AU)", "Change the transmit power levels for the vehicle's radio interfaces that use RTL8812AU chipset.")); m_pMenuItems[m_IndexTxPowerRTL8812AU]->showArrow(); } if ( g_pCurrentModel->hasRadioCardsRTL8812EU() > 0 ) { - m_IndexTxPowerRTL8812EU = addMenuItem(new MenuItem("Tx Power Level (RTL8812EU)", "Change the transmit power levels for the vehicle's radio interfaces that use RTL8812EU chipset.")); + m_IndexTxPowerRTL8812EU = addMenuItem(new MenuItem("Power Level (RTL8812EU)", "Change the transmit power levels for the vehicle's radio interfaces that use RTL8812EU chipset.")); m_pMenuItems[m_IndexTxPowerRTL8812EU]->showArrow(); } if ( g_pCurrentModel->hasRadioCardsAtheros() > 0 ) { - m_IndexTxPowerAtheros = addMenuItem(new MenuItem("Tx Power Level (Atheros)", "Change the transmit power levels for the vehicle's radio interfaces that use Atheros chipset.")); + m_IndexTxPowerAtheros = addMenuItem(new MenuItem("Power Level (Atheros)", "Change the transmit power levels for the vehicle's radio interfaces that use Atheros chipset.")); m_pMenuItems[m_IndexTxPowerAtheros]->showArrow(); } - m_IndexRadioConfig = addMenuItem(new MenuItem("Full Radio Config", "Full radio configuration")); + m_IndexRadioConfig = addMenuItem(new MenuItem("Radio Parameters", "Full radio configuration")); m_pMenuItems[m_IndexRadioConfig]->showArrow(); m_pItemsSelect[4] = new MenuItemSelect("Disable Uplinks", "Disable all uplinks, makes the system a one way system. Except for initial pairing and synching and sending commands to the vehicle. No video retransmissions happen, adaptive video is also disabled."); @@ -300,6 +301,9 @@ void MenuVehicleRadioConfig::populate() m_pItemsSelect[2]->addSelection("All Streams and Data"); m_pItemsSelect[2]->setIsEditable(); m_IndexEncryption = addMenuItem(m_pItemsSelect[2]); + + m_IndexOptimizeLinks = addMenuItem(new MenuItem("Optmize Radio Links Wizard", "Runs a process to optimize radio links parameters.")); + m_pMenuItems[m_IndexOptimizeLinks]->showArrow(); } void MenuVehicleRadioConfig::valuesToUI() @@ -337,7 +341,7 @@ void MenuVehicleRadioConfig::valuesToUI() if ( ! m_bControllerHasKey ) { m_pItemsSelect[2]->setSelectedIndex(0); - m_pItemsSelect[2]->setEnabled(false); + //m_pItemsSelect[2]->setEnabled(false); } if ( -1 != m_IndexTxPowerRTL8812AU ) @@ -459,9 +463,10 @@ void MenuVehicleRadioConfig::onSelectItem() { if ( ! m_bControllerHasKey ) { - MenuConfirmation* pMC = new MenuConfirmation("Missing Pass Code", "You have not set a pass code on the controller. You need first to set a pass code on the controller.", -1, true); + MenuConfirmation* pMC = new MenuConfirmation("Missing Pass Code", "You have not set a pass code on the controller. You need first to set a pass code on the controller from Menu->Controller->Encryption.", -1, true); pMC->m_yPos = 0.3; add_menu_to_stack(pMC); + m_pItemsSelect[2]->setSelectedIndex(0); return; } u8 params[128]; @@ -609,5 +614,16 @@ void MenuVehicleRadioConfig::onSelectItem() */ if ( m_IndexRadioConfig == m_SelectedIndex ) - add_menu_to_stack(new MenuRadioConfig()); + { + MenuRadioConfig* pM = new MenuRadioConfig(); + pM->m_bGoToFirstRadioLinkOnShow = true; + add_menu_to_stack(pM); + return; + } + + if ( m_IndexOptimizeLinks == m_SelectedIndex ) + { + add_menu_to_stack(new MenuNegociateRadio()); + return; + } } \ No newline at end of file diff --git a/code/r_central/menu/menu_vehicle_radio.h b/code/r_central/menu/menu_vehicle_radio.h index d358388a..5777c3eb 100644 --- a/code/r_central/menu/menu_vehicle_radio.h +++ b/code/r_central/menu/menu_vehicle_radio.h @@ -24,6 +24,7 @@ class MenuVehicleRadioConfig: public Menu int m_IndexTxPowerRTL8812EU; int m_IndexTxPowerAtheros; int m_IndexRadioConfig; + int m_IndexOptimizeLinks; int m_IndexFreq[MAX_RADIO_INTERFACES]; int m_IndexConfigureLinks[MAX_RADIO_INTERFACES]; u32 m_SupportedChannels[MAX_RADIO_INTERFACES][100]; diff --git a/code/r_central/menu/menu_vehicle_radio_interface.cpp b/code/r_central/menu/menu_vehicle_radio_interface.cpp index 4453f8b8..4445d3c9 100644 --- a/code/r_central/menu/menu_vehicle_radio_interface.cpp +++ b/code/r_central/menu/menu_vehicle_radio_interface.cpp @@ -56,9 +56,13 @@ MenuVehicleRadioInterface::MenuVehicleRadioInterface(int iRadioInterface) sprintf(szBuff, "Vehicle Radio Interface %d Parameters", m_iRadioInterface+1); setTitle(szBuff); - addTopLine("You have a single radio interface on the vehicle side for this radio link. You can't change the radio interface parameters. They are derived automatically from the radio link params."); + //addTopLine("You have a single radio interface on the vehicle side for this radio link. You can't change the radio interface parameters. They are derived automatically from the radio link params."); + //addMenuItem(new MenuItem("Ok")); + + m_pItemsSelect[1] = createMenuItemCardModelSelector("Card Model"); + m_pItemsSelect[1]->setIsEditable(); + m_IndexCardModel = addMenuItem(m_pItemsSelect[1]); - addMenuItem(new MenuItem("Ok")); } MenuVehicleRadioInterface::~MenuVehicleRadioInterface() @@ -75,6 +79,10 @@ void MenuVehicleRadioInterface::onShow() void MenuVehicleRadioInterface::valuesToUI() { + if ( g_pCurrentModel->radioInterfacesParams.interface_card_model[m_iRadioInterface] < 0 ) + m_pItemsSelect[1]->setSelection(-g_pCurrentModel->radioInterfacesParams.interface_card_model[m_iRadioInterface]); + else + m_pItemsSelect[1]->setSelection(g_pCurrentModel->radioInterfacesParams.interface_card_model[m_iRadioInterface]); } void MenuVehicleRadioInterface::Render() @@ -113,12 +121,6 @@ void MenuVehicleRadioInterface::onSelectItem() return; } - if ( 0 == m_SelectedIndex ) - { - menu_stack_pop(0); - return; - } - if ( link_is_reconfiguring_radiolink() ) { handle_commands_show_popup_progress(); @@ -128,5 +130,17 @@ void MenuVehicleRadioInterface::onSelectItem() if ( m_pMenuItems[m_SelectedIndex]->isEditing() ) return; - + if ( m_IndexCardModel == m_SelectedIndex ) + { + int iCardModel = m_pItemsSelect[1]->getSelectedIndex(); + u32 uParam = m_iRadioInterface; + if ( 0 == iCardModel ) + uParam = uParam | (128<<8); + else + uParam = uParam | ((128-iCardModel) << 8); + if ( ! handle_commands_send_to_vehicle(COMMAND_ID_SET_RADIO_CARD_MODEL, uParam, NULL, 0) ) + valuesToUI(); + + return; + } } \ No newline at end of file diff --git a/code/r_central/menu/menu_vehicle_radio_interface.h b/code/r_central/menu/menu_vehicle_radio_interface.h index 3918509c..84ae0bd5 100644 --- a/code/r_central/menu/menu_vehicle_radio_interface.h +++ b/code/r_central/menu/menu_vehicle_radio_interface.h @@ -23,12 +23,6 @@ class MenuVehicleRadioInterface: public Menu MenuItemSlider* m_pItemsSlider[20]; MenuItemSelect* m_pItemsSelect[20]; - int m_IndexTXPreferred; int m_IndexCardModel; - int m_IndexEnabled; - int m_IndexUsage; - int m_IndexCapabilities; - int m_IndexDataRate; int m_IndexName; - int m_IndexInternal; }; \ No newline at end of file diff --git a/code/r_central/menu/menu_vehicle_radio_link.cpp b/code/r_central/menu/menu_vehicle_radio_link.cpp index 95681e54..77eb1a24 100644 --- a/code/r_central/menu/menu_vehicle_radio_link.cpp +++ b/code/r_central/menu/menu_vehicle_radio_link.cpp @@ -45,10 +45,27 @@ const char* s_szMenuRadio_SingleCard2 = "Note: You can not change the usage and MenuVehicleRadioLink::MenuVehicleRadioLink(int iRadioLink) :Menu(MENU_ID_VEHICLE_RADIO_LINK, "Vehicle Radio Link Parameters", NULL) { - m_Width = 0.36; + m_Width = 0.46; m_xPos = menu_get_XStartPos(m_Width); m_yPos = 0.1; m_iRadioLink = iRadioLink; + m_bMustSelectDatarate = false; + m_bSwitchedDataRatesType = false; + + m_IndexFrequency = -1; + m_IndexUsage = -1; + m_IndexCapabilities = -1; + m_IndexDataRatesType = -1; + m_IndexDataRateVideo = -1; + m_IndexDataRateTypeDownlink = -1; + m_IndexDataRateTypeUplink = -1; + m_IndexDataRateDataDownlink = -1; + m_IndexDataRateDataUplink = -1; + m_IndexHT = -1; + m_IndexLDPC = -1; + m_IndexSGI = -1; + m_IndexSTBC = -1; + m_IndexReset = -1; if ( g_pCurrentModel->radioLinksParams.link_capabilities_flags[m_iRadioLink] & RADIO_HW_CAPABILITY_FLAG_USED_FOR_RELAY ) log_line("Opening menu for relay radio link %d ...", m_iRadioLink+1); @@ -60,157 +77,77 @@ MenuVehicleRadioLink::MenuVehicleRadioLink(int iRadioLink) sprintf(szBuff, "Vehicle Radio Link %d Parameters", m_iRadioLink+1); setTitle(szBuff); - for( int i=0; i<20; i++ ) - m_pItemsSelect[i] = NULL; - - char szBands[128]; int iRadioInterfaceId = g_pCurrentModel->getRadioInterfaceIndexForRadioLink(m_iRadioLink); if ( -1 == iRadioInterfaceId ) { log_softerror_and_alarm("Invalid radio link. No radio interfaces assigned to radio link %d.", m_iRadioLink+1); - m_IndexStart = -1; return; } - log_line("Opened menu to configure radio link %d, used by radio interface %d on the model side.", m_iRadioLink+1, iRadioInterfaceId+1); - - m_IndexStart = 0; - str_get_supported_bands_string(g_pCurrentModel->radioInterfacesParams.interface_supported_bands[iRadioInterfaceId], szBands); - - u32 uControllerSupportedBands = 0; + m_uControllerSupportedBands = 0; for( int i=0; iszMAC) ) continue; - uControllerSupportedBands |= pRadioHWInfo->supportedBands; + m_uControllerSupportedBands |= pRadioHWInfo->supportedBands; } - m_SupportedChannelsCount = getSupportedChannels( uControllerSupportedBands & g_pCurrentModel->radioInterfacesParams.interface_supported_bands[iRadioInterfaceId], 1, &(m_SupportedChannels[0]), MAX_MENU_CHANNELS); - - sprintf(szBuff, "Vehicle radio interface used for this radio link: %s", str_get_radio_card_model_string(g_pCurrentModel->radioInterfacesParams.interface_card_model[iRadioInterfaceId])); - - addMenuItem(new MenuItemText(szBuff, false, 0.0)); - addSection("General"); - - m_pItemsSelect[00] = new MenuItemSelect("Frequency"); + m_SupportedChannelsCount = getSupportedChannels( m_uControllerSupportedBands & g_pCurrentModel->radioInterfacesParams.interface_supported_bands[iRadioInterfaceId], 1, &(m_SupportedChannels[0]), MAX_MENU_CHANNELS); - if ( g_pCurrentModel->radioLinksParams.link_capabilities_flags[m_iRadioLink] & RADIO_HW_CAPABILITY_FLAG_USED_FOR_RELAY ) - { - sprintf(szBuff,"Relay %s", str_format_frequency(g_pCurrentModel->relay_params.uRelayFrequencyKhz)); - m_pItemsSelect[00]->addSelection(szBuff); - } - else - { - Preferences* pP = get_Preferences(); - int iCountChCurrentColumn = 0; - for( int ch=0; chiScaleMenus > 0) && (iCountChCurrentColumn > 14 ) ) - { - m_pItemsSelect[00]->addSeparator(); - iCountChCurrentColumn = 0; - } - if ( m_SupportedChannels[ch] == 0 ) - { - m_pItemsSelect[00]->addSeparator(); - iCountChCurrentColumn = 0; - continue; - } - strcpy(szBuff, str_format_frequency(m_SupportedChannels[ch])); - m_pItemsSelect[00]->addSelection(szBuff); - iCountChCurrentColumn++; - } + log_line("Opened menu to configure radio link %d, used by radio interface %d on the model side.", m_iRadioLink+1, iRadioInterfaceId+1); + addMenuItems(); + log_line("Created radio link menu for radio link %d", m_iRadioLink+1); +} - m_SupportedChannelsCount = getSupportedChannels( uControllerSupportedBands & g_pCurrentModel->radioInterfacesParams.interface_supported_bands[iRadioInterfaceId], 0, &(m_SupportedChannels[0]), MAX_MENU_CHANNELS); - if ( 0 == m_SupportedChannelsCount ) - { - sprintf(szBuff,"%s", str_format_frequency(g_pCurrentModel->radioLinksParams.link_frequency_khz[m_iRadioLink])); - m_pItemsSelect[00]->addSelection(szBuff); - m_pItemsSelect[00]->setSelectedIndex(0); - m_pItemsSelect[00]->setEnabled(false); - } - } +MenuVehicleRadioLink::~MenuVehicleRadioLink() +{ +} - m_pItemsSelect[00]->setIsEditable(); - m_IndexFrequency = addMenuItem(m_pItemsSelect[00]); +void MenuVehicleRadioLink::onShow() +{ + valuesToUI(); + Menu::onShow(); + invalidate(); + log_line("Showed radio link menu for radio link %d", m_iRadioLink+1); + addMenuItems(); +} - m_pItemsSelect[01] = new MenuItemSelect("Usage", "Selects what type of data gets sent/received on this radio link, or disable it."); - m_pItemsSelect[01]->addSelection("Disabled"); - m_pItemsSelect[01]->addSelection("Video & Data"); - m_pItemsSelect[01]->addSelection("Video"); - m_pItemsSelect[01]->addSelection("Data"); - m_pItemsSelect[01]->setIsEditable(); - m_IndexUsage = addMenuItem(m_pItemsSelect[01]); +void MenuVehicleRadioLink::addMenuItems() +{ + int iTmp = getSelectedMenuItemIndex(); + removeAllItems(); - m_pItemsSelect[02] = new MenuItemSelect("Capabilities", "Sets the uplink/downlink capabilities of this radio link. If the associated vehicle radio interface has attached an external LNA or a unidirectional booster, it can't be used for both uplink and downlink, it must be marked to be used only for uplink or downlink accordingly."); - m_pItemsSelect[02]->addSelection("Uplink & Downlink"); - m_pItemsSelect[02]->addSelection("Downlink Only"); - m_pItemsSelect[02]->addSelection("Uplink Only"); - m_pItemsSelect[02]->setIsEditable(); - m_IndexCapabilities = addMenuItem(m_pItemsSelect[02]); + for( int i=0; i<20; i++ ) + m_pItemsSelect[i] = NULL; + + m_IndexFrequency = -1; + m_IndexUsage = -1; + m_IndexCapabilities = -1; + m_IndexDataRatesType = -1; + m_IndexDataRateVideo = -1; + m_IndexDataRateTypeDownlink = -1; + m_IndexDataRateTypeUplink = -1; + m_IndexDataRateDataDownlink = -1; + m_IndexDataRateDataUplink = -1; + m_IndexHT = -1; + m_IndexLDPC = -1; + m_IndexSGI = -1; + m_IndexSTBC = -1; + m_IndexReset = -1; - if ( 1 == g_pCurrentModel->radioInterfacesParams.interfaces_count ) - addMenuItem( new MenuItemText(s_szMenuRadio_SingleCard2, true)); + int iRadioInterfaceId = g_pCurrentModel->getRadioInterfaceIndexForRadioLink(m_iRadioLink); + char szBuff[256]; + sprintf(szBuff, "Vehicle radio interface used for this radio link: %s", str_get_radio_card_model_string(g_pCurrentModel->radioInterfacesParams.interface_card_model[iRadioInterfaceId])); + addMenuItem(new MenuItemText(szBuff, false, 0.0)); + addSection("General"); + addMenuItemFrequencies(); + addMenuItemsCapabilities(); + addSection("Radio Data Rates"); - - m_pItemsSelect[03] = new MenuItemSelect("Radio Data Rate (for Video)", "Sets the physical radio data rate to use on this radio link for video data. If adaptive radio links is enabled, this will get lowered automatically by Ruby as needed."); - for( int i=0; iaddSelection(szBuff); - } - for( int i=0; i<=MAX_MCS_INDEX; i++ ) - { - str_getDataRateDescription(-1-i, 0, szBuff); - m_pItemsSelect[03]->addSelection(szBuff); - } - m_pItemsSelect[03]->setIsEditable(); - m_IndexDataRateVideo = addMenuItem(m_pItemsSelect[03]); - - m_pItemsSelect[04] = new MenuItemSelect("Radio Data Rate (for data downlink)", "Sets the physical radio downlink data rate for data packets (excluding video packets)."); - m_pItemsSelect[04]->addSelection("Same as Video Data Rate"); - m_pItemsSelect[04]->addSelection("Fixed"); - m_pItemsSelect[04]->addSelection("Lowest"); - m_pItemsSelect[04]->setIsEditable(); - m_IndexDataRateTypeDownlink = addMenuItem(m_pItemsSelect[04]); - - m_pItemsSelect[05] = new MenuItemSelect(" Fixed Radio Data Rate (data downlink)", "Sets the physical radio downlink data rate for data packets (excluding video packets)."); - for( int i=0; iaddSelection(szBuff); - } - for( int i=0; i<=MAX_MCS_INDEX; i++ ) - { - str_getDataRateDescription(-1-i, 0, szBuff); - m_pItemsSelect[05]->addSelection(szBuff); - } - m_pItemsSelect[05]->setIsEditable(); - m_IndexDataRateDataDownlink = addMenuItem(m_pItemsSelect[05]); - - m_pItemsSelect[06] = new MenuItemSelect("Radio Data Rate (for data uplink)", "Sets the physical radio uplink data rate for data packets."); - m_pItemsSelect[06]->addSelection("Same as Video Data Rate"); - m_pItemsSelect[06]->addSelection("Fixed"); - m_pItemsSelect[06]->addSelection("Lowest"); - m_pItemsSelect[06]->setIsEditable(); - m_IndexDataRateTypeUplink = addMenuItem(m_pItemsSelect[06]); - - m_pItemsSelect[07] = new MenuItemSelect(" Fixed Radio Data Rate (data uplink)", "Sets the physical radio uplink data rate for data packets."); - for( int i=0; iaddSelection(szBuff); - } - for( int i=0; i<=MAX_MCS_INDEX; i++ ) - { - str_getDataRateDescription(-1-i, 0, szBuff); - m_pItemsSelect[07]->addSelection(szBuff); - } - m_pItemsSelect[07]->setIsEditable(); - m_IndexDataRateDataUplink = addMenuItem(m_pItemsSelect[07]); + addMenuItemsDataRates(); addSection("Radio Link Flags"); @@ -221,77 +158,73 @@ MenuVehicleRadioLink::MenuVehicleRadioLink(int iRadioLink) m_pItemsSelect[11]->addSelection("40 Mhz Both Directions"); m_pItemsSelect[11]->setIsEditable(); m_IndexHT = addMenuItem(m_pItemsSelect[11]); + if ( m_bSwitchedDataRatesType ) + m_pItemsSelect[11]->setEnabled(false); - m_pItemsSelect[12] = new MenuItemSelect("Enable LDPC", "Enables LDPC (Low Density Parity Check) when using MCS data rates."); - m_pItemsSelect[12]->addSelection("No"); - m_pItemsSelect[12]->addSelection("Vehicle Downlink"); - m_pItemsSelect[12]->addSelection("Controller Uplink"); - m_pItemsSelect[12]->addSelection("Both Directions"); - m_pItemsSelect[12]->setIsEditable(); - m_IndexLDPC = addMenuItem(m_pItemsSelect[12]); - - m_pItemsSelect[13] = new MenuItemSelect("Enable SGI", "Enables SGI (Short Guard Interval) when using MCS data rates."); - m_pItemsSelect[13]->addSelection("No"); - m_pItemsSelect[13]->addSelection("Vehicle Downlink"); - m_pItemsSelect[13]->addSelection("Controller Uplink"); - m_pItemsSelect[13]->addSelection("Both Directions"); - m_pItemsSelect[13]->setIsEditable(); - m_IndexSGI = addMenuItem(m_pItemsSelect[13]); - - m_pItemsSelect[14] = new MenuItemSelect("Enable STBC", "Enables STBC when using MCS data rates."); - m_pItemsSelect[14]->addSelection("No"); - m_pItemsSelect[14]->addSelection("Vehicle Downlink"); - m_pItemsSelect[14]->addSelection("Controller Uplink"); - m_pItemsSelect[14]->addSelection("Both Directions"); - m_pItemsSelect[14]->setIsEditable(); - m_IndexSTBC = addMenuItem(m_pItemsSelect[14]); - + if ( g_pCurrentModel->radioLinksParams.link_datarate_video_bps[m_iRadioLink] < 0 ) + addMenuItemsMCS(); + else if ( m_bSwitchedDataRatesType ) + addMenuItemsMCS(); m_IndexReset = addMenuItem(new MenuItem("Reset To Default", "Resets this radio link parameters to default values.")); - log_line("Created radio link menu for radio link %d", m_iRadioLink+1); -} -MenuVehicleRadioLink::~MenuVehicleRadioLink() -{ -} - -void MenuVehicleRadioLink::onShow() -{ valuesToUI(); - Menu::onShow(); - invalidate(); - - log_line("Showed radio link menu for radio link %d", m_iRadioLink+1); + m_SelectedIndex = iTmp; + if ( m_SelectedIndex >= m_ItemsCount ) + m_SelectedIndex = m_ItemsCount-1; } -void MenuVehicleRadioLink::valuesToUI() +void MenuVehicleRadioLink::addMenuItemFrequencies() { - bool bDisableAll = false; - if ( -1 == m_IndexStart ) - bDisableAll = true; - int iRadioInterfaceId = g_pCurrentModel->getRadioInterfaceIndexForRadioLink(m_iRadioLink); - if ( -1 == iRadioInterfaceId ) - bDisableAll = true; - - log_line("MenuVehicleRadioLink: Update values for radio link %d", m_iRadioLink+1); + char szBuff[128]; + m_pItemsSelect[0] = new MenuItemSelect("Frequency"); - if ( bDisableAll ) + if ( g_pCurrentModel->radioLinksParams.link_capabilities_flags[m_iRadioLink] & RADIO_HW_CAPABILITY_FLAG_USED_FOR_RELAY ) { - for( int i=1; i<20; i++ ) - if ( NULL != m_pItemsSelect[i] ) - m_pItemsSelect[i]->setEnabled(false); + sprintf(szBuff,"Relay %s", str_format_frequency(g_pCurrentModel->relay_params.uRelayFrequencyKhz)); + m_pItemsSelect[0]->addSelection(szBuff); + m_pItemsSelect[0]->setIsEditable(); + m_IndexFrequency = addMenuItem(m_pItemsSelect[0]); return; } + + Preferences* pP = get_Preferences(); + int iCountChCurrentColumn = 0; + for( int ch=0; chiScaleMenus > 0) && (iCountChCurrentColumn > 14 ) ) + { + m_pItemsSelect[0]->addSeparator(); + iCountChCurrentColumn = 0; + } + if ( m_SupportedChannels[ch] == 0 ) + { + m_pItemsSelect[0]->addSeparator(); + iCountChCurrentColumn = 0; + continue; + } + strcpy(szBuff, str_format_frequency(m_SupportedChannels[ch])); + m_pItemsSelect[0]->addSelection(szBuff); + iCountChCurrentColumn++; + } - u32 linkCapabilitiesFlags = g_pCurrentModel->radioLinksParams.link_capabilities_flags[m_iRadioLink]; - u32 linkRadioFlags = g_pCurrentModel->radioLinksParams.link_radio_flags[m_iRadioLink]; + int iRadioInterfaceId = g_pCurrentModel->getRadioInterfaceIndexForRadioLink(m_iRadioLink); + m_SupportedChannelsCount = getSupportedChannels( m_uControllerSupportedBands & g_pCurrentModel->radioInterfacesParams.interface_supported_bands[iRadioInterfaceId], 0, &(m_SupportedChannels[0]), MAX_MENU_CHANNELS); + if ( 0 == m_SupportedChannelsCount ) + { + sprintf(szBuff,"%s", str_format_frequency(g_pCurrentModel->radioLinksParams.link_frequency_khz[m_iRadioLink])); + m_pItemsSelect[0]->addSelection(szBuff); + m_pItemsSelect[0]->setSelectedIndex(0); + m_pItemsSelect[0]->setEnabled(false); + } - // Frequency + m_pItemsSelect[0]->setIsEditable(); + m_IndexFrequency = addMenuItem(m_pItemsSelect[0]); if ( g_pCurrentModel->radioLinksParams.link_capabilities_flags[m_iRadioLink] & RADIO_HW_CAPABILITY_FLAG_USED_FOR_RELAY ) - m_pItemsSelect[00]->setEnabled(false); + m_pItemsSelect[0]->setEnabled(false); else - m_pItemsSelect[00]->setEnabled(true); + m_pItemsSelect[0]->setEnabled(true); int selectedIndex = 0; for( int ch=0; chsetSelection(selectedIndex); + m_pItemsSelect[0]->setSelection(selectedIndex); +} - // Capabilities +void MenuVehicleRadioLink::addMenuItemsCapabilities() +{ + u32 linkCapabilitiesFlags = g_pCurrentModel->radioLinksParams.link_capabilities_flags[m_iRadioLink]; + + m_pItemsSelect[1] = new MenuItemSelect("Link Usage", "Selects what type of data gets sent/received on this radio link, or disable it."); + m_pItemsSelect[1]->addSelection("Disabled"); + m_pItemsSelect[1]->addSelection("Video & Data"); + m_pItemsSelect[1]->addSelection("Video"); + m_pItemsSelect[1]->addSelection("Data"); + m_pItemsSelect[1]->setIsEditable(); + m_IndexUsage = addMenuItem(m_pItemsSelect[1]); + + m_pItemsSelect[2] = new MenuItemSelect("Capabilities", "Sets the uplink/downlink capabilities of this radio link. If the associated vehicle radio interface has attached an external LNA or a unidirectional booster, it can't be used for both uplink and downlink, it must be marked to be used only for uplink or downlink accordingly."); + m_pItemsSelect[2]->addSelection("Uplink & Downlink"); + m_pItemsSelect[2]->addSelection("Downlink Only"); + m_pItemsSelect[2]->addSelection("Uplink Only"); + m_pItemsSelect[2]->setIsEditable(); + m_IndexCapabilities = addMenuItem(m_pItemsSelect[2]); + + // Link usage if ( g_pCurrentModel->radioLinksParams.link_capabilities_flags[m_iRadioLink] & RADIO_HW_CAPABILITY_FLAG_USED_FOR_RELAY ) - m_pItemsSelect[01]->setEnabled(false); + m_pItemsSelect[1]->setEnabled(false); else - m_pItemsSelect[01]->setEnabled(true); + m_pItemsSelect[1]->setEnabled(true); - m_pItemsSelect[01]->setSelection(0); // Disabled + m_pItemsSelect[1]->setSelection(0); // Disabled if ( linkCapabilitiesFlags & RADIO_HW_CAPABILITY_FLAG_CAN_USE_FOR_DATA ) if ( linkCapabilitiesFlags & RADIO_HW_CAPABILITY_FLAG_CAN_USE_FOR_VIDEO ) - m_pItemsSelect[01]->setSelection(1); + m_pItemsSelect[1]->setSelection(1); if ( linkCapabilitiesFlags & RADIO_HW_CAPABILITY_FLAG_CAN_USE_FOR_DATA ) if ( !(linkCapabilitiesFlags & RADIO_HW_CAPABILITY_FLAG_CAN_USE_FOR_VIDEO) ) - m_pItemsSelect[01]->setSelection(3); + m_pItemsSelect[1]->setSelection(3); if ( !(linkCapabilitiesFlags & RADIO_HW_CAPABILITY_FLAG_CAN_USE_FOR_DATA) ) if ( linkCapabilitiesFlags & RADIO_HW_CAPABILITY_FLAG_CAN_USE_FOR_VIDEO ) - m_pItemsSelect[01]->setSelection(2); + m_pItemsSelect[1]->setSelection(2); if ( (1 == g_pCurrentModel->radioInterfacesParams.interfaces_count) || (1 == g_pCurrentModel->radioLinksParams.links_count) ) - m_pItemsSelect[01]->setSelection(1); + m_pItemsSelect[1]->setSelection(1); if ( linkCapabilitiesFlags & RADIO_HW_CAPABILITY_FLAG_DISABLED ) - m_pItemsSelect[01]->setSelection(0); + m_pItemsSelect[1]->setSelection(0); if ( (1 == g_pCurrentModel->radioInterfacesParams.interfaces_count) || (1 == g_pCurrentModel->radioLinksParams.links_count) ) - m_pItemsSelect[01]->setEnabled(false); - - if ( (g_pCurrentModel->radioLinksParams.link_capabilities_flags[m_iRadioLink] & RADIO_HW_CAPABILITY_FLAG_USED_FOR_RELAY) || - (linkCapabilitiesFlags & RADIO_HW_CAPABILITY_FLAG_DISABLED) ) - { - for( int i=2; i<20; i++ ) - if ( NULL != m_pItemsSelect[i] ) - m_pItemsSelect[i]->setEnabled(false); - m_pItemsSelect[0]->setEnabled(false); - } - else - { - for( int i=2; i<20; i++ ) - if ( NULL != m_pItemsSelect[i]) - m_pItemsSelect[i]->setEnabled(true); - m_pItemsSelect[0]->setEnabled(true); - } + m_pItemsSelect[1]->setEnabled(false); - // Usage + // Capabilities if ( linkCapabilitiesFlags & RADIO_HW_CAPABILITY_FLAG_CAN_TX ) if ( linkCapabilitiesFlags & RADIO_HW_CAPABILITY_FLAG_CAN_RX ) - m_pItemsSelect[02]->setSelection(0); + m_pItemsSelect[2]->setSelection(0); if ( linkCapabilitiesFlags & RADIO_HW_CAPABILITY_FLAG_CAN_RX ) if ( !(linkCapabilitiesFlags & RADIO_HW_CAPABILITY_FLAG_CAN_TX) ) - m_pItemsSelect[02]->setSelection(2); + m_pItemsSelect[2]->setSelection(2); if ( linkCapabilitiesFlags & RADIO_HW_CAPABILITY_FLAG_CAN_TX ) if ( !(linkCapabilitiesFlags & RADIO_HW_CAPABILITY_FLAG_CAN_RX) ) - m_pItemsSelect[02]->setSelection(1); + m_pItemsSelect[2]->setSelection(1); if ( (1 == g_pCurrentModel->radioInterfacesParams.interfaces_count) || (1 == g_pCurrentModel->radioLinksParams.links_count) ) { - m_pItemsSelect[02]->setSelection(0); - m_pItemsSelect[02]->setEnabled(false); + m_pItemsSelect[2]->setSelection(0); + m_pItemsSelect[2]->setEnabled(false); } - // Data rates + if ( 1 == g_pCurrentModel->radioInterfacesParams.interfaces_count ) + addMenuItem( new MenuItemText(s_szMenuRadio_SingleCard2, true)); - selectedIndex = 0; - for( int i=0; iradioLinksParams.link_datarate_video_bps[m_iRadioLink] == getDataRatesBPS()[i] ) - selectedIndex = i; - for( int i=0; i<=MAX_MCS_INDEX; i++ ) - if ( g_pCurrentModel->radioLinksParams.link_datarate_video_bps[m_iRadioLink] == (-1-i) ) - selectedIndex = i+getDataRatesCount(); +} - m_pItemsSelect[03]->setSelection(selectedIndex); +void MenuVehicleRadioLink::addMenuItemsDataRates() +{ + char szBuff[128]; + m_pItemsSelect[8] = new MenuItemSelect("Radio Modulations", "Use legacy modulation schemes or or the new Modulation Coding Schemes."); + m_pItemsSelect[8]->addSelection("Legacy Modulations"); + m_pItemsSelect[8]->addSelection("Modulation Coding Schemes"); + m_pItemsSelect[8]->setIsEditable(); + m_IndexDataRatesType = addMenuItem(m_pItemsSelect[8]); - selectedIndex = 0; - for( int i=0; iradioLinksParams.link_datarate_data_bps[m_iRadioLink] == getDataRatesBPS()[i] ) - selectedIndex = i; - for( int i=0; i<=MAX_MCS_INDEX; i++ ) - if ( g_pCurrentModel->radioLinksParams.link_datarate_data_bps[m_iRadioLink] == (-1-i) ) - selectedIndex = i+getDataRatesCount(); + bool bAddMCSRates = false; - m_pItemsSelect[05]->setSelection(selectedIndex); + if ( g_pCurrentModel->radioLinksParams.link_datarate_video_bps[m_iRadioLink] > 0 ) + { + if ( m_bSwitchedDataRatesType ) + { + m_pItemsSelect[8]->setSelectedIndex(1); + bAddMCSRates = true; + } + else + m_pItemsSelect[8]->setSelectedIndex(0); + } + else + { + if ( m_bSwitchedDataRatesType ) + m_pItemsSelect[8]->setSelectedIndex(0); + else + { + m_pItemsSelect[8]->setSelectedIndex(1); + bAddMCSRates = true; + } + } - selectedIndex = 0; - for( int i=0; iradioLinksParams.uplink_datarate_data_bps[m_iRadioLink] == getDataRatesBPS()[i] ) - selectedIndex = i; - for( int i=0; i<=MAX_MCS_INDEX; i++ ) - if ( g_pCurrentModel->radioLinksParams.uplink_datarate_data_bps[m_iRadioLink] == (-1-i) ) - selectedIndex = i+getDataRatesCount(); + m_pItemsSelect[3] = new MenuItemSelect("Video Radio Data Rate", "Sets the physical radio data rate to use on this radio link for video data. If adaptive radio links is enabled, this will get lowered automatically by Ruby as needed."); + if ( m_bMustSelectDatarate ) + m_pItemsSelect[3]->addSelection("Select a data rate"); + + if ( bAddMCSRates ) + { + for( int i=0; i<=MAX_MCS_INDEX; i++ ) + { + str_getDataRateDescription(-1-i, 0, szBuff); + m_pItemsSelect[3]->addSelection(szBuff); + } + } + else + { + for( int i=0; iaddSelection(szBuff); + } + } + m_pItemsSelect[3]->setIsEditable(); + m_IndexDataRateVideo = addMenuItem(m_pItemsSelect[3]); - m_pItemsSelect[07]->setSelection(selectedIndex); + // Set video data rates - // Fixed data rates + int selectedIndex = -1; - m_pItemsSelect[04]->setSelectedIndex(2); - if ( g_pCurrentModel->radioLinksParams.uDownlinkDataDataRateType[m_iRadioLink] == FLAG_RADIO_LINK_DATARATE_DATA_TYPE_SAME_AS_ADAPTIVE_VIDEO ) + if ( m_bMustSelectDatarate ) + m_pItemsSelect[3]->setSelectedIndex(0); + else { - m_pItemsSelect[04]->setSelectedIndex(0); - m_pItemsSelect[05]->setEnabled(false); - m_pItemsSelect[05]->setSelectedIndex(m_pItemsSelect[03]->getSelectedIndex()); + if ( bAddMCSRates ) + { + for( int i=0; i<=MAX_MCS_INDEX; i++ ) + if ( g_pCurrentModel->radioLinksParams.link_datarate_video_bps[m_iRadioLink] == (-1-i) ) + selectedIndex = i; + } + else + { + for( int i=0; iradioLinksParams.link_datarate_video_bps[m_iRadioLink] == getDataRatesBPS()[i] ) + selectedIndex = i; + } + if ( selectedIndex == -1 ) + selectedIndex = 2; + m_pItemsSelect[3]->setSelectedIndex(selectedIndex); } - if ( g_pCurrentModel->radioLinksParams.uDownlinkDataDataRateType[m_iRadioLink] == FLAG_RADIO_LINK_DATARATE_DATA_TYPE_FIXED ) + + m_pItemsSelect[4] = new MenuItemSelect("Telemetry Radio Data Rate (for downlink)", "Sets the physical radio downlink data rate for data packets (excluding video packets)."); + m_pItemsSelect[4]->addSelection("Same as Video Data Rate"); + m_pItemsSelect[4]->addSelection("Fixed"); + m_pItemsSelect[4]->addSelection("Lowest"); + m_pItemsSelect[4]->setIsEditable(); + m_IndexDataRateTypeDownlink = addMenuItem(m_pItemsSelect[4]); + + m_pItemsSelect[5] = new MenuItemSelect(" Fixed Radio Data Rate (for downlink)", "Sets the physical radio downlink data rate for data packets (excluding video packets)."); + if ( bAddMCSRates ) { - m_pItemsSelect[04]->setSelectedIndex(1); + for( int i=0; i<=MAX_MCS_INDEX; i++ ) + { + str_getDataRateDescription(-1-i, 0, szBuff); + m_pItemsSelect[5]->addSelection(szBuff); + } } - if ( g_pCurrentModel->radioLinksParams.uDownlinkDataDataRateType[m_iRadioLink] == FLAG_RADIO_LINK_DATARATE_DATA_TYPE_LOWEST ) + else { - m_pItemsSelect[04]->setSelectedIndex(2); - m_pItemsSelect[05]->setEnabled(false); - selectedIndex = 0; for( int i=0; iradioLinksParams.link_datarate_video_bps[m_iRadioLink] < 0 ) - selectedIndex = getDataRatesCount(); - m_pItemsSelect[05]->setSelectedIndex(selectedIndex); + { + str_getDataRateDescription(getDataRatesBPS()[i], 0, szBuff); + m_pItemsSelect[5]->addSelection(szBuff); + } + } + m_pItemsSelect[5]->setIsEditable(); + m_IndexDataRateDataDownlink = addMenuItem(m_pItemsSelect[5]); + + m_pItemsSelect[6] = new MenuItemSelect("Telemetry Radio Data Rate (for uplink)", "Sets the physical radio uplink data rate for data packets."); + m_pItemsSelect[6]->addSelection("Same as Video Data Rate"); + m_pItemsSelect[6]->addSelection("Fixed"); + m_pItemsSelect[6]->addSelection("Lowest"); + m_pItemsSelect[6]->setIsEditable(); + m_IndexDataRateTypeUplink = addMenuItem(m_pItemsSelect[6]); + + m_pItemsSelect[7] = new MenuItemSelect(" Fixed Radio Data Rate (for uplink)", "Sets the physical radio uplink data rate for data packets."); + if ( bAddMCSRates ) + { + for( int i=0; i<=MAX_MCS_INDEX; i++ ) + { + str_getDataRateDescription(-1-i, 0, szBuff); + m_pItemsSelect[7]->addSelection(szBuff); + } + } + else + { + for( int i=0; iaddSelection(szBuff); + } } + m_pItemsSelect[7]->setIsEditable(); + m_IndexDataRateDataUplink = addMenuItem(m_pItemsSelect[7]); + // Data/telemetry data rates - m_pItemsSelect[06]->setSelectedIndex(2); - if ( g_pCurrentModel->radioLinksParams.uUplinkDataDataRateType[m_iRadioLink] == FLAG_RADIO_LINK_DATARATE_DATA_TYPE_SAME_AS_ADAPTIVE_VIDEO ) + selectedIndex = -1; + if ( bAddMCSRates ) { - m_pItemsSelect[06]->setSelectedIndex(0); - m_pItemsSelect[07]->setEnabled(false); - m_pItemsSelect[07]->setSelectedIndex(m_pItemsSelect[03]->getSelectedIndex()); + for( int i=0; i<=MAX_MCS_INDEX; i++ ) + if ( g_pCurrentModel->radioLinksParams.link_datarate_data_bps[m_iRadioLink] == (-1-i) ) + selectedIndex = i; } - if ( g_pCurrentModel->radioLinksParams.uUplinkDataDataRateType[m_iRadioLink] == FLAG_RADIO_LINK_DATARATE_DATA_TYPE_FIXED ) + else { - m_pItemsSelect[06]->setSelectedIndex(1); + for( int i=0; iradioLinksParams.link_datarate_data_bps[m_iRadioLink] == getDataRatesBPS()[i] ) + selectedIndex = i; } - if ( g_pCurrentModel->radioLinksParams.uUplinkDataDataRateType[m_iRadioLink] == FLAG_RADIO_LINK_DATARATE_DATA_TYPE_LOWEST ) - { - m_pItemsSelect[06]->setSelectedIndex(2); - m_pItemsSelect[07]->setEnabled(false); - + + if ( (-1 == selectedIndex) || m_bMustSelectDatarate ) selectedIndex = 0; + m_pItemsSelect[5]->setSelection(selectedIndex); + + + selectedIndex = -1; + if ( bAddMCSRates ) + { + for( int i=0; i<=MAX_MCS_INDEX; i++ ) + if ( g_pCurrentModel->radioLinksParams.uplink_datarate_data_bps[m_iRadioLink] == (-1-i) ) + selectedIndex = i; + } + else + { for( int i=0; iradioLinksParams.uplink_datarate_data_bps[m_iRadioLink] == getDataRatesBPS()[i] ) selectedIndex = i; - if ( g_pCurrentModel->radioLinksParams.link_datarate_video_bps[m_iRadioLink] < 0 ) - selectedIndex = getDataRatesCount(); - m_pItemsSelect[07]->setSelectedIndex(selectedIndex); } + if ( (-1 == selectedIndex) || m_bMustSelectDatarate ) + selectedIndex = 0; + m_pItemsSelect[7]->setSelection(selectedIndex); + + if ( m_bMustSelectDatarate ) + { + m_pItemsSelect[4]->setSelectedIndex(2); + m_pItemsSelect[5]->setSelectedIndex(0); + m_pItemsSelect[5]->setEnabled(false); + m_pItemsSelect[6]->setSelectedIndex(2); + m_pItemsSelect[7]->setSelectedIndex(0); + m_pItemsSelect[7]->setEnabled(false); + } + else + { + if ( g_pCurrentModel->radioLinksParams.uDownlinkDataDataRateType[m_iRadioLink] == FLAG_RADIO_LINK_DATARATE_DATA_TYPE_SAME_AS_ADAPTIVE_VIDEO ) + { + m_pItemsSelect[4]->setSelectedIndex(0); + m_pItemsSelect[5]->setEnabled(false); + m_pItemsSelect[5]->setSelectedIndex(m_pItemsSelect[3]->getSelectedIndex()); + } + else if ( g_pCurrentModel->radioLinksParams.uDownlinkDataDataRateType[m_iRadioLink] == FLAG_RADIO_LINK_DATARATE_DATA_TYPE_FIXED ) + { + m_pItemsSelect[4]->setSelectedIndex(1); + m_pItemsSelect[5]->setEnabled(true); + } + else if ( g_pCurrentModel->radioLinksParams.uDownlinkDataDataRateType[m_iRadioLink] == FLAG_RADIO_LINK_DATARATE_DATA_TYPE_LOWEST ) + { + m_pItemsSelect[4]->setSelectedIndex(2); + m_pItemsSelect[5]->setEnabled(false); + m_pItemsSelect[5]->setSelectedIndex(0); + } + + if ( g_pCurrentModel->radioLinksParams.uUplinkDataDataRateType[m_iRadioLink] == FLAG_RADIO_LINK_DATARATE_DATA_TYPE_SAME_AS_ADAPTIVE_VIDEO ) + { + m_pItemsSelect[6]->setSelectedIndex(0); + m_pItemsSelect[7]->setEnabled(false); + m_pItemsSelect[7]->setSelectedIndex(m_pItemsSelect[3]->getSelectedIndex()); + } + else if ( g_pCurrentModel->radioLinksParams.uUplinkDataDataRateType[m_iRadioLink] == FLAG_RADIO_LINK_DATARATE_DATA_TYPE_FIXED ) + { + m_pItemsSelect[6]->setSelectedIndex(1); + m_pItemsSelect[7]->setEnabled(true); + } + else if ( g_pCurrentModel->radioLinksParams.uUplinkDataDataRateType[m_iRadioLink] == FLAG_RADIO_LINK_DATARATE_DATA_TYPE_LOWEST ) + { + m_pItemsSelect[6]->setSelectedIndex(2); + m_pItemsSelect[7]->setEnabled(false); + m_pItemsSelect[7]->setSelectedIndex(0); + } + } +} - // MCS flags +void MenuVehicleRadioLink::addMenuItemsMCS() +{ + u32 linkRadioFlags = g_pCurrentModel->radioLinksParams.link_radio_flags[m_iRadioLink]; - if ( (linkRadioFlags & RADIO_FLAG_HT40_VEHICLE) && (linkRadioFlags & RADIO_FLAG_HT40_CONTROLLER) ) - m_pItemsSelect[11]->setSelectedIndex(3); - else if ( linkRadioFlags & RADIO_FLAG_HT40_VEHICLE ) - m_pItemsSelect[11]->setSelectedIndex(1); - else if ( linkRadioFlags & RADIO_FLAG_HT40_CONTROLLER ) - m_pItemsSelect[11]->setSelectedIndex(2); - else - m_pItemsSelect[11]->setSelectedIndex(0); + m_pItemsSelect[12] = new MenuItemSelect("Enable LDPC", "Enables LDPC (Low Density Parity Check) when using MCS data rates."); + m_pItemsSelect[12]->addSelection("No"); + m_pItemsSelect[12]->addSelection("Downlink"); + m_pItemsSelect[12]->addSelection("Uplink"); + m_pItemsSelect[12]->addSelection("Both Directions"); + m_pItemsSelect[12]->setIsEditable(); + m_IndexLDPC = addMenuItem(m_pItemsSelect[12]); + + m_pItemsSelect[13] = new MenuItemSelect("Enable SGI", "Enables SGI (Short Guard Interval) when using MCS data rates."); + m_pItemsSelect[13]->addSelection("No"); + m_pItemsSelect[13]->addSelection("Downlink"); + m_pItemsSelect[13]->addSelection("Uplink"); + m_pItemsSelect[13]->addSelection("Both Directions"); + m_pItemsSelect[13]->setIsEditable(); + m_IndexSGI = addMenuItem(m_pItemsSelect[13]); + + m_pItemsSelect[14] = new MenuItemSelect("Enable STBC", "Enables STBC when using MCS data rates."); + m_pItemsSelect[14]->addSelection("No"); + m_pItemsSelect[14]->addSelection("Downlink"); + m_pItemsSelect[14]->addSelection("Uplink"); + m_pItemsSelect[14]->addSelection("Both Directions"); + m_pItemsSelect[14]->setIsEditable(); + m_IndexSTBC = addMenuItem(m_pItemsSelect[14]); if ( (linkRadioFlags & RADIO_FLAG_LDPC_VEHICLE) && (linkRadioFlags & RADIO_FLAG_LDPC_CONTROLLER) ) m_pItemsSelect[12]->setSelectedIndex(3); @@ -492,12 +587,76 @@ void MenuVehicleRadioLink::valuesToUI() else m_pItemsSelect[14]->setSelectedIndex(0); + if ( m_bMustSelectDatarate ) + { + m_pItemsSelect[12]->setEnabled(false); + m_pItemsSelect[13]->setEnabled(false); + m_pItemsSelect[14]->setEnabled(false); + } +} + +void MenuVehicleRadioLink::valuesToUI() +{ + u32 linkCapabilitiesFlags = g_pCurrentModel->radioLinksParams.link_capabilities_flags[m_iRadioLink]; + u32 linkRadioFlags = g_pCurrentModel->radioLinksParams.link_radio_flags[m_iRadioLink]; + + if ( (g_pCurrentModel->radioLinksParams.link_capabilities_flags[m_iRadioLink] & RADIO_HW_CAPABILITY_FLAG_USED_FOR_RELAY) || + (linkCapabilitiesFlags & RADIO_HW_CAPABILITY_FLAG_DISABLED) ) + { + for( int i=2; i<20; i++ ) + if ( NULL != m_pItemsSelect[i] ) + m_pItemsSelect[i]->setEnabled(false); + m_pItemsSelect[0]->setEnabled(false); + } + else + { + for( int i=2; i<20; i++ ) + if ( NULL != m_pItemsSelect[i]) + m_pItemsSelect[i]->setEnabled(true); + m_pItemsSelect[0]->setEnabled(true); + + if ( m_bMustSelectDatarate ) + if ( 0 == m_pItemsSelect[3]->getSelectedIndex() ) + { + if ( NULL != m_pItemsSelect[4] ) + m_pItemsSelect[4]->setEnabled(false); + if ( NULL != m_pItemsSelect[5] ) + m_pItemsSelect[5]->setEnabled(false); + if ( NULL != m_pItemsSelect[6] ) + m_pItemsSelect[6]->setEnabled(false); + if ( NULL != m_pItemsSelect[7] ) + m_pItemsSelect[7]->setEnabled(false); + if ( NULL != m_pItemsSelect[12] ) + m_pItemsSelect[12]->setEnabled(false); + if ( NULL != m_pItemsSelect[13] ) + m_pItemsSelect[13]->setEnabled(false); + if ( NULL != m_pItemsSelect[14] ) + m_pItemsSelect[14]->setEnabled(false); + } + } + + if ( (linkRadioFlags & RADIO_FLAG_HT40_VEHICLE) && (linkRadioFlags & RADIO_FLAG_HT40_CONTROLLER) ) + m_pItemsSelect[11]->setSelectedIndex(3); + else if ( linkRadioFlags & RADIO_FLAG_HT40_VEHICLE ) + m_pItemsSelect[11]->setSelectedIndex(1); + else if ( linkRadioFlags & RADIO_FLAG_HT40_CONTROLLER ) + m_pItemsSelect[11]->setSelectedIndex(2); + else + m_pItemsSelect[11]->setSelectedIndex(0); + + if ( m_bMustSelectDatarate ) + { + m_pItemsSelect[11]->setSelectedIndex(0); + m_pItemsSelect[11]->setEnabled(false); + } + //char szBands[128]; + //str_get_supported_bands_string(g_pCurrentModel->radioInterfacesParams.interface_supported_bands[iRadioInterfaceId], szBands); + char szCapab[256]; char szFlags[256]; str_get_radio_capabilities_description(linkCapabilitiesFlags, szCapab); str_get_radio_frame_flags_description(linkRadioFlags, szFlags); log_line("MenuVehicleRadioLink: Update UI: Current radio link %d capabilities: %s, flags: %s.", m_iRadioLink+1, szCapab, szFlags); - } void MenuVehicleRadioLink::Render() @@ -513,15 +672,10 @@ void MenuVehicleRadioLink::Render() RenderEnd(yTop); } -void MenuVehicleRadioLink::onReturnFromChild(int iChildMenuId, int returnValue) -{ - Menu::onReturnFromChild(iChildMenuId, returnValue); -} - void MenuVehicleRadioLink::sendRadioLinkCapabilities(int iRadioLink) { - int usage = m_pItemsSelect[01]->getSelectedIndex(); - int capab = m_pItemsSelect[02]->getSelectedIndex(); + int usage = m_pItemsSelect[1]->getSelectedIndex(); + int capab = m_pItemsSelect[2]->getSelectedIndex(); u32 link_capabilities = g_pCurrentModel->radioLinksParams.link_capabilities_flags[iRadioLink]; @@ -555,7 +709,7 @@ void MenuVehicleRadioLink::sendRadioLinkCapabilities(int iRadioLink) log_line("Sending new link capabilities: %d for radio link %d", link_capabilities, iRadioLink+1); u32 param = (((u32)iRadioLink) & 0xFF) | (link_capabilities<<8); if ( ! handle_commands_send_to_vehicle(COMMAND_ID_SET_RADIO_LINK_CAPABILITIES, param, NULL, 0) ) - valuesToUI(); + addMenuItems(); } void MenuVehicleRadioLink::sendRadioLinkConfig(int iRadioLink) @@ -564,14 +718,14 @@ void MenuVehicleRadioLink::sendRadioLinkConfig(int iRadioLink) ((get_sw_version_major(g_pCurrentModel) == 9) && (get_sw_version_minor(g_pCurrentModel) <= 20)) ) { addMessageWithTitle(0, "Can't update radio links", "You need to update your vehicle to version 9.2 or newer"); - return; + addMenuItems(); } type_radio_links_parameters newRadioLinkParams; memcpy((u8*)&newRadioLinkParams, (u8*)&(g_pCurrentModel->radioLinksParams), sizeof(type_radio_links_parameters)); - int usage = m_pItemsSelect[01]->getSelectedIndex(); - int capab = m_pItemsSelect[02]->getSelectedIndex(); + int usage = m_pItemsSelect[1]->getSelectedIndex(); + int capab = m_pItemsSelect[2]->getSelectedIndex(); u32 link_capabilities = g_pCurrentModel->radioLinksParams.link_capabilities_flags[iRadioLink]; @@ -589,42 +743,60 @@ void MenuVehicleRadioLink::sendRadioLinkConfig(int iRadioLink) newRadioLinkParams.link_capabilities_flags[iRadioLink] = link_capabilities; - int indexRate = m_pItemsSelect[03]->getSelectedIndex(); + int indexRate = m_pItemsSelect[3]->getSelectedIndex(); + if ( m_bMustSelectDatarate ) + indexRate--; + if ( indexRate < 0 ) + indexRate = 0; - if ( indexRate < getDataRatesCount() ) - newRadioLinkParams.link_datarate_video_bps[iRadioLink] = getDataRatesBPS()[indexRate]; + bool bIsMCSRates = false; + if ( g_pCurrentModel->radioLinksParams.link_datarate_video_bps[m_iRadioLink] > 0 ) + { + if ( m_bSwitchedDataRatesType ) + bIsMCSRates = true; + } else - newRadioLinkParams.link_datarate_video_bps[iRadioLink] = -1-(indexRate - getDataRatesCount()); + { + if ( ! m_bSwitchedDataRatesType ) + bIsMCSRates = true; + } + + if ( bIsMCSRates ) + newRadioLinkParams.link_datarate_video_bps[iRadioLink] = -1-indexRate; + else if ( indexRate < getDataRatesCount() ) + newRadioLinkParams.link_datarate_video_bps[iRadioLink] = getDataRatesBPS()[indexRate]; - if ( 0 == m_pItemsSelect[04]->getSelectedIndex() ) + if ( 0 == m_pItemsSelect[4]->getSelectedIndex() ) newRadioLinkParams.uDownlinkDataDataRateType[iRadioLink] = FLAG_RADIO_LINK_DATARATE_DATA_TYPE_SAME_AS_ADAPTIVE_VIDEO; - if ( 1 == m_pItemsSelect[04]->getSelectedIndex() ) + if ( 1 == m_pItemsSelect[4]->getSelectedIndex() ) newRadioLinkParams.uDownlinkDataDataRateType[iRadioLink] = FLAG_RADIO_LINK_DATARATE_DATA_TYPE_FIXED; - if ( 2 == m_pItemsSelect[04]->getSelectedIndex() ) + if ( 2 == m_pItemsSelect[4]->getSelectedIndex() ) newRadioLinkParams.uDownlinkDataDataRateType[iRadioLink] = FLAG_RADIO_LINK_DATARATE_DATA_TYPE_LOWEST; - if ( 0 == m_pItemsSelect[06]->getSelectedIndex() ) + if ( 0 == m_pItemsSelect[6]->getSelectedIndex() ) newRadioLinkParams.uUplinkDataDataRateType[iRadioLink] = FLAG_RADIO_LINK_DATARATE_DATA_TYPE_SAME_AS_ADAPTIVE_VIDEO; - if ( 1 == m_pItemsSelect[06]->getSelectedIndex() ) + if ( 1 == m_pItemsSelect[6]->getSelectedIndex() ) newRadioLinkParams.uUplinkDataDataRateType[iRadioLink] = FLAG_RADIO_LINK_DATARATE_DATA_TYPE_FIXED; - if ( 2 == m_pItemsSelect[06]->getSelectedIndex() ) + if ( 2 == m_pItemsSelect[6]->getSelectedIndex() ) newRadioLinkParams.uUplinkDataDataRateType[iRadioLink] = FLAG_RADIO_LINK_DATARATE_DATA_TYPE_LOWEST; - if ( m_pItemsSelect[05]->isEnabled() ) + if ( 1 == m_pItemsSelect[4]->getSelectedIndex() ) + if ( m_pItemsSelect[5]->isEnabled() ) { - indexRate = m_pItemsSelect[05]->getSelectedIndex(); - if ( indexRate < getDataRatesCount() ) - newRadioLinkParams.link_datarate_data_bps[iRadioLink] = getDataRatesBPS()[indexRate]; + indexRate = m_pItemsSelect[5]->getSelectedIndex(); + if ( bIsMCSRates ) + newRadioLinkParams.link_datarate_data_bps[iRadioLink] = -1-indexRate; else - newRadioLinkParams.link_datarate_data_bps[iRadioLink] = -1-(indexRate - getDataRatesCount()); + newRadioLinkParams.link_datarate_data_bps[iRadioLink] = getDataRatesBPS()[indexRate]; } - if ( m_pItemsSelect[07]->isEnabled() ) + if ( 1 == m_pItemsSelect[6]->getSelectedIndex() ) + if ( m_pItemsSelect[7]->isEnabled() ) { - indexRate = m_pItemsSelect[07]->getSelectedIndex(); - if ( indexRate < getDataRatesCount() ) - newRadioLinkParams.uplink_datarate_data_bps[iRadioLink] = getDataRatesBPS()[indexRate]; + indexRate = m_pItemsSelect[7]->getSelectedIndex(); + if ( bIsMCSRates ) + newRadioLinkParams.uplink_datarate_data_bps[iRadioLink] = -1-indexRate; else - newRadioLinkParams.uplink_datarate_data_bps[iRadioLink] = -1-(indexRate - getDataRatesCount()); + newRadioLinkParams.uplink_datarate_data_bps[iRadioLink] = getDataRatesBPS()[indexRate]; } u32 radioFlags = g_pCurrentModel->radioLinksParams.link_radio_flags[iRadioLink]; @@ -632,34 +804,39 @@ void MenuVehicleRadioLink::sendRadioLinkConfig(int iRadioLink) // Clear all MCS flags radioFlags &= 0xFF00FFFF; - if ( 1 == m_pItemsSelect[11]->getSelectedIndex() ) - radioFlags |= RADIO_FLAG_HT40_VEHICLE; - else if ( 2 == m_pItemsSelect[11]->getSelectedIndex() ) - radioFlags |= RADIO_FLAG_HT40_CONTROLLER; - else if ( 3 == m_pItemsSelect[11]->getSelectedIndex() ) - radioFlags |= RADIO_FLAG_HT40_VEHICLE | RADIO_FLAG_HT40_CONTROLLER; - - if ( 1 == m_pItemsSelect[12]->getSelectedIndex() ) - radioFlags |= RADIO_FLAG_LDPC_VEHICLE; - else if ( 2 == m_pItemsSelect[12]->getSelectedIndex() ) - radioFlags |= RADIO_FLAG_LDPC_CONTROLLER; - else if ( 3 == m_pItemsSelect[12]->getSelectedIndex() ) - radioFlags |= RADIO_FLAG_LDPC_VEHICLE | RADIO_FLAG_LDPC_CONTROLLER; - - if ( 1 == m_pItemsSelect[13]->getSelectedIndex() ) - radioFlags |= RADIO_FLAG_SGI_VEHICLE; - else if ( 2 == m_pItemsSelect[13]->getSelectedIndex() ) - radioFlags |= RADIO_FLAG_SGI_CONTROLLER; - else if ( 3 == m_pItemsSelect[13]->getSelectedIndex() ) - radioFlags |= RADIO_FLAG_SGI_VEHICLE | RADIO_FLAG_SGI_CONTROLLER; - - if ( 1 == m_pItemsSelect[14]->getSelectedIndex() ) - radioFlags |= RADIO_FLAG_STBC_VEHICLE; - else if ( 2 == m_pItemsSelect[14]->getSelectedIndex() ) - radioFlags |= RADIO_FLAG_STBC_CONTROLLER; - else if ( 3 == m_pItemsSelect[14]->getSelectedIndex() ) - radioFlags |= RADIO_FLAG_STBC_VEHICLE | RADIO_FLAG_STBC_CONTROLLER; - + if ( bIsMCSRates ) + { + if ( 1 == m_pItemsSelect[11]->getSelectedIndex() ) + radioFlags |= RADIO_FLAG_HT40_VEHICLE; + else if ( 2 == m_pItemsSelect[11]->getSelectedIndex() ) + radioFlags |= RADIO_FLAG_HT40_CONTROLLER; + else if ( 3 == m_pItemsSelect[11]->getSelectedIndex() ) + radioFlags |= RADIO_FLAG_HT40_VEHICLE | RADIO_FLAG_HT40_CONTROLLER; + + if ( 1 == m_pItemsSelect[12]->getSelectedIndex() ) + radioFlags |= RADIO_FLAG_LDPC_VEHICLE; + else if ( 2 == m_pItemsSelect[12]->getSelectedIndex() ) + radioFlags |= RADIO_FLAG_LDPC_CONTROLLER; + else if ( 3 == m_pItemsSelect[12]->getSelectedIndex() ) + radioFlags |= RADIO_FLAG_LDPC_VEHICLE | RADIO_FLAG_LDPC_CONTROLLER; + + if ( 1 == m_pItemsSelect[13]->getSelectedIndex() ) + radioFlags |= RADIO_FLAG_SGI_VEHICLE; + else if ( 2 == m_pItemsSelect[13]->getSelectedIndex() ) + radioFlags |= RADIO_FLAG_SGI_CONTROLLER; + else if ( 3 == m_pItemsSelect[13]->getSelectedIndex() ) + radioFlags |= RADIO_FLAG_SGI_VEHICLE | RADIO_FLAG_SGI_CONTROLLER; + } + + if ( NULL != m_pItemsSelect[14] ) + { + if ( 1 == m_pItemsSelect[14]->getSelectedIndex() ) + radioFlags |= RADIO_FLAG_STBC_VEHICLE; + else if ( 2 == m_pItemsSelect[14]->getSelectedIndex() ) + radioFlags |= RADIO_FLAG_STBC_CONTROLLER; + else if ( 3 == m_pItemsSelect[14]->getSelectedIndex() ) + radioFlags |= RADIO_FLAG_STBC_VEHICLE | RADIO_FLAG_STBC_CONTROLLER; + } newRadioLinkParams.link_radio_flags[iRadioLink] = radioFlags; if ( 0 == memcmp((u8*)&newRadioLinkParams, (u8*)&(g_pCurrentModel->radioLinksParams), sizeof(type_radio_links_parameters)) ) @@ -748,6 +925,16 @@ void MenuVehicleRadioLink::sendNewRadioLinkFrequency(int iVehicleLinkIndex, u32 warnings_add_configuring_radio_link(iVehicleLinkIndex, "Changing Frequency"); } +void MenuVehicleRadioLink::onReturnFromChild(int iChildMenuId, int returnValue) +{ + Menu::onReturnFromChild(iChildMenuId, returnValue); +} + +int MenuVehicleRadioLink::onBack() +{ + return Menu::onBack(); +} + void MenuVehicleRadioLink::onSelectItem() { Menu::onSelectItem(); @@ -760,7 +947,7 @@ void MenuVehicleRadioLink::onSelectItem() if ( link_is_reconfiguring_radiolink() ) { handle_commands_show_popup_progress(); - valuesToUI(); + addMenuItems(); return; } @@ -769,17 +956,14 @@ void MenuVehicleRadioLink::onSelectItem() char szBuff[256]; - if ( -1 == m_IndexStart ) - return; - int iRadioInterfaceId = g_pCurrentModel->getRadioInterfaceIndexForRadioLink(m_iRadioLink); log_line("MenuVehicleRadioLink: Current radio interface assigned to radio link %d: %d", m_iRadioLink+1, iRadioInterfaceId+1); if ( -1 == iRadioInterfaceId ) return; - if ( m_IndexFrequency == m_SelectedIndex ) + if ( (-1 != m_IndexFrequency) && (m_IndexFrequency == m_SelectedIndex) ) { - int index = m_pItemsSelect[00]->getSelectedIndex(); + int index = m_pItemsSelect[0]->getSelectedIndex(); u32 freq = m_SupportedChannels[index]; int band = getBand(freq); if ( freq == g_pCurrentModel->radioLinksParams.link_frequency_khz[m_iRadioLink] ) @@ -804,7 +988,7 @@ void MenuVehicleRadioLink::onSelectItem() { log_line(szError); add_menu_to_stack(new MenuConfirmation("Invalid option",szError, 0, true)); - valuesToUI(); + addMenuItems(); return; } @@ -824,7 +1008,7 @@ void MenuVehicleRadioLink::onSelectItem() { sprintf(szBuff, "%s frequency is not supported by your controller.", str_format_frequency(freq)); add_menu_to_stack(new MenuConfirmation("Invalid option",szBuff, 0, true)); - valuesToUI(); + addMenuItems(); return; } if ( (! allSupported) && (1 == g_pCurrentModel->radioLinksParams.links_count) ) @@ -832,21 +1016,23 @@ void MenuVehicleRadioLink::onSelectItem() char szBuff[256]; sprintf(szBuff, "Not all radio interfaces on your controller support %s frequency. Some radio interfaces on the controller will not be used to communicate with this vehicle.", str_format_frequency(freq)); add_menu_to_stack(new MenuConfirmation("Confirmation",szBuff, 0, true)); + addMenuItems(); } if ( (get_sw_version_major(g_pCurrentModel) < 9) || ((get_sw_version_major(g_pCurrentModel) == 9) && (get_sw_version_minor(g_pCurrentModel) <= 20)) ) { addMessageWithTitle(0, "Can't update radio links", "You need to update your vehicle to version 9.2 or newer"); + addMenuItems(); return; } sendNewRadioLinkFrequency(m_iRadioLink, freq); return; } - if ( m_IndexUsage == m_SelectedIndex ) + if ( (-1 != m_IndexUsage) && (m_IndexUsage == m_SelectedIndex) ) { - int usage = m_pItemsSelect[01]->getSelectedIndex(); + int usage = m_pItemsSelect[1]->getSelectedIndex(); u32 nicFreq[MAX_RADIO_INTERFACES]; for( int i=0; igetSelectedIndex() ) + if ( g_pCurrentModel->radioLinksParams.link_datarate_video_bps[m_iRadioLink] > 0 ) + { + m_bSwitchedDataRatesType = false; + m_bMustSelectDatarate = false; + addMenuItems(); + return; + } + if ( 1 == m_pItemsSelect[8]->getSelectedIndex() ) + if ( g_pCurrentModel->radioLinksParams.link_datarate_video_bps[m_iRadioLink] < 0 ) + { + m_bSwitchedDataRatesType = false; + m_bMustSelectDatarate = false; + addMenuItems(); + return; + } + + m_bSwitchedDataRatesType = true; + m_bMustSelectDatarate = true; + addMenuItems(); + return; + } + + if ( (-1 != m_IndexDataRateVideo) && (m_IndexDataRateVideo == m_SelectedIndex) ) { + if ( m_bMustSelectDatarate ) + if ( 0 == m_pItemsSelect[3]->getSelectedIndex() ) + { + addMessage("Please select a radio datarate."); + return; + } + sendRadioLinkConfig(m_iRadioLink); + return; + } + if ( ((-1 != m_IndexDataRateDataDownlink) && (m_IndexDataRateDataDownlink == m_SelectedIndex)) || + ((-1 != m_IndexDataRateDataUplink) && (m_IndexDataRateDataUplink == m_SelectedIndex)) || + ((-1 != m_IndexDataRateTypeDownlink) && (m_IndexDataRateTypeDownlink == m_SelectedIndex)) || + ((-1 != m_IndexDataRateTypeUplink) && (m_IndexDataRateTypeUplink == m_SelectedIndex)) ) + { + if ( m_bMustSelectDatarate ) + if ( 0 == m_pItemsSelect[3]->getSelectedIndex() ) + { + addMessage("Please select a radio datarate first."); + return; + } sendRadioLinkConfig(m_iRadioLink); return; } - if ( m_IndexSGI == m_SelectedIndex || m_IndexLDPC == m_SelectedIndex || m_IndexSTBC == m_SelectedIndex || m_IndexHT == m_SelectedIndex) + if ( ((-1 != m_IndexSGI) && (m_IndexSGI == m_SelectedIndex)) || + ((-1 != m_IndexLDPC) && (m_IndexLDPC == m_SelectedIndex)) || + ((-1 != m_IndexSTBC) && (m_IndexSTBC == m_SelectedIndex)) || + ((-1 != m_IndexHT) && (m_IndexHT == m_SelectedIndex)) ) { + if ( m_bMustSelectDatarate ) + if ( 0 == m_pItemsSelect[3]->getSelectedIndex() ) + { + addMessage("Please select a radio datarate first."); + return; + } sendRadioLinkConfig(m_iRadioLink); return; } - if ( m_IndexReset == m_SelectedIndex ) + if ( (-1 != m_IndexReset) && (m_IndexReset == m_SelectedIndex) ) { if ( ! handle_commands_send_to_vehicle(COMMAND_ID_RESET_RADIO_LINK, m_iRadioLink, NULL, 0) ) - valuesToUI(); + addMenuItems(); } -} \ No newline at end of file +} + +void MenuVehicleRadioLink::onChangeRadioConfigFinished(bool bSucceeded) +{ + if ( bSucceeded ) + { + m_bSwitchedDataRatesType = false; + m_bMustSelectDatarate = false; + } + addMenuItems(); +} diff --git a/code/r_central/menu/menu_vehicle_radio_link.h b/code/r_central/menu/menu_vehicle_radio_link.h index e9616daf..1981c58b 100644 --- a/code/r_central/menu/menu_vehicle_radio_link.h +++ b/code/r_central/menu/menu_vehicle_radio_link.h @@ -13,10 +13,17 @@ class MenuVehicleRadioLink: public Menu virtual void valuesToUI(); virtual void Render(); virtual void onShow(); + virtual int onBack(); virtual void onReturnFromChild(int iChildMenuId, int returnValue); virtual void onSelectItem(); - + void onChangeRadioConfigFinished(bool bSucceeded); + private: + void addMenuItems(); + void addMenuItemFrequencies(); + void addMenuItemsCapabilities(); + void addMenuItemsDataRates(); + void addMenuItemsMCS(); void sendRadioLinkCapabilities(int iRadioLink); void sendRadioLinkConfig(int linkIndex); void sendNewRadioLinkFrequency(int iVehicleLinkIndex, u32 uNewFreqKhz); @@ -26,13 +33,13 @@ class MenuVehicleRadioLink: public Menu MenuItemSlider* m_pItemsSlider[20]; MenuItemSelect* m_pItemsSelect[20]; + u32 m_uControllerSupportedBands; u32 m_SupportedChannels[MAX_MENU_CHANNELS]; int m_SupportedChannelsCount; - - int m_IndexStart; int m_IndexFrequency; int m_IndexUsage; int m_IndexCapabilities; + int m_IndexDataRatesType; int m_IndexDataRateVideo; int m_IndexDataRateTypeDownlink; int m_IndexDataRateTypeUplink; @@ -43,4 +50,7 @@ class MenuVehicleRadioLink: public Menu int m_IndexSGI; int m_IndexSTBC; int m_IndexReset; + + bool m_bMustSelectDatarate; + bool m_bSwitchedDataRatesType; }; \ No newline at end of file diff --git a/code/r_central/menu/menu_vehicle_rc.cpp b/code/r_central/menu/menu_vehicle_rc.cpp index 2fcdeadb..d807f211 100644 --- a/code/r_central/menu/menu_vehicle_rc.cpp +++ b/code/r_central/menu/menu_vehicle_rc.cpp @@ -1013,11 +1013,16 @@ void MenuVehicleRC::onSelectItem() if ( g_VehiclesRuntimeInfo[g_iCurrentActiveVehicleRuntimeInfoIndex].headerFCTelemetry.flags & FC_TELE_FLAGS_ARMED ) { m_bPendingEnable = enable; + char szVehicleType[256]; + if ( NULL != g_VehiclesRuntimeInfo[g_iCurrentActiveVehicleRuntimeInfoIndex].pModel ) + strcpy(szVehicleType, g_VehiclesRuntimeInfo[g_iCurrentActiveVehicleRuntimeInfoIndex].pModel->getVehicleTypeString()); + else + strcpy(szVehicleType, "vehicle"); char szBuff[256]; if ( ! enable ) - sprintf(szBuff, "Your vehicle is armed. Are you sure you want to disable the RC link? You will lose RC controll of the vehicle."); + snprintf(szBuff, sizeof(szBuff)/sizeof(szBuff[0]), "Your %s is armed. Are you sure you want to disable the RC link? You will lose RC control of your %s.", szVehicleType, szVehicleType); else - sprintf(szBuff, "Your vehicle is armed. Are you sure you want to enable the RC link? It can have unintended consequences depending on how the flight controller is setup."); + snprintf(szBuff, sizeof(szBuff)/sizeof(szBuff[0]), "Your %s is armed. Are you sure you want to enable the RC link? It can have unintended consequences depending on how the flight controller is setup.", szVehicleType); MenuConfirmation* pMC = new MenuConfirmation("WARNING!",szBuff,2); add_menu_to_stack(pMC); return; diff --git a/code/r_central/menu/menu_vehicle_rc_channels.h b/code/r_central/menu/menu_vehicle_rc_channels.h index 6ab024d8..5f71628e 100644 --- a/code/r_central/menu/menu_vehicle_rc_channels.h +++ b/code/r_central/menu/menu_vehicle_rc_channels.h @@ -29,7 +29,7 @@ typedef struct int m_IndexClear; int m_IndexAssign; -} __attribute__((packed)) t_menu_group_rc_channel; +} ALIGN_STRUCT_SPEC_INFO t_menu_group_rc_channel; class MenuVehicleRCChannels: public Menu diff --git a/code/r_central/menu/menu_vehicle_rc_expo.h b/code/r_central/menu/menu_vehicle_rc_expo.h index fbcdc2d6..72476abb 100644 --- a/code/r_central/menu/menu_vehicle_rc_expo.h +++ b/code/r_central/menu/menu_vehicle_rc_expo.h @@ -15,7 +15,7 @@ typedef struct int m_IndexTitle; int m_IndexValue; -} __attribute__((packed)) t_menu_group_rc_expo; +} ALIGN_STRUCT_SPEC_INFO t_menu_group_rc_expo; class MenuVehicleRCExpo: public Menu diff --git a/code/r_central/menu/menu_vehicle_rc_failsafe.h b/code/r_central/menu/menu_vehicle_rc_failsafe.h index 3832314c..38c37f14 100644 --- a/code/r_central/menu/menu_vehicle_rc_failsafe.h +++ b/code/r_central/menu/menu_vehicle_rc_failsafe.h @@ -15,7 +15,7 @@ typedef struct int m_IndexTitle; int m_IndexType; int m_IndexValue; -} __attribute__((packed)) t_menu_group_rc_failsafe; +} ALIGN_STRUCT_SPEC_INFO t_menu_group_rc_failsafe; class MenuVehicleRCFailsafe: public Menu diff --git a/code/r_central/menu/menu_vehicle_video.cpp b/code/r_central/menu/menu_vehicle_video.cpp index 05fb6d7d..2d0327f0 100644 --- a/code/r_central/menu/menu_vehicle_video.cpp +++ b/code/r_central/menu/menu_vehicle_video.cpp @@ -42,6 +42,7 @@ #include "menu_item_section.h" #include "../osd/osd_common.h" +#include "../process_router_messages.h" MenuVehicleVideo::MenuVehicleVideo(void) :Menu(MENU_ID_VEHICLE_VIDEO, "Video Settings", NULL) @@ -440,6 +441,8 @@ void MenuVehicleVideo::sendVideoLinkProfiles() log_line("Sending new video link profiles to vehicle."); if ( ! handle_commands_send_to_vehicle(COMMAND_ID_UPDATE_VIDEO_LINK_PROFILES, 0, buffer, MAX_VIDEO_LINK_PROFILES*sizeof(type_video_link_profile)) ) valuesToUI(); + else + send_control_message_to_router(PACEKT_TYPE_LOCAL_CONTROLLER_ADAPTIVE_VIDEO_PAUSE, 10000); } @@ -598,7 +601,9 @@ void MenuVehicleVideo::onSelectItem() #endif if ( ! g_pCurrentModel->isRunningOnOpenIPCHardware() ) { - addMessage("Your vehicle raspberry Pi hardware supports only H264 video encoder/decoder."); + char szTextW[256]; + sprintf(szTextW, "Your %s's Raspberry Pi hardware supports only H264 video encoder/decoder.", g_pCurrentModel->getVehicleTypeString()); + addMessage(szTextW); valuesToUI(); return; } @@ -612,5 +617,7 @@ void MenuVehicleVideo::onSelectItem() if ( g_pCurrentModel->video_params.uVideoExtraFlags != paramsNew.uVideoExtraFlags ) if ( ! handle_commands_send_to_vehicle(COMMAND_ID_SET_VIDEO_PARAMS, 0, (u8*)¶msNew, sizeof(video_parameters_t)) ) valuesToUI(); + else + send_control_message_to_router(PACEKT_TYPE_LOCAL_CONTROLLER_ADAPTIVE_VIDEO_PAUSE, 10000); } } diff --git a/code/r_central/menu/menu_vehicle_video_adaptive.cpp b/code/r_central/menu/menu_vehicle_video_adaptive.cpp deleted file mode 100644 index 9e3c31ef..00000000 --- a/code/r_central/menu/menu_vehicle_video_adaptive.cpp +++ /dev/null @@ -1,215 +0,0 @@ -/* - Ruby Licence - Copyright (c) 2024 Petru Soroaga petrusoroaga@yahoo.com - All rights reserved. - - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Copyright info and developer info must be preserved as is in the user - interface, additions could be made to that info. - * Neither the name of the organization nor the - names of its contributors may be used to endorse or promote products - derived from this software without specific prior written permission. - * Military use is not permited. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - DISCLAIMED. IN NO EVENT SHALL Julien Verneuil BE LIABLE FOR ANY - DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*/ - -#include "../../base/video_capture_res.h" -#include "../../base/utils.h" -#include "../../base/controller_utils.h" -#include "menu.h" -#include "menu_vehicle_video_adaptive.h" -#include "menu_vehicle_video_profile.h" -#include "menu_vehicle_video_encodings.h" -#include "menu_item_select.h" -#include "menu_item_slider.h" -#include "menu_item_section.h" - -#include "../osd/osd_common.h" - -MenuVehicleVideoAdaptive::MenuVehicleVideoAdaptive(void) -:Menu(MENU_ID_VEHICLE_VIDEO_ADAPTIVE, "Adaptive Video Settings", NULL) -{ - m_Width = 0.36; - m_xPos = menu_get_XStartPos(m_Width); m_yPos = 0.1; - - float dxMargin = 0;//0.03 * Menu::getScaleFactor(); - m_pItemsSelect[3] = new MenuItemSelect("Auto Keyframing", "Automatic keyframe adjustment based on radio link conditions."); - m_pItemsSelect[3]->addSelection("Off"); - m_pItemsSelect[3]->addSelection("On"); - m_pItemsSelect[3]->setIsEditable(); - m_pItemsSelect[3]->setMargin(dxMargin); - m_IndexAutoKeyframe = addMenuItem(m_pItemsSelect[3]); - - m_pItemsSelect[6] = new MenuItemSelect("Auto Video Quality", "Reduce the video quality when radio link quality goes down."); - m_pItemsSelect[6]->addSelection("Off"); - m_pItemsSelect[6]->addSelection("On"); - m_pItemsSelect[6]->setIsEditable(); - m_pItemsSelect[6]->setMargin(dxMargin); - m_IndexAdaptiveVideo = addMenuItem(m_pItemsSelect[6]); - - m_pItemsSelect[8] = new MenuItemSelect("Auto Quantization", "Adjust the camera H264/H265 quantization to try to maintain a constant video bitrate."); - m_pItemsSelect[8]->addSelection("Off"); - m_pItemsSelect[8]->addSelection("On"); - m_pItemsSelect[8]->setIsEditable(); - m_pItemsSelect[8]->setMargin(dxMargin); - m_IndexAutoQuantization = addMenuItem(m_pItemsSelect[8]); - - m_pItemsSelect[7] = new MenuItemSelect("Retransmissions", "Enable request and retransmissions of video data."); - m_pItemsSelect[7]->addSelection("Off"); - m_pItemsSelect[7]->addSelection("On"); - m_pItemsSelect[7]->setIsEditable(); - m_pItemsSelect[7]->setMargin(dxMargin); - m_pItemsSelect[7]->setExtraHeight(1.5* g_pRenderEngine->textHeight(g_idFontMenu) * MENU_ITEM_SPACING); - m_IndexRetransmissions = addMenuItem(m_pItemsSelect[7]); -} - -MenuVehicleVideoAdaptive::~MenuVehicleVideoAdaptive() -{ -} - -void MenuVehicleVideoAdaptive::valuesToUI() -{ - int keyframe_ms = g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].keyframe_ms; - - if ( keyframe_ms > 0 ) - m_pItemsSelect[3]->setSelectedIndex(0); - else - m_pItemsSelect[3]->setSelectedIndex(1); - - if ( g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].uEncodingFlags & VIDEO_ENCODINGS_FLAGS_ENABLE_RETRANSMISSIONS ) - m_pItemsSelect[7]->setSelectedIndex(1); - else - m_pItemsSelect[7]->setSelectedIndex(0); - - if ( g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].uEncodingFlags & VIDEO_ENCODINGS_FLAGS_ENABLE_ADAPTIVE_VIDEO_LINK_PARAMS ) - m_pItemsSelect[6]->setSelectedIndex(1); - else - m_pItemsSelect[6]->setSelectedIndex(0); - - if ( g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].uEncodingFlags & VIDEO_ENCODINGS_FLAGS_ENABLE_VIDEO_ADAPTIVE_QUANTIZATION ) - m_pItemsSelect[8]->setSelectedIndex(1); - else - m_pItemsSelect[8]->setSelectedIndex(0); -} - -void MenuVehicleVideoAdaptive::Render() -{ - RenderPrepare(); - - float yTop = RenderFrameAndTitle(); - float y = yTop; - - for( int i=0; ivideo_link_profiles[i]), sizeof(type_video_link_profile)); - - type_video_link_profile* pProfile = &(profiles[g_pCurrentModel->video_params.user_selected_video_link_profile]); - int keyframe_ms = g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].keyframe_ms; - - if ( 0 == m_pItemsSelect[3]->getSelectedIndex() ) - pProfile->keyframe_ms = (keyframe_ms>0)?keyframe_ms:(-keyframe_ms); - else - pProfile->keyframe_ms = (keyframe_ms>0)?-keyframe_ms:(keyframe_ms); - - if ( m_pItemsSelect[8]->getSelectedIndex() == 0 ) - pProfile->uEncodingFlags = pProfile->uEncodingFlags & (~VIDEO_ENCODINGS_FLAGS_ENABLE_VIDEO_ADAPTIVE_QUANTIZATION ); - else - pProfile->uEncodingFlags = pProfile->uEncodingFlags | VIDEO_ENCODINGS_FLAGS_ENABLE_VIDEO_ADAPTIVE_QUANTIZATION; - - if ( m_pItemsSelect[7]->getSelectedIndex() == 0 ) - pProfile->uEncodingFlags = pProfile->uEncodingFlags & (~VIDEO_ENCODINGS_FLAGS_ENABLE_RETRANSMISSIONS ); - else - pProfile->uEncodingFlags = pProfile->uEncodingFlags | VIDEO_ENCODINGS_FLAGS_ENABLE_RETRANSMISSIONS; - - - if ( pProfile->uEncodingFlags == g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].uEncodingFlags ) - if ( pProfile->keyframe_ms == g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].keyframe_ms ) - return; - - // Propagate changes to lower video profiles - - propagate_video_profile_changes( &g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile], pProfile, &(profiles[0])); - - log_line("Sending video encoding flags: %s", str_format_video_encoding_flags(pProfile->uEncodingFlags)); - - u8 buffer[1024]; - memcpy( buffer, &(profiles[0]), MAX_VIDEO_LINK_PROFILES * sizeof(type_video_link_profile) ); - - log_line("Sending new video link profiles to vehicle."); - if ( ! handle_commands_send_to_vehicle(COMMAND_ID_UPDATE_VIDEO_LINK_PROFILES, 0, buffer, MAX_VIDEO_LINK_PROFILES*sizeof(type_video_link_profile)) ) - valuesToUI(); -} - - -void MenuVehicleVideoAdaptive::onReturnFromChild(int iChildMenuId, int returnValue) -{ - Menu::onReturnFromChild(iChildMenuId, returnValue); -} - -void MenuVehicleVideoAdaptive::onSelectItem() -{ - Menu::onSelectItem(); - - if ( m_pMenuItems[m_SelectedIndex]->isEditing() ) - return; - - if ( handle_commands_is_command_in_progress() ) - { - handle_commands_show_popup_progress(); - return; - } - - g_TimeLastVideoCameraChangeCommand = g_TimeNow; - - - if ( (m_IndexAutoKeyframe == m_SelectedIndex) || (m_IndexAdaptiveVideo == m_SelectedIndex) ) - if ( hardware_board_is_goke(g_pCurrentModel->hwCapabilities.uBoardType) ) - { - addUnsupportedMessageOpenIPCGoke(NULL); - valuesToUI(); - return; - } - - if ( m_IndexAutoQuantization == m_SelectedIndex) - if ( hardware_board_is_openipc(g_pCurrentModel->hwCapabilities.uBoardType) ) - { - addUnsupportedMessageOpenIPC(NULL); - valuesToUI(); - return; - } - - if ( m_IndexAutoKeyframe == m_SelectedIndex || m_IndexAdaptiveVideo == m_SelectedIndex) - { - sendVideoLinkProfiles(); - return; - } - - if ( m_IndexRetransmissions == m_SelectedIndex || m_IndexAutoQuantization == m_SelectedIndex ) - { - sendVideoLinkProfiles(); - return; - } -} diff --git a/code/r_central/menu/menu_vehicle_video_adaptive.h b/code/r_central/menu/menu_vehicle_video_adaptive.h deleted file mode 100644 index 5b0fdb25..00000000 --- a/code/r_central/menu/menu_vehicle_video_adaptive.h +++ /dev/null @@ -1,30 +0,0 @@ -#pragma once -#include "menu_objects.h" -#include "menu_item_select.h" -#include "menu_item_slider.h" -#include "menu_item_radio.h" -#include "menu_item_checkbox.h" -#include "menu_item_text.h" -#include "../../base/video_capture_res.h" - -class MenuVehicleVideoAdaptive: public Menu -{ - public: - MenuVehicleVideoAdaptive(); - virtual ~MenuVehicleVideoAdaptive(); - virtual void Render(); - virtual void onReturnFromChild(int iChildMenuId, int returnValue); - virtual void onSelectItem(); - virtual void valuesToUI(); - - private: - int m_IndexAutoKeyframe; - int m_IndexAdaptiveVideo; - int m_IndexAutoQuantization; - int m_IndexRetransmissions; - MenuItemSlider* m_pItemsSlider[15]; - MenuItemSelect* m_pItemsSelect[15]; - MenuItemRadio* m_pItemsRadio[5]; - - void sendVideoLinkProfiles(); -}; \ No newline at end of file diff --git a/code/r_central/menu/menu_vehicle_video_bidir.cpp b/code/r_central/menu/menu_vehicle_video_bidir.cpp index 2a5c7782..078efa38 100644 --- a/code/r_central/menu/menu_vehicle_video_bidir.cpp +++ b/code/r_central/menu/menu_vehicle_video_bidir.cpp @@ -41,6 +41,7 @@ #include "menu_item_section.h" #include "../osd/osd_common.h" +#include "../process_router_messages.h" MenuVehicleVideoBidirectional::MenuVehicleVideoBidirectional(void) :Menu(MENU_ID_VEHICLE_VIDEO_BIDIRECTIONAL, "Bidirectional Video Settings", NULL) @@ -263,6 +264,8 @@ void MenuVehicleVideoBidirectional::sendVideoLinkProfiles() log_line("Sending new video link profiles to vehicle."); if ( ! handle_commands_send_to_vehicle(COMMAND_ID_UPDATE_VIDEO_LINK_PROFILES, 0, buffer, MAX_VIDEO_LINK_PROFILES*sizeof(type_video_link_profile)) ) valuesToUI(); + else + send_control_message_to_router(PACEKT_TYPE_LOCAL_CONTROLLER_ADAPTIVE_VIDEO_PAUSE, 10000); } diff --git a/code/r_central/notifications.cpp b/code/r_central/notifications.cpp index f7ec517b..fa901891 100644 --- a/code/r_central/notifications.cpp +++ b/code/r_central/notifications.cpp @@ -51,6 +51,13 @@ void notification_add_armed(u32 uVehicleId) if ( g_VehiclesRuntimeInfo[i].uVehicleId == uVehicleId ) iRuntimeInfoIndex = i; + if ( (-1 == iRuntimeInfoIndex) || (g_VehiclesRuntimeInfo[iRuntimeInfoIndex].pModel == NULL) ) + return; + + int layoutIndex = g_VehiclesRuntimeInfo[iRuntimeInfoIndex].pModel->osd_params.layout; + if ( ! (g_VehiclesRuntimeInfo[iRuntimeInfoIndex].pModel->osd_params.osd_flags[layoutIndex] & OSD_FLAG_SHOW_FLIGHT_MODE_CHANGE) ) + return; + char szBuff[64]; strcpy(szBuff, "ARMED"); if ( (iRuntimeInfoIndex != -1) && (NULL != g_pCurrentModel) && (g_pCurrentModel->relay_params.isRelayEnabledOnRadioLinkId >= 0) ) @@ -69,6 +76,13 @@ void notification_add_disarmed(u32 uVehicleId) if ( g_VehiclesRuntimeInfo[i].uVehicleId == uVehicleId ) iRuntimeInfoIndex = i; + if ( (-1 == iRuntimeInfoIndex) || (g_VehiclesRuntimeInfo[iRuntimeInfoIndex].pModel == NULL) ) + return; + + int layoutIndex = g_VehiclesRuntimeInfo[iRuntimeInfoIndex].pModel->osd_params.layout; + if ( ! (g_VehiclesRuntimeInfo[iRuntimeInfoIndex].pModel->osd_params.osd_flags[layoutIndex] & OSD_FLAG_SHOW_FLIGHT_MODE_CHANGE) ) + return; + char szBuff[64]; strcpy(szBuff, "DISARMED"); if ( (iRuntimeInfoIndex != -1) && (NULL != g_pCurrentModel) && (g_pCurrentModel->relay_params.isRelayEnabledOnRadioLinkId >= 0) ) @@ -89,6 +103,13 @@ void notification_add_flight_mode(u32 uVehicleId, u32 flightMode) if ( g_VehiclesRuntimeInfo[i].uVehicleId == uVehicleId ) iRuntimeInfoIndex = i; + if ( (-1 == iRuntimeInfoIndex) || (g_VehiclesRuntimeInfo[iRuntimeInfoIndex].pModel == NULL) ) + return; + + int layoutIndex = g_VehiclesRuntimeInfo[iRuntimeInfoIndex].pModel->osd_params.layout; + if ( ! (g_VehiclesRuntimeInfo[iRuntimeInfoIndex].pModel->osd_params.osd_flags[layoutIndex] & OSD_FLAG_SHOW_FLIGHT_MODE_CHANGE) ) + return; + sprintf(szBuff, "Flight mode changed to: %s", model_getShortFlightMode(flightMode)); if ( (iRuntimeInfoIndex != -1) && (NULL != g_pCurrentModel) && (g_pCurrentModel->relay_params.isRelayEnabledOnRadioLinkId >= 0) ) if ( NULL != g_VehiclesRuntimeInfo[iRuntimeInfoIndex].pModel ) diff --git a/code/r_central/osd/osd.cpp b/code/r_central/osd/osd.cpp index ae98f9e3..868b4398 100644 --- a/code/r_central/osd/osd.cpp +++ b/code/r_central/osd/osd.cpp @@ -1198,7 +1198,9 @@ void osd_show_recording(bool bShowWhenStopped, float xPos, float yPos) else pColorRed[3] = 1.0; g_pRenderEngine->setColors(pColorRed); + bool bBB = g_pRenderEngine->drawBackgroundBoundingBoxes(false); w = 1.1*osd_show_value(xPos,yPos+dy, "REC", g_idFontOSD); + g_pRenderEngine->drawBackgroundBoundingBoxes(bBB); float m = w*0.18+0.5*w*0.1; pColorRed[3] = 1.0; @@ -1212,10 +1214,12 @@ void osd_show_recording(bool bShowWhenStopped, float xPos, float yPos) else pColorRed[3] = 1.0; g_pRenderEngine->setColors(pColorRed); + bBB = g_pRenderEngine->drawBackgroundBoundingBoxes(false); w = osd_show_value(xPos,yPos+dy, "REC", g_idFontOSD); osd_set_colors(); osd_show_value(xPos-height_text*0.1, yPos + dy + height_text*1.2, szTime, g_idFontOSD); + g_pRenderEngine->drawBackgroundBoundingBoxes(bBB); return; } @@ -1230,7 +1234,9 @@ void osd_show_recording(bool bShowWhenStopped, float xPos, float yPos) float m = w*0.18+0.5*w*0.1; xPos -= w + 2*m; + bool bBB = g_pRenderEngine->drawBackgroundBoundingBoxes(false); osd_show_value(xPos,yPos+dy, "REC", g_idFontOSDSmall); + g_pRenderEngine->drawBackgroundBoundingBoxes(bBB); if ( (g_TimeNow / 1000 ) % 2 ) g_pRenderEngine->setColors(pColorRed); @@ -1244,10 +1250,11 @@ void osd_show_recording(bool bShowWhenStopped, float xPos, float yPos) else g_pRenderEngine->setColors(pColorYellow); + bBB = g_pRenderEngine->drawBackgroundBoundingBoxes(false); osd_show_value(xPos,yPos+dy, "REC", g_idFontOSDSmall); - osd_set_colors(); osd_show_value(xPos-m*0.3,yPos+dy+height_text_small*1.1, szTime, g_idFontOSDSmall); + g_pRenderEngine->drawBackgroundBoundingBoxes(bBB); } void render_bars() @@ -1255,6 +1262,7 @@ void render_bars() if ( ! (g_pCurrentModel->osd_params.osd_flags2[osd_get_current_layout_index()] & OSD_FLAG2_SHOW_BACKGROUND_BARS) ) return; + float fGlobalAlpha = g_pRenderEngine->setGlobalAlfa(1.0); osd_set_colors_background_fill(); if ( g_pCurrentModel->osd_params.osd_flags2[osd_get_current_layout_index()] & OSD_FLAG2_LAYOUT_LEFT_RIGHT ) @@ -1262,14 +1270,16 @@ void render_bars() float fWidth = osd_getVerticalBarWidth(); g_pRenderEngine->drawRect(osd_getMarginX(), osd_getMarginY(), fWidth, 1.0-2*osd_getMarginY()); g_pRenderEngine->drawRect(1.0-osd_getMarginX()-fWidth, osd_getMarginY(), fWidth, 1.0-2*osd_getMarginY()); + g_pRenderEngine->setGlobalAlfa(fGlobalAlpha); osd_set_colors(); return; } float hBar = osd_getBarHeight(); hBar += osd_getSecondBarHeight(); + g_pRenderEngine->drawRect(osd_getMarginX(), osd_getMarginY(), 1.0-2*osd_getMarginX(),hBar); g_pRenderEngine->drawRect(osd_getMarginX(), 1.0-hBar-osd_getMarginY(), 1.0-2*osd_getMarginX(),hBar); - + g_pRenderEngine->setGlobalAlfa(fGlobalAlpha); /* { float widthRight = 0.0; @@ -2741,8 +2751,8 @@ void osd_render_all() osd_setMarginX(0.0); osd_setMarginY(0.0); - u32 tr = ((pModel->osd_params.osd_preferences[osd_get_current_layout_index()])>>8) & 0xFF; - osd_set_transparency((int)tr); + u32 uTransparency = ((pModel->osd_params.osd_preferences[osd_get_current_layout_index()]) & OSD_PREFERENCES_OSD_TRANSPARENCY_BITMASK) >> OSD_PREFERENCES_OSD_TRANSPARENCY_SHIFT; + osd_set_transparency((int)uTransparency); u32 scale = pModel->osd_params.osd_preferences[osd_get_current_layout_index()] & 0xFF; osd_setScaleOSD((int)scale); @@ -2774,12 +2784,13 @@ void osd_render_all() color2[0] = color2[1] = color2[2] = 0; color2[0] = 0; color2[3] = 1.0; - switch ( tr ) + switch ( uTransparency ) { case 0: color2[3] = 0.05; break; case 1: color2[3] = 0.26; break; case 2: color2[3] = 0.5; break; case 3: color2[3] = 0.8; break; + case 4: color2[3] = 1.0; break; } g_pRenderEngine->setFontBackgroundBoundingBoxFillColor(color2); g_pRenderEngine->setBackgroundBoundingBoxPadding(0.0); diff --git a/code/r_central/osd/osd_common.cpp b/code/r_central/osd/osd_common.cpp index 0a9abe05..4b91dc87 100644 --- a/code/r_central/osd/osd_common.cpp +++ b/code/r_central/osd/osd_common.cpp @@ -311,7 +311,8 @@ void osd_set_transparency(int value) case 0: g_pRenderEngine->setGlobalAlfa(0.85); break; case 1: g_pRenderEngine->setGlobalAlfa(0.9); break; case 2: g_pRenderEngine->setGlobalAlfa(0.95); break; - case 3: g_pRenderEngine->setGlobalAlfa(1.0); break; + case 3: + case 4: g_pRenderEngine->setGlobalAlfa(1.0); break; } } @@ -356,6 +357,7 @@ void osd_set_colors_background_fill() case 1: alfa = 0.15; break; case 2: alfa = 0.3; break; case 3: alfa = 0.7; break; + case 4: alfa = 1.0; break; } if ( p->iInvertColorsOSD ) @@ -366,15 +368,24 @@ void osd_set_colors_background_fill() case 1: alfa = 0.62; break; case 2: alfa = 0.67; break; case 3: alfa = 0.75; break; + case 4: alfa = 1.0; break; } } - g_pRenderEngine->setColors(get_Color_OSDBackground(), alfa); + double pC[4]; + memcpy(pC, get_Color_OSDBackground(), 4*sizeof(double)); + if ( s_iOSDTransparency == 4 ) + pC[3] = 1.0; + g_pRenderEngine->setColors(pC, alfa); } void osd_set_colors_background_fill(float fAlpha) { - g_pRenderEngine->setColors(get_Color_OSDBackground(), fAlpha); + double pC[4]; + memcpy(pC, get_Color_OSDBackground(), 4*sizeof(double)); + if ( s_iOSDTransparency == 4 ) + pC[3] = 1.0; + g_pRenderEngine->setColors(pC, fAlpha); } float osd_course_to(double lat1, double long1, double lat2, double long2) diff --git a/code/r_central/osd/osd_debug_stats.cpp b/code/r_central/osd/osd_debug_stats.cpp index ea1b8b46..fe92e52a 100644 --- a/code/r_central/osd/osd_debug_stats.cpp +++ b/code/r_central/osd/osd_debug_stats.cpp @@ -120,7 +120,6 @@ float _osd_render_debug_stats_graph_lines(float xPos, float yPos, float hGraph, g_pRenderEngine->drawLine(xx, yPos + hGraph*0.5, xx + 0.008, yPos+hGraph*0.5); } - float xBar = xPos; float fWidthBar = fWidth / iCountValues; float hPoint = 0; @@ -180,7 +179,7 @@ float _osd_render_debug_stats_graph_lines(float xPos, float yPos, float hGraph, return hGraph; } -float _osd_render_debug_stats_graph_bars(float xPos, float yPos, float hGraph, float fWidth, u8* pValues, u8* pValues2, int iCountValues, int iLog) +float _osd_render_debug_stats_graph_bars(float xPos, float yPos, float hGraph, float fWidth, u8* pValues, u8* pValues2, u8* pValues3, int iCountValues, int iLog) { char szBuff[32]; float height_text = g_pRenderEngine->textHeight(g_idFontStats); @@ -194,6 +193,14 @@ float _osd_render_debug_stats_graph_bars(float xPos, float yPos, float hGraph, f if ( NULL != pValues2 ) if ( (int)pValues[i] + (int)pValues2[i] > iMax ) iMax = (int)pValues[i] + (int)pValues2[i]; + if ( NULL != pValues3 ) + if ( (int)pValues[i] + (int)pValues3[i] > iMax ) + iMax = (int)pValues[i] + (int)pValues3[i]; + + if ( NULL != pValues2 ) + if ( NULL != pValues3 ) + if ( (int)pValues[i] + (int)pValues2[i] + (int)pValues3[i] > iMax ) + iMax = (int)pValues[i] + (int)pValues2[i] + (int)pValues3[i]; } float fMaxLog = logf((float)iMax); @@ -211,37 +218,83 @@ float _osd_render_debug_stats_graph_bars(float xPos, float yPos, float hGraph, f xPos += dx; fWidth -= dx; float xBar = xPos; + float fWidthPixel = g_pRenderEngine->getPixelWidth(); + float fHeightPixel = g_pRenderEngine->getPixelHeight(); float fWidthBar = fWidth / iCountValues; for( int i=0; i 0 ) - hBar = hGraph * logf((float)pValues[i]) / fMaxLog; + hBar1 = hGraph * logf((float)pValues[i]) / fMaxLog; + + if ( NULL != pValues2 ) + if ( pValues2[i] > 0 ) + hBar2 = hGraph * logf(fAllSum) / fMaxLog - hBar1; + + if ( NULL != pValues3 ) + if ( pValues3[i] > 0 ) + hBar3 = hGraph * logf(fAllSum) / fMaxLog - hBar1; + if ( NULL != pValues2 ) if ( pValues2[i] > 0 ) - // hBar2 = hGraph * logf((float)pValues2[i]) / fMaxLog; - hBar2 = hGraph * logf((float)pValues[i] + (float)pValues2[i]) / fMaxLog - hBar; + if ( NULL != pValues3 ) + if ( pValues3[i] > 0 ) + { + float hBar23 = hGraph * logf(fAllSum) / fMaxLog - hBar1; + hBar2 = hBar23 * pValues2[i]/(pValues2[i] + pValues3[i]); + hBar3 = hBar23 - hBar2; + } } + float yBar = yPos + hGraph; if ( pValues[i] > 0 ) { + yBar -= hBar1; g_pRenderEngine->setStroke(200, 200, 200, OSD_STRIKE_WIDTH); g_pRenderEngine->setFill(200, 200, 200, s_fOSDStatsGraphLinesAlpha); - g_pRenderEngine->drawRect(xBar, yPos + hGraph - hBar, fWidthBar - g_pRenderEngine->getPixelWidth(), hBar); + g_pRenderEngine->drawRect(xBar, yBar, fWidthBar - g_pRenderEngine->getPixelWidth(), hBar1); } if ( NULL != pValues2 ) if ( pValues2[i] > 0 ) { + yBar -= hBar2; g_pRenderEngine->setStroke(0, 200, 0, OSD_STRIKE_WIDTH); g_pRenderEngine->setFill(0, 200, 0, s_fOSDStatsGraphLinesAlpha); - g_pRenderEngine->drawRect(xBar, yPos + hGraph - hBar-hBar2, fWidthBar - g_pRenderEngine->getPixelWidth(), hBar2); + g_pRenderEngine->drawRect(xBar, yBar, fWidthBar - g_pRenderEngine->getPixelWidth(), hBar2); + } + + if ( NULL != pValues3 ) + if ( pValues3[i] > 0 ) + { + yBar -= hBar3; + g_pRenderEngine->setStroke(0, 0, 250, OSD_STRIKE_WIDTH); + g_pRenderEngine->setFill(0, 0, 250, s_fOSDStatsGraphLinesAlpha); + g_pRenderEngine->drawRect(xBar, yBar, fWidthBar - g_pRenderEngine->getPixelWidth(), hBar3); + } + + if ( pValues[i] == 0 ) + if ( (NULL == pValues2) || (0 == pValues2[i]) ) + if ( (NULL == pValues3) || (0 == pValues3[i]) ) + { + g_pRenderEngine->setStroke(200, 200, 200, OSD_STRIKE_WIDTH); + g_pRenderEngine->setFill(200, 200, 200, s_fOSDStatsGraphLinesAlpha); + g_pRenderEngine->drawRect(xBar + fWidthBar*0.5 - fWidthPixel, yPos + 0.5*hGraph - fHeightPixel, 2.0*fWidthPixel, 2.0*fHeightPixel); } xBar += fWidthBar; } @@ -325,6 +378,85 @@ float _osd_render_debug_stats_graph_values(float xPos, float yPos, float hGraph, return hGraph; } +float _osd_render_ack_time_hist(controller_runtime_info_vehicle* pRTInfoVehicle, float xPos, float fGraphXStart, float yPos, float hGraph, float fWidthGraph ) +{ + char szBuff[128]; + float height_text = g_pRenderEngine->textHeight(s_idFontStats); + float height_text_small = g_pRenderEngine->textHeight(s_idFontStatsSmall); + + for( int iLink=0; iLinkuAckTimes[pRTInfoVehicle->iAckTimeIndex[iLink]][iLink]; + int iMinValue = 1000; + int iAvgValueSum = 0; + int iAvgCount = 0; + for( int i=0; iuAckTimes[i][iLink] > iMaxValue ) + iMaxValue = pRTInfoVehicle->uAckTimes[i][iLink]; + if ( pRTInfoVehicle->uAckTimes[i][iLink] != 0 ) + { + iAvgValueSum += pRTInfoVehicle->uAckTimes[i][iLink]; + iAvgCount++; + if ( pRTInfoVehicle->uAckTimes[i][iLink] < iMinValue ) + iMinValue = pRTInfoVehicle->uAckTimes[i][iLink]; + } + } + + int iAvgValue = 0; + if ( iAvgCount > 0 ) + iAvgValue = iAvgValueSum / iAvgCount; + sprintf(szBuff, "RadioLink-%d Ack Time History: Min: %d ms, Max: %d ms, Avg: %d ms", + iLink+1, iMinValue, iMaxValue, iAvgValue); + g_pRenderEngine->drawText(xPos, yPos, s_idFontStats, szBuff); + yPos += height_text*1.3; + + g_pRenderEngine->setStrokeSize(OSD_STRIKE_WIDTH); + osd_set_colors(); + g_pRenderEngine->setColors(get_Color_Dev()); + g_pRenderEngine->drawText(xPos, yPos+hGraph - height_text_small*0.7, g_idFontStatsSmall, "0"); + g_pRenderEngine->drawText(xPos + fWidthGraph, yPos+hGraph - height_text_small*0.7, g_idFontStatsSmall, "0"); + sprintf(szBuff, "%d", iMaxValue); + g_pRenderEngine->drawText(xPos, yPos - height_text_small*0.3, g_idFontStatsSmall, szBuff); + g_pRenderEngine->drawText(xPos + fWidthGraph, yPos - height_text_small*0.3, g_idFontStatsSmall, szBuff); + sprintf(szBuff, "%d", iMaxValue/2); + g_pRenderEngine->drawText(xPos, yPos+hGraph*0.5 - height_text_small*0.5, g_idFontStatsSmall, szBuff); + g_pRenderEngine->drawText(xPos+fWidthGraph, yPos+hGraph*0.5 - height_text_small*0.5, g_idFontStatsSmall, szBuff); + + float dx = g_pRenderEngine->textWidth(g_idFontStats, "000"); + xPos += dx; + fWidthGraph -= dx; + float xBar = xPos; + float fWidthBar = fWidthGraph / (SYSTEM_RT_INFO_INTERVALS/4); + + g_pRenderEngine->setStroke(250,250,250,0.5); + for( float xx=xPos; xxdrawLine(xx, yPos, xx + 0.008, yPos); + g_pRenderEngine->drawLine(xx, yPos + hGraph*0.5, xx + 0.008, yPos+hGraph*0.5); + } + + for( int i=0; iiAckTimeIndex[iLink] + 1) ) + g_pRenderEngine->drawLine(xBar + 0.5*fWidthBar, yPos, xBar + 0.5*fWidthBar, yPos + hGraph); + else + { + float hBar = hGraph * (float)pRTInfoVehicle->uAckTimes[i][iLink] / (float)iMaxValue; + g_pRenderEngine->setStroke(200, 200, 200, OSD_STRIKE_WIDTH); + g_pRenderEngine->setFill(200, 200, 200, s_fOSDStatsGraphLinesAlpha); + g_pRenderEngine->drawRect(xBar, yPos + hGraph - hBar, fWidthBar - g_pRenderEngine->getPixelWidth(), hBar); + } + xBar += fWidthBar; + } + osd_set_colors(); + g_pRenderEngine->setColors(get_Color_Dev()); + yPos += hGraph; + yPos += height_text_small; + } + return yPos; +} + void osd_render_debug_stats() { Preferences* pP = get_Preferences(); @@ -362,7 +494,7 @@ void osd_render_debug_stats() float height_text = g_pRenderEngine->textHeight(s_idFontStats); float height_text_small = g_pRenderEngine->textHeight(s_idFontStatsSmall); float hGraph = height_text * 3.0; - float hGraphSmall = height_text * 1.2; + float hGraphSmall = height_text * 1.6; float xPos = 0.03; float yPos = 0.17; @@ -371,6 +503,8 @@ void osd_render_debug_stats() char szBuff[128]; + int iCountGraphs = 0; + osd_set_colors_background_fill(g_fOSDStatsBgTransparency); g_pRenderEngine->drawRoundRect(xPos, yPos, width, height, 1.5*POPUP_ROUND_MARGIN); osd_set_colors(); @@ -439,22 +573,30 @@ void osd_render_debug_stats() g_pRenderEngine->drawText(xPos, y, s_idFontStats, "Vehicle Tx Video Packets"); y += height_text*1.3; - y += _osd_render_debug_stats_graph_bars(fGraphXStart, y, hGraphSmall, fWidthGraph, uTmp, uTmp2, iCountIntervals, 1); + y += _osd_render_debug_stats_graph_bars(fGraphXStart, y, hGraphSmall, fWidthGraph, uTmp, uTmp2, NULL, iCountIntervals, 1); y += height_text_small; /**/ //-------------------------------------------- if ( pP->uDebugStatsFlags & CTRL_RT_DEBUG_INFO_FLAG_SHOW_RX_VIDEO_DATA_PACKETS ) { - for( int i=0; iuRxVideoPackets[i+iStartIntervals][0]; - uTmp2[i] = pCRTInfo->uRxDataPackets[i+iStartIntervals][0]; - } - g_pRenderEngine->drawText(xPos, y, s_idFontStats, "Rx Video/Data Packets"); - y += height_text*1.3; - y += _osd_render_debug_stats_graph_bars(fGraphXStart, y, hGraphSmall, fWidthGraph, uTmp, uTmp2, iCountIntervals, 1); - y += height_text_small; + for( int i=0; iuRxVideoPackets[i+iStartIntervals][0]; + uTmp2[i]+= pCRTInfo->uRxVideoECPackets[i+iStartIntervals][0]; + uTmp3[i]+= 5*pCRTInfo->uRxVideoRetrPackets[i+iStartIntervals][0]; + } + } + g_pRenderEngine->drawText(xPos, y, s_idFontStats, "Rx Video/EC/Retr Packets"); + y += height_text*1.3; + y += _osd_render_debug_stats_graph_bars(fGraphXStart, y, hGraphSmall, fWidthGraph, uTmp, uTmp2, uTmp3, iCountIntervals, 1); + y += height_text_small; + iCountGraphs++; } //-------------------------------------------- @@ -468,6 +610,7 @@ void osd_render_debug_stats() y += height_text*1.3; y += _osd_render_debug_stats_graph_values(fGraphXStart, y, hGraphSmall, fWidthGraph, uTmp, iCountIntervals); y += height_text_small; + iCountGraphs++; } //----------------------------------------------- @@ -492,6 +635,7 @@ void osd_render_debug_stats() y += height_text_small; } } + iCountGraphs++; } //----------------------------------------------- @@ -530,10 +674,13 @@ void osd_render_debug_stats() y += _osd_render_debug_stats_graph_values(fGraphXStart, y, hGraphSmall, fWidthGraph, uTmp, iCountIntervals); y += height_text_small; } + iCountGraphs++; } //-------------------------------------------------------------------------- + if ( iCountGraphs >=3 ) + { g_pRenderEngine->setStrokeSize(1.0); g_pRenderEngine->setStroke(255,255,255, 1.0); xPosSlice = fGraphXStart + dx; @@ -548,6 +695,7 @@ void osd_render_debug_stats() } } y += height_text_small; + } //-------------------------------------------- if ( pP->uDebugStatsFlags & CTRL_RT_DEBUG_INFO_FLAG_SHOW_RX_MISSING_PACKETS_MAX_GAP ) @@ -561,9 +709,10 @@ void osd_render_debug_stats() sprintf(szBuff, "Rx Missing Packets Max Gap (interface %d)", iInt+1); g_pRenderEngine->drawText(xPos, y, s_idFontStats, szBuff); y += height_text*1.3; - y += _osd_render_debug_stats_graph_bars(fGraphXStart, y, hGraphSmall, fWidthGraph, uTmp, NULL, iCountIntervals, 0); + y += _osd_render_debug_stats_graph_bars(fGraphXStart, y, hGraphSmall, fWidthGraph, uTmp, NULL, NULL, iCountIntervals, 0); y += height_text_small; } + iCountGraphs++; } //-------------------------------------------- @@ -575,8 +724,9 @@ void osd_render_debug_stats() } g_pRenderEngine->drawText(xPos, y, s_idFontStats, "Rx Consumed Packets"); y += height_text*1.3; - y += _osd_render_debug_stats_graph_bars(fGraphXStart, y, hGraph, fWidthGraph, uTmp, NULL, iCountIntervals, 1); + y += _osd_render_debug_stats_graph_bars(fGraphXStart, y, hGraph, fWidthGraph, uTmp, NULL, NULL, iCountIntervals, 1); y += height_text_small; + iCountGraphs++; } //-------------------------------------------- @@ -587,7 +737,7 @@ void osd_render_debug_stats() } g_pRenderEngine->drawText(xPos, y, s_idFontStats, "Outputed clean video packets"); y += height_text*1.3; - y += _osd_render_debug_stats_graph_bars(fGraphXStart, y, hGraphSmall, fWidthGraph, uTmp, NULL, iCountIntervals, 1); + y += _osd_render_debug_stats_graph_bars(fGraphXStart, y, hGraphSmall, fWidthGraph, uTmp, NULL, NULL, iCountIntervals, 1); y += height_text_small; */ @@ -625,17 +775,38 @@ void osd_render_debug_stats() // ---------------------------------------- if ( pP->uDebugStatsFlags & CTRL_RT_DEBUG_INFO_FLAG_SHOW_MIN_MAX_ACK_TIME ) - { - controller_runtime_info_vehicle* pRTInfoVehicle = controller_rt_info_get_vehicle_info(pCRTInfo, pActiveModel->uVehicleId); - for( int i=0; iuMinAckTime[i+iStartIntervals]; - iTmp2[i] = pRTInfoVehicle->uMaxAckTime[i+iStartIntervals]; + { + controller_runtime_info_vehicle* pRTInfoVehicle = controller_rt_info_get_vehicle_info(pCRTInfo, pActiveModel->uVehicleId); + for( int iLink=0; iLinkuMinAckTime[i+iStartIntervals][iLink]; + iTmp2[i] = pRTInfoVehicle->uMaxAckTime[i+iStartIntervals][iLink]; + if ( iTmp2[i] > 0 ) + { + g_pRenderEngine->setColors(get_Color_OSDText(), 0.3); + g_pRenderEngine->drawRect(xPosSlice, yTop, fWidthBar, 0.9 - yTop); + osd_set_colors(); + } + xPosSlice += fWidthBar; + } + sprintf(szBuff, "RadioLink-%d Min/Max Ack Time (ms)", iLink+1); + g_pRenderEngine->drawText(xPos, y, s_idFontStats, szBuff); + y += height_text*1.3; + y += _osd_render_debug_stats_graph_lines(fGraphXStart, y, hGraphSmall*1.3, fWidthGraph, NULL, iTmp1, iTmp2, NULL, iCountIntervals); + y += height_text_small; + iCountGraphs++; + } } - g_pRenderEngine->drawText(xPos, y, s_idFontStats, "Min/Max Ack Time (ms)"); - y += height_text*1.3; - y += _osd_render_debug_stats_graph_lines(fGraphXStart, y, hGraphSmall*1.3, fWidthGraph, NULL, iTmp1, iTmp2, NULL, iCountIntervals); - y += height_text_small; + + // ---------------------------------------- + if ( pP->uDebugStatsFlags & CTRL_RT_DEBUG_INFO_FLAG_SHOW_ACK_TIME_HISTORY ) + { + controller_runtime_info_vehicle* pRTInfoVehicle = controller_rt_info_get_vehicle_info(pCRTInfo, pActiveModel->uVehicleId); + y = _osd_render_ack_time_hist(pRTInfoVehicle, xPos, fGraphXStart, y, hGraphSmall, fWidthGraph*0.5 ); + iCountGraphs++; } // ---------------------------------------- @@ -650,6 +821,7 @@ void osd_render_debug_stats() y += height_text*1.3; y += _osd_render_debug_stats_graph_values(fGraphXStart, y, hGraphSmall, fWidthGraph, uTmp, iCountIntervals); y += height_text_small; + iCountGraphs++; } //-------------------------------------------- @@ -663,6 +835,7 @@ void osd_render_debug_stats() y += height_text*1.3; y += _osd_render_debug_stats_graph_values(fGraphXStart, y, hGraphSmall, fWidthGraph, uTmp, iCountIntervals); y += height_text_small; + iCountGraphs++; } //-------------------------------------------- @@ -674,7 +847,7 @@ void osd_render_debug_stats() } g_pRenderEngine->drawText(xPos, y, s_idFontStats, "Requested Retransmissions"); y += height_text*1.3; - y += _osd_render_debug_stats_graph_bars(fGraphXStart, y, hGraphSmall, fWidthGraph, uTmp, NULL, iCountIntervals, 0); + y += _osd_render_debug_stats_graph_bars(fGraphXStart, y, hGraphSmall, fWidthGraph, uTmp, NULL, NULL, iCountIntervals, 0); y += height_text_small; for( int i=0; idrawText(xPos, y, s_idFontStats, "Ack Retransmissions"); y += height_text*1.3; - y += _osd_render_debug_stats_graph_bars(fGraphXStart, y, hGraphSmall, fWidthGraph, uTmp, NULL, iCountIntervals, 0); + y += _osd_render_debug_stats_graph_bars(fGraphXStart, y, hGraphSmall, fWidthGraph, uTmp, NULL, NULL, iCountIntervals, 0); y += height_text_small; + iCountGraphs++; } //-------------------------------------------- @@ -696,7 +870,7 @@ void osd_render_debug_stats() } g_pRenderEngine->drawText(xPos, y, s_idFontStats, "Rx Video Decode Data/EC"); y += height_text*1.3; - y += _osd_render_debug_stats_graph_bars(fGraphXStart, y, hGraph, fWidthGraph, uTmp, uTmp2, iCountIntervals, 1); + y += _osd_render_debug_stats_graph_bars(fGraphXStart, y, hGraph, fWidthGraph, uTmp, uTmp2, NULL, iCountIntervals, 1); y += height_text_small; /**/ //------------------------------------------------ @@ -770,6 +944,7 @@ void osd_render_debug_stats() xPosSlice += fWidthBar; } + iCountGraphs++; } float xLine = fGraphXStart + pCRTInfo->iCurrentIndex * fWidthBar; diff --git a/code/r_central/osd/osd_plugins.h b/code/r_central/osd/osd_plugins.h index 29952db0..0a34cfc3 100644 --- a/code/r_central/osd/osd_plugins.h +++ b/code/r_central/osd/osd_plugins.h @@ -34,7 +34,7 @@ typedef struct bool bBoundingBox; bool bHighlight; -} __attribute__((packed)) plugin_osd_t; +} ALIGN_STRUCT_SPEC_INFO plugin_osd_t; extern plugin_osd_t* g_pPluginsOSD[MAX_OSD_PLUGINS]; extern int g_iPluginsOSDCount; diff --git a/code/r_central/osd/osd_stats.cpp b/code/r_central/osd/osd_stats.cpp index e3791e72..676b4aa9 100644 --- a/code/r_central/osd/osd_stats.cpp +++ b/code/r_central/osd/osd_stats.cpp @@ -3521,8 +3521,8 @@ float osd_render_stats_dev(float xPos, float yPos, float scale) float y = yPos + height_text*1.3*s_OSDStatsLineSpacing; - u32 linkMin = 2000; - u32 linkMax = 0; + u8 uLinkMinAck = 2000; + u8 uLinkMaxAck = 0; Model* pActiveModel = osd_get_current_data_source_vehicle_model(); u32 uActiveVehicleId = osd_get_current_data_source_vehicle_id(); @@ -3536,31 +3536,32 @@ float osd_render_stats_dev(float xPos, float yPos, float scale) break; } } - - if ( (-1 != iIndexVehicleRuntimeInfo) && g_bIsRouterReady && (NULL != pActiveModel) ) + controller_runtime_info_vehicle* pRTInfoVehicle = controller_rt_info_get_vehicle_info(&g_SMControllerRTInfo, uActiveVehicleId); + int iCountInterfaces = hardware_get_radio_interfaces_count(); + if ( (-1 != iIndexVehicleRuntimeInfo) && g_bIsRouterReady && (NULL != pActiveModel) && (NULL != pRTInfoVehicle) ) { - for( int i=0; iradioLinksParams.links_count; i++ ) + for( int i=0; i linkMax ) - linkMax = g_SM_RouterVehiclesRuntimeInfo.uRadioLinksDelayRoundtripMs[iIndexVehicleRuntimeInfo][i]; - if ( g_SM_RouterVehiclesRuntimeInfo.uRadioLinksDelayRoundtripMsMin[iIndexVehicleRuntimeInfo][i] < linkMin ) - linkMin = g_SM_RouterVehiclesRuntimeInfo.uRadioLinksDelayRoundtripMsMin[iIndexVehicleRuntimeInfo][i]; + if ( 0 != pRTInfoVehicle->uMinAckTime[i][k] ) + if ( pRTInfoVehicle->uMinAckTime[i][k] < uLinkMinAck ) + uLinkMinAck = pRTInfoVehicle->uMinAckTime[i][k]; + if ( pRTInfoVehicle->uMaxAckTime[i][k] < uLinkMaxAck ) + uLinkMaxAck = pRTInfoVehicle->uMaxAckTime[i][k]; } } if ( p->iDebugShowDevRadioStats ) { - if ( 0 != linkMax ) - sprintf(szBuff, "%d ms", linkMax); + if ( uLinkMaxAck < 2000 ) + sprintf(szBuff, "%d ms", uLinkMaxAck); else strcpy(szBuff, "N/A"); _osd_stats_draw_line(xPos, rightMargin, y, s_idFontStats, "Link RT (max):", szBuff); y += height_text*s_OSDStatsLineSpacing; - if ( 0 != linkMin ) - sprintf(szBuff, "%d ms", linkMin); + if ( 0 != uLinkMaxAck ) + sprintf(szBuff, "%d ms", uLinkMaxAck); else strcpy(szBuff, "N/A"); _osd_stats_draw_line(xPos, rightMargin, y, s_idFontStats, "Link RT (minim):", szBuff); diff --git a/code/r_central/osd/osd_stats_radio.cpp b/code/r_central/osd/osd_stats_radio.cpp index 646687b3..de22f7cc 100644 --- a/code/r_central/osd/osd_stats_radio.cpp +++ b/code/r_central/osd/osd_stats_radio.cpp @@ -1072,7 +1072,7 @@ float osd_render_stats_local_radio_links_get_height(shared_mem_radio_stats* pRad // Retransmissions roundtrip if ( NULL != pActiveModel ) if ( pActiveModel->bDeveloperMode || s_bDebugStatsShowAll ) - height += 3.0 * height_text * s_OSDStatsLineSpacing; + height += 4.0 * height_text * s_OSDStatsLineSpacing; if ( NULL != pActiveModel ) if ( pActiveModel->bDeveloperMode || s_bDebugStatsShowAll ) @@ -1232,19 +1232,15 @@ float osd_render_stats_local_radio_links( float xPos, float yPos, const char* sz int iLocalRadioLinkId = iLocalRadioLinkIdForVehicleRadioLinks[iVehicleRadioLink]; if ( 1 == iCountVehicles ) { - u32 uRTDelay = MAX_U32; - for( int k=0; kuVehicleId ) - continue; - uRTDelay = g_SM_RouterVehiclesRuntimeInfo.uPingRoundtripTimeVehiclesOnLocalRadioLinks[k][iLocalRadioLinkId]; - break; - } + int iRTDelay = 0; + controller_runtime_info_vehicle* pRTInfoVehicle = controller_rt_info_get_vehicle_info(&g_SMControllerRTInfo, g_pCurrentModel->uVehicleId); + if ( NULL != pRTInfoVehicle ) + iRTDelay = pRTInfoVehicle->uAckTimes[pRTInfoVehicle->iAckTimeIndex[iLocalRadioLinkId]][iLocalRadioLinkId]; sprintf(szBuff, "Link-%d RT delay:", iVehicleRadioLink+1); - if ( (uRTDelay == MAX_U32) || (g_pCurrentModel->radioLinksParams.uGlobalRadioLinksFlags & MODEL_RADIOLINKS_FLAGS_DOWNLINK_ONLY) ) + if ( (iRTDelay == 0) || (g_pCurrentModel->radioLinksParams.uGlobalRadioLinksFlags & MODEL_RADIOLINKS_FLAGS_DOWNLINK_ONLY) ) strcpy(szBuff2, "N/A"); else - sprintf(szBuff2, "%u ms", uRTDelay/1000); + sprintf(szBuff2, "%u ms", iRTDelay); _osd_stats_draw_line(xPos, rightMargin, y, s_idFontStats, szBuff, szBuff2); y += height_text*s_OSDStatsLineSpacing; continue; @@ -1259,17 +1255,22 @@ float osd_render_stats_local_radio_links( float xPos, float yPos, const char* sz { if ( g_SM_RouterVehiclesRuntimeInfo.uVehiclesIds[k] == 0 ) continue; - u32 uRTDelay = g_SM_RouterVehiclesRuntimeInfo.uPingRoundtripTimeVehiclesOnLocalRadioLinks[k][iLocalRadioLinkId]; + + int iRTDelay = 0; + controller_runtime_info_vehicle* pRTInfoVehicle = controller_rt_info_get_vehicle_info(&g_SMControllerRTInfo, g_SM_RouterVehiclesRuntimeInfo.uVehiclesIds[k]); + if ( NULL != pRTInfoVehicle ) + iRTDelay = pRTInfoVehicle->uAckTimes[pRTInfoVehicle->iAckTimeIndex[iLocalRadioLinkId]][iLocalRadioLinkId]; + Model* pModel = findModelWithId(g_SM_RouterVehiclesRuntimeInfo.uVehiclesIds[k], 31); if ( NULL != pModel ) sprintf(szBuff, "%s:",pModel->getLongName()); else sprintf(szBuff, "Vehicle %d:", k+1); str_capitalize_first_letter(szBuff); - if ( uRTDelay == MAX_U32 ) + if ( iRTDelay == 0 ) strcpy(szBuff2, "N/A"); else - sprintf(szBuff2, "%u ms", uRTDelay/1000); + sprintf(szBuff2, "%u ms", iRTDelay); _osd_stats_draw_line(xPos + height_text, rightMargin, y, s_idFontStats, szBuff, szBuff2); y += height_text*s_OSDStatsLineSpacing; } @@ -1305,6 +1306,13 @@ float osd_render_stats_local_radio_links( float xPos, float yPos, const char* sz } */ g_pRenderEngine->setColors(get_Color_Dev()); + strcpy(szBuff, "None"); + if ( pActiveModel->rxtx_sync_type == RXTX_SYNC_TYPE_BASIC ) + strcpy(szBuff, "Basic"); + if ( pActiveModel->rxtx_sync_type == RXTX_SYNC_TYPE_ADV ) + strcpy(szBuff, "Adv"); + _osd_stats_draw_line(xPos, rightMargin, y, s_idFontStats, "Clock sync type:", szBuff); + y += height_text*s_OSDStatsLineSpacing; // To fix /* diff --git a/code/r_central/osd/osd_widgets.h b/code/r_central/osd/osd_widgets.h index 5d328eff..57e24b99 100644 --- a/code/r_central/osd/osd_widgets.h +++ b/code/r_central/osd/osd_widgets.h @@ -21,7 +21,7 @@ typedef struct bool bShow; int iParams[MAX_OSD_WIDGET_PARAMS]; int iParamsCount; -} type_osd_widget_display_info; +} ALIGN_STRUCT_SPEC_INFO type_osd_widget_display_info; typedef struct { @@ -29,13 +29,13 @@ typedef struct int iVersion; char szName[MAX_OSD_WIDGET_NAME]; -} __attribute__((packed)) type_osd_widget_info; +} ALIGN_STRUCT_SPEC_INFO type_osd_widget_info; typedef struct { type_osd_widget_info info; type_osd_widget_display_info display_info[MAX_MODELS][MODEL_MAX_OSD_PROFILES]; -} type_osd_widget; +} ALIGN_STRUCT_SPEC_INFO type_osd_widget; int osd_widgets_get_count(); type_osd_widget* osd_widgets_get(int iIndex); diff --git a/code/r_central/pairing.cpp b/code/r_central/pairing.cpp index 4c516e41..2a8e0f32 100644 --- a/code/r_central/pairing.cpp +++ b/code/r_central/pairing.cpp @@ -154,7 +154,8 @@ bool pairing_start_normal() g_iSearchSiKECC = -1; g_iSearchSiKLBT = -1; g_iSearchSiKMCSTR = -1; - + g_bDidAnUpdate = false; + onEventBeforePairing(); if ( NULL == g_pCurrentModel ) diff --git a/code/r_central/process_router_messages.cpp b/code/r_central/process_router_messages.cpp index 5abf9b96..0006d094 100644 --- a/code/r_central/process_router_messages.cpp +++ b/code/r_central/process_router_messages.cpp @@ -39,6 +39,8 @@ #include "menu/menu_objects.h" #include "menu/menu_search.h" #include "menu/menu_diagnose_radio_link.h" +#include "menu/menu_vehicle_radio_link.h" +#include "menu/menu_negociate_radio.h" #include "process_router_messages.h" #include #include "shared_vars.h" @@ -669,6 +671,16 @@ int _process_received_message_from_router(u8* pPacketBuffer) return 0; } + if ( pPH->packet_type == PACKET_TYPE_NEGOCIATE_RADIO_LINKS ) + { + if ( menu_has_menu(MENU_ID_NEGOCIATE_RADIO) ) + { + MenuNegociateRadio* pMenu = (MenuNegociateRadio*) menu_get_menu_by_id(MENU_ID_NEGOCIATE_RADIO); + if ( NULL != pMenu ) + pMenu->onReceivedVehicleResponse(pPacketBuffer, pPH->total_length); + } + return 0; + } if ( pPH->packet_type == PACKET_TYPE_LOCAL_CONTROLL_VIDEO_DETECTED_ON_SEARCH ) { MenuSearch::onVideoReceived(pPH->vehicle_id_src); @@ -757,9 +769,21 @@ int _process_received_message_from_router(u8* pPacketBuffer) } else { - Popup* p = new Popup("Failed to set the new parameters.",0.25,0.44, 0.5, 4); - p->setIconId(g_idIconError, get_Color_IconError()); - popups_add_topmost(p); + //Popup* p = new Popup("Failed to set the new parameters.",0.25,0.44, 0.5, 4); + //p->setIconId(g_idIconError, get_Color_IconError()); + //popups_add_topmost(p); + Menu* pm = new Menu(MENU_ID_SIMPLE_MESSAGE, "Change failed",NULL); + pm->m_xPos = 0.32; pm->m_yPos = 0.4; + pm->m_Width = 0.36; + pm->m_bDisableStacking = true; + pm->addTopLine("The selected parameters are not supported by the radio interfaces."); + add_menu_to_stack(pm); + } + if ( menu_has_menu(MENU_ID_VEHICLE_RADIO_LINK) ) + { + MenuVehicleRadioLink* pMenuRadioLink = (MenuVehicleRadioLink*) menu_get_menu_by_id(MENU_ID_VEHICLE_RADIO_LINK); + if ( NULL != pMenuRadioLink ) + pMenuRadioLink->onChangeRadioConfigFinished(bSucceeded); } menu_update_ui_all_menus(); } diff --git a/code/r_central/shared_vars.cpp b/code/r_central/shared_vars.cpp index 2db0a138..3a311867 100644 --- a/code/r_central/shared_vars.cpp +++ b/code/r_central/shared_vars.cpp @@ -57,6 +57,7 @@ bool g_bToglleOSDOff = false; bool g_bToglleStatsOff = false; bool g_bToglleAllOSDOff = false; bool g_bFreezeOSD = false; +bool g_bDidAnUpdate = false; bool g_bIsRouterPacketsHistoryGraphOn = false; bool g_bIsRouterPacketsHistoryGraphPaused = false; diff --git a/code/r_central/shared_vars.h b/code/r_central/shared_vars.h index be1c4e80..4e5e4387 100644 --- a/code/r_central/shared_vars.h +++ b/code/r_central/shared_vars.h @@ -1,5 +1,6 @@ #pragma once #include "../base/base.h" +#include "../base/config.h" #include "../radio/radiopackets2.h" #include "../radio/radiopackets_rc.h" #include "../base/shared_mem.h" @@ -57,6 +58,7 @@ extern bool g_bToglleOSDOff; extern bool g_bToglleStatsOff; extern bool g_bToglleAllOSDOff; extern bool g_bFreezeOSD; +extern bool g_bDidAnUpdate; extern bool g_bIsRouterPacketsHistoryGraphOn; extern bool g_bIsRouterPacketsHistoryGraphPaused; diff --git a/code/r_central/shared_vars_state.h b/code/r_central/shared_vars_state.h index 0876b270..f09f9fea 100644 --- a/code/r_central/shared_vars_state.h +++ b/code/r_central/shared_vars_state.h @@ -1,5 +1,6 @@ #pragma once #include "../base/base.h" +#include "../base/config.h" #include "../base/models.h" #include "../base/commands.h" #include "../base/msp.h" @@ -19,7 +20,7 @@ typedef struct u32 uTimeLastUploadSegment; u32 uLastSegmentIndexUploaded; -} __attribute__((packed)) t_structure_file_upload; +} ALIGN_STRUCT_SPEC_INFO t_structure_file_upload; extern t_structure_file_upload g_CurrentUploadingFile; extern bool g_bHasFileUploadInProgress; @@ -42,7 +43,7 @@ typedef struct u16 uScreenChars[MAX_MSP_CHARS_BUFFER]; // Max 64x24 u16 uScreenCharsTmp[MAX_MSP_CHARS_BUFFER]; // Max 64x24 bool bEmptyBuffer; -} __attribute__((packed)) type_msp_parse_state; +} ALIGN_STRUCT_SPEC_INFO type_msp_parse_state; typedef struct { @@ -127,7 +128,7 @@ typedef struct int tmp_iCountFCTelemetryPacketsShort; u32 tmp_uTimeLastTelemetryFrequencyComputeTime; -} t_structure_vehicle_info; +} ALIGN_STRUCT_SPEC_INFO t_structure_vehicle_info; extern int s_StartSequence; extern Model* g_pCurrentModel; diff --git a/code/r_central/ui_alarms.cpp b/code/r_central/ui_alarms.cpp index 78b61e77..7a3d5bb6 100644 --- a/code/r_central/ui_alarms.cpp +++ b/code/r_central/ui_alarms.cpp @@ -232,6 +232,12 @@ void alarms_add_from_vehicle(u32 uVehicleId, u32 uAlarms, u32 uFlags1, u32 uFlag uIconId = g_idIconCamera; strcpy(szAlarmText, "Video capture process on vehicle is malfunctioning."); strcpy(szAlarmText2, "Reinstall your vehicle firmware."); + + if ( uFlags1 == 1 ) + { + strcpy(szAlarmText, "Video capture process on vehicle is not responding."); + strcpy(szAlarmText2, "Vehicle will restart now..."); + } } diff --git a/code/r_central/warnings.cpp b/code/r_central/warnings.cpp index b886a676..5af1105d 100644 --- a/code/r_central/warnings.cpp +++ b/code/r_central/warnings.cpp @@ -43,6 +43,7 @@ #include "osd/osd_common.h" #include "shared_vars.h" +#include #include "link_watch.h" #include "ruby_central.h" #include "popup_log.h" @@ -206,6 +207,7 @@ void warnings_add(u32 uVehicleId, const char* szTitle, u32 iconId, double* pColo if ( NULL != g_VehiclesRuntimeInfo[iRuntimeInfoIndex].pModel ) sprintf(szComposedTitle, "%s: %s", g_VehiclesRuntimeInfo[iRuntimeInfoIndex].pModel->getShortName(), szTitle); + szComposedTitle[0] = toupper(szComposedTitle[0]); if ( s_PopupsWarningsCount > 0 ) if ( 0 == strcmp(szComposedTitle, s_PopupsWarnings[0]->getTitle()) ) if ( s_TimeLastWarningAdded+2000 > g_TimeNow ) diff --git a/code/r_player/mpp_core.cpp b/code/r_player/mpp_core.cpp index 33413cfe..fe272bb6 100644 --- a/code/r_player/mpp_core.cpp +++ b/code/r_player/mpp_core.cpp @@ -31,8 +31,8 @@ #include "mpp_core.h" -#define READ_VIDEO_BUF_SIZE (1024*1024) // SZ_1M https://github.com/rockchip-linux/mpp/blob/ed377c99a733e2cdbcc457a6aa3f0fcd438a9dff/osal/inc/mpp_common.h#L179 -#define MAX_VIDEO_FRAMES 24 // min 16 and 20+ recommended (mpp/readme.txt) +#define READ_VIDEO_BUF_SIZE (2*1024*1024) // SZ_1M https://github.com/rockchip-linux/mpp/blob/ed377c99a733e2cdbcc457a6aa3f0fcd438a9dff/osal/inc/mpp_common.h#L179 +#define MAX_VIDEO_FRAMES 36 // min 16 and 20+ recommended (mpp/readme.txt) #define CODEC_ALIGN(x, a) (((x)+(a)-1)&~((a)-1)) typedef struct diff --git a/code/r_start/first_boot.cpp b/code/r_start/first_boot.cpp index 563559f2..2ce9c884 100644 --- a/code/r_start/first_boot.cpp +++ b/code/r_start/first_boot.cpp @@ -62,7 +62,7 @@ void do_first_boot_pre_initialization() { hw_execute_bash_command("cp -rf /home/88XXau_wfb.ko /lib/modules/$(uname -r)/kernel/drivers/net/wireless/", NULL); hw_execute_bash_command("rmmod 88XXau_wfb 2>&1 1>/dev/null", NULL); - hw_execute_bash_command("insmod /lib/modules/$(uname -r)/kernel/drivers/net/wireless/88XXau_wfb.ko", NULL); + hw_execute_bash_command("insmod /lib/modules/$(uname -r)/kernel/drivers/net/wireless/88XXau_wfb.ko 2>&1 1>/dev/null", NULL); } if ( access("/home/8812eu_radxa.ko", R_OK) != -1 ) { @@ -73,8 +73,8 @@ void do_first_boot_pre_initialization() } hw_execute_bash_command("depmod -a", NULL); hw_execute_bash_command("lsusb", NULL); - hw_execute_bash_command("sudo modprobe -f 88XXau_wfb", NULL); - hw_execute_bash_command("sudo modprobe -f 8812eu_radxa.ko", NULL); + hw_execute_bash_command("sudo modprobe -f 88XXau_wfb 2>&1 1>/dev/null", NULL); + hw_execute_bash_command("sudo modprobe -f 8812eu_radxa.ko 2>&1 1>/dev/null", NULL); hw_execute_bash_command("sudo modprobe -r aic8800_fdrv 2>&1 1>/dev/null", NULL); hw_execute_bash_command("sudo modprobe -r aic8800_bsp 2>&1 1>/dev/null", NULL); hw_execute_bash_command("lsusb", NULL); @@ -224,10 +224,10 @@ void do_first_boot_initialization(bool bIsVehicle, u32 uBoardType) } else { - load_ControllerSettings(); - ControllerSettings* pcs = get_ControllerSettings(); + load_ControllerSettings(); #ifdef HW_PLATFORM_RASPBERRY + ControllerSettings* pcs = get_ControllerSettings(); if ( NULL != pcs ) { pcs->iFreqARM = DEFAULT_ARM_FREQ; @@ -253,6 +253,7 @@ void do_first_boot_initialization(bool bIsVehicle, u32 uBoardType) #endif #if defined (HW_PLATFORM_RADXA_ZERO3) + ControllerSettings* pcs = get_ControllerSettings(); if ( NULL != pcs ) pcs->iFreqARM = hardware_get_cpu_speed(); #endif diff --git a/code/r_start/r_start_vehicle.cpp b/code/r_start/r_start_vehicle.cpp index 0dff401c..5a2ea719 100644 --- a/code/r_start/r_start_vehicle.cpp +++ b/code/r_start/r_start_vehicle.cpp @@ -669,7 +669,7 @@ int r_start_vehicle(int argc, char *argv[]) int counter = 0; bool bMustRestart = false; int iRestartCount = 0; - u32 uTimeToAdjustAffinities = get_current_timestamp_ms() + 5000; + u32 uTimeToAdjustAffinities = get_current_timestamp_ms() + 1000; u32 uTimeLoopLog = g_TimeStart; char szFileUpdate[MAX_FILE_PATH_SIZE]; diff --git a/code/r_start/ruby_start.cpp b/code/r_start/ruby_start.cpp index 65425a08..a0e70ef8 100644 --- a/code/r_start/ruby_start.cpp +++ b/code/r_start/ruby_start.cpp @@ -1039,6 +1039,10 @@ int main(int argc, char *argv[]) fflush(stdout); hardware_i2c_reset_enumerated_flag(); hardware_enumerate_i2c_busses(); + // Load existing settings first + hardware_i2c_load_device_settings(); + hardware_i2c_log_devices(); + // Save existing settings and any new devices hardware_i2c_save_device_settings(); int iKnown = hardware_get_i2c_found_count_known_devices(); int iConfigurable = hardware_get_i2c_found_count_configurable_devices(); @@ -1244,10 +1248,10 @@ int main(int argc, char *argv[]) if ( NULL == strstr(szOutput, "performance") ) hardware_set_default_radxa_cpu_freq(); - hw_execute_bash_command("systemctl disable systemd-journald.service", NULL); - hw_execute_bash_command("systemctl mask systemd-journald.service", NULL); - hw_execute_bash_command("systemctl disable rknpu2.service", NULL); - hw_execute_bash_command("systemctl mask rknpu2.service", NULL); + hw_execute_bash_command("systemctl disable systemd-journald.service 2>&1 1>/dev/null", NULL); + hw_execute_bash_command("systemctl mask systemd-journald.service 2>&1 1>/dev/null", NULL); + hw_execute_bash_command("systemctl disable rknpu2.service 2>&1 1>/dev/null", NULL); + hw_execute_bash_command("systemctl mask rknpu2.service 2>&1 1>/dev/null", NULL); #endif } diff --git a/code/r_station/adaptive_video.cpp b/code/r_station/adaptive_video.cpp index 3f0a6b82..5027e313 100644 --- a/code/r_station/adaptive_video.cpp +++ b/code/r_station/adaptive_video.cpp @@ -42,8 +42,26 @@ #include "shared_vars_state.h" #include "timers.h" -extern t_packet_queue s_QueueRadioPackets; +extern t_packet_queue s_QueueRadioPacketsHighPrio; +u32 s_uTimePauseAdaptiveVideoUntil = 0; + +void adaptive_video_pause(u32 uMilisec) +{ + if ( 0 == uMilisec ) + { + s_uTimePauseAdaptiveVideoUntil = 0; + log_line("[AdaptiveVideo] Resumed"); + + } + else + { + if ( uMilisec > 20000 ) + uMilisec = 20000; + s_uTimePauseAdaptiveVideoUntil = g_TimeNow + uMilisec; + log_line("[AdaptiveVideo] Pause for %u milisec", uMilisec); + } +} int _adaptive_video_get_lower_video_profile(int iVideoProfile) { @@ -91,13 +109,15 @@ void _adaptive_video_send_video_profile_to_vehicle(int iVideoProfile, u32 uVehic memcpy(packet+sizeof(t_packet_header), (u8*)&(pRuntimeInfo->uVideoProfileRequestId), sizeof(u32)); memcpy(packet+sizeof(t_packet_header) + sizeof(u32), (u8*)&uVideoProfile, sizeof(u8)); memcpy(packet+sizeof(t_packet_header) + sizeof(u32) + sizeof(u8), (u8*)&uVideoStreamIndex, sizeof(u8)); - packets_queue_inject_packet_first(&s_QueueRadioPackets, packet); + packets_queue_inject_packet_first(&s_QueueRadioPacketsHighPrio, packet); } void adaptive_video_switch_to_video_profile(int iVideoProfile, u32 uVehicleId) { if ( ! isPairingDoneWithVehicle(uVehicleId) ) return; + if ( g_bNegociatingRadioLinks ) + return; type_global_state_vehicle_runtime_info* pRuntimeInfo = getVehicleRuntimeInfo(uVehicleId); if ( NULL == pRuntimeInfo ) return; @@ -107,7 +127,7 @@ void adaptive_video_switch_to_video_profile(int iVideoProfile, u32 uVehicleId) _adaptive_video_send_video_profile_to_vehicle(iVideoProfile, uVehicleId); } -void adaptive_video_received_video_profile_switch_confirmation(u32 uRequestId, u8 uVideoProfile, u32 uVehicleId) +void adaptive_video_received_video_profile_switch_confirmation(u32 uRequestId, u8 uVideoProfile, u32 uVehicleId, int iInterfaceIndex) { type_global_state_vehicle_runtime_info* pRuntimeInfo = getVehicleRuntimeInfo(uVehicleId); if ( NULL == pRuntimeInfo ) @@ -123,7 +143,7 @@ void adaptive_video_received_video_profile_switch_confirmation(u32 uRequestId, u if ( pRuntimeInfo->uVideoProfileRequestId == uRequestId ) { u32 uDeltaTime = g_TimeNow - pRuntimeInfo->uLastTimeSentVideoProfileRequest; - controller_rt_info_update_ack_rt_time(&g_SMControllerRTInfo, uVehicleId, uDeltaTime); + controller_rt_info_update_ack_rt_time(&g_SMControllerRTInfo, uVehicleId, g_SM_RadioStats.radio_interfaces[iInterfaceIndex].assignedLocalRadioLinkId, uDeltaTime); } } @@ -263,9 +283,17 @@ void _adaptive_video_check_vehicle(Model* pModel, type_global_state_vehicle_runt void adaptive_video_periodic_loop() { - if (g_TimeNow < g_TimeStart + 4000 ) + if ( (g_TimeNow < g_TimeStart + 4000) || g_bNegociatingRadioLinks ) return; + if ( 0 != s_uTimePauseAdaptiveVideoUntil ) + { + if ( g_TimeNow < s_uTimePauseAdaptiveVideoUntil ) + return; + log_line("[AdaptiveVideo] Resumed after pause."); + s_uTimePauseAdaptiveVideoUntil = 0; + } + for( int i=0; iiRadioType == RADIO_TYPE_ATHEROS) || (pRadioHWInfo->iRadioType == RADIO_TYPE_RALINK) ) { @@ -508,10 +515,6 @@ int send_packet_to_radio_interfaces(u8* pPacketData, int nPacketLength, int iSen u8* pData = pPacketData; int nLength = nPacketLength; - // To remove - static int siDebugCCC = 0; - siDebugCCC++; - while ( nLength > 0 ) { iTotalPackets++; diff --git a/code/r_station/process_local_packets.cpp b/code/r_station/process_local_packets.cpp index a5fdc769..fcdb08fa 100644 --- a/code/r_station/process_local_packets.cpp +++ b/code/r_station/process_local_packets.cpp @@ -443,7 +443,7 @@ void _process_local_notification_model_changed(t_packet_header* pPH, u8 uChangeT return; } - // Signal other components about the model change if it's not from central or if settings where synchronised form vehicle + // Signal other components about the model change if it's not from central or if settings where synchronised from vehicle // Signal other components too if the RC parameters where changed bool bNotify = false; if ( (pPH->vehicle_id_src == PACKET_COMPONENT_COMMANDS) || @@ -483,6 +483,17 @@ void process_local_control_packet(t_packet_header* pPH) return; return; } + + if ( pPH->packet_type == PACEKT_TYPE_LOCAL_CONTROLLER_ADAPTIVE_VIDEO_PAUSE ) + { + if ( 0 == pPH->vehicle_id_dest ) + log_line("Received notification to resume adaptive video."); + else + log_line("Received notification to pause adaptive video for %u ms", pPH->vehicle_id_dest); + adaptive_video_pause(pPH->vehicle_id_dest); + return; + } + if ( pPH->packet_type == PACKET_TYPE_LOCAL_CONTROLLER_RELOAD_CORE_PLUGINS ) { log_line("Router received a local message to reload core plugins."); diff --git a/code/r_station/process_radio_in_packets.cpp b/code/r_station/process_radio_in_packets.cpp index 84e23b0d..e936e11e 100644 --- a/code/r_station/process_radio_in_packets.cpp +++ b/code/r_station/process_radio_in_packets.cpp @@ -301,26 +301,16 @@ int _process_received_ruby_message(int iInterfaceIndex, u8* pPacketBuffer) int iIndex = getVehicleRuntimeIndex(pPH->vehicle_id_src); if ( iIndex >= 0 ) { - for( int k=0; kvehicle_id_src, g_SM_RadioStats.radio_interfaces[iInterfaceIndex].assignedLocalRadioLinkId, uRoundtripMilis); if ( uPingId != g_State.vehiclesRuntimeInfo[iIndex].uLastPingIdReceivedFromVehicleOnLocalRadioLinks[uOriginalLocalRadioLinkId] ) { - u32 uRoundtripMicros = get_current_timestamp_micros() - g_State.vehiclesRuntimeInfo[iIndex].uTimeLastPingSentToVehicleOnLocalRadioLinks[uOriginalLocalRadioLinkId][k]; - u32 uRoundtripMilis = uRoundtripMicros / 1000; - - controller_rt_info_update_ack_rt_time(&g_SMControllerRTInfo, pPH->vehicle_id_src, uRoundtripMilis); - g_State.vehiclesRuntimeInfo[iIndex].uLastPingIdReceivedFromVehicleOnLocalRadioLinks[uOriginalLocalRadioLinkId] = uPingId; g_State.vehiclesRuntimeInfo[iIndex].uTimeLastPingReplyReceivedFromVehicleOnLocalRadioLinks[uOriginalLocalRadioLinkId] = g_TimeNow; - - g_State.vehiclesRuntimeInfo[iIndex].uPingRoundtripTimeOnLocalRadioLinks[uOriginalLocalRadioLinkId] = uRoundtripMicros; - g_SM_RouterVehiclesRuntimeInfo.uPingRoundtripTimeVehiclesOnLocalRadioLinks[iIndex][uOriginalLocalRadioLinkId] = uRoundtripMicros; - addLinkRTTimeToRuntimeInfoIndex(iIndex, (int)uOriginalLocalRadioLinkId, uRoundtripMilis, uVehicleLocalTimeMs); - - if ( NULL != g_pProcessStats ) - g_pProcessStats->lastIPCOutgoingTime = g_TimeNow; - break; + g_State.vehiclesRuntimeInfo[iIndex].uPingRoundtripTimeOnLocalRadioLinks[uOriginalLocalRadioLinkId] = uRoundtripMilis; + adjustLinkClockDeltasForVehicleRuntimeIndex(iIndex, uRoundtripMilis, uVehicleLocalTimeMs); } } } @@ -371,6 +361,19 @@ int _process_received_ruby_message(int iInterfaceIndex, u8* pPacketBuffer) } return 0; } + + if ( pPH->packet_type == PACKET_TYPE_NEGOCIATE_RADIO_LINKS ) + { + u8 uCommand = pPacketBuffer[sizeof(t_packet_header) + sizeof(u8)]; + if ( NEGOCIATE_RADIO_STEP_DATA_RATE == uCommand ) + { + radio_stats_reset_interfaces_rx_info(&g_SM_RadioStats); + } + ruby_ipc_channel_send_message(g_fIPCToCentral, pPacketBuffer, pPH->total_length); + if ( NULL != g_pProcessStats ) + g_pProcessStats->lastIPCOutgoingTime = g_TimeNow; + return 0; + } return 0; } diff --git a/code/r_station/process_video_packets.cpp b/code/r_station/process_video_packets.cpp index fd723f45..f0527aa4 100644 --- a/code/r_station/process_video_packets.cpp +++ b/code/r_station/process_video_packets.cpp @@ -78,7 +78,7 @@ ProcessorRxVideo* _find_create_rx_video_processor(u32 uVehicleId, u32 uVideoStre int _process_received_video_data_packet(int iInterfaceIndex, u8* pPacket, int iPacketLength) { t_packet_header* pPH = (t_packet_header*)pPacket; - t_packet_header_video_full_98* pPHVF = (t_packet_header_video_full_98*) (pPacket+sizeof(t_packet_header)); + //t_packet_header_video_full_98* pPHVF = (t_packet_header_video_full_98*) (pPacket+sizeof(t_packet_header)); u32 uVehicleId = pPH->vehicle_id_src; Model* pModel = findModelWithId(uVehicleId, 111); if ( NULL == pModel ) @@ -110,7 +110,7 @@ int _process_received_video_data_packet(int iInterfaceIndex, u8* pPacket, int iP */ - bool bIsRelayedPacket = relay_controller_is_vehicle_id_relayed_vehicle(g_pCurrentModel, uVehicleId); + //bool bIsRelayedPacket = relay_controller_is_vehicle_id_relayed_vehicle(g_pCurrentModel, uVehicleId); u32 uVideoStreamIndex = 0; ProcessorRxVideo* pProcessorVideo = _find_create_rx_video_processor(uVehicleId, uVideoStreamIndex); @@ -181,7 +181,7 @@ int process_received_video_packet(int iInterfaceIndex, u8* pPacket, int iPacketL { if ( pPH->packet_flags & PACKET_FLAGS_BIT_RETRANSMITED ) { - t_packet_header_video_full_98* pPHVF = (t_packet_header_video_full_98*) (pPacket+sizeof(t_packet_header)); + //t_packet_header_video_full_98* pPHVF = (t_packet_header_video_full_98*) (pPacket+sizeof(t_packet_header)); //log_line("DEBUG recv retr video [%u/%u]", pPHVF->uCurrentBlockIndex, pPHVF->uCurrentBlockPacketIndex); } @@ -194,7 +194,7 @@ int process_received_video_packet(int iInterfaceIndex, u8* pPacket, int iPacketL u8 uVideoProfile = 0; memcpy((u8*)&uRequestId, pPacket + sizeof(t_packet_header), sizeof(u32)); memcpy((u8*)&uVideoProfile, pPacket + sizeof(t_packet_header) + sizeof(u32), sizeof(u8)); - adaptive_video_received_video_profile_switch_confirmation(uRequestId, uVideoProfile, pPH->vehicle_id_src); + adaptive_video_received_video_profile_switch_confirmation(uRequestId, uVideoProfile, pPH->vehicle_id_src, iInterfaceIndex); } return nRet; } \ No newline at end of file diff --git a/code/r_station/processor_rx_video.cpp b/code/r_station/processor_rx_video.cpp index 5f566342..3d6aefbe 100644 --- a/code/r_station/processor_rx_video.cpp +++ b/code/r_station/processor_rx_video.cpp @@ -83,7 +83,7 @@ type_fec_info; type_fec_info s_FECInfo; -extern t_packet_queue s_QueueRadioPackets; +extern t_packet_queue s_QueueRadioPacketsHighPrio; int ProcessorRxVideo::m_siInstancesCount = 0; FILE* ProcessorRxVideo::m_fdLogFile = NULL; @@ -147,7 +147,7 @@ ProcessorRxVideo::ProcessorRxVideo(u32 uVehicleId, u8 uVideoStreamIndex) m_uTimeLastReceivedNewVideoPacket = 0; m_TimeLastHistoryStatsUpdate = 0; m_TimeLastRetransmissionsStatsUpdate = 0; - m_uTimeLastReceivedVideoPacket = 0; + m_uLatestVideoPacketReceiveTime = 0; m_bPaused = false; @@ -709,9 +709,9 @@ u32 ProcessorRxVideo::getLastTimeVideoStreamChanged() return m_uTimeLastVideoStreamChanged; } -u32 ProcessorRxVideo::getLastTimeReceivedVideoPacket() +u32 ProcessorRxVideo::getLastestVideoPacketReceiveTime() { - return m_uTimeLastReceivedVideoPacket; + return m_uLatestVideoPacketReceiveTime; } int ProcessorRxVideo::getCurrentlyReceivedVideoProfile() @@ -754,30 +754,69 @@ int ProcessorRxVideo::getCurrentlyMissingVideoPackets() int ProcessorRxVideo::getVideoWidth() { - return 0; - // To fix? - //return m_SM_VideoDecodeStats.width; + int iVideoWidth = 0; + if ( m_iIndexVideoDecodeStats != -1 ) + { + if ( (0 != g_SM_VideoDecodeStats.video_streams[m_iIndexVideoDecodeStats].iCurrentVideoWidth) && (0 != g_SM_VideoDecodeStats.video_streams[m_iIndexVideoDecodeStats].iCurrentVideoHeight) ) + iVideoWidth = g_SM_VideoDecodeStats.video_streams[m_iIndexVideoDecodeStats].iCurrentVideoWidth; + } + else + { + Model* pModel = findModelWithId(m_uVehicleId, 177); + if ( NULL != pModel ) + iVideoWidth = pModel->video_link_profiles[pModel->video_params.user_selected_video_link_profile].width; + } + return iVideoWidth; } int ProcessorRxVideo::getVideoHeight() { - return 0; - // To fix? - //return m_SM_VideoDecodeStats.height; + int iVideoHeight = 0; + if ( m_iIndexVideoDecodeStats != -1 ) + { + if ( (0 != g_SM_VideoDecodeStats.video_streams[m_iIndexVideoDecodeStats].iCurrentVideoWidth) && (0 != g_SM_VideoDecodeStats.video_streams[m_iIndexVideoDecodeStats].iCurrentVideoHeight) ) + iVideoHeight = g_SM_VideoDecodeStats.video_streams[m_iIndexVideoDecodeStats].iCurrentVideoHeight; + } + else + { + Model* pModel = findModelWithId(m_uVehicleId, 177); + if ( NULL != pModel ) + iVideoHeight = pModel->video_link_profiles[pModel->video_params.user_selected_video_link_profile].height; + } + return iVideoHeight; } int ProcessorRxVideo::getVideoFPS() { - return 0; - // To fix? - //return m_SM_VideoDecodeStats.fps; + int iFPS = 0; + if ( -1 != m_iIndexVideoDecodeStats ) + iFPS = g_SM_VideoDecodeStats.video_streams[m_iIndexVideoDecodeStats].iCurrentVideoFPS; + if ( 0 == iFPS ) + { + Model* pModel = findModelWithId(m_uVehicleId, 177); + if ( NULL != pModel ) + iFPS = pModel->video_link_profiles[pModel->video_params.user_selected_video_link_profile].fps; + } + return iFPS; } int ProcessorRxVideo::getVideoType() { - return 0; - // To fix? - //return m_SM_VideoDecodeStats.video_stream_and_type >> 4; + int iVideoType = 0; + if ( (-1 != m_iIndexVideoDecodeStats ) && + (0 != ((g_SM_VideoDecodeStats.video_streams[m_iIndexVideoDecodeStats].PHVF.uVideoStreamIndexAndType >> 4) & 0x0F) ) ) + iVideoType = (g_SM_VideoDecodeStats.video_streams[m_iIndexVideoDecodeStats].PHVF.uVideoStreamIndexAndType >> 4) & 0x0F; + else + { + Model* pModel = findModelWithId(m_uVehicleId, 177); + if ( NULL != pModel ) + { + iVideoType = VIDEO_TYPE_H264; + if ( pModel->video_params.uVideoExtraFlags & VIDEO_FLAG_GENERATE_H265 ) + iVideoType = VIDEO_TYPE_H265; + } + } + return iVideoType; } shared_mem_video_stream_stats_history* ProcessorRxVideo::getVideoDecodeStatsHistory() @@ -852,7 +891,7 @@ int ProcessorRxVideo::handleReceivedVideoPacket(int interfaceNb, u8* pBuffer, in return 1; #ifdef PROFILE_RX - u32 uTimeStart = get_current_timestamp_ms(); + //u32 uTimeStart = get_current_timestamp_ms(); //int iStackIndexStart = m_iRXBlocksStackTopIndex; #endif @@ -860,8 +899,10 @@ int ProcessorRxVideo::handleReceivedVideoPacket(int interfaceNb, u8* pBuffer, in { bool bNewestOnStream = m_pVideoRxBuffer->checkAddVideoPacket(pBuffer, length); if ( bNewestOnStream && (m_iIndexVideoDecodeStats != -1) ) + { updateVideoDecodingStats(pBuffer, length); - + m_uLatestVideoPacketReceiveTime = g_TimeNow; + } t_packet_header* pPH = (t_packet_header*)pBuffer; t_packet_header_video_full_98* pPHVF = (t_packet_header_video_full_98*) (pBuffer+sizeof(t_packet_header)); @@ -875,7 +916,7 @@ int ProcessorRxVideo::handleReceivedVideoPacket(int interfaceNb, u8* pBuffer, in if ( pPHVF->uStreamInfo == m_uRequestRetransmissionUniqueId ) { u32 uDeltaTime = g_TimeNow - m_uLastTimeRequestedRetransmission; - controller_rt_info_update_ack_rt_time(&g_SMControllerRTInfo, pPH->vehicle_id_src, uDeltaTime); + controller_rt_info_update_ack_rt_time(&g_SMControllerRTInfo, pPH->vehicle_id_src, g_SM_RadioStats.radio_interfaces[interfaceNb].assignedLocalRadioLinkId, uDeltaTime); } } } @@ -898,38 +939,21 @@ int ProcessorRxVideo::handleReceivedVideoPacket(int interfaceNb, u8* pBuffer, in u8* pVideoSource = pVideoPacket->pVideoData; if ( pVideoPacket->pPHVF->uVideoStatusFlags2 & VIDEO_STATUS_FLAGS2_HAS_DEBUG_TIMESTAMPS ) { - t_packet_header_video_full_98_debug_info* pPHVFDebugInfo = (t_packet_header_video_full_98_debug_info*)pVideoSource; + //t_packet_header_video_full_98_debug_info* pPHVFDebugInfo = (t_packet_header_video_full_98_debug_info*)pVideoSource; //log_line("DEBUG output skip debug info for [%u/%u], CRC %u", pVideoPacket->pPHVF->uCurrentBlockIndex, pVideoPacket->pPHVF->uCurrentBlockPacketIndex, pPHVFDebugInfo->uVideoCRC); pVideoSource += sizeof(t_packet_header_video_full_98_debug_info); } u16 uVideoSize = 0; memcpy(&uVideoSize, pVideoSource, sizeof(u16)); - u32 crc = base_compute_crc32(pVideoSource, pVideoPacket->pPHVF->uCurrentBlockPacketSize); + //u32 crc = base_compute_crc32(pVideoSource, pVideoPacket->pPHVF->uCurrentBlockPacketSize); //log_line("DEBUG output [%u/%u] %d bytes, block size %d, packet length: %d, CRC %u", // pVideoPacket->pPHVF->uCurrentBlockIndex, pVideoPacket->pPHVF->uCurrentBlockPacketIndex, // uVideoSize, pVideoPacket->pPHVF->uCurrentBlockPacketSize, pVideoPacket->pPH->total_length, crc); pVideoSource += sizeof(u16); - int iVideoWidth = 0; - int iVideoHeight = 0; - if ( m_iIndexVideoDecodeStats != -1 ) - { - if ( (0 != g_SM_VideoDecodeStats.video_streams[m_iIndexVideoDecodeStats].iCurrentVideoWidth) && (0 != g_SM_VideoDecodeStats.video_streams[m_iIndexVideoDecodeStats].iCurrentVideoHeight) ) - { - iVideoWidth = g_SM_VideoDecodeStats.video_streams[m_iIndexVideoDecodeStats].iCurrentVideoWidth; - iVideoHeight = g_SM_VideoDecodeStats.video_streams[m_iIndexVideoDecodeStats].iCurrentVideoHeight; - } - } - else - { - Model* pModel = findModelWithId(m_uVehicleId, 177); - if ( NULL != pModel ) - { - iVideoWidth = pModel->video_link_profiles[pModel->video_params.user_selected_video_link_profile].width; - iVideoHeight = pModel->video_link_profiles[pModel->video_params.user_selected_video_link_profile].height; - } - } + int iVideoWidth = getVideoWidth(); + int iVideoHeight = getVideoHeight(); rx_video_output_video_data(m_uVehicleId, (pVideoPacket->pPHVF->uVideoStreamIndexAndType >> 4) & 0x0F , iVideoWidth, iVideoHeight, pVideoSource, uVideoSize, pVideoPacket->pPH->total_length); @@ -984,7 +1008,6 @@ int ProcessorRxVideo::handleReceivedVideoPacket(int interfaceNb, u8* pBuffer, in u32 video_block_index = pPHVF->video_block_index; u8 video_block_packet_index = pPHVF->video_block_packet_index; - m_uTimeLastReceivedVideoPacket = g_TimeNow; checkAndDiscardBlocksTooOld(); @@ -1165,7 +1188,7 @@ void ProcessorRxVideo::checkAndRequestMissingPackets() type_rx_video_block_info* pVideoBlockFirst = m_pVideoRxBuffer->getFirstVideoBlockInBuffer(); type_rx_video_block_info* pVideoBlockLast = m_pVideoRxBuffer->getVideoBlockInBuffer(iCountBlocks-1); if ( (NULL != pVideoBlockFirst) && (NULL != pVideoBlockLast) ) - if ( pVideoBlockLast->uReceivedTime - pVideoBlockFirst->uReceivedTime > m_iMilisecondsMaxRetransmissionWindow ) + if ( (int)pVideoBlockLast->uReceivedTime - (int)pVideoBlockFirst->uReceivedTime > m_iMilisecondsMaxRetransmissionWindow ) { m_pVideoRxBuffer->emptyBuffers("No new video past retransmission window"); //resetReceiveBuffers(m_iRXBlocksStackTopIndex); @@ -1309,8 +1332,7 @@ void ProcessorRxVideo::checkAndRequestMissingPackets() m_uLastTimeRequestedRetransmission = g_TimeNow; - type_rx_video_block_info* pVideoBlockFirst = m_pVideoRxBuffer->getFirstVideoBlockInBuffer(); - + //type_rx_video_block_info* pVideoBlockFirst = m_pVideoRxBuffer->getFirstVideoBlockInBuffer(); //log_line("DEBUG sent retr %u, %d (%s) (video min/max: %u - %u)", // m_uRequestRetransmissionUniqueId, iCountPacketsRequested, szDebug, pVideoBlockFirst->uVideoBlockIndex, m_pVideoRxBuffer->getMaxReceivedVideoBlockIndex()); @@ -1318,7 +1340,7 @@ void ProcessorRxVideo::checkAndRequestMissingPackets() if ( NULL != pRTInfo ) pRTInfo->uCountReqRetransmissions[g_SMControllerRTInfo.iCurrentIndex]++; - packets_queue_add_packet(&s_QueueRadioPackets, packet); + packets_queue_add_packet(&s_QueueRadioPacketsHighPrio, packet); return; @@ -1709,8 +1731,8 @@ int ProcessorRxVideo::processRetransmittedVideoPacket(u8* pBuffer, int length) //if ( -1 == m_iRXBlocksStackTopIndex ) // return -1; - u32 video_block_index = 0; - u8 video_block_packet_index = 0; + //u32 video_block_index = 0; + //u8 video_block_packet_index = 0; // To fix /* diff --git a/code/r_station/processor_rx_video.h b/code/r_station/processor_rx_video.h index 020b8200..2acf3fae 100644 --- a/code/r_station/processor_rx_video.h +++ b/code/r_station/processor_rx_video.h @@ -81,7 +81,7 @@ class ProcessorRxVideo void resumeProcessing(); u32 getLastTimeVideoStreamChanged(); - u32 getLastTimeReceivedVideoPacket(); + u32 getLastestVideoPacketReceiveTime(); int getCurrentlyReceivedVideoProfile(); int getCurrentlyReceivedVideoFPS(); int getCurrentlyReceivedVideoKeyframe(); @@ -102,7 +102,8 @@ class ProcessorRxVideo u32 m_uVehicleId; u8 m_uVideoStreamIndex; int m_iIndexVideoDecodeStats; - u32 m_uTimeLastReceivedVideoPacket; + u32 m_uLatestVideoPacketReceiveTime; + VideoRxPacketsBuffer* m_pVideoRxBuffer; protected: void resetReceiveState(); @@ -131,7 +132,6 @@ class ProcessorRxVideo bool m_bInitialized; int m_iInstanceIndex; bool m_bPaused; - VideoRxPacketsBuffer* m_pVideoRxBuffer; // Configuration @@ -170,4 +170,3 @@ class ProcessorRxVideo u32 m_uLastBlockReceivedSetVideoBitrate; u32 m_uLastBlockReceivedEncodingExtraFlags2; }; - diff --git a/code/r_station/ruby_rt_station.cpp b/code/r_station/ruby_rt_station.cpp index d9816244..1bfb0817 100644 --- a/code/r_station/ruby_rt_station.cpp +++ b/code/r_station/ruby_rt_station.cpp @@ -89,7 +89,8 @@ u8 s_BufferRCUplink[MAX_PACKET_TOTAL_SIZE]; u8 s_PipeBufferRCUplink[MAX_PACKET_TOTAL_SIZE]; int s_PipeBufferRCUplinkPos = 0; -t_packet_queue s_QueueRadioPackets; +t_packet_queue s_QueueRadioPacketsHighPrio; +t_packet_queue s_QueueRadioPacketsRegPrio; t_packet_queue s_QueueControlPackets; u32 s_debugLastFPSTime = 0; @@ -104,9 +105,6 @@ int s_iSearchSikMCSTR = -1; u32 s_uTimeLastTryReadIPCMessages = 0; -#define MAX_RADIO_PACKETS_TO_CACHE_LOCALLY 100 -type_received_radio_packet s_ReceivedRadioPacketsBuffer[MAX_RADIO_PACKETS_TO_CACHE_LOCALLY]; - void _broadcast_radio_interface_init_failed(int iInterfaceIndex) { t_packet_header PH; @@ -916,7 +914,7 @@ int _must_inject_ping_now() // returns true if it added a packet to the radio queue -bool _check_send_or_queue_ping() +bool _check_queue_ping() { if ( (NULL == g_pCurrentModel) || g_bSearching ) return false; @@ -994,15 +992,12 @@ bool _check_send_or_queue_ping() for( int i=0; i0; k-- ) + if ( g_State.vehiclesRuntimeInfo[i].uVehicleId == uDestinationVehicleId ) { - g_State.vehiclesRuntimeInfo[i].uLastPingIdSentToVehicleOnLocalRadioLinks[s_uPingToSendLocalRadioLinkId][k] = g_State.vehiclesRuntimeInfo[i].uLastPingIdSentToVehicleOnLocalRadioLinks[s_uPingToSendLocalRadioLinkId][k-1]; - g_State.vehiclesRuntimeInfo[i].uTimeLastPingSentToVehicleOnLocalRadioLinks[s_uPingToSendLocalRadioLinkId][k] = g_State.vehiclesRuntimeInfo[i].uTimeLastPingSentToVehicleOnLocalRadioLinks[s_uPingToSendLocalRadioLinkId][k-1]; + g_State.vehiclesRuntimeInfo[i].uLastPingIdSentToVehicleOnLocalRadioLinks[s_uPingToSendLocalRadioLinkId] = s_uPingToSendId; + g_State.vehiclesRuntimeInfo[i].uTimeLastPingInitiatedToVehicleOnLocalRadioLinks[s_uPingToSendLocalRadioLinkId] = get_current_timestamp_ms(); + break; } - g_State.vehiclesRuntimeInfo[i].uLastPingIdSentToVehicleOnLocalRadioLinks[s_uPingToSendLocalRadioLinkId][0] = s_uPingToSendId; - g_State.vehiclesRuntimeInfo[i].uTimeLastPingSentToVehicleOnLocalRadioLinks[s_uPingToSendLocalRadioLinkId][0] = get_current_timestamp_micros(); } t_packet_header PH; @@ -1043,226 +1038,37 @@ bool _check_send_or_queue_ping() } #endif - packets_queue_add_packet(&s_QueueRadioPackets, packet); + packets_queue_add_packet(&s_QueueRadioPacketsRegPrio, packet); if ( g_bDebugIsPacketsHistoryGraphOn && (!g_bDebugIsPacketsHistoryGraphPaused) ) add_detailed_history_tx_packets(g_pDebug_SM_RouterPacketsStatsHistory, g_TimeNow % 1000, 0, 0, 1, 0, 0, 0); return true; } -void _process_and_send_packets_individually() +void _preprocess_radio_out_packet(u8* pPacketBuffer) { - Preferences* pP = get_Preferences(); - int maxLengthAllowedInRadioPacket = pP->iDebugMaxPacketSize; - if ( maxLengthAllowedInRadioPacket > MAX_VIDEO_PACKET_DATA_SIZE ) - maxLengthAllowedInRadioPacket = MAX_VIDEO_PACKET_DATA_SIZE; - - while ( packets_queue_has_packets(&s_QueueRadioPackets) ) - { - if ( NULL != g_pProcessStats ) - g_pProcessStats->lastIPCIncomingTime = g_TimeNow; - - int iPacketLength = -1; - u8* pPacketBuffer = packets_queue_pop_packet(&s_QueueRadioPackets, &iPacketLength); - if ( NULL == pPacketBuffer || -1 == iPacketLength ) - break; - - _check_for_atheros_datarate_change_command_to_vehicle(pPacketBuffer); - - t_packet_header* pPH = (t_packet_header*)pPacketBuffer; - pPH->vehicle_id_src = g_uControllerId; - - if ( (pPH->packet_flags & PACKET_FLAGS_MASK_MODULE) == PACKET_COMPONENT_COMMANDS ) - { - t_packet_header_command* pCom = (t_packet_header_command*)(pPacketBuffer + sizeof(t_packet_header)); - type_global_state_vehicle_runtime_info* pRuntimeInfo = getVehicleRuntimeInfo(pPH->vehicle_id_dest); - if ( NULL != pRuntimeInfo ) - { - pRuntimeInfo->uTimeLastCommandIdSent = g_TimeNow; - pRuntimeInfo->uLastCommandIdSent = pCom->command_counter; - pRuntimeInfo->uLastCommandIdRetrySent = pCom->command_resend_counter; - } - if ( pPH->packet_type == PACKET_TYPE_COMMAND ) - { - t_packet_header_command* pPHC = (t_packet_header_command*)(pPacketBuffer + sizeof(t_packet_header)); - if ( (pPHC->command_type & COMMAND_TYPE_MASK) == COMMAND_ID_SET_CAMERA_PARAMETERS ) - { - type_camera_parameters* pPHCamP = (type_camera_parameters*)(pPacketBuffer + sizeof(t_packet_header)+sizeof(t_packet_header_command)); - - int iProfile = pPHCamP->iCurrentProfile; - - log_line("Sending cam params: br: %d, co: %d, sa: %d, sh: %d, hue: %d, exp: %d, shutter: %d, drc: %d", - (int)pPHCamP->profiles[iProfile].brightness, - (int)pPHCamP->profiles[iProfile].contrast, - (int)pPHCamP->profiles[iProfile].saturation, - (int)pPHCamP->profiles[iProfile].sharpness, - (int)pPHCamP->profiles[iProfile].hue, - (int)pPHCamP->profiles[iProfile].exposure, - (int)pPHCamP->profiles[iProfile].shutterspeed, - (int)pPHCamP->profiles[iProfile].drc ); - } - } - } - - int send_count = 1; - - if ( (pPH->packet_flags & PACKET_FLAGS_MASK_MODULE) == PACKET_COMPONENT_COMMANDS ) - if ( pPH->packet_type == COMMAND_ID_SET_RADIO_LINK_FREQUENCY ) - send_count = 10; + if ( NULL == pPacketBuffer ) + return; - if ( (pPH->packet_type == PACKET_TYPE_VEHICLE_RECORDING) ) - send_count = 5; - + t_packet_header* pPH = (t_packet_header*)pPacketBuffer; - for( int i=0; iis_spectator)) ) + if ( pPH->packet_type == PACKET_TYPE_SIK_CONFIG ) { - // Empty queue - packets_queue_init(&s_QueueRadioPackets); - return; + u8 uVehicleLinkId = *(pPacketBuffer + sizeof(t_packet_header)); + u8 uCommandId = *(pPacketBuffer + sizeof(t_packet_header) + sizeof(u8)); + log_line("Received message to send to vehicle to configure SiK vehicle radio link %d, command: %d", (int) uVehicleLinkId+1, (int)uCommandId); } - int iCountPendingPackets = packets_queue_has_packets(&s_QueueRadioPackets); - if ( 0 == iCountPendingPackets ) - return; - - #ifndef FEATURE_CONCATENATE_SMALL_RADIO_PACKETS - _process_and_send_packets_individually(); - return; - #endif - - Preferences* pP = get_Preferences(); - - int maxLengthAllowedInRadioPacket = pP->iDebugMaxPacketSize; - if ( maxLengthAllowedInRadioPacket > MAX_VIDEO_PACKET_DATA_SIZE ) - maxLengthAllowedInRadioPacket = MAX_VIDEO_PACKET_DATA_SIZE; - - u8 composed_packet[MAX_PACKET_TOTAL_SIZE]; - int composed_packet_length = 0; - int send_count = 1; - int countComm = 0; - int countRC = 0; - - // Send retransmissions first, if any - - - #ifdef FEATURE_CONCATENATE_SMALL_RADIO_PACKETS - u32 uDestinationVehicleIdLastPacket = 0; - u32 uLastPacketType = 0; - #endif - - // Send all the other outstanding packets to radio - - while ( packets_queue_has_packets(&s_QueueRadioPackets) ) + if ( (pPH->packet_flags & PACKET_FLAGS_MASK_MODULE) == PACKET_COMPONENT_COMMANDS ) { - if ( NULL != g_pProcessStats ) - g_pProcessStats->lastIPCIncomingTime = g_TimeNow; - - int iPacketLength = -1; - u8* pPacketBuffer = packets_queue_pop_packet(&s_QueueRadioPackets, &iPacketLength); - if ( NULL == pPacketBuffer || -1 == iPacketLength ) - break; - - _check_for_atheros_datarate_change_command_to_vehicle(pPacketBuffer); - - t_packet_header* pPH = (t_packet_header*)pPacketBuffer; - pPH->vehicle_id_src = g_uControllerId; - - - if ( (pPH->packet_flags & PACKET_FLAGS_MASK_MODULE) == PACKET_COMPONENT_COMMANDS ) - { - t_packet_header_command* pCom = (t_packet_header_command*)(pPacketBuffer + sizeof(t_packet_header)); - type_global_state_vehicle_runtime_info* pRuntimeInfo = getVehicleRuntimeInfo(pPH->vehicle_id_dest); - if ( NULL != pRuntimeInfo ) - { - pRuntimeInfo->uTimeLastCommandIdSent = g_TimeNow; - pRuntimeInfo->uLastCommandIdSent = pCom->command_counter; - pRuntimeInfo->uLastCommandIdRetrySent = pCom->command_resend_counter; - } - } - - #ifdef FEATURE_CONCATENATE_SMALL_RADIO_PACKETS - - bool bSendNow = false; - - if ( (composed_packet_length + iPacketLength > maxLengthAllowedInRadioPacket) ) - bSendNow = true; - - if ( g_bUpdateInProgress ) - bSendNow = true; - - if ( (pPH->packet_type == PACKET_TYPE_RUBY_PING_CLOCK) || (uLastPacketType == PACKET_TYPE_RUBY_PING_CLOCK) ) - bSendNow = true; - - if ( uDestinationVehicleIdLastPacket != pPH->vehicle_id_dest ) - if ( 0 != uDestinationVehicleIdLastPacket ) - bSendNow = true; - - uDestinationVehicleIdLastPacket = pPH->vehicle_id_dest; - - if ( bSendNow && (composed_packet_length > 0) ) + t_packet_header_command* pCom = (t_packet_header_command*)(pPacketBuffer + sizeof(t_packet_header)); + type_global_state_vehicle_runtime_info* pRuntimeInfo = getVehicleRuntimeInfo(pPH->vehicle_id_dest); + if ( NULL != pRuntimeInfo ) { - for( int i=0; iuTimeLastCommandIdSent = g_TimeNow; + pRuntimeInfo->uLastCommandIdSent = pCom->command_counter; + pRuntimeInfo->uLastCommandIdRetrySent = pCom->command_resend_counter; } - - if ( (pPH->packet_flags & PACKET_FLAGS_MASK_MODULE) == PACKET_COMPONENT_COMMANDS ) - if ( pPH->packet_type == COMMAND_ID_SET_RADIO_LINK_FREQUENCY ) - send_count = 10; - - if ( (pPH->packet_type == PACKET_TYPE_VEHICLE_RECORDING) ) - send_count = 5; - - if ( (pPH->packet_flags & PACKET_FLAGS_MASK_MODULE) == PACKET_COMPONENT_COMMANDS ) - countComm = 1; - if ( (pPH->packet_flags & PACKET_FLAGS_MASK_MODULE) == PACKET_COMPONENT_RC ) - countRC = 1; - - memcpy(&composed_packet[composed_packet_length], pPacketBuffer, iPacketLength); - composed_packet_length += iPacketLength; - uLastPacketType = pPH->packet_type; - #else - - send_count = 1; - countComm = 0; - countRC = 0; - - if ( (pPH->packet_flags & PACKET_FLAGS_MASK_MODULE) == PACKET_COMPONENT_COMMANDS ) - if ( pPH->packet_type == COMMAND_ID_SET_RADIO_LINK_FREQUENCY ) - send_count = 10; - - if ( (pPH->packet_type == PACKET_TYPE_VEHICLE_RECORDING) ) - send_count = 5; - - if ( (pPH->packet_flags & PACKET_FLAGS_MASK_MODULE) == PACKET_COMPONENT_COMMANDS ) - countComm = 1; - if ( (pPH->packet_flags & PACKET_FLAGS_MASK_MODULE) == PACKET_COMPONENT_RC ) - countRC = 1; - - if ( (pPH->packet_flags & PACKET_FLAGS_MASK_MODULE) == PACKET_COMPONENT_COMMANDS ) if ( pPH->packet_type == PACKET_TYPE_COMMAND ) { t_packet_header_command* pPHC = (t_packet_header_command*)(pPacketBuffer + sizeof(t_packet_header)); @@ -1283,49 +1089,84 @@ void _process_and_send_packets() (int)pPHCamP->profiles[iProfile].drc ); } } + } + if ( pPH->packet_type == PACKET_TYPE_NEGOCIATE_RADIO_LINKS ) + { + u8 uCommand = pPacketBuffer[sizeof(t_packet_header) + sizeof(u8)]; + if ( (uCommand == NEGOCIATE_RADIO_STEP_END) || (uCommand == NEGOCIATE_RADIO_STEP_CANCEL) ) + g_bNegociatingRadioLinks = false; + else + g_bNegociatingRadioLinks = true; + } +} - for( int i=0; ivehicle_id_src = g_uControllerId; - composed_packet_length = 0; - - #endif - } + _preprocess_radio_out_packet(pPacketBuffer); - if ( composed_packet_length > 0 ) + int send_count = 1; + + if ( (pPH->packet_flags & PACKET_FLAGS_MASK_MODULE) == PACKET_COMPONENT_COMMANDS ) + if ( pPH->packet_type == COMMAND_ID_SET_RADIO_LINK_FREQUENCY ) + send_count = 10; + + if ( (pPH->packet_type == PACKET_TYPE_VEHICLE_RECORDING) ) + send_count = 5; + + if ( (pPH->packet_flags & PACKET_FLAGS_MASK_MODULE) == PACKET_COMPONENT_RUBY ) + if ( pPH->packet_type == PACKET_TYPE_RUBY_PING_CLOCK ) { - for( int i=0; ivehicle_id_dest ) + { + u8 uLocalRadioLinkId = 0; + memcpy(&uLocalRadioLinkId, pPacketBuffer+sizeof(t_packet_header)+sizeof(u8), sizeof(u8)); + if ( uLocalRadioLinkId < MAX_RADIO_INTERFACES ) + { + g_State.vehiclesRuntimeInfo[i].uTimeLastPingSentToVehicleOnLocalRadioLinks[uLocalRadioLinkId] = g_TimeNow; + break; + } + } } } + + for( int i=0; iis_spectator)) ) + { + // Empty queue + packets_queue_init(pRadioQueue); + return; + } - t_packet_header* pPH = (t_packet_header*)pPacketBuffer; - - if ( pPH->packet_type == PACKET_TYPE_SIK_CONFIG ) + while ( packets_queue_has_packets(pRadioQueue) ) { - u8 uVehicleLinkId = *(pPacketBuffer + sizeof(t_packet_header)); - u8 uCommandId = *(pPacketBuffer + sizeof(t_packet_header) + sizeof(u8)); - log_line("Received message to send to vehicle to configure SiK vehicle radio link %d, command: %d", (int) uVehicleLinkId+1, (int)uCommandId); + if ( NULL != g_pProcessStats ) + g_pProcessStats->lastIPCIncomingTime = g_TimeNow; + + int iPacketLength = -1; + u8* pPacketBuffer = packets_queue_pop_packet(pRadioQueue, &iPacketLength); + if ( NULL == pPacketBuffer || -1 == iPacketLength ) + break; + + _process_and_send_packet(pPacketBuffer, iPacketLength); } } @@ -1344,10 +1185,7 @@ void _read_ipc_pipes(u32 uTimeNow) if ( (pPH->packet_flags & PACKET_FLAGS_MASK_MODULE) == PACKET_COMPONENT_LOCAL_CONTROL ) packets_queue_add_packet(&s_QueueControlPackets, s_BufferCommands); else - { - _preprocess_radio_out_packet(s_BufferCommands); - packets_queue_add_packet(&s_QueueRadioPackets, s_BufferCommands); - } + packets_queue_add_packet(&s_QueueRadioPacketsRegPrio, s_BufferCommands); } if ( maxToRead - maxPacketsToRead > 6 ) log_line("Read %d messages from central msgqueue.", maxToRead - maxPacketsToRead); @@ -1363,8 +1201,7 @@ void _read_ipc_pipes(u32 uTimeNow) { if ( ! isPairingDoneWithVehicle(pPH->vehicle_id_dest) ) continue; - _preprocess_radio_out_packet(s_BufferMessageFromTelemetry); - packets_queue_add_packet(&s_QueueRadioPackets, s_BufferMessageFromTelemetry); + packets_queue_add_packet(&s_QueueRadioPacketsRegPrio, s_BufferMessageFromTelemetry); } } if ( maxToRead - maxPacketsToRead > 6 ) @@ -1381,8 +1218,7 @@ void _read_ipc_pipes(u32 uTimeNow) { if ( ! isPairingDoneWithVehicle(pPH->vehicle_id_dest) ) continue; - _preprocess_radio_out_packet(s_BufferRCUplink); - packets_queue_add_packet(&s_QueueRadioPackets, s_BufferRCUplink); + packets_queue_add_packet(&s_QueueRadioPacketsRegPrio, s_BufferRCUplink); } } if ( maxToRead - maxPacketsToRead > 6 ) @@ -1661,79 +1497,6 @@ void _consume_ipc_messages() log_softerror_and_alarm("Consuming %d IPC messages took too long: %d ms", 20 - iMaxToConsume, uTime - uTimeStart); } -int _consume_radio_rx_packets() -{ - /* - static int s_iCountFrames = 0; - static u32 s_uLastCountFrames = 0; - - s_iCountFrames++; - if ( g_TimeNow >= s_uLastCountFrames + 1000 ) - { - s_uLastCountFrames = g_TimeNow; - log_line("DBG fps: %d", s_iCountFrames); - s_iCountFrames = 0; - } - */ - - int iReceivedAnyPackets = radio_rx_has_packets_to_consume(); - if ( iReceivedAnyPackets <= 0 ) - return 0; - - if ( iReceivedAnyPackets > g_SM_RadioRxQueueInfo.uPendingRxPackets[g_SM_RadioRxQueueInfo.uCurrentIndex] ) - { - if ( iReceivedAnyPackets < 255 ) - g_SM_RadioRxQueueInfo.uPendingRxPackets[g_SM_RadioRxQueueInfo.uCurrentIndex] = iReceivedAnyPackets; - else - g_SM_RadioRxQueueInfo.uPendingRxPackets[g_SM_RadioRxQueueInfo.uCurrentIndex] = 254; - } - - if ( iReceivedAnyPackets > MAX_RADIO_PACKETS_TO_CACHE_LOCALLY-5 ) - iReceivedAnyPackets = MAX_RADIO_PACKETS_TO_CACHE_LOCALLY-5; - - u32 uTimeStart = get_current_timestamp_ms(); - - // To fix remove - for( int i=0; i uTimeStart + 500 ) - { - log_softerror_and_alarm("Consuming radio rx packets takes too long (%u ms), read ipc messages.", g_TimeNow - uTimeStart); - uTimeStart = g_TimeNow; - _read_ipc_pipes(g_TimeNow); - } - if ( (0 != s_uTimeLastTryReadIPCMessages) && (g_TimeNow > s_uTimeLastTryReadIPCMessages + 500) ) - { - log_softerror_and_alarm("Too much time since last ipc messages read (%u ms) while consuming radio messages, read ipc messages.", g_TimeNow - s_uTimeLastTryReadIPCMessages); - uTimeStart = g_TimeNow; - _read_ipc_pipes(g_TimeNow); - } - } - - g_SMControllerRTInfo.uRxProcessedPackets[g_SMControllerRTInfo.iCurrentIndex] += iCount; - return iCount; -} - void _check_send_pairing_requests() { if ( ! g_bFirstModelPairingDone ) @@ -2039,7 +1802,56 @@ void _synchronize_shared_mems() } } -void _main_loop(); +int _try_read_consume_high_priority_packets(int iCountMax, u32 uTimeoutMicrosec) +{ + int iCountConsumedHighPrio = 0; + + int iPacketLength = 0; + int iPacketIsShort = 0; + int iRadioInterfaceIndex = 0; + u8* pPacket = NULL; + + while ( (iCountConsumedHighPrio < iCountMax) && (!g_bQuit) ) + { + pPacket = radio_rx_wait_get_next_received_high_prio_packet(uTimeoutMicrosec, &iPacketLength, &iPacketIsShort, &iRadioInterfaceIndex); + if ( (NULL == pPacket) || g_bQuit ) + break; + + iCountConsumedHighPrio++; + + process_received_single_radio_packet(iRadioInterfaceIndex, pPacket, iPacketLength); + shared_mem_radio_stats_rx_hist_update(&g_SM_HistoryRxStats, iRadioInterfaceIndex, pPacket, g_TimeNow); + g_SMControllerRTInfo.uRxProcessedPackets[g_SMControllerRTInfo.iCurrentIndex]++; + } + return iCountConsumedHighPrio; +} + +int _try_read_consume_reg_priority_packets(int iCountMax, u32 uTimeoutMicrosec) +{ + int iCountConsumedRegPrio = 0; + + int iPacketLength = 0; + int iPacketIsShort = 0; + int iRadioInterfaceIndex = 0; + u8* pPacket = NULL; + + while ( (iCountConsumedRegPrio < iCountMax) && (!g_bQuit) ) + { + pPacket = radio_rx_wait_get_next_received_reg_prio_packet(uTimeoutMicrosec, &iPacketLength, &iPacketIsShort, &iRadioInterfaceIndex); + if ( (NULL == pPacket) || g_bQuit ) + break; + + iCountConsumedRegPrio++; + + process_received_single_radio_packet(iRadioInterfaceIndex, pPacket, iPacketLength); + shared_mem_radio_stats_rx_hist_update(&g_SM_HistoryRxStats, iRadioInterfaceIndex, pPacket, g_TimeNow); + g_SMControllerRTInfo.uRxProcessedPackets[g_SMControllerRTInfo.iCurrentIndex]++; + } + return iCountConsumedRegPrio; +} + +void _main_loop_no_sync(); +void _main_loop_basic_sync(); void handle_sigint(int sig) { @@ -2240,7 +2052,8 @@ int main(int argc, char *argv[]) radio_links_open_rxtx_radio_interfaces(); } - packets_queue_init(&s_QueueRadioPackets); + packets_queue_init(&s_QueueRadioPacketsHighPrio); + packets_queue_init(&s_QueueRadioPacketsRegPrio); packets_queue_init(&s_QueueControlPackets); log_line("IPC Queues Init Complete."); @@ -2294,12 +2107,6 @@ int main(int argc, char *argv[]) send_alarm_to_central(ALARM_ID_FIRMWARE_OLD, i, 0); } - for( int i=0; iuLoopCounter++; g_pProcessStats->lastActiveTime = g_TimeNow; } - _main_loop(); + + if ( g_bSearching ) + _main_loop_no_sync(); + else if ( g_pCurrentModel->rxtx_sync_type == RXTX_SYNC_TYPE_BASIC ) + _main_loop_basic_sync(); + else + _main_loop_no_sync(); if ( g_bQuit ) break; } @@ -2427,72 +2240,74 @@ void video_processors_cleanup() rx_video_output_uninit(); } -void _main_loop() +void _main_loop_no_sync() { static u32 uMaxLoopTime = DEFAULT_MAX_LOOP_TIME_MILISECONDS; - //hardware_sleep_ms(1); - //hardware_sleep_micros(300); - g_TimeNow = get_current_timestamp_ms(); - g_TimeNowMicros = get_current_timestamp_micros(); u32 tTime0 = g_TimeNow; + u32 uTimeStart = g_TimeNow; + u32 tTime1 = 0; + while ( g_TimeNow < uTimeStart + 2 ) + { + //--------------------------------------------- + // Check and process retransmissions received and pings received and other high priority radio messages + _try_read_consume_high_priority_packets(10, 100); + + //------------------------------------------ + // Process all the other radio-in packets + int iMaxCountRegPrio = 50; + int iWaitMicroSec = 0; + while ( (iMaxCountRegPrio > 0) && (!g_bQuit) ) + { + int iConsumedReg = _try_read_consume_reg_priority_packets(5, iWaitMicroSec); + _try_read_consume_high_priority_packets(1, 0); + if ( iConsumedReg < 5 ) + break; + if ( g_bQuit ) + break; - if ( (g_pProcessStats->uLoopCounter % 10) == 0 ) - { - _router_periodic_loop(); - _synchronize_shared_mems(); - _check_rx_loop_consistency(); - _check_send_or_queue_ping(); - } + iMaxCountRegPrio -= iConsumedReg; + } - u32 tTime1 = get_current_timestamp_ms(); + g_TimeNow = get_current_timestamp_ms(); + tTime1 = g_TimeNow; + } - if ( (g_pProcessStats->uLoopCounter % 10) == 0 ) + if ( g_bSearching ) { + _synchronize_shared_mems(); _read_ipc_pipes(tTime1); _consume_ipc_messages(); - } - u32 tTime2 = get_current_timestamp_ms(); - - int iCounter = 4; - int iRxPackets = 0; - while ( iCounter > 0 ) - { - iCounter--; - int k = _consume_radio_rx_packets(); - if ( k <= 0 ) - break; - iRxPackets += k; + if ( g_TimeNow > tTime0 + uMaxLoopTime ) + log_softerror_and_alarm("Router loop took too long to complete (%d milisec)!!!", g_TimeNow - tTime0); + else + s_iCountCPULoopOverflows = 0; + return; } - if ( iRxPackets == 0 ) - hardware_sleep_ms(1); - u32 tTime3 = get_current_timestamp_ms(); - - int nEndOfVideoBlock = 0; - /* - for( int i=0; i<6; i++ ) + for( int i=0; i 0 ) - nEndOfVideoBlock |= process_received_radio_packets(); - else + if ( g_pVideoProcessorRxList[i] == NULL ) break; - receivedAny = try_receive_radio_packets(200); - } - */ - u32 tTime4 = get_current_timestamp_ms(); - - if ( g_bSearching ) - { - u32 tNow = get_current_timestamp_ms(); - if ( tNow > g_TimeNow + uMaxLoopTime ) - log_softerror_and_alarm("Router loop took too long to complete (%d milisec)!!!", tNow - g_TimeNow); - else - s_iCountCPULoopOverflows = 0; - return; + g_pVideoProcessorRxList[i]->periodicLoop(g_TimeNow); } + if ( controller_rt_info_will_advance_index(&g_SMControllerRTInfo, g_TimeNow) ) + adaptive_video_periodic_loop(); + + _router_periodic_loop(); + _synchronize_shared_mems(); + _check_rx_loop_consistency(); + _check_queue_ping(); + _read_ipc_pipes(tTime1); + _consume_ipc_messages(); + + if ( (NULL != g_pCurrentModel) && g_pCurrentModel->hasCamera() ) + rx_video_output_periodic_loop(); + + g_TimeNow = get_current_timestamp_ms(); + u32 tTime2 = g_TimeNow; if ( controller_rt_info_will_advance_index(&g_SMControllerRTInfo, g_TimeNow) ) { @@ -2511,81 +2326,60 @@ void _main_loop() } radio_stats_reset_signal_info_for_card(&g_SM_RadioStats, i); } - - for( int i=0; iperiodicLoop(g_TimeNow); - } - adaptive_video_periodic_loop(); - } - - if ( (!g_bSearching) && (NULL != g_pCurrentModel) && g_pCurrentModel->hasCamera() ) - { - rx_video_output_periodic_loop(); - // To fix video_link_adaptive_periodic_loop(); - // To fix video_link_keyframe_periodic_loop(); } - u32 tTime5 = get_current_timestamp_ms(); - - int iCountHighPriorityPackets = 0; - for( int i=0; ipacket_type) ) - iCountHighPriorityPackets++; - - // To fix, record to new structures - /* - if ( (pPH->packet_flags & PACKET_FLAGS_MASK_MODULE) == PACKET_COMPONENT_VIDEO ) - if ( pPH->packet_type == PACKET_TYPE_VIDEO_SWITCH_TO_ADAPTIVE_VIDEO_LEVEL ) - { - u32 uLevel = 0; - memcpy((u8*)&uLevel, pData + sizeof(t_packet_header), sizeof(u32)); - - int iIndex = getVehicleRuntimeIndex(pPH->vehicle_id_dest); - if ( -1 != iIndex ) - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iIndex].iCurrentTargetLevelShift = (int) uLevel; - } - */ - } + g_TimeNow = get_current_timestamp_ms(); + u32 tTime3 = g_TimeNow; bool bSendNow = false; if ( !g_pCurrentModel->hasCamera()) bSendNow = true; - if ( g_pCurrentModel->rxtx_sync_type == RXTX_SYNC_TYPE_NONE ) + if ( g_bUpdateInProgress ) bSendNow = true; - if ( g_bUpdateInProgress || nEndOfVideoBlock ) - bSendNow = true; - if ( s_QueueRadioPackets.timeFirstPacket + 100 < g_TimeNow ) + if ( g_pCurrentModel->rxtx_sync_type == RXTX_SYNC_TYPE_NONE ) bSendNow = true; - if ( iCountHighPriorityPackets > 0 ) + if ( g_TimeNow > s_QueueRadioPacketsRegPrio.timeFirstPacket + 55 ) bSendNow = true; + if ( g_pCurrentModel->rxtx_sync_type != RXTX_SYNC_TYPE_NONE ) + { + ProcessorRxVideo* pProcessorRxVideo = ProcessorRxVideo::getVideoProcessorForVehicleId(g_pCurrentModel->uVehicleId, 0); + if ( NULL != pProcessorRxVideo ) + if ( NULL != pProcessorRxVideo->m_pVideoRxBuffer ) + if ( pProcessorRxVideo->m_pVideoRxBuffer->isFrameEnded() ) + { + u32 uFrameDuration = 1000 / g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].fps; + if ( pProcessorRxVideo->getLastestVideoPacketReceiveTime() >= g_TimeNow - uFrameDuration ) + { + if ( g_TimeNow < pProcessorRxVideo->m_pVideoRxBuffer->getLastFrameEndTime() + uFrameDuration - 5 ) + bSendNow = true; + } + else if ( g_TimeNow > pProcessorRxVideo->getLastestVideoPacketReceiveTime() + 2 ) + bSendNow = true; + } + } + if ( bSendNow ) - _process_and_send_packets(); + { + _process_and_send_packets_individually(&s_QueueRadioPacketsHighPrio); + _process_and_send_packets_individually(&s_QueueRadioPacketsRegPrio); + } - u32 tTime6 = get_current_timestamp_ms(); - if ( (g_TimeNow > g_TimeStart + 10000) && (tTime6 > tTime0 + uMaxLoopTime) ) + g_TimeNow = get_current_timestamp_ms(); + u32 tTime4 = g_TimeNow; + if ( (g_TimeNow > g_TimeStart + 10000) && (tTime4 > tTime0 + uMaxLoopTime) ) { - log_softerror_and_alarm("Router loop took too long to complete (%d milisec: %u + %u + %u + %u + %u + %u), repeat count: %u!!!", tTime6 - tTime0, tTime1-tTime0, tTime2-tTime1, tTime3-tTime2, tTime4-tTime3, tTime5-tTime4, tTime6-tTime5, s_iCountCPULoopOverflows+1); + log_softerror_and_alarm("Router loop took too long to complete (%d milisec: %u + %u + %u + %u), repeat count: %u!!!", tTime4 - tTime0, tTime1-tTime0, tTime2-tTime1, tTime3-tTime2, tTime4-tTime3, s_iCountCPULoopOverflows+1); s_iCountCPULoopOverflows++; if ( s_iCountCPULoopOverflows > 5 ) if ( g_TimeNow > g_TimeLastSetRadioFlagsCommandSent + 5000 ) - send_alarm_to_central(ALARM_ID_CONTROLLER_CPU_LOOP_OVERLOAD,(tTime6-tTime0), 0); + send_alarm_to_central(ALARM_ID_CONTROLLER_CPU_LOOP_OVERLOAD,(tTime4-tTime0), 0); - if ( tTime6 >= tTime0 + 300 ) + if ( tTime4 >= tTime0 + 300 ) if ( g_TimeNow > g_TimeLastSetRadioFlagsCommandSent + 5000 ) - send_alarm_to_central(ALARM_ID_CONTROLLER_CPU_LOOP_OVERLOAD,(tTime6-tTime0)<<16, 0); + send_alarm_to_central(ALARM_ID_CONTROLLER_CPU_LOOP_OVERLOAD,(tTime4-tTime0)<<16, 0); } else { @@ -2593,15 +2387,22 @@ void _main_loop() } if ( controller_rt_info_check_advance_index(&g_SMControllerRTInfo, g_TimeNow) ) - radio_rx_set_packet_counter_output(&(g_SMControllerRTInfo.uRxVideoPackets[g_SMControllerRTInfo.iCurrentIndex][0]), &(g_SMControllerRTInfo.uRxDataPackets[g_SMControllerRTInfo.iCurrentIndex][0]), &(g_SMControllerRTInfo.uRxMissingPackets[g_SMControllerRTInfo.iCurrentIndex][0]), &(g_SMControllerRTInfo.uRxMissingPacketsMaxGap[g_SMControllerRTInfo.iCurrentIndex][0])); + radio_rx_set_packet_counter_output(&(g_SMControllerRTInfo.uRxVideoPackets[g_SMControllerRTInfo.iCurrentIndex][0]), + &(g_SMControllerRTInfo.uRxVideoECPackets[g_SMControllerRTInfo.iCurrentIndex][0]), + &(g_SMControllerRTInfo.uRxVideoRetrPackets[g_SMControllerRTInfo.iCurrentIndex][0]), + &(g_SMControllerRTInfo.uRxDataPackets[g_SMControllerRTInfo.iCurrentIndex][0]), &(g_SMControllerRTInfo.uRxMissingPackets[g_SMControllerRTInfo.iCurrentIndex][0]), &(g_SMControllerRTInfo.uRxMissingPacketsMaxGap[g_SMControllerRTInfo.iCurrentIndex][0])); if ( NULL != g_pProcessStats ) { - if ( g_pProcessStats->uMaxLoopTimeMs < tTime6 - tTime0 ) - g_pProcessStats->uMaxLoopTimeMs = tTime6 - tTime0; - g_pProcessStats->uTotalLoopTime += tTime6 - tTime0; + if ( g_pProcessStats->uMaxLoopTimeMs < tTime4 - tTime0 ) + g_pProcessStats->uMaxLoopTimeMs = tTime4 - tTime0; + g_pProcessStats->uTotalLoopTime += tTime4 - tTime0; if ( 0 != g_pProcessStats->uLoopCounter ) g_pProcessStats->uAverageLoopTimeMs = g_pProcessStats->uTotalLoopTime / g_pProcessStats->uLoopCounter; } +} +void _main_loop_basic_sync() +{ + _main_loop_no_sync(); } \ No newline at end of file diff --git a/code/r_station/rx_video_output.cpp b/code/r_station/rx_video_output.cpp index 95c4398b..e5e6c76c 100644 --- a/code/r_station/rx_video_output.cpp +++ b/code/r_station/rx_video_output.cpp @@ -149,10 +149,10 @@ void _rx_video_output_launch_video_player() strcpy(s_szOutputVideoPlayerFilename, VIDEO_PLAYER_PIPE); log_line("[VideoOutput] Starting video player [%s]", s_szOutputVideoPlayerFilename); - - if ( hw_process_exists(s_szOutputVideoPlayerFilename) ) + int iPID = hw_process_exists(s_szOutputVideoPlayerFilename); + if ( iPID > 0 ) { - log_line("[VideoOutput] Video player process already running. Do nothing."); + log_line("[VideoOutput] Video player process already running (PID %d). Do nothing.", iPID); return; } @@ -206,9 +206,10 @@ void _rx_video_output_launch_video_player() strcpy(s_szOutputVideoPlayerFilename, VIDEO_PLAYER_UDP); log_line("[VideoOutput] Starting video player [%s]", s_szOutputVideoPlayerFilename); - if ( hw_process_exists(s_szOutputVideoPlayerFilename) ) + int iPID = hw_process_exists(s_szOutputVideoPlayerFilename); + if ( iPID > 0 ) { - log_line("[VideoOutput] Video player process already running. Do nothing."); + log_line("[VideoOutput] Video player process already running (PID: %d). Do nothing.", iPID); return; } @@ -540,6 +541,21 @@ void rx_video_output_uninit() void rx_video_output_enable_pipe_output() { + #if defined(HW_PLATFORM_RASPBERRY) + strcpy(s_szOutputVideoPlayerFilename, VIDEO_PLAYER_PIPE); + #endif + #if defined(HW_PLATFORM_RADXA_ZERO3) + strcpy(s_szOutputVideoPlayerFilename, VIDEO_PLAYER_UDP); + #endif + + hardware_sleep_ms(100); + int iPID = hw_process_exists(s_szOutputVideoPlayerFilename); + if ( iPID < 10 ) + { + log_line("[VideoOutput] Player is not running, start it..."); + _rx_video_output_launch_video_player(); + } + log_line("[VideoOutput] Opening video output pipe write endpoint: %s", FIFO_RUBY_STATION_VIDEO_STREAM); if ( -1 != s_fPipeVideoOutToPlayer ) { diff --git a/code/r_station/rx_video_recording.cpp b/code/r_station/rx_video_recording.cpp index 5a80869a..978a89f5 100644 --- a/code/r_station/rx_video_recording.cpp +++ b/code/r_station/rx_video_recording.cpp @@ -94,9 +94,9 @@ void rx_video_recording_init() log_error_and_alarm("[VideoRecording] Failed to open semaphore for reading: %s", SEMAPHORE_STOP_VIDEO_RECORD); if ( (NULL != s_pSemaphoreStopRecord) && (NULL != s_pSemaphoreStartRecord) ) - log_line("[VideoOutput] Opened semaphores for signaling video recording start/stop."); + log_line("[VideoRecording] Opened semaphores for signaling video recording start/stop."); - log_line("[VideoOutput] Init start complete."); + log_line("[VideoRecording] Init start complete."); } void rx_video_recording_uninit() @@ -122,6 +122,12 @@ void rx_video_recording_start() log_line("[VideoRecording] Received request to start recording video."); + char szComm[MAX_FILE_PATH_SIZE]; + sprintf(szComm, "chmod 777 %s 2>&1 1>/dev/null", FOLDER_MEDIA); + hw_execute_bash_command(szComm, NULL); + sprintf(szComm, "chmod 777 %s* 2>&1 1>/dev/null", FOLDER_MEDIA); + hw_execute_bash_command(szComm, NULL); + s_TimeStartRecording = get_current_timestamp_ms(); strcpy(s_szFileRecordingOutput, FOLDER_RUBY_TEMP); strcat(s_szFileRecordingOutput, FILE_TEMP_VIDEO_FILE); @@ -169,7 +175,7 @@ void rx_video_recording_start() log_softerror_and_alarm("[VideoRecording] Failed to set nonblock flag on video recording file"); log_line("[VideoRecording] Video recording file flags: %s", str_get_pipe_flags(fcntl(s_iFileVideoRecordingOutput, F_GETFL))); - log_line("[VideoOutput] Recording started."); + log_line("[VideoRecording] Recording started."); s_bRecording = true; } @@ -189,7 +195,7 @@ void rx_video_recording_stop() int width = 1280; int height = 720; - int fps = 30; + int fps = 0; int iVideoType = VIDEO_TYPE_H264; for( int i=0; igetVideoHeight(); fps = g_pVideoProcessorRxList[i]->getVideoFPS(); iVideoType = g_pVideoProcessorRxList[i]->getVideoType(); + log_line("Found info for VID %u: w/h/fps: %dx%d@%d, type: %d", g_pCurrentModel->uVehicleId, width, height, fps, iVideoType); break; } + if ( 0 == width ) + log_softerror_and_alarm("Can't find processor rx video stream info for VID: %u", g_pCurrentModel->uVehicleId); char szFile[128]; strcpy(szFile, FOLDER_RUBY_TEMP); diff --git a/code/r_station/shared_vars.cpp b/code/r_station/shared_vars.cpp index 72bd02b2..462d87ac 100644 --- a/code/r_station/shared_vars.cpp +++ b/code/r_station/shared_vars.cpp @@ -45,6 +45,7 @@ u32 g_uSearchFrequency = 0; u32 g_uAcceptedFirmwareType = MODEL_FIRMWARE_TYPE_RUBY; bool g_bUpdateInProgress = false; bool s_bReceivedInvalidRadioPackets = false; +bool g_bNegociatingRadioLinks = false; bool g_bDebugIsPacketsHistoryGraphOn = false; bool g_bDebugIsPacketsHistoryGraphPaused = false; diff --git a/code/r_station/shared_vars.h b/code/r_station/shared_vars.h index 9106443f..d0c8843d 100644 --- a/code/r_station/shared_vars.h +++ b/code/r_station/shared_vars.h @@ -31,6 +31,7 @@ extern u32 g_uSearchFrequency; extern u32 g_uAcceptedFirmwareType; extern bool g_bUpdateInProgress; extern bool s_bReceivedInvalidRadioPackets; +extern bool g_bNegociatingRadioLinks; extern bool g_bDebugIsPacketsHistoryGraphOn; extern bool g_bDebugIsPacketsHistoryGraphPaused; diff --git a/code/r_station/shared_vars_state.cpp b/code/r_station/shared_vars_state.cpp index 2c450f88..62c9e1c7 100644 --- a/code/r_station/shared_vars_state.cpp +++ b/code/r_station/shared_vars_state.cpp @@ -49,28 +49,16 @@ void resetVehicleRuntimeInfo(int iIndex) for( int i=0; i= MAX_CONCURENT_VEHICLES) ) return; - if ( (iLocalRadioLink < 0) || (iLocalRadioLink >= MAX_RADIO_INTERFACES) ) - return; - - u32 avg = 0; - for( int i=0; i g_State.vehiclesRuntimeInfo[iRuntimeInfoIndex].uRadioLinkRoundtripMsMax[iLocalRadioLink] ) - g_State.vehiclesRuntimeInfo[iRuntimeInfoIndex].uRadioLinkRoundtripMsMax[iLocalRadioLink] = uRoundtripTimeMs; - - if ( g_State.vehiclesRuntimeInfo[iRuntimeInfoIndex].uRadioLinkRoundtripMsMin[iLocalRadioLink] == MAX_U32 ) - g_State.vehiclesRuntimeInfo[iRuntimeInfoIndex].uRadioLinkRoundtripMsMin[iLocalRadioLink] = uRoundtripTimeMs; - else if ( uRoundtripTimeMs < g_State.vehiclesRuntimeInfo[iRuntimeInfoIndex].uRadioLinkRoundtripMsMin[iLocalRadioLink] ) - g_State.vehiclesRuntimeInfo[iRuntimeInfoIndex].uRadioLinkRoundtripMsMin[iLocalRadioLink] = uRoundtripTimeMs; - - g_State.vehiclesRuntimeInfo[iRuntimeInfoIndex].uRadioLinkRoundtripMsAvg[iLocalRadioLink] = (avg*3 + uRoundtripTimeMs)/4; - g_State.vehiclesRuntimeInfo[iRuntimeInfoIndex].uRadioLinkRoundtripLastComputedTime[iLocalRadioLink] = g_TimeNow; - - if ( g_State.vehiclesRuntimeInfo[iRuntimeInfoIndex].uRadioLinksMinimumRoundtripMs == MAX_U32 ) - { - g_State.vehiclesRuntimeInfo[iRuntimeInfoIndex].uRadioLinksMinimumRoundtripMs = uRoundtripTimeMs; + if ( g_State.vehiclesRuntimeInfo[iRuntimeInfoIndex].iVehicleClockIsBehindThisMilisec == 500000000 ) g_State.vehiclesRuntimeInfo[iRuntimeInfoIndex].iVehicleClockIsBehindThisMilisec = (int) get_current_timestamp_ms() - (int)uRoundtripTimeMs/2 - (int)uLocalTimeVehicleMs; - } - else if ( uRoundtripTimeMs < g_State.vehiclesRuntimeInfo[iRuntimeInfoIndex].uRadioLinksMinimumRoundtripMs ) - { - g_State.vehiclesRuntimeInfo[iRuntimeInfoIndex].uRadioLinksMinimumRoundtripMs = uRoundtripTimeMs; + else if ( (int)uRoundtripTimeMs < g_State.vehiclesRuntimeInfo[iRuntimeInfoIndex].iVehicleClockIsBehindThisMilisec ) g_State.vehiclesRuntimeInfo[iRuntimeInfoIndex].iVehicleClockIsBehindThisMilisec = (int) get_current_timestamp_ms() - (int)uRoundtripTimeMs/2 - (int)uLocalTimeVehicleMs; - } - - radio_set_link_clock_delta(g_State.vehiclesRuntimeInfo[iRuntimeInfoIndex].iVehicleClockIsBehindThisMilisec); - g_SM_RouterVehiclesRuntimeInfo.uRadioLinksDelayRoundtripMsLastTime[iRuntimeInfoIndex][iLocalRadioLink] = g_TimeNow; - g_SM_RouterVehiclesRuntimeInfo.uRadioLinksDelayRoundtripMs[iRuntimeInfoIndex][iLocalRadioLink] = g_State.vehiclesRuntimeInfo[iRuntimeInfoIndex].uRadioLinkRoundtripMsAvg[iLocalRadioLink] ; - g_SM_RouterVehiclesRuntimeInfo.uRadioLinksDelayRoundtripMsMin[iRuntimeInfoIndex][iLocalRadioLink] = g_State.vehiclesRuntimeInfo[iRuntimeInfoIndex].uRadioLinkRoundtripMsMin[iLocalRadioLink]; + radio_set_link_clock_delta(g_State.vehiclesRuntimeInfo[iRuntimeInfoIndex].iVehicleClockIsBehindThisMilisec); } \ No newline at end of file diff --git a/code/r_station/shared_vars_state.h b/code/r_station/shared_vars_state.h index b8208c34..b37c2478 100644 --- a/code/r_station/shared_vars_state.h +++ b/code/r_station/shared_vars_state.h @@ -1,7 +1,6 @@ #pragma once +#include "../base/config.h" -#define MAX_RUNTIME_INFO_PINGS_HISTORY 3 -#define MAX_RUNTIME_INFO_LINKS_RT_TIMES 5 #define MAX_RUNTIME_INFO_COMMANDS_RT_TIMES 5 typedef struct @@ -16,22 +15,14 @@ typedef struct // Radio link roundtrip/ping times - u32 uPingRoundtripTimeOnLocalRadioLinks[MAX_RADIO_INTERFACES]; - u32 uTimeLastPingSentToVehicleOnLocalRadioLinks[MAX_RADIO_INTERFACES][MAX_RUNTIME_INFO_PINGS_HISTORY]; + u32 uTimeLastPingInitiatedToVehicleOnLocalRadioLinks[MAX_RADIO_INTERFACES]; + u32 uTimeLastPingSentToVehicleOnLocalRadioLinks[MAX_RADIO_INTERFACES]; + u8 uLastPingIdSentToVehicleOnLocalRadioLinks[MAX_RADIO_INTERFACES]; u32 uTimeLastPingReplyReceivedFromVehicleOnLocalRadioLinks[MAX_RADIO_INTERFACES]; - u8 uLastPingIdSentToVehicleOnLocalRadioLinks[MAX_RADIO_INTERFACES][MAX_RUNTIME_INFO_PINGS_HISTORY]; u8 uLastPingIdReceivedFromVehicleOnLocalRadioLinks[MAX_RADIO_INTERFACES]; - - u32 uLastLinkRoundtripTimesMs[MAX_RADIO_INTERFACES][MAX_RUNTIME_INFO_LINKS_RT_TIMES]; - - u32 uRadioLinkRoundtripLastComputedTime[MAX_RADIO_INTERFACES]; - u32 uRadioLinkRoundtripMsLast[MAX_RADIO_INTERFACES]; - u32 uRadioLinkRoundtripMsAvg[MAX_RADIO_INTERFACES]; - u32 uRadioLinkRoundtripMsMin[MAX_RADIO_INTERFACES]; - u32 uRadioLinkRoundtripMsMax[MAX_RADIO_INTERFACES]; + u32 uPingRoundtripTimeOnLocalRadioLinks[MAX_RADIO_INTERFACES]; u32 uLastTimeReceivedAckFromVehicle; - u32 uRadioLinksMinimumRoundtripMs; int iVehicleClockIsBehindThisMilisec; // Commands roundtrip info @@ -55,13 +46,13 @@ typedef struct u32 uLastTimeRecvVideoProfileAck; bool bReceivedKeyframeInfoInVideoStream; -} __attribute__((packed)) type_global_state_vehicle_runtime_info; +} ALIGN_STRUCT_SPEC_INFO type_global_state_vehicle_runtime_info; typedef struct { type_global_state_vehicle_runtime_info vehiclesRuntimeInfo[MAX_CONCURENT_VEHICLES]; -} __attribute__((packed)) type_global_state_station; +} ALIGN_STRUCT_SPEC_INFO type_global_state_station; extern type_global_state_station g_State; @@ -75,4 +66,4 @@ type_global_state_vehicle_runtime_info* getVehicleRuntimeInfo(u32 uVehicleId); void logCurrentVehiclesRuntimeInfo(); void addCommandRTTimeToRuntimeInfo(type_global_state_vehicle_runtime_info* pRuntimeInfo, u32 uRoundtripTimeMs); -void addLinkRTTimeToRuntimeInfoIndex(int iRuntimeInfoIndex, int iLocalRadioLink, u32 uRoundtripTimeMs, u32 uLocalTimeVehicleMs); +void adjustLinkClockDeltasForVehicleRuntimeIndex(int iRuntimeInfoIndex, u32 uRoundtripTimeMs, u32 uLocalTimeVehicleMs); diff --git a/code/r_station/test_link_params.cpp b/code/r_station/test_link_params.cpp index 54e65098..bd660e07 100644 --- a/code/r_station/test_link_params.cpp +++ b/code/r_station/test_link_params.cpp @@ -43,7 +43,7 @@ #include "radio_links.h" #include "packets_utils.h" -extern t_packet_queue s_QueueRadioPackets; +extern t_packet_queue s_QueueRadioPacketsRegPrio; int s_iTestLinkState = TEST_LINK_STATE_NONE; u32 s_uTestLinkVehicleId = 0; @@ -595,7 +595,7 @@ void test_link_loop() uBuffer[sizeof(t_packet_header)+4] = PACKET_TYPE_TEST_RADIO_LINK_COMMAND_START; memcpy(uBuffer + sizeof(t_packet_header) + PACKET_TYPE_TEST_RADIO_LINK_HEADER_SIZE, (u8*)&s_RadioLinksParamsToTest, sizeof(type_radio_links_parameters)); - packets_queue_add_packet(&s_QueueRadioPackets, uBuffer); + packets_queue_add_packet(&s_QueueRadioPacketsRegPrio, uBuffer); log_line("[TestLink-%d] Sent start message (count %u) to vehicle", s_iTestLinkRunCount, s_iTestLinkCurrentStepSendCount); } return; @@ -654,7 +654,7 @@ void test_link_loop() u32 uTmp = (u32)s_iTestLinkCurrentStepSendCount; memcpy(uBuffer + sizeof(t_packet_header) + PACKET_TYPE_TEST_RADIO_LINK_HEADER_SIZE, &uTmp, sizeof(u32)); memcpy(uBuffer + sizeof(t_packet_header) + PACKET_TYPE_TEST_RADIO_LINK_HEADER_SIZE + sizeof(u32), &s_uTestLinkCountPingsReceived, sizeof(u32)); - packets_queue_add_packet(&s_QueueRadioPackets, uBuffer); + packets_queue_add_packet(&s_QueueRadioPacketsRegPrio, uBuffer); log_line("[TestLink-%d] Sent ping uplink %d to vehicle", s_iTestLinkRunCount, s_iTestLinkCurrentStepSendCount); } return; @@ -690,7 +690,7 @@ void test_link_loop() u32 uTmp = (u32)s_iTestLinkCurrentStepSendCount; memcpy(uBuffer + sizeof(t_packet_header) + PACKET_TYPE_TEST_RADIO_LINK_HEADER_SIZE, &uTmp, sizeof(u32)); memcpy(uBuffer + sizeof(t_packet_header) + PACKET_TYPE_TEST_RADIO_LINK_HEADER_SIZE + sizeof(u32), &s_uTestLinkCountPingsReceived, sizeof(u32)); - packets_queue_add_packet(&s_QueueRadioPackets, uBuffer); + packets_queue_add_packet(&s_QueueRadioPacketsRegPrio, uBuffer); log_line("[TestLink-%d] Sent ping downlink %d to vehicle", s_iTestLinkRunCount, s_iTestLinkCurrentStepSendCount); } return; @@ -745,7 +745,7 @@ void test_link_loop() uBuffer[sizeof(t_packet_header)+3] = (u8)s_iTestLinkRunCount; uBuffer[sizeof(t_packet_header)+4] = PACKET_TYPE_TEST_RADIO_LINK_COMMAND_END; uBuffer[sizeof(t_packet_header)+5] = 1; - packets_queue_add_packet(&s_QueueRadioPackets, uBuffer); + packets_queue_add_packet(&s_QueueRadioPacketsRegPrio, uBuffer); log_line("[TestLink-%d] Sent end message %d to vehicle", s_iTestLinkRunCount, s_iTestLinkCurrentStepSendCount); } return; diff --git a/code/r_station/timers.cpp b/code/r_station/timers.cpp index 1ca8154d..200de05d 100644 --- a/code/r_station/timers.cpp +++ b/code/r_station/timers.cpp @@ -4,7 +4,6 @@ u32 g_TimeNow = 0; u32 g_TimeStart = 0; -u32 g_TimeNowMicros = 0; u32 g_TimeLastPeriodicCheck = 0; // RC Tx diff --git a/code/r_station/timers.h b/code/r_station/timers.h index b11b7da9..eaad17eb 100644 --- a/code/r_station/timers.h +++ b/code/r_station/timers.h @@ -5,7 +5,6 @@ extern u32 g_TimeNow; extern u32 g_TimeStart; -extern u32 g_TimeNowMicros; extern u32 g_TimeLastPeriodicCheck; // RC Tx diff --git a/code/r_station/video_link_adaptive.cpp b/code/r_station/video_link_adaptive.cpp deleted file mode 100644 index f65dd199..00000000 --- a/code/r_station/video_link_adaptive.cpp +++ /dev/null @@ -1,610 +0,0 @@ -/* - Ruby Licence - Copyright (c) 2024 Petru Soroaga petrusoroaga@yahoo.com - All rights reserved. - - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Copyright info and developer info must be preserved as is in the user - interface, additions could be made to that info. - * Neither the name of the organization nor the - names of its contributors may be used to endorse or promote products - derived from this software without specific prior written permission. - * Military use is not permited. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - DISCLAIMED. IN NO EVENT SHALL Julien Verneuil BE LIABLE FOR ANY - DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*/ - -#include "../base/base.h" -#include "../base/config.h" -#include "../base/models.h" -#include "../base/models_list.h" -#include "../radio/radiopacketsqueue.h" -#include "../common/string_utils.h" - -#include "shared_vars.h" -#include "timers.h" - -#include "video_link_adaptive.h" -#include "video_link_keyframe.h" -#include "processor_rx_video.h" -#include "links_utils.h" - -extern t_packet_queue s_QueueRadioPackets; - -u32 s_uPauseAdjustmensUntilTime = 0; -u32 s_uTimeStartGoodIntervalForProfileShiftUp = 0; - -void video_link_adaptive_init(u32 uVehicleId) -{ - s_uPauseAdjustmensUntilTime = 0; - log_line("Initialized adaptive video info for VID: %u", uVehicleId); - video_link_keyframe_init(uVehicleId); -} - -void video_link_adaptive_switch_to_med_level(u32 uVehicleId) -{ - video_link_adaptive_init(uVehicleId); - - Model* pModel = findModelWithId(uVehicleId, 140); - if ( NULL == pModel ) - return; - - int isAdaptiveVideoOn = ((pModel->video_link_profiles[pModel->video_params.user_selected_video_link_profile].uProfileEncodingFlags) & VIDEO_PROFILE_ENCODING_FLAG_ENABLE_ADAPTIVE_VIDEO_LINK)?1:0; - if ( ! isAdaptiveVideoOn ) - return; - - int iIndex = getVehicleRuntimeIndex(uVehicleId); - if ( -1 == iIndex ) - return; - - int iLevelsHQ = pModel->get_video_profile_total_levels(pModel->video_params.user_selected_video_link_profile); - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iIndex].iCurrentTargetLevelShift = iLevelsHQ; - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iIndex].iLastAcknowledgedLevelShift = -1; - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iIndex].uTimeLastAckLevelShift = 0; - s_uPauseAdjustmensUntilTime = g_TimeNow + 3000; - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iIndex].uTimeLastLevelShiftDown = g_TimeNow+3000; - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iIndex].uTimeLastLevelShiftUp = g_TimeNow+3000; - s_uTimeStartGoodIntervalForProfileShiftUp = 0; - log_line("Video adaptive: reset to MQ level for VID %u (name: %s)", uVehicleId, pModel->getLongName()); -} - -void video_link_adaptive_set_intial_video_adjustment_level(u32 uVehicleId, int iCurrentVideoProfile, u8 uDataPackets, u8 uECPackets) -{ - Model* pModel = findModelWithId(uVehicleId, 141); - if ( NULL == pModel ) - return; - - log_line("Adaptive video: received initial video stream from vehicle %u: video profile: %d (%s), data/ec: %d/%d", - uVehicleId, iCurrentVideoProfile, str_get_video_profile_name((u32)iCurrentVideoProfile), uDataPackets, uECPackets); - if ( iCurrentVideoProfile < 0 || iCurrentVideoProfile >= MAX_VIDEO_LINK_PROFILES ) - iCurrentVideoProfile = pModel->video_params.user_selected_video_link_profile; - - int iIndex = getVehicleRuntimeIndex(uVehicleId); - if ( -1 == iIndex ) - return; - - if ( iCurrentVideoProfile == pModel->video_params.user_selected_video_link_profile ) - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iIndex].iCurrentTargetLevelShift = 0; - else if ( iCurrentVideoProfile == VIDEO_PROFILE_MQ ) - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iIndex].iCurrentTargetLevelShift = pModel->get_video_profile_total_levels(pModel->video_params.user_selected_video_link_profile); - else - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iIndex].iCurrentTargetLevelShift = pModel->get_video_profile_total_levels(pModel->video_params.user_selected_video_link_profile) + pModel->get_video_profile_total_levels(VIDEO_PROFILE_MQ); - - if ( g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iIndex].iCurrentTargetLevelShift < 0 ) - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iIndex].iCurrentTargetLevelShift = 0; - - if ( (int)uECPackets > pModel->video_link_profiles[iCurrentVideoProfile].block_fecs ) - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iIndex].iCurrentTargetLevelShift += ((int)uECPackets - pModel->video_link_profiles[iCurrentVideoProfile].block_fecs); - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iIndex].iLastAcknowledgedLevelShift = g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iIndex].iCurrentTargetLevelShift; - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iIndex].iLastRequestedLevelShiftRetryCount = 0; - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iIndex].uTimeLastLevelShiftDown = g_TimeNow+1000; - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iIndex].uTimeLastLevelShiftUp = g_TimeNow+1000; - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iIndex].uTimeLastRequestedLevelShift = g_TimeNow+1000; - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iIndex].uTimeLastAckLevelShift = 0; - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iIndex].uTimeLastLevelShiftCheckConsistency = g_TimeNow; - s_uTimeStartGoodIntervalForProfileShiftUp = 0; - log_line("Adaptive video did set initial/current adaptive video level to %d, from received video stream for VID %u, last received video profile: %d", - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iIndex].iCurrentTargetLevelShift, uVehicleId, iCurrentVideoProfile); -} - -void _video_link_adaptive_check_send_to_vehicle(u32 uVehicleId, bool bForce) -{ - Model* pModel = findModelWithId(uVehicleId, 142); - if ( NULL == pModel ) - return; - - int iIndex = getVehicleRuntimeIndex(uVehicleId); - if ( -1 == iIndex ) - return; - - if ( ! bForce ) - { - if ( g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iIndex].iCurrentTargetLevelShift == -1 ) - return; - if ( g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iIndex].iCurrentTargetLevelShift == g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iIndex].iLastAcknowledgedLevelShift ) - return; - if ( g_TimeNow < g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iIndex].uTimeLastRequestedLevelShift + g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iIndex].uUpdateInterval + g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iIndex].iLastRequestedLevelShiftRetryCount*10 ) - return; - } - - t_packet_header PH; - radio_packet_init(&PH, PACKET_COMPONENT_VIDEO, PACKET_TYPE_VIDEO_SWITCH_TO_ADAPTIVE_VIDEO_LEVEL, STREAM_ID_DATA); - PH.vehicle_id_src = g_uControllerId; - PH.vehicle_id_dest = uVehicleId; - PH.total_length = sizeof(t_packet_header) + sizeof(u32) + sizeof(u8); - - u32 uLevel = (u32) g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iIndex].iCurrentTargetLevelShift; - u8 uVideoStreamIndex = 0; - u8 packet[MAX_PACKET_TOTAL_SIZE]; - memcpy(packet, (u8*)&PH, sizeof(t_packet_header)); - memcpy(packet+sizeof(t_packet_header), (u8*)&uLevel, sizeof(u32)); - memcpy(packet+sizeof(t_packet_header) + sizeof(u32), (u8*)&uVideoStreamIndex, sizeof(u8)); - packets_queue_inject_packet_first(&s_QueueRadioPackets, packet); - - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iIndex].uTimeLastRequestedLevelShift = g_TimeNow; - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iIndex].iLastRequestedLevelShiftRetryCount++; - if ( g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iIndex].iLastRequestedLevelShiftRetryCount > 100 ) - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iIndex].iLastRequestedLevelShiftRetryCount = 20; - -} - -void _video_link_adaptive_check_adjust_video_params(u32 uVehicleId) -{ - Model* pModel = findModelWithId(uVehicleId, 143); - if ( NULL == pModel ) - return; - - int isAdaptiveVideoOn = ((pModel->video_link_profiles[pModel->video_params.user_selected_video_link_profile].uProfileEncodingFlags) & VIDEO_PROFILE_ENCODING_FLAG_ENABLE_ADAPTIVE_VIDEO_LINK)?1:0; - if ( ! isAdaptiveVideoOn ) - return; - - _video_link_adaptive_check_send_to_vehicle(uVehicleId, false); - - static u32 s_uTimeLastPeriodicResentCurrentAdaptiveLevel = 0; - if ( ! pModel->isVideoLinkFixedOneWay() ) - if ( g_TimeNow > s_uTimeLastPeriodicResentCurrentAdaptiveLevel + 5000 ) - { - s_uTimeLastPeriodicResentCurrentAdaptiveLevel = g_TimeNow; - _video_link_adaptive_check_send_to_vehicle(uVehicleId, true); - } - - int iVehicleIndex = getVehicleRuntimeIndex(uVehicleId); - if ( -1 == iVehicleIndex ) - return; - - ProcessorRxVideo* pProcessorVideo = NULL; - for( int i=0; im_uVehicleId == uVehicleId ) - { - pProcessorVideo = g_pVideoProcessorRxList[i]; - break; - } - } - - if ( g_TimeNow < g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uTimeLastLevelShiftDown + 50 ) - return; - if ( g_TimeNow < g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uTimeLastLevelShiftUp + 50 ) - return; - - int iLevelsHQ = pModel->get_video_profile_total_levels(pModel->video_params.user_selected_video_link_profile); - int iLevelsMQ = pModel->get_video_profile_total_levels(VIDEO_PROFILE_MQ); - int iLevelsLQ = pModel->get_video_profile_total_levels(VIDEO_PROFILE_LQ); - int iMaxLevels = iLevelsHQ; - iMaxLevels += iLevelsMQ; - if ( ! (pModel->video_link_profiles[pModel->video_params.user_selected_video_link_profile].uProfileEncodingFlags & VIDEO_PROFILE_ENCODING_FLAG_USE_MEDIUM_ADAPTIVE_VIDEO) ) - iMaxLevels += iLevelsLQ; - - // videoAdjustmentStrength is 1 (lowest strength) to 10 (highest strength) - // fParamsChangeStrength: higher value means more aggresive adjustments. From 0.1 to 1 - float fParamsChangeStrength = (float)pModel->video_params.videoAdjustmentStrength / 10.0; - - // When on HQ video profile, switch faster to MQ video profile; - /* - if ( g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[0].iCurrentTargetLevelShift < iLevelsHQ ) - { - fParamsChangeStrength += 0.2; - if ( fParamsChangeStrength > 1.0 ) - fParamsChangeStrength = 1.0; - } - */ - - // When in MQ video profile, switch slower to LQ video profile - - if ( g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentTargetLevelShift > iLevelsHQ ) - { - fParamsChangeStrength -= 0.1; - if ( fParamsChangeStrength < 0.1 ) - fParamsChangeStrength = 0.1; - } - - // Check for video profile received from vehicle missmatch - if ( g_TimeNow > g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uTimeLastLevelShiftCheckConsistency + 4000 ) - { - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uTimeLastLevelShiftCheckConsistency = g_TimeNow; - - bool bHasRecentRxData = false; - for( int i=0; i 2000) && (g_SM_RadioStats.radio_links[i].timeLastRxPacket >= g_TimeNow-1000) ) - { - bHasRecentRxData = true; - break; - } - } - if ( bHasRecentRxData && (NULL != pProcessorVideo) ) - { - if ( g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentTargetLevelShift < iLevelsHQ ) - { - if ( pProcessorVideo->getCurrentlyReceivedVideoProfile() != VIDEO_PROFILE_USER ) - if ( pProcessorVideo->getCurrentlyReceivedVideoProfile() != VIDEO_PROFILE_HIGH_QUALITY ) - if ( pProcessorVideo->getCurrentlyReceivedVideoProfile() != VIDEO_PROFILE_BEST_PERF ) - { - char szTmp[64]; - strcpy(szTmp, str_get_video_profile_name(pModel->video_params.user_selected_video_link_profile)); - log_softerror_and_alarm("Adaptive video: Missmatch between last requested video profile (%s) and currently received video profile (%s)", szTmp, str_get_video_profile_name(pProcessorVideo->getCurrentlyReceivedVideoProfile())); - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iLastAcknowledgedLevelShift = -1; - _video_link_adaptive_check_send_to_vehicle(uVehicleId, false); - - u32 uParam = (u32)pProcessorVideo->getCurrentlyReceivedVideoProfile(); - uParam = uParam | (((u32)g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentTargetLevelShift)<<16); - if ( pModel->bDeveloperMode ) - send_alarm_to_central(ALARM_ID_GENERIC, ALARM_ID_GENERIC_TYPE_ADAPTIVE_VIDEO_LEVEL_MISSMATCH, uParam ); - } - } - else if ( g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentTargetLevelShift < iLevelsHQ + iLevelsMQ ) - { - if ( pProcessorVideo->getCurrentlyReceivedVideoProfile() != VIDEO_PROFILE_MQ ) - { - char szTmp[64]; - strcpy(szTmp, str_get_video_profile_name(VIDEO_PROFILE_MQ)); - log_softerror_and_alarm("Adaptive video: Missmatch between last requested video profile (%s) and currently received video profile (%s)", szTmp, str_get_video_profile_name(pProcessorVideo->getCurrentlyReceivedVideoProfile())); - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iLastAcknowledgedLevelShift = -1; - _video_link_adaptive_check_send_to_vehicle(uVehicleId, false); - - u32 uParam = (u32)pProcessorVideo->getCurrentlyReceivedVideoProfile(); - uParam = uParam | (((u32)g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentTargetLevelShift)<<16); - if ( pModel->bDeveloperMode ) - send_alarm_to_central(ALARM_ID_GENERIC, ALARM_ID_GENERIC_TYPE_ADAPTIVE_VIDEO_LEVEL_MISSMATCH, uParam ); - } - } - else if ( g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentTargetLevelShift < iLevelsHQ + iLevelsMQ + iLevelsLQ ) - { - if ( pProcessorVideo->getCurrentlyReceivedVideoProfile() != VIDEO_PROFILE_LQ ) - { - char szTmp[64]; - strcpy(szTmp, str_get_video_profile_name(VIDEO_PROFILE_LQ)); - log_softerror_and_alarm("Adaptive video: Missmatch between last requested video profile (%s) and currently received video profile (%s)", szTmp, str_get_video_profile_name(pProcessorVideo->getCurrentlyReceivedVideoProfile())); - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iLastAcknowledgedLevelShift = -1; - _video_link_adaptive_check_send_to_vehicle(uVehicleId, false); - - u32 uParam = (u32)pProcessorVideo->getCurrentlyReceivedVideoProfile(); - uParam = uParam | (((u32)g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentTargetLevelShift)<<16); - if ( pModel->bDeveloperMode ) - send_alarm_to_central(ALARM_ID_GENERIC, ALARM_ID_GENERIC_TYPE_ADAPTIVE_VIDEO_LEVEL_MISSMATCH, uParam ); - } - } - } - } - - // Max interval time we can check is: - // MAX_CONTROLLER_ADAPTIVE_VIDEO_INFO_INTERVALS * CONTROLLER_ADAPTIVE_VIDEO_SAMPLE_INTERVAL ms - // = 3.6 seconds - // Minus one because the current index is still processing/invalid - - int iIntervalsToCheckDown = (1.0-0.5*fParamsChangeStrength) * MAX_CONTROLLER_ADAPTIVE_VIDEO_INFO_INTERVALS; - int iIntervalsToCheckUp = (1.0-0.3*fParamsChangeStrength) * MAX_CONTROLLER_ADAPTIVE_VIDEO_INFO_INTERVALS; - if ( iIntervalsToCheckDown >= MAX_CONTROLLER_ADAPTIVE_VIDEO_INFO_INTERVALS-1 ) - iIntervalsToCheckDown = MAX_CONTROLLER_ADAPTIVE_VIDEO_INFO_INTERVALS-2; - if ( iIntervalsToCheckUp >= MAX_CONTROLLER_ADAPTIVE_VIDEO_INFO_INTERVALS-1 ) - iIntervalsToCheckUp = MAX_CONTROLLER_ADAPTIVE_VIDEO_INFO_INTERVALS-2; - - int iIntervalsSinceLastShiftDown = (g_TimeNow - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uTimeLastLevelShiftDown) / g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uUpdateInterval; - if ( iIntervalsToCheckDown > iIntervalsSinceLastShiftDown ) - iIntervalsToCheckDown = iIntervalsSinceLastShiftDown; - if ( iIntervalsToCheckDown <= 0 ) - iIntervalsToCheckDown = 1; - - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uIntervalsAdaptive1 = ((u16)iIntervalsToCheckDown) | (((u16)iIntervalsToCheckUp)<<16); - - int iCountReconstructedDown = 0; - int iLongestReconstructionDown = 0; - int iCountRetransmissionsDown = 0; - int iCountReRetransmissionsDown = 0; - int iCountMissingSegmentsDown = 0; - - int iCountReconstructedUp = 0; - int iLongestReconstructionUp = 0; - int iCountRetransmissionsUp = 0; - int iCountReRetransmissionsUp = 0; - int iCountMissingSegmentsUp = 0; - - int iIndex = g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentIntervalIndex - 1; - if ( iIndex < 0 ) - iIndex = MAX_CONTROLLER_ADAPTIVE_VIDEO_INFO_INTERVALS-1; - - int iCurrentReconstructionLength = 0; - - for( int i=0; i iLongestReconstructionDown ) - iLongestReconstructionDown = iCurrentReconstructionLength; - iCurrentReconstructionLength = 0; - } - } - - if ( iCurrentReconstructionLength > iLongestReconstructionDown ) - iLongestReconstructionDown = iCurrentReconstructionLength; - iCurrentReconstructionLength = 0; - - iCountReconstructedUp = iCountReconstructedDown; - iCountRetransmissionsUp = iCountRetransmissionsDown; - iCountReRetransmissionsUp = iCountReRetransmissionsDown; - iCountMissingSegmentsUp = iCountMissingSegmentsDown; - iLongestReconstructionUp = iLongestReconstructionDown; - - for( int i=iIntervalsToCheckDown; i iLongestReconstructionUp ) - iLongestReconstructionUp = iCurrentReconstructionLength; - iCurrentReconstructionLength = 0; - } - } - - if ( iCurrentReconstructionLength > iLongestReconstructionUp ) - iLongestReconstructionUp = iCurrentReconstructionLength; - iCurrentReconstructionLength = 0; - - bool bDidAnyShift = false; - - // Check for shift down - - int iThresholdReconstructedDown = 2 + (1.0-fParamsChangeStrength)*iIntervalsToCheckDown*0.9; - int iThresholdLongestRecontructionDown = 2 + (1.0-fParamsChangeStrength)*iIntervalsToCheckDown*0.7; - int iThresholdRetransmissionsDown = 1 + (1.0-fParamsChangeStrength)*iIntervalsToCheckDown/8.0; - - u32 uMinTimeSinceLastShift = iIntervalsToCheckDown * g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uUpdateInterval; - if ( iThresholdReconstructedDown * g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uUpdateInterval < uMinTimeSinceLastShift ) - uMinTimeSinceLastShift = iThresholdReconstructedDown * g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uUpdateInterval; - if ( iThresholdRetransmissionsDown * g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uUpdateInterval < uMinTimeSinceLastShift ) - uMinTimeSinceLastShift = iThresholdRetransmissionsDown * g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uUpdateInterval; - - if ( uMinTimeSinceLastShift < 100 ) - uMinTimeSinceLastShift = 100; - if ( uMinTimeSinceLastShift > 500 ) - uMinTimeSinceLastShift = 500; - if ( g_TimeNow > g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uTimeLastLevelShiftDown + uMinTimeSinceLastShift ) - { - if ( iCountRetransmissionsDown > iThresholdRetransmissionsDown ) - { - bDidAnyShift = true; - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uTimeLastLevelShiftDown = g_TimeNow; - - // Go directly to next video profile down - if ( g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentTargetLevelShift < iLevelsHQ ) - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentTargetLevelShift = iLevelsHQ; - else if ( g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentTargetLevelShift < iLevelsHQ + iLevelsMQ) - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentTargetLevelShift = iLevelsHQ+iLevelsMQ; - else - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentTargetLevelShift = iMaxLevels - 1; - - if ( g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentTargetLevelShift >= iMaxLevels ) - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentTargetLevelShift = iMaxLevels - 1; - } - - if ( ! bDidAnyShift ) - if ( (iCountReconstructedDown > iThresholdReconstructedDown) || - (iLongestReconstructionDown > iThresholdLongestRecontructionDown) ) - { - bDidAnyShift = true; - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uTimeLastLevelShiftDown = g_TimeNow; - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentTargetLevelShift++; - - if ( g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentTargetLevelShift >= iMaxLevels ) - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentTargetLevelShift = iMaxLevels - 1; - } - - if ( ! bDidAnyShift ) - if ( (g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentTargetLevelShift == 0) || - (g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentTargetLevelShift == iLevelsHQ) || - (g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentTargetLevelShift == iLevelsMQ) ) - if ( (iCountReconstructedDown > iThresholdReconstructedDown/2+1) || - (iLongestReconstructionDown > iThresholdLongestRecontructionDown/2+1) ) - { - bDidAnyShift = true; - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uTimeLastLevelShiftDown = g_TimeNow; - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentTargetLevelShift++; - - if ( g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentTargetLevelShift >= iMaxLevels ) - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentTargetLevelShift = iMaxLevels - 1; - } - - } - - // Check for shift up ? - // Only if we did not shifted down or up recently - - int iThresholdReconstructedUp = 1 + (1.0-fParamsChangeStrength)*iIntervalsToCheckUp*0.4; - int iThresholdLongestRecontructionUp = 1 + (1.0-fParamsChangeStrength)*iIntervalsToCheckUp*0.2; - int iThresholdRetransmissionsUp = 1 + (1.0-fParamsChangeStrength)*iIntervalsToCheckUp*0.05; - - u32 uTimeForShiftUp = 1000 - (500.0*fParamsChangeStrength); - if ( uTimeForShiftUp < 100 ) - uTimeForShiftUp = 100; - if ( uTimeForShiftUp > 1000 ) - uTimeForShiftUp = 1000; - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uIntervalsAdaptive2 = ((u32)iThresholdReconstructedUp) | (((u32)iThresholdLongestRecontructionUp)<<8); - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uIntervalsAdaptive2 |= (((u32)iThresholdReconstructedDown)<<16) | (((u32)iThresholdLongestRecontructionDown)<<24); - - if ( g_TimeNow > g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uTimeLastLevelShiftDown + uTimeForShiftUp ) - if ( g_TimeNow > g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uTimeLastLevelShiftUp + uTimeForShiftUp ) - { - if ( iCountReconstructedUp < iThresholdReconstructedUp ) - if ( iLongestReconstructionUp < iThresholdLongestRecontructionUp ) - if ( iCountRetransmissionsUp < iThresholdRetransmissionsUp ) - { - bDidAnyShift = true; - - if ( 0 == s_uTimeStartGoodIntervalForProfileShiftUp ) - s_uTimeStartGoodIntervalForProfileShiftUp = g_TimeNow; - else if ( g_TimeNow >= s_uTimeStartGoodIntervalForProfileShiftUp + DEFAULT_MINIMUM_OK_INTERVAL_MS_TO_SWITCH_VIDEO_PROFILE_UP ) - { - s_uTimeStartGoodIntervalForProfileShiftUp = 0; - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uTimeLastLevelShiftUp = g_TimeNow; - if ( g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentTargetLevelShift > 0 ) - { - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentTargetLevelShift--; - // Skip data:fec == 1:1 leves - if ( g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentTargetLevelShift == iLevelsHQ-1 ) - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentTargetLevelShift--; - if ( g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentTargetLevelShift == iLevelsHQ + iLevelsMQ - 1 ) - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentTargetLevelShift--; - if ( g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentTargetLevelShift == iLevelsHQ + iLevelsMQ + iLevelsLQ - 1 ) - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentTargetLevelShift--; - } - if ( g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentTargetLevelShift < 0 ) - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentTargetLevelShift = 0; - } - } - } - - if ( ! bDidAnyShift ) - s_uTimeStartGoodIntervalForProfileShiftUp = 0; -} - -void video_link_adaptive_periodic_loop() -{ - if ( g_bSearching || (NULL == g_pCurrentModel) || g_bUpdateInProgress ) - return; - - if ( g_bDebugState ) - return; - - for( int i=0; iis_spectator) ) - continue; - if ( hardware_board_is_goke(pModel->hwCapabilities.uBoardType) ) - continue; - if ( ! g_State.vehiclesRuntimeInfo[i].bIsPairingDone ) - continue; - - if ( pModel->isVideoLinkFixedOneWay() ) - { - if ( g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[i].iCurrentTargetLevelShift != 0 ) - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[i].iCurrentTargetLevelShift = 0; - if ( g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[i].iLastAcknowledgedLevelShift != 0 ) - _video_link_adaptive_check_send_to_vehicle(g_State.vehiclesRuntimeInfo[i].uVehicleId, false); - continue; - } - - int isAdaptiveVideoOn = ((pModel->video_link_profiles[pModel->video_params.user_selected_video_link_profile].uProfileEncodingFlags) & VIDEO_PROFILE_ENCODING_FLAG_ENABLE_ADAPTIVE_VIDEO_LINK)?1:0; - int isAdaptiveKeyframe = ((pModel->video_link_profiles[pModel->video_params.user_selected_video_link_profile].uProfileEncodingFlags) & VIDEO_PROFILE_ENCODING_FLAG_ENABLE_ADAPTIVE_VIDEO_KEYFRAME)?1:0; - - if ( ! isAdaptiveVideoOn ) - { - if ( g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[i].iCurrentTargetLevelShift != 0 ) - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[i].iCurrentTargetLevelShift = 0; - if ( g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[i].iLastAcknowledgedLevelShift != 0 ) - _video_link_adaptive_check_send_to_vehicle(g_State.vehiclesRuntimeInfo[i].uVehicleId, false); - } - - if ( g_TimeNow < g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[i].uLastUpdateTime + g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[i].uUpdateInterval ) - continue; - - if ( (! isAdaptiveVideoOn) && (! isAdaptiveKeyframe) ) - continue; - - // Update info - - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[i].uLastUpdateTime = g_TimeNow; - - // Compute total missing video packets as of now - - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[i].iChangeStrength = pModel->video_params.videoAdjustmentStrength; - - int iMissing = 0; - for( int k=0; kuVehicleId != g_pVideoProcessorRxList[k]->m_uVehicleId ) - continue; - iMissing = g_pVideoProcessorRxList[k]->getCurrentlyMissingVideoPackets(); - break; - } - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[i].uIntervalsMissingVideoPackets[g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[i].iCurrentIntervalIndex] = (u16) iMissing; - - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[i].iCurrentIntervalIndex++; - if ( g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[i].iCurrentIntervalIndex >= MAX_CONTROLLER_ADAPTIVE_VIDEO_INFO_INTERVALS ) - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[i].iCurrentIntervalIndex = 0; - - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[i].uIntervalsOuputCleanVideoPackets[g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[i].iCurrentIntervalIndex] = 0; - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[i].uIntervalsOuputRecontructedVideoPackets[g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[i].iCurrentIntervalIndex] = 0; - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[i].uIntervalsMissingVideoPackets[g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[i].iCurrentIntervalIndex] = 0; - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[i].uIntervalsRequestedRetransmissions[g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[i].iCurrentIntervalIndex] = 0; - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[i].uIntervalsRetriedRetransmissions[g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[i].iCurrentIntervalIndex] = 0; - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[i].uIntervalsFlags[g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[i].iCurrentIntervalIndex] = 0; - - // Do adaptive video calculations only after we start receiving the video stream - // (a valid initial last requested level shift is obtained from the received video stream) - - if ( (0 != s_uPauseAdjustmensUntilTime) && (g_TimeNow < s_uPauseAdjustmensUntilTime) ) - { - _video_link_adaptive_check_send_to_vehicle(g_State.vehiclesRuntimeInfo[i].uVehicleId, false); - } - else if ( isAdaptiveVideoOn ) - { - if ( g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[i].iCurrentTargetLevelShift != -1 ) - _video_link_adaptive_check_adjust_video_params(g_State.vehiclesRuntimeInfo[i].uVehicleId); - } - } -} diff --git a/code/r_station/video_link_adaptive.h b/code/r_station/video_link_adaptive.h deleted file mode 100644 index 57204e48..00000000 --- a/code/r_station/video_link_adaptive.h +++ /dev/null @@ -1,7 +0,0 @@ -#pragma once - -void video_link_adaptive_init(u32 uVehicleId); -void video_link_adaptive_switch_to_med_level(u32 uVehicleId); -void video_link_adaptive_set_intial_video_adjustment_level(u32 uVehicleId, int iCurrentVideoProfile, u8 uDataPackets, u8 uECPackets); - -void video_link_adaptive_periodic_loop(); diff --git a/code/r_station/video_link_keyframe.cpp b/code/r_station/video_link_keyframe.cpp deleted file mode 100644 index 90dfbb96..00000000 --- a/code/r_station/video_link_keyframe.cpp +++ /dev/null @@ -1,508 +0,0 @@ -/* - Ruby Licence - Copyright (c) 2024 Petru Soroaga petrusoroaga@yahoo.com - All rights reserved. - - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Copyright info and developer info must be preserved as is in the user - interface, additions could be made to that info. - * Neither the name of the organization nor the - names of its contributors may be used to endorse or promote products - derived from this software without specific prior written permission. - * Military use is not permited. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - DISCLAIMED. IN NO EVENT SHALL Julien Verneuil BE LIABLE FOR ANY - DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*/ - -#include "../base/base.h" -#include "../base/config.h" -#include "../base/models.h" -#include "../base/models_list.h" -#include "../common/string_utils.h" -#include "../common/relay_utils.h" -#include "../radio/radiopacketsqueue.h" - -#include "shared_vars.h" -#include "timers.h" - -#include "video_link_adaptive.h" -#include "processor_rx_video.h" - -extern t_packet_queue s_QueueRadioPackets; - -void video_link_keyframe_init(u32 uVehicleId) -{ - Model* pModel = findModelWithId(uVehicleId, 150); - if ( NULL == pModel ) - return; - - int iInfoIndex = getVehicleRuntimeIndex(uVehicleId); - if ( -1 == iInfoIndex ) - return; - - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iInfoIndex].iLastRequestedKeyFrameMs = -1; - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iInfoIndex].iLastAcknowledgedKeyFrameMs = -1; - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iInfoIndex].iLastRequestedKeyFrameMsRetryCount = 0; - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iInfoIndex].uTimeLastRequestedKeyFrame = 0; - g_State.vehiclesRuntimeInfo[iInfoIndex].bReceivedKeyframeInfoInVideoStream = false; - log_line("Initialized adaptive video keyframe info, default start keyframe interval (last requested): %d ms, for VID %u (name: %s)", g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[0].iLastRequestedKeyFrameMs, uVehicleId, pModel->getLongName()); -} - -void video_link_keyframe_set_intial_received_level(u32 uVehicleId, int iReceivedKeyframeMs) -{ - Model* pModel = findModelWithId(uVehicleId, 151); - if ( NULL == pModel ) - return; - - int iInfoIndex = getVehicleRuntimeIndex(uVehicleId); - if ( -1 == iInfoIndex ) - return; - - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iInfoIndex].iLastRequestedKeyFrameMs = iReceivedKeyframeMs; - - if ( g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iInfoIndex].iLastRequestedKeyFrameMs < 0 ) - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iInfoIndex].iLastRequestedKeyFrameMs = pModel->video_params.uMaxAutoKeyframeIntervalMs; - - log_line("Initial keyframe received from the video stream: %d ms", g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iInfoIndex].iLastRequestedKeyFrameMs); - - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iInfoIndex].iLastAcknowledgedKeyFrameMs = g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iInfoIndex].iLastRequestedKeyFrameMs; - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iInfoIndex].iLastRequestedKeyFrameMsRetryCount = 0; - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iInfoIndex].uTimeLastRequestedKeyFrame = g_TimeNow; - g_State.vehiclesRuntimeInfo[iInfoIndex].bReceivedKeyframeInfoInVideoStream = true; - log_line("Done setting initial keyframe state for VID %u based on received keframe interval of %d ms from video stream.", uVehicleId, iReceivedKeyframeMs); - -} - -void video_link_keyframe_set_current_level_to_request(u32 uVehicleId, int iKeyframeMs) -{ - Model* pModel = findModelWithId(uVehicleId, 152); - if ( NULL == pModel ) - return; - - int iInfoIndex = getVehicleRuntimeIndex(uVehicleId); - - if ( -1 == iInfoIndex ) - return; - - g_State.vehiclesRuntimeInfo[iInfoIndex].bReceivedKeyframeInfoInVideoStream = true; - - if ( g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iInfoIndex].iLastRequestedKeyFrameMs == iKeyframeMs ) - return; - - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iInfoIndex].iLastRequestedKeyFrameMs = iKeyframeMs; - - if ( g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iInfoIndex].iLastRequestedKeyFrameMs < 0 ) - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iInfoIndex].iLastRequestedKeyFrameMs = pModel->video_params.uMaxAutoKeyframeIntervalMs; - - log_line("Set current keyframe interval to request from VID %u to %d ms", uVehicleId, g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iInfoIndex].iLastRequestedKeyFrameMs); - - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iInfoIndex].iLastRequestedKeyFrameMsRetryCount = 0; - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iInfoIndex].uTimeLastRequestedKeyFrame = g_TimeNow; -} - -void _video_link_keyframe_check_send_to_vehicle(u32 uVehicleId) -{ - Model* pModel = findModelWithId(uVehicleId, 153); - if ( NULL == pModel ) - return; - - int iInfoIndex = getVehicleRuntimeIndex(uVehicleId); - if ( -1 == iInfoIndex ) - return; - if ( ! g_State.vehiclesRuntimeInfo[iInfoIndex].bIsPairingDone ) - return; - if ( g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iInfoIndex].iLastRequestedKeyFrameMs == -1 ) - return; - - if ( g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iInfoIndex].iLastRequestedKeyFrameMs == g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iInfoIndex].iLastAcknowledgedKeyFrameMs ) - return; - - if ( g_TimeNow < g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iInfoIndex].uTimeLastRequestedKeyFrame + g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iInfoIndex].uUpdateInterval + g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iInfoIndex].iLastRequestedKeyFrameMsRetryCount*10 ) - return; - - t_packet_header PH; - radio_packet_init(&PH, PACKET_COMPONENT_VIDEO, PACKET_TYPE_VIDEO_SWITCH_VIDEO_KEYFRAME_TO_VALUE, STREAM_ID_DATA); - PH.vehicle_id_src = g_uControllerId; - PH.vehicle_id_dest = uVehicleId; - PH.total_length = sizeof(t_packet_header) + sizeof(u32) + sizeof(u8); - - u32 uKeyframeMs = (u32) g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iInfoIndex].iLastRequestedKeyFrameMs; - u8 uVideoStreamIndex = 0; - u8 packet[MAX_PACKET_TOTAL_SIZE]; - memcpy(packet, (u8*)&PH, sizeof(t_packet_header)); - memcpy(packet+sizeof(t_packet_header), (u8*)&uKeyframeMs, sizeof(u32)); - memcpy(packet+sizeof(t_packet_header) + sizeof(u32), (u8*)&uVideoStreamIndex, sizeof(u8)); - packets_queue_inject_packet_first(&s_QueueRadioPackets, packet); - - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iInfoIndex].uTimeLastRequestedKeyFrame = g_TimeNow; - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iInfoIndex].iLastRequestedKeyFrameMsRetryCount++; - if ( g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iInfoIndex].iLastRequestedKeyFrameMsRetryCount > 100 ) - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iInfoIndex].iLastRequestedKeyFrameMsRetryCount = 20; -} - -void _video_link_keyframe_request_new_keyframe_interval(u32 uVehicleId, int iIntervalMs) -{ - if ( iIntervalMs < DEFAULT_VIDEO_MIN_AUTO_KEYFRAME_INTERVAL ) - iIntervalMs = DEFAULT_VIDEO_MIN_AUTO_KEYFRAME_INTERVAL; - if ( iIntervalMs > 20000 ) - iIntervalMs = 20000; - - Model* pModel = findModelWithId(uVehicleId, 154); - if ( NULL == pModel ) - return; - - if ( iIntervalMs > (int)pModel->video_params.uMaxAutoKeyframeIntervalMs ) - iIntervalMs = pModel->video_params.uMaxAutoKeyframeIntervalMs; - - int iInfoIndex = getVehicleRuntimeIndex(uVehicleId); - if ( -1 == iInfoIndex ) - return; - - if ( iIntervalMs == g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iInfoIndex].iLastRequestedKeyFrameMs ) - return; - - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iInfoIndex].iLastRequestedKeyFrameMs = iIntervalMs; - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iInfoIndex].iLastRequestedKeyFrameMsRetryCount = 0; - - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iInfoIndex].uTimeLastRequestedKeyFrame = g_TimeNow - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iInfoIndex].uUpdateInterval - 1; -} - -void _video_link_keyframe_do_adaptive_check(int iVehicleIndex) -{ - Model* pModel = findModelWithId(g_State.vehiclesRuntimeInfo[iVehicleIndex].uVehicleId, 157); - if ( (NULL == pModel) || (pModel->is_spectator) ) - return; - - ProcessorRxVideo* pProcessorVideo = NULL; - for( int i=0; im_uVehicleId == g_State.vehiclesRuntimeInfo[iVehicleIndex].uVehicleId ) - { - pProcessorVideo = g_pVideoProcessorRxList[i]; - break; - } - } - - // Max interval time we can check is: - // MAX_CONTROLLER_ADAPTIVE_VIDEO_INFO_INTERVALS * CONTROLLER_ADAPTIVE_VIDEO_SAMPLE_INTERVAL ms - // = 3.6 seconds - // Minus one because the current index is still processing/invalid - - int iIntervalsToCheckDown = MAX_CONTROLLER_ADAPTIVE_VIDEO_INFO_INTERVALS-1; - int iIntervalsToCheckUp = MAX_CONTROLLER_ADAPTIVE_VIDEO_INFO_INTERVALS-1; - - int iIntervalsSinceLastShiftDown = (int)(g_TimeNow - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uTimeLastKFShiftDown) / (int)g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uUpdateInterval; - if ( iIntervalsSinceLastShiftDown < iIntervalsToCheckDown ) - iIntervalsToCheckDown = iIntervalsSinceLastShiftDown; - - if ( iIntervalsToCheckDown < 0 ) - iIntervalsToCheckDown = 0; - - int iIntervalsSinceLastShiftUp = (int)(g_TimeNow - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uTimeLastKFShiftUp) / (int)g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uUpdateInterval; - if ( iIntervalsSinceLastShiftUp < iIntervalsToCheckUp ) - iIntervalsToCheckUp = iIntervalsSinceLastShiftUp; - - if ( iIntervalsToCheckUp < 0 ) - iIntervalsToCheckUp = 0; - - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uStats_CheckIntervalsKeyFrame = ((u16)iIntervalsToCheckDown) | (((u16)iIntervalsToCheckUp)<<16); - - // videoAdjustmentStrength is 1 (lowest strength) to 10 (highest strength) - // fParamsChangeStrength: higher value means more aggresive adjustments. From 0.1 to 1 - float fParamsChangeStrength = (float)pModel->video_params.videoAdjustmentStrength / 10.0; - - int iThresholdDownLevelSlow = 2 + 20.0 * (1.0-fParamsChangeStrength); - int iThresholdDownLevelFast = 5 + 30.0 * (1.0-fParamsChangeStrength); - int iThresholdDownLevelRetrSlow = 1 + 5.0 * (1.0-fParamsChangeStrength); - int iThresholdDownLevelRetrFast = 2 + 10.0 * (1.0-fParamsChangeStrength); - - // Up levels thresholds must be smaller than down levels thresholds - int iThresholdUpLevel = iThresholdDownLevelSlow - 2; - if ( iThresholdUpLevel < 1 ) - iThresholdUpLevel = 1; - int iThresholdUpLevelRetr = iThresholdDownLevelRetrSlow-2; - if ( iThresholdUpLevelRetr < 1 ) - iThresholdUpLevelRetr = 1; - - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uStats_KeyFrameThresholdIntervalsUp = (iThresholdUpLevel & 0xFF) | ((iThresholdUpLevelRetr & 0xFF) << 8); - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uStats_KeyFrameThresholdIntervalsDown = (iThresholdDownLevelSlow & 0xFF) | ((iThresholdDownLevelFast & 0xFF) << 8) | ((iThresholdDownLevelRetrSlow & 0xFF) << 16) | ((iThresholdDownLevelRetrFast & 0xFF)<<24); - - // Check if we need to down shift keyframe (decrease keyframe interval) - - if ( (NULL != pProcessorVideo) && (pProcessorVideo->getLastTimeReceivedVideoPacket() < g_TimeNow - (10-pModel->video_params.videoAdjustmentStrength)*20) ) - { - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uIntervalsFlags[g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentIntervalIndex] |= ADAPTIVE_STATS_FLAG_NO_VIDEO_PACKETS; - if ( g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iLastRequestedKeyFrameMs > DEFAULT_VIDEO_MIN_AUTO_KEYFRAME_INTERVAL ) - { - int iNewKeyFrameIntervalMs = g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iLastRequestedKeyFrameMs / 2; - if ( iNewKeyFrameIntervalMs < DEFAULT_VIDEO_MIN_AUTO_KEYFRAME_INTERVAL ) - iNewKeyFrameIntervalMs = DEFAULT_VIDEO_MIN_AUTO_KEYFRAME_INTERVAL; - if ( iNewKeyFrameIntervalMs != g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iLastRequestedKeyFrameMs ) - { - _video_link_keyframe_request_new_keyframe_interval(pModel->uVehicleId, iNewKeyFrameIntervalMs); - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uTimeLastKFShiftDown = g_TimeNow; - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uIntervalsFlags[g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentIntervalIndex] |= ADAPTIVE_STATS_FLAGS_KEYFRAME_SHIFTED; - } - } - return; - } - - if ( g_TimeNow > g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uTimeLastRequestedKeyFrame + 100 ) - if ( g_TimeNow > g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uTimeLastKFShiftDown + 100 ) - if ( g_TimeNow > g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uTimeLastKFShiftUp + 100 ) - if ( iIntervalsToCheckDown > 0 ) - { - int iCountReconstructed = 0; - int iCountRetransmissions = 0; - int iCountReRetransmissions = 0; - int iCountMissingSegments = 0; - - int iIndex = g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentIntervalIndex; - for( int k=0; k<=iIntervalsToCheckDown; k++ ) - { - iCountReconstructed += (g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uIntervalsOuputRecontructedVideoPackets[iIndex]!=0)?1:0; - iCountRetransmissions += (g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uIntervalsRequestedRetransmissions[iIndex]!=0)?1:0; - iCountReRetransmissions += (g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uIntervalsRetriedRetransmissions[iIndex]!=0)?1:0; - iCountMissingSegments += (g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uIntervalsMissingVideoPackets[iIndex]!=0)?1:0; - iIndex--; - if ( iIndex < 0 ) - iIndex = MAX_CONTROLLER_ADAPTIVE_VIDEO_INFO_INTERVALS-1; - } - - int iNewKeyFrameIntervalMs = g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iLastRequestedKeyFrameMs; - // Check larger thresholds first - - if ( iIntervalsToCheckDown >= iThresholdDownLevelSlow ) - if ( iCountReconstructed >= iThresholdDownLevelSlow ) - if ( iNewKeyFrameIntervalMs > 4*DEFAULT_VIDEO_MIN_AUTO_KEYFRAME_INTERVAL ) - { - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uIntervalsFlags[g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentIntervalIndex] |= ADAPTIVE_STATS_FLAGS_SHIFT_DOWN; - iNewKeyFrameIntervalMs = (iNewKeyFrameIntervalMs * 3)/4; - } - - if ( iIntervalsToCheckDown >= iThresholdDownLevelFast ) - if ( iCountReconstructed >= iThresholdDownLevelFast ) - { - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uIntervalsFlags[g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentIntervalIndex] |= ADAPTIVE_STATS_FLAGS_SHIFT_DOWN_FAST; - iNewKeyFrameIntervalMs = iNewKeyFrameIntervalMs/2; - } - - if ( iIntervalsToCheckDown >= iThresholdDownLevelRetrSlow ) - if ( (iCountRetransmissions >= iThresholdDownLevelRetrSlow) || (iCountMissingSegments >= iThresholdDownLevelRetrSlow/2+1) ) - if ( iNewKeyFrameIntervalMs > 2*DEFAULT_VIDEO_MIN_AUTO_KEYFRAME_INTERVAL ) - { - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uIntervalsFlags[g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentIntervalIndex] |= ADAPTIVE_STATS_FLAGS_SHIFT_DOWN; - iNewKeyFrameIntervalMs /= 2; - } - - if ( iIntervalsToCheckDown >= iThresholdDownLevelRetrFast ) - if ( (iCountRetransmissions >= iThresholdDownLevelRetrFast) || (iCountMissingSegments >= iThresholdDownLevelRetrFast/2+1) ) - { - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uIntervalsFlags[g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentIntervalIndex] |= ADAPTIVE_STATS_FLAGS_SHIFT_DOWN_FAST; - iNewKeyFrameIntervalMs = DEFAULT_VIDEO_MIN_AUTO_KEYFRAME_INTERVAL; - } - - // Apply the change, if any - - if ( iNewKeyFrameIntervalMs < DEFAULT_VIDEO_MIN_AUTO_KEYFRAME_INTERVAL ) - iNewKeyFrameIntervalMs = DEFAULT_VIDEO_MIN_AUTO_KEYFRAME_INTERVAL; - - if ( iNewKeyFrameIntervalMs != g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iLastRequestedKeyFrameMs ) - { - _video_link_keyframe_request_new_keyframe_interval(pModel->uVehicleId, iNewKeyFrameIntervalMs); - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uTimeLastKFShiftDown = g_TimeNow; - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uIntervalsFlags[g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentIntervalIndex] |= ADAPTIVE_STATS_FLAGS_KEYFRAME_SHIFTED; - } - } - - // Check if we need to up shift keyframe (increase keyframe interval ) - - if ( g_TimeNow > g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uTimeLastRequestedKeyFrame + 900 ) - if ( g_TimeNow > g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uTimeLastKFShiftUp + 900 ) - if ( g_TimeNow > g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uTimeLastKFShiftDown + 900 ) - if ( iIntervalsToCheckUp >= iThresholdUpLevel ) - { - int iCountReconstructed = 0; - int iCountRetransmissions = 0; - int iCountReRetransmissions = 0; - int iCountMissingSegments = 0; - - int iIndex = g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentIntervalIndex; - for( int k=0; k<=iIntervalsToCheckUp; k++ ) - { - iCountReconstructed += (g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uIntervalsOuputRecontructedVideoPackets[iIndex]!=0)?1:0; - iCountRetransmissions += (g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uIntervalsRequestedRetransmissions[iIndex]!=0)?1:0; - iCountReRetransmissions += (g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uIntervalsRetriedRetransmissions[iIndex]!=0)?1:0; - iCountMissingSegments += (g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uIntervalsMissingVideoPackets[iIndex]!=0)?1:0; - iIndex--; - if ( iIndex < 0 ) - iIndex = MAX_CONTROLLER_ADAPTIVE_VIDEO_INFO_INTERVALS-1; - } - - if ( iIntervalsToCheckUp >= iThresholdUpLevel ) - if ( iCountReconstructed < iThresholdUpLevel ) - if ( iIntervalsToCheckUp >= iThresholdUpLevelRetr ) - if ( iCountRetransmissions < iThresholdUpLevelRetr ) - { - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uIntervalsFlags[g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentIntervalIndex] |= ADAPTIVE_STATS_FLAGS_SHIFT_UP; - int iNewKeyFrameIntervalMs = g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iLastRequestedKeyFrameMs * 4 / 3; - if ( iNewKeyFrameIntervalMs > (int)pModel->video_params.uMaxAutoKeyframeIntervalMs ) - iNewKeyFrameIntervalMs = pModel->video_params.uMaxAutoKeyframeIntervalMs; - if ( iNewKeyFrameIntervalMs != g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iLastRequestedKeyFrameMs ) - { - _video_link_keyframe_request_new_keyframe_interval(pModel->uVehicleId, iNewKeyFrameIntervalMs); - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uTimeLastKFShiftUp = g_TimeNow; - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uIntervalsFlags[g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentIntervalIndex] |= ADAPTIVE_STATS_FLAGS_KEYFRAME_SHIFTED; - } - } - - /* - if ( iIntervalsToCheckUp >= iThresholdUpLevelRetr ) - if ( iCountRetransmissions < iThresholdUpLevelRetr ) - { - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uIntervalsFlags[g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentIntervalIndex] |= ADAPTIVE_STATS_FLAGS_SHIFT_UP; - int iNewKeyFrameIntervalMs = g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iLastRequestedKeyFrameMs * 4 / 3; - if ( iNewKeyFrameIntervalMs > (int)pModel->video_params.uMaxAutoKeyframeIntervalMs ) - iNewKeyFrameIntervalMs = pModel->video_params.uMaxAutoKeyframeIntervalMs; - if ( iNewKeyFrameIntervalMs != g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iLastRequestedKeyFrameMs ) - { - _video_link_keyframe_request_new_keyframe_interval(pModel->uVehicleId, iNewKeyFrameIntervalMs); - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uTimeLastKFShiftUp = g_TimeNow; - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].uIntervalsFlags[g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[iVehicleIndex].iCurrentIntervalIndex] |= ADAPTIVE_STATS_FLAGS_KEYFRAME_SHIFTED; - } - } - */ - } -} - - -void video_link_keyframe_periodic_loop() -{ - if ( g_bSearching || (NULL == g_pCurrentModel) || g_bUpdateInProgress ) - return; - - if ( g_bIsControllerLinkToVehicleLost || g_bIsVehicleLinkToControllerLost ) - return; - - if ( g_bDebugState ) - return; - - u32 s_uTimeLastKeyFrameCheckVehicles = 0; - - - // Send/Resend keyframe to vehicle if needed - - for( int i=0; iis_spectator) ) - continue; - - if ( ! (pModel->video_link_profiles[pModel->video_params.user_selected_video_link_profile].uProfileEncodingFlags & VIDEO_PROFILE_ENCODING_FLAG_ENABLE_ADAPTIVE_VIDEO_KEYFRAME) ) - continue; - if ( pModel->isVideoLinkFixedOneWay() ) - continue; - - _video_link_keyframe_check_send_to_vehicle(pModel->uVehicleId); - } - - // Do adjustments only once ever 100 ms - if ( g_TimeNow < s_uTimeLastKeyFrameCheckVehicles + 100 ) - return; - - s_uTimeLastKeyFrameCheckVehicles = g_TimeNow; - - for( int i=0; iis_spectator) ) - continue; - - if ( ! (pModel->video_link_profiles[pModel->video_params.user_selected_video_link_profile].uProfileEncodingFlags & VIDEO_PROFILE_ENCODING_FLAG_ENABLE_ADAPTIVE_VIDEO_KEYFRAME) ) - continue; - if ( pModel->isVideoLinkFixedOneWay() ) - continue; - - if ( hardware_board_is_goke(pModel->hwCapabilities.uBoardType) ) - continue; - - int iCurrentVideoProfile = 0; - int iCurrentFPS = 0; - for( int k=0; kuVehicleId != g_pVideoProcessorRxList[k]->m_uVehicleId ) - continue; - iCurrentVideoProfile = g_pVideoProcessorRxList[k]->getCurrentlyReceivedVideoProfile(); - iCurrentFPS = g_pVideoProcessorRxList[k]->getCurrentlyReceivedVideoFPS(); - break; - } - if ( iCurrentVideoProfile == -1 ) - iCurrentVideoProfile = pModel->video_params.user_selected_video_link_profile; - - int iCurrentProfileKeyFrameMs = pModel->video_link_profiles[iCurrentVideoProfile].keyframe_ms; - if ( iCurrentProfileKeyFrameMs < 0 ) - iCurrentProfileKeyFrameMs = -iCurrentProfileKeyFrameMs; - if ( iCurrentFPS < 1 ) - iCurrentFPS = pModel->video_link_profiles[iCurrentVideoProfile].fps; - - _video_link_keyframe_check_send_to_vehicle(pModel->uVehicleId); - - // If relaying is enabled on this vehicle and video from this vehicle is not visible, do not adjust anything (will default to relay keyframe interval) - - if ( ! relay_controller_must_display_video_from(g_pCurrentModel, pModel->uVehicleId) ) - continue; - - // User set a fixed keyframe value? Then do nothing (just set it if not set); - - if ( iCurrentProfileKeyFrameMs > 0 ) - { - if ( g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[i].iLastRequestedKeyFrameMs != iCurrentProfileKeyFrameMs ) - g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[i].iLastRequestedKeyFrameMs = iCurrentProfileKeyFrameMs; - - if ( g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[i].iLastRequestedKeyFrameMs == g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[i].iLastAcknowledgedKeyFrameMs ) - continue; - - _video_link_keyframe_check_send_to_vehicle(pModel->uVehicleId); - continue; - } - - if ( g_TimeNow < g_SM_RouterVehiclesRuntimeInfo.vehicles_adaptive_video[i].uTimeLastRequestedKeyFrame + 50 ) - continue; - - // Do actual check and adjustments for autokeyframe - _video_link_keyframe_do_adaptive_check(i); - } -} - diff --git a/code/r_station/video_link_keyframe.h b/code/r_station/video_link_keyframe.h deleted file mode 100644 index 3ce89934..00000000 --- a/code/r_station/video_link_keyframe.h +++ /dev/null @@ -1,6 +0,0 @@ -#pragma once - -void video_link_keyframe_init(u32 uVehicleId); -void video_link_keyframe_set_intial_received_level(u32 uVehicleId, int iReceivedKeyframeMs); -void video_link_keyframe_set_current_level_to_request(u32 uVehicleId, int iKeyframeMs); -void video_link_keyframe_periodic_loop(); diff --git a/code/r_station/video_rx_buffers.cpp b/code/r_station/video_rx_buffers.cpp index 71865136..23e89f02 100644 --- a/code/r_station/video_rx_buffers.cpp +++ b/code/r_station/video_rx_buffers.cpp @@ -177,6 +177,9 @@ void VideoRxPacketsBuffer::_empty_block_buffer_index(int iBufferIndex) void VideoRxPacketsBuffer::_empty_buffers(const char* szReason) { m_bEndOfFirstIFrameDetected = false; + m_bIsInsideIFrame = false; + m_bFrameEnded = true; + m_uFrameEndedTime = 0; if ( NULL == szReason ) log_line("Empty buffers (no reason)"); @@ -463,6 +466,18 @@ bool VideoRxPacketsBuffer::_add_video_packet_to_buffer(int iBufferIndex, u8* pPa { m_uMaxVideoBlockIndexReceived = pPHVF->uCurrentBlockIndex; m_uMaxVideoBlockPacketIndexReceived = pPHVF->uCurrentBlockPacketIndex; + + if ( pPHVF->uVideoStatusFlags2 & VIDEO_STATUS_FLAGS2_IS_IFRAME ) + m_bIsInsideIFrame = true; + else + m_bIsInsideIFrame = false; + + m_bFrameEnded = false; + if ( pPHVF->uVideoStatusFlags2 & VIDEO_STATUS_FLAGS2_END_FRAME ) + { + m_bFrameEnded = true; + m_uFrameEndedTime = g_TimeNow; + } return true; } return false; @@ -725,3 +740,18 @@ void VideoRxPacketsBuffer::advanceStartPositionToVideoBlock(u32 uVideoBlockIndex break; } } + +bool VideoRxPacketsBuffer::isFrameEnded() +{ + return m_bFrameEnded; +} + +u32 VideoRxPacketsBuffer::getLastFrameEndTime() +{ + return m_uFrameEndedTime; +} + +bool VideoRxPacketsBuffer::isInsideIFrame() +{ + return m_bIsInsideIFrame; +} diff --git a/code/r_station/video_rx_buffers.h b/code/r_station/video_rx_buffers.h index adc9019c..ce580f7c 100644 --- a/code/r_station/video_rx_buffers.h +++ b/code/r_station/video_rx_buffers.h @@ -57,6 +57,10 @@ class VideoRxPacketsBuffer void advanceStartPosition(); void advanceStartPositionToVideoBlock(u32 uVideoBlockIndex); + bool isFrameEnded(); + u32 getLastFrameEndTime(); + bool isInsideIFrame(); + protected: bool _check_allocate_video_block_in_buffer(int iBufferIndex); @@ -74,6 +78,9 @@ class VideoRxPacketsBuffer int m_iCameraIndex; u32 m_uMaxVideoBlockIndexInBuffer; bool m_bEndOfFirstIFrameDetected; + bool m_bIsInsideIFrame; + bool m_bFrameEnded; + u32 m_uFrameEndedTime; int m_iBufferIndexFirstReceivedBlock; int m_iBufferIndexFirstReceivedPacketIndex; diff --git a/code/r_utils/ruby_update.cpp b/code/r_utils/ruby_update.cpp index d6f3370d..63a8e74a 100644 --- a/code/r_utils/ruby_update.cpp +++ b/code/r_utils/ruby_update.cpp @@ -91,7 +91,6 @@ void validate_camera(Model* pModel) } } - void update_openipc_cpu(Model* pModel) { hardware_set_default_sigmastar_cpu_freq(); @@ -99,6 +98,58 @@ void update_openipc_cpu(Model* pModel) pModel->processesPriorities.iFreqARM = DEFAULT_FREQ_OPENIPC_SIGMASTAR; } + +void do_update_to_99() +{ + log_line("Doing update to 9.9"); + + if ( ! s_isVehicle ) + { + load_Preferences(); + Preferences* pP = get_Preferences(); + pP->iColorOSDOutline[0] = 10; + pP->iColorOSDOutline[1] = 10; + pP->iColorOSDOutline[2] = 10; + pP->iColorOSDOutline[3] = 90; // 90% + save_Preferences(); + } + + Model* pModel = getCurrentModel(); + if ( NULL == pModel ) + return; + + pModel->setDefaultVideoBitrate(); + pModel->video_params.user_selected_video_link_profile = VIDEO_PROFILE_HIGH_QUALITY; + + pModel->rxtx_sync_type = RXTX_SYNC_TYPE_BASIC; + pModel->processesPriorities.uProcessesFlags = PROCESSES_FLAGS_BALANCE_INT_CORES; + + for( int k=0; kcamera_params[k].profiles[i].uFlags |= CAMERA_FLAG_OPENIPC_3A_SIGMASTAR; + } + } + + for( int i=0; iosd_params.osd_flags[i] |= OSD_FLAG_SHOW_FLIGHT_MODE_CHANGE; + } + + pModel->resetVideoLinkProfiles(-1); + + #if defined (HW_PLATFORM_OPENIPC_CAMERA) + if ( hardware_board_is_sigmastar(pModel->hwCapabilities.uBoardType & BOARD_TYPE_MASK) ) + { + pModel->processesPriorities.iFreqGPU = 0; + hardware_set_oipc_freq_boost(pModel->processesPriorities.iFreqARM, pModel->processesPriorities.iFreqGPU); + } + #endif + + log_line("Updated model VID %u (%s) to v9.9", pModel->uVehicleId, pModel->getLongName()); +} + void do_update_to_98() { log_line("Doing update to 9.8"); @@ -106,11 +157,11 @@ void do_update_to_98() if ( ! s_isVehicle ) { load_ControllerSettings(); - ControllerSettings* pCS = get_ControllerSettings(); + //ControllerSettings* pCS = get_ControllerSettings(); save_ControllerSettings(); load_Preferences(); - Preferences* pP = get_Preferences(); + //Preferences* pP = get_Preferences(); save_Preferences(); #if defined (HW_PLATFORM_RADXA_ZERO3) @@ -134,7 +185,7 @@ void do_update_to_98() #if defined (HW_PLATFORM_OPENIPC_CAMERA) if ( hardware_board_is_sigmastar(pModel->hwCapabilities.uBoardType & BOARD_TYPE_MASK) ) { - pModel->processesPriorities.iFreqGPU = 1; + pModel->processesPriorities.iFreqGPU = 0; hardware_set_oipc_freq_boost(pModel->processesPriorities.iFreqARM, pModel->processesPriorities.iFreqGPU); for( int i=0; ivideo_link_profiles[i].h264refresh = 2; } - pModel->video_link_profiles[VIDEO_PROFILE_LQ].fps = DEFAULT_LQ_VIDEO_FPS; + pModel->video_link_profiles[VIDEO_PROFILE_LQ].fps = DEFAULT_VIDEO_FPS; pModel->video_link_profiles[VIDEO_PROFILE_LQ].video_data_length = DEFAULT_LQ_VIDEO_DATA_LENGTH; pModel->video_link_profiles[VIDEO_PROFILE_LQ].block_packets = DEFAULT_LQ_VIDEO_BLOCK_PACKETS; pModel->video_link_profiles[VIDEO_PROFILE_LQ].block_fecs = DEFAULT_LQ_VIDEO_BLOCK_FECS; pModel->video_link_profiles[VIDEO_PROFILE_LQ].bitrate_fixed_bps = DEFAULT_LQ_VIDEO_BITRATE; - pModel->video_link_profiles[VIDEO_PROFILE_MQ].fps = DEFAULT_MQ_VIDEO_FPS; + pModel->video_link_profiles[VIDEO_PROFILE_MQ].fps = DEFAULT_VIDEO_FPS; pModel->video_link_profiles[VIDEO_PROFILE_MQ].video_data_length = DEFAULT_MQ_VIDEO_DATA_LENGTH; pModel->video_link_profiles[VIDEO_PROFILE_MQ].block_packets = DEFAULT_MQ_VIDEO_BLOCK_PACKETS; pModel->video_link_profiles[VIDEO_PROFILE_MQ].block_fecs = DEFAULT_MQ_VIDEO_BLOCK_FECS; @@ -1239,7 +1290,7 @@ void do_update_to_68() pModel->video_params.videoAdjustmentStrength = DEFAULT_VIDEO_PARAMS_ADJUSTMENT_STRENGTH; - pModel->video_link_profiles[VIDEO_PROFILE_LQ].fps = DEFAULT_LQ_VIDEO_FPS; + pModel->video_link_profiles[VIDEO_PROFILE_LQ].fps = DEFAULT_VIDEO_FPS; pModel->video_link_profiles[VIDEO_PROFILE_LQ].video_data_length = DEFAULT_LQ_VIDEO_DATA_LENGTH; pModel->video_link_profiles[VIDEO_PROFILE_HIGH_QUALITY].uProfileEncodingFlags |= (DEFAULT_VIDEO_RETRANS_MS5_HQ<<8); @@ -1289,9 +1340,9 @@ void do_update_to_67() pModel->video_link_profiles[i].keyframe_ms = DEFAULT_VIDEO_KEYFRAME; } - pModel->video_link_profiles[VIDEO_PROFILE_MQ].fps = DEFAULT_MQ_VIDEO_FPS; + pModel->video_link_profiles[VIDEO_PROFILE_MQ].fps = DEFAULT_VIDEO_FPS; - pModel->video_link_profiles[VIDEO_PROFILE_LQ].fps = DEFAULT_LQ_VIDEO_FPS; + pModel->video_link_profiles[VIDEO_PROFILE_LQ].fps = DEFAULT_VIDEO_FPS; pModel->video_params.videoAdjustmentStrength = DEFAULT_VIDEO_PARAMS_ADJUSTMENT_STRENGTH; } else @@ -1773,6 +1824,8 @@ int main(int argc, char *argv[]) do_update_to_97(); if ( (iMajor < 9) || (iMajor == 9 && iMinor <= 8) ) do_update_to_98(); + if ( (iMajor < 9) || (iMajor == 9 && iMinor <= 9) ) + do_update_to_99(); saveCurrentModel(); diff --git a/code/r_utils/ruby_video_proc.cpp b/code/r_utils/ruby_video_proc.cpp index a8e09784..973aeb8e 100644 --- a/code/r_utils/ruby_video_proc.cpp +++ b/code/r_utils/ruby_video_proc.cpp @@ -341,7 +341,9 @@ int main(int argc, char *argv[]) log_enable_stdout(); char szComm[256]; - sprintf(szComm, "chmod 777 %s*", FOLDER_MEDIA); + sprintf(szComm, "chmod 777 %s 2>&1 1>/dev/null", FOLDER_MEDIA); + hw_execute_bash_command(szComm, NULL); + sprintf(szComm, "chmod 777 %s* 2>&1 1>/dev/null", FOLDER_MEDIA); hw_execute_bash_command(szComm, NULL); if ( bStoreOnly ) @@ -361,7 +363,9 @@ int main(int argc, char *argv[]) process_video(szFileInfo, szFileOut); } - sprintf(szComm, "chmod 777 %s*", FOLDER_MEDIA); + sprintf(szComm, "chmod 777 %s 2>&1 1>/dev/null", FOLDER_MEDIA); + hw_execute_bash_command(szComm, NULL); + sprintf(szComm, "chmod 777 %s* 2>&1 1>/dev/null", FOLDER_MEDIA); hw_execute_bash_command(szComm, NULL); log_line("Finished processing video file."); diff --git a/code/r_vehicle/adaptive_video.cpp b/code/r_vehicle/adaptive_video.cpp index ba24273e..98b35cd9 100644 --- a/code/r_vehicle/adaptive_video.cpp +++ b/code/r_vehicle/adaptive_video.cpp @@ -44,14 +44,23 @@ u8 s_uLastVideoProfileRequestedByController = 0xFF; u32 s_uTimeLastVideoProfileRequestedByController = 0; +u32 s_uTimeLastTimeAdaptivePeriodicLoop = 0; u16 s_uCurrentKFValue = 0; u16 s_uPendingKFValue = 0; +int s_iPendingAdaptiveRadioDataRate = 0; +u32 s_uTimeSetPendingAdaptiveRadioDataRate = 0; + +u32 s_uLastAdaptiveAppliedVideoBitrate = 0; + void adaptive_video_init() { log_line("[AdaptiveVideo] Init..."); s_uCurrentKFValue = g_pCurrentModel->getInitialKeyframeIntervalMs(g_pCurrentModel->video_params.user_selected_video_link_profile); + s_uTimeLastTimeAdaptivePeriodicLoop = get_current_timestamp_ms(); + s_uTimeSetPendingAdaptiveRadioDataRate = 0; + s_iPendingAdaptiveRadioDataRate = 0; log_line("[AdaptiveVideo] Current KF ms: %d, pending KF ms: %d", s_uCurrentKFValue, s_uPendingKFValue); } @@ -74,18 +83,26 @@ void adaptive_video_set_last_profile_requested_by_controller(int iVideoProfile) // Update capture video bitrate u32 uBitrateBPS = g_pCurrentModel->video_link_profiles[iVideoProfile].bitrate_fixed_bps; - if ( g_pCurrentModel->hasCamera() ) - if ( g_pCurrentModel->isActiveCameraCSICompatible() || g_pCurrentModel->isActiveCameraVeye() ) - video_source_csi_send_control_message(RASPIVID_COMMAND_ID_VIDEO_BITRATE, uBitrateBPS/100000, 0); - - if ( g_pCurrentModel->hasCamera() ) - if ( g_pCurrentModel->isActiveCameraOpenIPC() ) - video_source_majestic_set_videobitrate_value(uBitrateBPS); + if ( uBitrateBPS != s_uLastAdaptiveAppliedVideoBitrate ) + { + s_uLastAdaptiveAppliedVideoBitrate = uBitrateBPS; + if ( g_pCurrentModel->hasCamera() ) + if ( g_pCurrentModel->isActiveCameraCSICompatible() || g_pCurrentModel->isActiveCameraVeye() ) + video_source_csi_send_control_message(RASPIVID_COMMAND_ID_VIDEO_BITRATE, uBitrateBPS/100000, 0); + + if ( g_pCurrentModel->hasCamera() ) + if ( g_pCurrentModel->isActiveCameraOpenIPC() ) + video_source_majestic_set_videobitrate_value(uBitrateBPS); + } // Update adaptive video rate for tx radio: if ( s_uLastVideoProfileRequestedByController == g_pCurrentModel->video_params.user_selected_video_link_profile ) + { packet_utils_set_adaptive_video_datarate(0); + s_iPendingAdaptiveRadioDataRate = 0; + s_uTimeSetPendingAdaptiveRadioDataRate = 0; + } else { int nRateTxVideo = DEFAULT_RADIO_DATARATE_VIDEO; @@ -98,13 +115,17 @@ void adaptive_video_set_last_profile_requested_by_controller(int iVideoProfile) // If datarate is increasing, set it right away if ( (0 != packet_utils_get_last_set_adaptive_video_datarate()) && (getRealDataRateFromRadioDataRate(nRateTxVideo, 0) >= getRealDataRateFromRadioDataRate(packet_utils_get_last_set_adaptive_video_datarate(), 0)) ) + { packet_utils_set_adaptive_video_datarate(nRateTxVideo); - // To fix + s_iPendingAdaptiveRadioDataRate = 0; + s_uTimeSetPendingAdaptiveRadioDataRate = 0; + } // If datarate is decreasing, set it after a short period - //else if ( g_TimeNow > s_uTimeLastVideoProfileRequestedByController + DEFAULT_LOWER_VIDEO_RADIO_DATARATE_AFTER_MS ) - // packet_utils_set_adaptive_video_datarate(nRateTxVideo); - else - packet_utils_set_adaptive_video_datarate(nRateTxVideo); + else + { + s_iPendingAdaptiveRadioDataRate = nRateTxVideo; + s_uTimeSetPendingAdaptiveRadioDataRate = g_TimeNow; + } } s_uTimeLastVideoProfileRequestedByController = g_TimeNow; @@ -149,6 +170,11 @@ bool _adaptive_video_send_kf_to_capture_program(u16 uNewKeyframeMs) return true; } +void adaptive_video_on_capture_restarted() +{ + s_uLastAdaptiveAppliedVideoBitrate = 0; +} + void adaptive_video_on_new_camera_read(bool bEndOfFrame, bool bIsInsideIFrame) { if ( 0 != s_uPendingKFValue ) @@ -170,4 +196,29 @@ void adaptive_video_on_new_camera_read(bool bEndOfFrame, bool bIsInsideIFrame) g_pVideoTxBuffers->updateCurrentKFValue(); } } +} + +void adaptive_video_periodic_loop() +{ + if ( g_TimeNow < s_uTimeLastTimeAdaptivePeriodicLoop + 10 ) + return; + if ( g_bNegociatingRadioLinks ) + { + if ( g_TimeNow > g_uTimeStartNegociatingRadioLinks + 60*1000 ) + { + g_uTimeStartNegociatingRadioLinks = 0; + g_bNegociatingRadioLinks = false; + } + return; + } + + s_uTimeLastTimeAdaptivePeriodicLoop = g_TimeNow; + + if ( (0 != s_iPendingAdaptiveRadioDataRate) && (0 != s_uTimeSetPendingAdaptiveRadioDataRate) ) + if ( g_TimeNow >= s_uTimeSetPendingAdaptiveRadioDataRate + DEFAULT_LOWER_VIDEO_RADIO_DATARATE_AFTER_MS ) + { + packet_utils_set_adaptive_video_datarate(s_iPendingAdaptiveRadioDataRate); + s_iPendingAdaptiveRadioDataRate = 0; + s_uTimeSetPendingAdaptiveRadioDataRate = 0; + } } \ No newline at end of file diff --git a/code/r_vehicle/adaptive_video.h b/code/r_vehicle/adaptive_video.h index 074f6d2b..5f20359e 100644 --- a/code/r_vehicle/adaptive_video.h +++ b/code/r_vehicle/adaptive_video.h @@ -6,4 +6,6 @@ void adaptive_video_set_kf_for_current_video_profile(u16 uKeyframe); void adaptive_video_set_last_profile_requested_by_controller(int iVideoProfile); int adaptive_video_get_current_active_video_profile(); u16 adaptive_video_get_current_kf(); +void adaptive_video_on_capture_restarted(); void adaptive_video_on_new_camera_read(bool bEndOfFrame, bool bIsInsideIFrame); +void adaptive_video_periodic_loop(); diff --git a/code/r_vehicle/packets_utils.cpp b/code/r_vehicle/packets_utils.cpp index d762ce8d..b2ed0493 100644 --- a/code/r_vehicle/packets_utils.cpp +++ b/code/r_vehicle/packets_utils.cpp @@ -31,6 +31,7 @@ #include "packets_utils.h" #include "../base/base.h" +#include "../base/config.h" #include "../base/flags.h" #include "../base/encr.h" #include "../base/commands.h" @@ -66,7 +67,7 @@ typedef struct u32 uFlags2; u32 uRepeatCount; u32 uStartTime; -} __attribute__((packed)) t_alarm_info; +} ALIGN_STRUCT_SPEC_INFO t_alarm_info; #define MAX_ALARMS_QUEUE 20 @@ -399,6 +400,12 @@ bool _send_packet_to_wifi_radio_interface(int iLocalRadioLinkId, int iRadioInter radio_set_out_datarate(nRateTx); + if ( test_link_is_in_progress() ) + { + t_packet_header* pPH = (t_packet_header*)pPacketData; + if ( (pPH->packet_flags & PACKET_FLAGS_MASK_MODULE) != PACKET_COMPONENT_VIDEO ) + log_line("Test link in progress. Sending radio packet using datarate: %d", nRateTx); + } u32 radioFlags = g_pCurrentModel->radioInterfacesParams.interface_current_radio_flags[iRadioInterfaceIndex]; radio_set_frames_flags(radioFlags); @@ -964,6 +971,40 @@ void send_alarm_to_controller(u32 uAlarm, u32 uFlags1, u32 uFlags2, u32 uRepeatC s_AlarmsPendingInQueue++; } + +void send_alarm_to_controller_now(u32 uAlarm, u32 uFlags1, u32 uFlags2, u32 uRepeatCount) +{ + char szBuff[128]; + alarms_to_string(uAlarm, uFlags1, uFlags2, szBuff); + + s_uAlarmsIndex++; + log_line("Sending alarm to controller: %s, alarm index: %u, repeat count: %u", szBuff, s_uAlarmsIndex, uRepeatCount); + + for( u32 u=0; uuVehicleId; + PH.vehicle_id_dest = 0; + PH.total_length = sizeof(t_packet_header) + 4*sizeof(u32); + + u8 packet[MAX_PACKET_TOTAL_SIZE]; + memcpy(packet, (u8*)&PH, sizeof(t_packet_header)); + memcpy(packet+sizeof(t_packet_header), &s_uAlarmsIndex, sizeof(u32)); + memcpy(packet+sizeof(t_packet_header)+sizeof(u32), &uAlarm, sizeof(u32)); + memcpy(packet+sizeof(t_packet_header)+2*sizeof(u32), &uFlags1, sizeof(u32)); + memcpy(packet+sizeof(t_packet_header)+3*sizeof(u32), &uFlags2, sizeof(u32)); + send_packet_to_radio_interfaces(packet, PH.total_length, -1); + + char szBuff[128]; + alarms_to_string(uAlarm, uFlags1, uFlags2, szBuff); + log_line("Sent alarm packet to radio: %s, alarm index: %u, repeat count: %u", szBuff, s_uAlarmsIndex, uRepeatCount); + + s_uTimeLastAlarmSentToRouter = g_TimeNow; + hardware_sleep_ms(50); + } +} + void send_pending_alarms_to_controller() { if ( s_AlarmsPendingInQueue == 0 ) diff --git a/code/r_vehicle/packets_utils.h b/code/r_vehicle/packets_utils.h index c1170209..bcdb77c6 100644 --- a/code/r_vehicle/packets_utils.h +++ b/code/r_vehicle/packets_utils.h @@ -13,4 +13,5 @@ int send_packet_to_radio_interfaces(u8* pPacketData, int nPacketLength, int iSen void send_packet_vehicle_log(u8* pBuffer, int length); void send_alarm_to_controller(u32 uAlarm, u32 uFlags1, u32 uFlags2, u32 uRepeatCount); +void send_alarm_to_controller_now(u32 uAlarm, u32 uFlags1, u32 uFlags2, u32 uRepeatCount); void send_pending_alarms_to_controller(); diff --git a/code/r_vehicle/periodic_loop.cpp b/code/r_vehicle/periodic_loop.cpp index 4eb9a12a..0dbb751e 100644 --- a/code/r_vehicle/periodic_loop.cpp +++ b/code/r_vehicle/periodic_loop.cpp @@ -66,7 +66,7 @@ extern u16 s_countTXVideoPacketsOutPerSec[2]; extern u16 s_countTXDataPacketsOutPerSec[2]; extern u16 s_countTXCompactedPacketsOutPerSec[2]; -static u32 s_uTimeLastCheckForRaspiDebugMessages = 0; +//static u32 s_uTimeLastCheckForRaspiDebugMessages = 0; static void * _reinit_sik_thread_func(void *ignored_argument) { @@ -577,7 +577,7 @@ void _update_videobitrate_history_data() g_SM_DevVideoBitrateHistory.uCurrentDataPoint++; if ( g_SM_DevVideoBitrateHistory.uCurrentDataPoint >= MAX_INTERVALS_VIDEO_BITRATE_HISTORY ) g_SM_DevVideoBitrateHistory.uCurrentDataPoint = 0; - int iIndex = (int)g_SM_DevVideoBitrateHistory.uCurrentDataPoint; + //int iIndex = (int)g_SM_DevVideoBitrateHistory.uCurrentDataPoint; // To fix g_SM_DevVideoBitrateHistory.uQuantizationOverflowValue = video_link_get_oveflow_quantization_value(); // To fix diff --git a/code/r_vehicle/process_received_ruby_messages.cpp b/code/r_vehicle/process_received_ruby_messages.cpp index 6482acfc..4ab62bcb 100644 --- a/code/r_vehicle/process_received_ruby_messages.cpp +++ b/code/r_vehicle/process_received_ruby_messages.cpp @@ -47,6 +47,8 @@ #include "processor_tx_video.h" #include "events.h" #include "test_link_params.h" +#include "video_source_csi.h" +#include "video_source_majestic.h" u32 s_uResendPairingConfirmationCounter = 0; @@ -81,10 +83,9 @@ int _process_received_ping_messages(int iInterfaceIndex, u8* pPacketBuffer) memcpy(packet+sizeof(t_packet_header)+sizeof(u8)+sizeof(u32), &uSenderLocalRadioLinkId, sizeof(u8)); memcpy(packet+sizeof(t_packet_header)+2*sizeof(u8)+sizeof(u32), &uLocalRadioLinkId, sizeof(u8)); - if ( radio_packet_type_is_high_priority(pPH->packet_type) ) + if ( radio_packet_type_is_high_priority(PH.packet_type) ) send_packet_to_radio_interfaces(packet, PH.total_length, -1); else - //packets_queue_add_packet(&g_QueueRadioPacketsOut, packet); packets_queue_inject_packet_first(&g_QueueRadioPacketsOut, packet); if ( g_pCurrentModel->relay_params.uCurrentRelayMode != uTargetRelayMode ) @@ -138,6 +139,7 @@ int process_received_ruby_message(int iInterfaceIndex, u8* pPacketBuffer) } radio_duplicate_detection_init(); + /* u32 uRefreshIntervalMs = 100; switch ( g_pCurrentModel->m_iRadioInterfacesGraphRefreshInterval ) { @@ -147,7 +149,8 @@ int process_received_ruby_message(int iInterfaceIndex, u8* pPacketBuffer) case 3: uRefreshIntervalMs = 100; break; case 4: uRefreshIntervalMs = 200; break; case 5: uRefreshIntervalMs = 500; break; - } + } + */ radio_stats_reset_received_info(&g_SM_RadioStats); process_data_tx_video_reset_retransmissions_history_info(); } @@ -293,7 +296,81 @@ int process_received_ruby_message(int iInterfaceIndex, u8* pPacketBuffer) return 0; } + + if ( pPH->packet_type == PACKET_TYPE_NEGOCIATE_RADIO_LINKS ) + { + u8 uCommand = pPacketBuffer[sizeof(t_packet_header) + sizeof(u8)]; + u32 uParam = 0; + int iParam = 0; + memcpy(&uParam, pPacketBuffer + sizeof(t_packet_header) + 2*sizeof(u8), sizeof(u32)); + memcpy(&iParam, &uParam, sizeof(int)); + log_line("Received negociate radio link, command %d, param: %d", uCommand, iParam); + + if ( (uCommand == NEGOCIATE_RADIO_STEP_END) || (uCommand == NEGOCIATE_RADIO_STEP_CANCEL) ) + { + g_bNegociatingRadioLinks = false; + g_uTimeStartNegociatingRadioLinks = 0; + packet_utils_set_adaptive_video_datarate(0); + + if ( uCommand == NEGOCIATE_RADIO_STEP_END ) + { + g_pCurrentModel->resetVideoLinkProfilesToDataRates(iParam, iParam); + g_pCurrentModel->radioLinksParams.uGlobalRadioLinksFlags |= MODEL_RADIOLINKS_FLAGS_HAS_NEGOCIATED_LINKS; + saveCurrentModel(); + t_packet_header PH; + radio_packet_init(&PH, PACKET_COMPONENT_LOCAL_CONTROL, PACKET_TYPE_LOCAL_CONTROL_MODEL_CHANGED, STREAM_ID_DATA); + PH.vehicle_id_src = PACKET_COMPONENT_RUBY | (MODEL_CHANGED_GENERIC<<8); + PH.total_length = sizeof(t_packet_header); + + ruby_ipc_channel_send_message(s_fIPCRouterToTelemetry, (u8*)&PH, PH.total_length); + ruby_ipc_channel_send_message(s_fIPCRouterToCommands, (u8*)&PH, PH.total_length); + if ( g_pCurrentModel->rc_params.rc_enabled ) + ruby_ipc_channel_send_message(s_fIPCRouterToRC, (u8*)&PH, PH.total_length); + + if ( NULL != g_pProcessStats ) + g_pProcessStats->lastIPCOutgoingTime = g_TimeNow; + if ( NULL != g_pProcessStats ) + g_pProcessStats->lastActiveTime = get_current_timestamp_ms(); + } + } + else + { + if ( ! g_bNegociatingRadioLinks ) + { + g_uTimeStartNegociatingRadioLinks = g_TimeNow; + u32 uBitrateBPS = 2000000; + if ( g_pCurrentModel->hasCamera() ) + if ( g_pCurrentModel->isActiveCameraCSICompatible() || g_pCurrentModel->isActiveCameraVeye() ) + video_source_csi_send_control_message(RASPIVID_COMMAND_ID_VIDEO_BITRATE, uBitrateBPS/100000, 0); + + if ( g_pCurrentModel->hasCamera() ) + if ( g_pCurrentModel->isActiveCameraOpenIPC() ) + video_source_majestic_set_videobitrate_value(uBitrateBPS); + } + g_bNegociatingRadioLinks = true; + } + + if ( uCommand == NEGOCIATE_RADIO_STEP_DATA_RATE ) + { + log_line("DEBUG set dr: %d", iParam); + packet_utils_set_adaptive_video_datarate(iParam); + } + t_packet_header PH; + radio_packet_init(&PH, PACKET_COMPONENT_RUBY, PACKET_TYPE_NEGOCIATE_RADIO_LINKS, STREAM_ID_DATA); + PH.vehicle_id_src = g_pCurrentModel->uVehicleId; + PH.vehicle_id_dest = g_uControllerId; + PH.total_length = sizeof(t_packet_header) + 2*sizeof(u8) + sizeof(u32); + + u8 packet[MAX_PACKET_TOTAL_SIZE]; + memcpy(packet, (u8*)&PH, sizeof(t_packet_header)); + packet[sizeof(t_packet_header)] = 1; + packet[sizeof(t_packet_header)+sizeof(u8)] = uCommand; + memcpy(packet+sizeof(t_packet_header) + 2*sizeof(u8), &uParam, sizeof(u32)); + packets_queue_add_packet(&g_QueueRadioPacketsOut, packet); + return 0; + } log_line("Received unprocessed Ruby message from controller, message type: %d", pPH->packet_type); return 0; } + diff --git a/code/r_vehicle/process_upload.cpp b/code/r_vehicle/process_upload.cpp index 52e4efb7..c3513322 100644 --- a/code/r_vehicle/process_upload.cpp +++ b/code/r_vehicle/process_upload.cpp @@ -201,6 +201,7 @@ static void * _thread_process_upload(void *argument) _process_upload_send_status_to_controller(OTA_UPDATE_STATUS_START_PROCESSING, 5); + #if defined(HW_PLATFORM_RASPBERRY) log_line("Save received update archive for backup..."); sprintf(szComm, "rm -rf %slast_update_received.tar 2>&1", FOLDER_UPDATES); hw_execute_bash_command(szComm, NULL); @@ -208,6 +209,7 @@ static void * _thread_process_upload(void *argument) hw_execute_bash_command(szComm, NULL); sprintf(szComm, "chmod 777 %slast_update_received.tar 2>&1", FOLDER_UPDATES); hw_execute_bash_command(szComm, NULL); + #endif vehicle_stop_rx_rc(); @@ -479,12 +481,12 @@ void process_sw_upload_new(u32 command_param, u8* pBuffer, int length) if ( params->type == 0 ) { sprintf(s_szUpdateArchiveFile, "%s%s", FOLDER_UPDATES, "ruby_update.zip"); - log_line("Receiving update zip file."); + log_line("Receiving update zip file, to save it in (%s)", s_szUpdateArchiveFile); } else { sprintf(s_szUpdateArchiveFile, "%s%s", FOLDER_UPDATES, "ruby_update.tar"); - log_line("Receiving update tar file."); + log_line("Receiving update tar file, to save it in (%s)", s_szUpdateArchiveFile); } } @@ -496,8 +498,11 @@ void process_sw_upload_new(u32 command_param, u8* pBuffer, int length) return; } - s_pSWPacketsReceived[params->file_block_index]++; s_pSWPacketsSize[params->file_block_index] = length-sizeof(t_packet_header)-sizeof(t_packet_header_command)-sizeof(command_packet_sw_package); + s_pSWPacketsReceived[params->file_block_index]++; + if ( 1 == s_pSWPacketsReceived[params->file_block_index] ) + s_uCurrentReceivedSoftwareSize += s_pSWPacketsSize[params->file_block_index]; + if ( s_pSWPacketsSize[params->file_block_index] > s_uSWPacketsMaxSize ) { log_softerror_and_alarm("Received SW Upload packet index %d too big (%u bytes, max allowed: %u)", params->file_block_index, s_pSWPacketsSize[params->file_block_index], s_uSWPacketsMaxSize); @@ -579,13 +584,19 @@ void process_sw_upload_new(u32 command_param, u8* pBuffer, int length) s_bUpdateInProgress = true; + + sprintf(szComm, "mkdir -p %s", FOLDER_UPDATES); + hw_execute_bash_command(szComm, NULL); + sprintf(szComm, "chmod 777 %s", FOLDER_UPDATES); + hw_execute_bash_command(szComm, NULL); s_pFileSoftware = fopen(s_szUpdateArchiveFile, "wb"); if ( NULL == s_pFileSoftware ) { - log_softerror_and_alarm("Failed to create file for the downloaded software package."); + log_softerror_and_alarm("Failed to create file for the downloaded software package. (file (%s))", s_szUpdateArchiveFile); log_softerror_and_alarm("The download did not completed correctly. Expected size: %d, received size: %d", params->total_size, s_uCurrentReceivedSoftwareSize ); _sw_update_close_remove_temp_files(); s_bUpdateInProgress = false; + _process_upload_send_status_to_controller(OTA_UPDATE_STATUS_FAILED_DISK_SPACE, 10); return; } @@ -598,6 +609,7 @@ void process_sw_upload_new(u32 command_param, u8* pBuffer, int length) log_softerror_and_alarm("Failed to write to file for the downloaded software package."); _sw_update_close_remove_temp_files(); s_bUpdateInProgress = false; + _process_upload_send_status_to_controller(OTA_UPDATE_STATUS_FAILED_DISK_SPACE, 10); return; } fileSize += s_pSWPacketsSize[i]; @@ -610,8 +622,6 @@ void process_sw_upload_new(u32 command_param, u8* pBuffer, int length) if ( fileSize != params->total_size ) log_softerror_and_alarm("Missmatch between expected file size (%u) and created file size (%u)!", params->total_size, fileSize); - sprintf(szComm, "mkdir -p %s", FOLDER_UPDATES); - hw_execute_bash_command(szComm, NULL); sync(); log_line("Received software package correctly (6.3 method). Update file: [%s]. Applying it.", s_szUpdateArchiveFile); @@ -620,6 +630,7 @@ void process_sw_upload_new(u32 command_param, u8* pBuffer, int length) { log_softerror_and_alarm("Failed to create worker thread to process upload."); s_bUpdateInProgress = false; + _process_upload_send_status_to_controller(OTA_UPDATE_STATUS_FAILED, 10); return; } } diff --git a/code/r_vehicle/ruby_rt_vehicle.cpp b/code/r_vehicle/ruby_rt_vehicle.cpp index 09917c32..f2fd678e 100644 --- a/code/r_vehicle/ruby_rt_vehicle.cpp +++ b/code/r_vehicle/ruby_rt_vehicle.cpp @@ -131,9 +131,6 @@ extern u32 s_uLastAlarmsCount; u32 s_uTimeLastTryReadIPCMessages = 0; -#define MAX_RADIO_PACKETS_TO_CACHE_LOCALLY 100 -type_received_radio_packet s_ReceivedRadioPacketsBuffer[MAX_RADIO_PACKETS_TO_CACHE_LOCALLY]; - bool links_set_cards_frequencies_and_params(int iLinkId) { if ( NULL == g_pCurrentModel ) @@ -697,6 +694,12 @@ void _read_ipc_pipes(u32 uTimeNow) packets_queue_add_packet(&s_QueueControlPackets, s_BufferTelemetryDownlink); else { + if ( get_video_capture_start_program_time() != 0 ) + if ( g_TimeNow < get_video_capture_start_program_time() + 2000 ) + { + log_line("Skipped telemetry packet as video capture just started."); + continue; + } packets_queue_add_packet(&g_QueueRadioPacketsOut, s_BufferTelemetryDownlink); /* if ( (pPH->packet_flags & PACKET_FLAGS_MASK_MODULE) == PACKET_COMPONENT_TELEMETRY ) @@ -1130,69 +1133,6 @@ void _check_rx_loop_consistency() } } -void _consume_radio_rx_packets() -{ - int iReceivedAnyPackets = radio_rx_has_packets_to_consume(); - - if ( iReceivedAnyPackets <= 0 ) - return; - - if ( iReceivedAnyPackets > 15 ) - iReceivedAnyPackets = 15; - - u32 uTimeStart = get_current_timestamp_ms(); - - for( int i=0; i uTimeStart + 500 ) - { - log_softerror_and_alarm("Consuming radio rx packets takes too long (%u ms), read ipc messages.", uTime - uTimeStart); - uTimeStart = uTime; - _read_ipc_pipes(uTime); - } - if ( (0 != s_uTimeLastTryReadIPCMessages) && (uTime > s_uTimeLastTryReadIPCMessages + 500) ) - { - log_softerror_and_alarm("Too much time since last ipc messages read (%u ms) while consuming radio messages, read ipc messages.", uTime - s_uTimeLastTryReadIPCMessages); - uTimeStart = uTime; - _read_ipc_pipes(uTime); - } - } - - if ( iReceivedAnyPackets <= 0 ) - if ( (NULL != g_pProcessStats) && (0 != g_pProcessStats->lastRadioRxTime) && (g_TimeNow > TIMEOUT_LINK_TO_CONTROLLER_LOST) && (g_pProcessStats->lastRadioRxTime + TIMEOUT_LINK_TO_CONTROLLER_LOST < g_TimeNow) ) - { - if ( g_TimeLastReceivedRadioPacketFromController + TIMEOUT_LINK_TO_CONTROLLER_LOST < g_TimeNow ) - if ( g_bHasLinkToController ) - { - g_bHasLinkToController = false; - if ( g_pCurrentModel->osd_params.osd_preferences[g_pCurrentModel->osd_params.layout] & OSD_PREFERENCES_BIT_FLAG_SHOW_CONTROLLER_LINK_LOST_ALARM ) - send_alarm_to_controller(ALARM_ID_LINK_TO_CONTROLLER_LOST, 0, 0, 5); - } - - if ( g_pCurrentModel->relay_params.isRelayEnabledOnRadioLinkId >= 0 ) - if ( g_pCurrentModel->relay_params.uRelayedVehicleId != 0 ) - if ( (g_pCurrentModel->relay_params.uCurrentRelayMode & RELAY_MODE_REMOTE) || - (g_pCurrentModel->relay_params.uCurrentRelayMode & RELAY_MODE_PIP_MAIN) || - (g_pCurrentModel->relay_params.uCurrentRelayMode & RELAY_MODE_PIP_REMOTE) ) - { - u32 uOldRelayMode = g_pCurrentModel->relay_params.uCurrentRelayMode; - g_pCurrentModel->relay_params.uCurrentRelayMode = RELAY_MODE_MAIN | RELAY_MODE_IS_RELAY_NODE; - saveCurrentModel(); - onEventRelayModeChanged(uOldRelayMode, g_pCurrentModel->relay_params.uCurrentRelayMode, "stop"); - } - } -} - u32 get_video_capture_start_program_time() { if ( (NULL == g_pCurrentModel) || ( ! g_pCurrentModel->hasCamera()) ) @@ -1544,9 +1484,6 @@ int main(int argc, char *argv[]) log_line("Start sequence: Done creating audio processor."); - for( int i=0; igetVehicleFirmwareType()); @@ -1608,6 +1545,7 @@ int main(int argc, char *argv[]) g_iDefaultRouterThreadPriority = hw_increase_current_thread_priority("Main thread", g_pCurrentModel->processesPriorities.iThreadPriorityRouter); + // ----------------------------------------------------------- // Main loop here @@ -1724,36 +1662,26 @@ void _main_loop() _update_main_loop_debug_info(); - int iCountRadioRxPacketsToConsume = 0; - int iCountRadioRxPacketsToProcess = 0; - + //--------------------------------------------- // Check and process retransmissions received and pings received and other high priority radio messages + int iCountConsumedHighPrio = 0; + + int iPacketLength = 0; + int iPacketIsShort = 0; + int iRadioInterfaceIndex = 0; + u8* pPacket = NULL; - iCountRadioRxPacketsToConsume = radio_rx_has_packets_to_consume(); - if ( iCountRadioRxPacketsToConsume > 0 ) + while ( (iCountConsumedHighPrio < 10) && (!g_bQuit) ) { - if ( iCountRadioRxPacketsToConsume >= MAX_RADIO_PACKETS_TO_CACHE_LOCALLY-2 ) - iCountRadioRxPacketsToConsume = MAX_RADIO_PACKETS_TO_CACHE_LOCALLY-2; - - iCountRadioRxPacketsToProcess = radio_rx_get_received_packets(iCountRadioRxPacketsToConsume, s_ReceivedRadioPacketsBuffer); - - for( int i=0; ipacket_type) ) - { - process_received_single_radio_packet(iRadioInterfaceIndex, pPacket, iPacketLength); - shared_mem_radio_stats_rx_hist_update(&g_SM_HistoryRxStats, iRadioInterfaceIndex, pPacket, g_TimeNow); - } - } + process_received_single_radio_packet(iRadioInterfaceIndex, pPacket, iPacketLength); + shared_mem_radio_stats_rx_hist_update(&g_SM_HistoryRxStats, iRadioInterfaceIndex, pPacket, g_TimeNow); } //-------------------------------------------- @@ -1761,130 +1689,142 @@ void _main_loop() if ( g_pCurrentModel->hasCamera() ) { - // Read multiple times for large I-frames - int iMaxRepeatCount = 3; + int iReadSize = 0; + u8* pVideoData = NULL; + bool bIsInsideIFrame = false; + bool bEndOfFrame = false; - while ( iMaxRepeatCount > 0 ) + // Clear up video pipes on start + if ( g_TimeNow < g_TimeStart + 2000 ) { - iMaxRepeatCount--; - int iReadSize = 0; - u8* pVideoData = NULL; - bool bIsInsideIFrame = false; - bool bEndOfFrame = false; + for( int i=0; i<50; i++ ) + { + if ( g_pCurrentModel->isActiveCameraCSICompatible() || g_pCurrentModel->isActiveCameraVeye() ) + pVideoData = video_source_csi_read(&iReadSize, &bIsInsideIFrame); + if ( g_pCurrentModel->isActiveCameraOpenIPC() ) + pVideoData = video_source_majestic_read(&iReadSize, true); + } + g_TimeNow = get_current_timestamp_ms(); + } + else + { + // Read multiple times for large I-frames + int iMaxRepeatCount = 4; - if ( g_pCurrentModel->isActiveCameraCSICompatible() || g_pCurrentModel->isActiveCameraVeye() ) + while ( iMaxRepeatCount > 0 ) { - pVideoData = video_source_csi_read(&iReadSize, &bIsInsideIFrame); - if ( iReadSize > 0 ) + iMaxRepeatCount--; + iReadSize = 0; + pVideoData = NULL; + bIsInsideIFrame = false; + bEndOfFrame = false; + + if ( g_pCurrentModel->isActiveCameraCSICompatible() || g_pCurrentModel->isActiveCameraVeye() ) { - int iBuffSize = video_source_csi_get_buffer_size(); - bEndOfFrame = (iReadSize < iBuffSize)?true:false; - if ( ! bDebugNoVideoOutput ) - g_pVideoTxBuffers->fillVideoPackets(pVideoData, iReadSize, bEndOfFrame, bIsInsideIFrame); - if ( iReadSize < iBuffSize ) + pVideoData = video_source_csi_read(&iReadSize, &bIsInsideIFrame); + if ( iReadSize > 0 ) + { + int iBuffSize = video_source_csi_get_buffer_size(); + bEndOfFrame = (iReadSize < iBuffSize)?true:false; + if ( ! bDebugNoVideoOutput ) + g_pVideoTxBuffers->fillVideoPackets(pVideoData, iReadSize, bEndOfFrame, bIsInsideIFrame); + if ( iReadSize < iBuffSize ) + iMaxRepeatCount = 0; + } + else iMaxRepeatCount = 0; } - else - iMaxRepeatCount = 0; - } - - if ( g_pCurrentModel->isActiveCameraOpenIPC() ) - { - pVideoData = video_source_majestic_read(&iReadSize, true); - if ( iReadSize > 0 ) + + if ( g_pCurrentModel->isActiveCameraOpenIPC() ) { - bool bSingle = video_source_majestic_last_read_is_single_nal(); - bool bEnd = video_source_majestic_last_read_is_end_nal(); - bIsInsideIFrame = video_source_majestic_is_inside_iframe(); - bEndOfFrame = (bSingle || bEnd); - if ( ! bDebugNoVideoOutput ) + pVideoData = video_source_majestic_read(&iReadSize, true); + if ( iReadSize > 0 ) { - g_pVideoTxBuffers->fillVideoPackets(pVideoData, iReadSize, bEndOfFrame, bIsInsideIFrame); + bool bSingle = video_source_majestic_last_read_is_single_nal(); + bool bEnd = video_source_majestic_last_read_is_end_nal(); + bIsInsideIFrame = video_source_majestic_is_inside_iframe(); + bEndOfFrame = (bSingle || bEnd); + if ( ! bDebugNoVideoOutput ) + { + g_pVideoTxBuffers->fillVideoPackets(pVideoData, iReadSize, bEndOfFrame, bIsInsideIFrame); + } } + else + iMaxRepeatCount = 0; } - else - iMaxRepeatCount = 0; - } - adaptive_video_on_new_camera_read(bEndOfFrame, bIsInsideIFrame); + adaptive_video_on_new_camera_read(bEndOfFrame, bIsInsideIFrame); - // Send telemetry/commands/etc before video data - if ( g_pVideoTxBuffers->hasPendingPacketsToSend() ) - if ( packets_queue_has_packets(&g_QueueRadioPacketsOut) ) - process_and_send_packets(); + // Send telemetry/commands/etc before video data + if ( g_pVideoTxBuffers->hasPendingPacketsToSend() ) + if ( packets_queue_has_packets(&g_QueueRadioPacketsOut) ) + process_and_send_packets(); - g_pVideoTxBuffers->sendAvailablePackets(); - - if ( g_pCurrentModel->bDeveloperMode ) - _check_compute_send_rt_debug_info(); + // Intermix video packets and try again to see if we got any new high priority packets + while ( g_pVideoTxBuffers->hasPendingPacketsToSend() ) + { + int iPending = g_pVideoTxBuffers->hasPendingPacketsToSend(); + int iCountSent = g_pVideoTxBuffers->sendAvailablePackets(10); + g_TimeNow = get_current_timestamp_ms(); + int iCount2 = 0; + while ( (iCount2 < 3) && (!g_bQuit) ) + { + pPacket = radio_rx_wait_get_next_received_high_prio_packet(0, &iPacketLength, &iPacketIsShort, &iRadioInterfaceIndex); + if ( (NULL == pPacket) || g_bQuit ) + break; - g_TimeNow = get_current_timestamp_ms(); + iCount2++; + iCountConsumedHighPrio++; - // To fix - /* - if ( (NULL != pVideoData) && (iReadSize > 0) ) - if ( ! bDebugNoVideoOutput ) - { - if ( process_data_tx_video_on_new_data(pVideoData, iReadSize) ) - s_debugVideoBlocksInCount++; - int videoPacketsReadyToSend = process_data_tx_video_has_packets_ready_to_send(); - if ( videoPacketsReadyToSend > 0 ) - if ( ! bDebugNoVideoOutput ) - { - //log_line("DEBUG sent %d video packs", videoPacketsReadyToSend); - //if ( videoPacketsReadyToSend > 10 ) - // log_line("DEBUG video stall %d", videoPacketsReadyToSend ); - process_data_tx_video_send_packets_ready_to_send(videoPacketsReadyToSend); + process_received_single_radio_packet(iRadioInterfaceIndex, pPacket, iPacketLength); + shared_mem_radio_stats_rx_hist_update(&g_SM_HistoryRxStats, iRadioInterfaceIndex, pPacket, g_TimeNow); + } } + + if ( g_pCurrentModel->bDeveloperMode ) + _check_compute_send_rt_debug_info(); + + g_TimeNow = get_current_timestamp_ms(); } - */ } } //------------------------------------------ // Process all the other radio-in packets - // Consume remaining packets cached locally - u32 uTimeStart = get_current_timestamp_ms(); - if ( iCountRadioRxPacketsToProcess > 0 ) + int iCountConsumedRegPrio = 0; + while ( (iCountConsumedRegPrio < 50) && (!g_bQuit) ) { - for( int i=0; ipacket_type) ) - continue; - shared_mem_radio_stats_rx_hist_update(&g_SM_HistoryRxStats, iRadioInterfaceIndex, pPacket, g_TimeNow); - process_received_single_radio_packet(iRadioInterfaceIndex, pPacket, iPacketLength); - + shared_mem_radio_stats_rx_hist_update(&g_SM_HistoryRxStats, iRadioInterfaceIndex, pPacket, g_TimeNow); + process_received_single_radio_packet(iRadioInterfaceIndex, pPacket, iPacketLength); + - u32 uTime = get_current_timestamp_ms(); - if ( uTime > uTimeStart + 500 ) - { - log_softerror_and_alarm("Consuming radio rx packets takes too long (%u ms), read ipc messages.", uTime - uTimeStart); - uTimeStart = uTime; - _read_ipc_pipes(uTime); - } - if ( (0 != s_uTimeLastTryReadIPCMessages) && (uTime > s_uTimeLastTryReadIPCMessages + 500) ) - { - log_softerror_and_alarm("Too much time since last ipc messages read (%u ms) while consuming radio messages, read ipc messages.", uTime - s_uTimeLastTryReadIPCMessages); - uTimeStart = uTime; - _read_ipc_pipes(uTime); - } + u32 uTime = get_current_timestamp_ms(); + if ( uTime > uTimeStart + 500 ) + { + log_softerror_and_alarm("Consuming radio rx packets takes too long (%u ms), read ipc messages.", uTime - uTimeStart); + uTimeStart = uTime; + _read_ipc_pipes(uTime); + } + if ( (0 != s_uTimeLastTryReadIPCMessages) && (uTime > s_uTimeLastTryReadIPCMessages + 500) ) + { + log_softerror_and_alarm("Too much time since last ipc messages read (%u ms) while consuming radio messages, read ipc messages.", uTime - s_uTimeLastTryReadIPCMessages); + uTimeStart = uTime; + _read_ipc_pipes(uTime); } } + // Check Radio Rx state - if ( iCountRadioRxPacketsToProcess <= 0 ) + if ( (0 == iCountConsumedHighPrio) && (0 == iCountConsumedRegPrio) ) if ( (NULL != g_pProcessStats) && (0 != g_pProcessStats->lastRadioRxTime) && (g_TimeNow > TIMEOUT_LINK_TO_CONTROLLER_LOST) && (g_pProcessStats->lastRadioRxTime + TIMEOUT_LINK_TO_CONTROLLER_LOST < g_TimeNow) ) { if ( g_TimeLastReceivedRadioPacketFromController + TIMEOUT_LINK_TO_CONTROLLER_LOST < g_TimeNow ) @@ -1908,6 +1848,8 @@ void _main_loop() } } + adaptive_video_periodic_loop(); + //------------------------------------------- // Process IPCs diff --git a/code/r_vehicle/ruby_rx_commands.cpp b/code/r_vehicle/ruby_rx_commands.cpp index 04530130..4fbf0761 100644 --- a/code/r_vehicle/ruby_rx_commands.cpp +++ b/code/r_vehicle/ruby_rx_commands.cpp @@ -114,7 +114,7 @@ typedef struct u32 uSegmentsSize[MAX_SEGMENTS_FILE_UPLOAD]; bool bSegmentsReceived[MAX_SEGMENTS_FILE_UPLOAD]; u32 uLastCommandIdForThisFile; -} __attribute__((packed)) t_structure_file_upload_info; +} ALIGN_STRUCT_SPEC_INFO t_structure_file_upload_info; t_structure_file_upload_info s_InfoLastFileUploaded; @@ -868,7 +868,7 @@ bool process_command(u8* pBuffer, int length) if ( uCommandType == COMMAND_ID_FACTORY_RESET ) { - for( int i=0; i<5; i++ ) + for( int i=0; i<10; i++ ) { sendCommandReply(COMMAND_RESPONSE_FLAGS_OK, 0, 0); hardware_sleep_ms(50); @@ -879,7 +879,9 @@ bool process_command(u8* pBuffer, int length) hw_execute_bash_command("rm -rf /home/pi/ruby/config", NULL); hw_execute_bash_command("rm -rf /home/pi/ruby/tmp", NULL); hw_execute_bash_command("mkdir -p config", NULL); + #if defined(HW_PLATFORM_RASPBERRY) hw_execute_bash_command("touch /home/pi/ruby/config/firstboot.txt", NULL); + #endif char szBuff[128]; sprintf(szBuff, "touch %s%s", FOLDER_CONFIG, LOG_USE_PROCESS); hw_execute_bash_command(szBuff, NULL); @@ -995,19 +997,25 @@ bool process_command(u8* pBuffer, int length) if ( uCommandType == COMMAND_ID_SET_OVERCLOCKING_PARAMS ) { + if ( iParamsLength != sizeof(command_packet_overclocking_params) ) + { + sendCommandReply(COMMAND_RESPONSE_FLAGS_FAILED, 0, 0); + return true; + } command_packet_overclocking_params* params = (command_packet_overclocking_params*)(pBuffer + sizeof(t_packet_header)+sizeof(t_packet_header_command)); sendCommandReply(COMMAND_RESPONSE_FLAGS_OK, 0, 0); g_pCurrentModel->processesPriorities.iFreqARM = params->freq_arm; g_pCurrentModel->processesPriorities.iFreqGPU = params->freq_gpu; g_pCurrentModel->processesPriorities.iOverVoltage = params->overvoltage; + g_pCurrentModel->processesPriorities.uProcessesFlags = params->uProcessesFlags; log_line("Received overclocking params: %d mhz arm freq, %d mhz gpu freq, %d overvoltage", g_pCurrentModel->processesPriorities.iFreqARM, g_pCurrentModel->processesPriorities.iFreqGPU, g_pCurrentModel->processesPriorities.iOverVoltage); + log_line("Received processes flags: %u", g_pCurrentModel->processesPriorities.uProcessesFlags); saveCurrentModel(); save_config_file(); - + signalReloadModel(MODEL_CHANGED_GENERIC, 0); #if defined (HW_PLATFORM_OPENIPC_CAMERA) - hw_execute_bash_command_raw("pidof majestic | xargs kill -9 2>/dev/null ", NULL); - hardware_set_oipc_freq_boost(g_pCurrentModel->processesPriorities.iFreqARM, g_pCurrentModel->processesPriorities.iFreqGPU); - hw_execute_bash_command("/usr/bin/majestic -s 2>&1 1>/dev/null &", NULL); + // Force restart of majestic and update priorities + sendControlMessage(PACKET_TYPE_LOCAL_CONTROL_UPDATE_VIDEO_PROGRAM, MODEL_CHANGED_VIDEO_CODEC); #endif return true; @@ -1060,11 +1068,44 @@ bool process_command(u8* pBuffer, int length) strcat(szBuffer, str_get_hardware_board_name(g_pCurrentModel->hwCapabilities.uBoardType)); strcat(szBuffer, "#"); - strcat(szBuffer, "Build: "); char szTmpBuild[256]; + strcat(szBuffer, "OpenIPC firmware build: "); szTmpBuild[0] = 0; hw_execute_bash_command("cat /etc/os-release | grep VERSION_ID", szTmpBuild); - for( int i=0; icommand_param) & 0xFF; int cardType = ((int)(((pPHC->command_param) >> 8) & 0xFF)) - 128; - if ( cardIndex < 0 || cardIndex >= g_pCurrentModel->radioInterfacesParams.interfaces_count ) + if ( (cardIndex < 0) || (cardIndex >= g_pCurrentModel->radioInterfacesParams.interfaces_count) ) { sendCommandReply(COMMAND_RESPONSE_FLAGS_FAILED, 0, 0); return true; } sendCommandReply(COMMAND_RESPONSE_FLAGS_OK, 0, 0); g_pCurrentModel->radioInterfacesParams.interface_card_model[cardIndex] = cardType; + if ( 0 == cardType ) + { + radio_hw_info_t* pRadioHWInfo = hardware_get_radio_info(cardIndex); + if ( NULL != pRadioHWInfo ) + g_pCurrentModel->radioInterfacesParams.interface_card_model[cardIndex] = pRadioHWInfo->iCardModel; + } saveCurrentModel(); signalReloadModel(0, 0); return true; @@ -2371,6 +2418,7 @@ bool process_command(u8* pBuffer, int length) if ( (g_pCurrentModel->video_params.uVideoExtraFlags & VIDEO_FLAG_GENERATE_H265) != (oldParams.uVideoExtraFlags & VIDEO_FLAG_GENERATE_H265) ) log_line("Changed video codec. New codec: %s", (g_pCurrentModel->video_params.uVideoExtraFlags & VIDEO_FLAG_GENERATE_H265)?"H265":"H264"); + bool bMustSignalTXVideo = false; bool bVideoResolutionChanged = false; bool bMustRestartCapture = false; @@ -2453,6 +2501,9 @@ bool process_command(u8* pBuffer, int length) if ( g_pCurrentModel->video_params.uVideoExtraFlags != oldParams.uVideoExtraFlags ) bMustRestartCapture = true; + if ( (g_pCurrentModel->video_params.uVideoExtraFlags & VIDEO_FLAG_GENERATE_H265) != (oldParams.uVideoExtraFlags & VIDEO_FLAG_GENERATE_H265) ) + bMustRestartCapture = true; + if ( (! bVideoResolutionChanged) && (! bMustRestartCapture) ) if ( g_pCurrentModel->video_params.uMaxAutoKeyframeIntervalMs != oldParams.uMaxAutoKeyframeIntervalMs ) { @@ -2460,8 +2511,6 @@ bool process_command(u8* pBuffer, int length) return true; } - signalReloadModel(0, 0); - if ( (g_pCurrentModel->hasCamera()) && ( g_pCurrentModel->video_params.user_selected_video_link_profile != oldParams.user_selected_video_link_profile ) ) signalReloadModel(MODEL_CHANGED_USER_SELECTED_VIDEO_PROFILE, 0); else @@ -2478,7 +2527,6 @@ bool process_command(u8* pBuffer, int length) else sendControlMessage(PACKET_TYPE_LOCAL_CONTROL_UPDATE_VIDEO_PROGRAM, MODEL_CHANGED_VIDEO_RESOLUTION); } - signalReloadModel(0, 0); } if ( bMustSignalTXVideo ) { @@ -3296,31 +3344,13 @@ bool process_command(u8* pBuffer, int length) if ( uCommandType == COMMAND_ID_SET_TX_POWERS ) { - sendCommandReply(COMMAND_RESPONSE_FLAGS_OK, 0, 0); u8* pData = pBuffer + sizeof(t_packet_header)+sizeof(t_packet_header_command); - u8 txPowerRTL8812AU = *pData; - pData++; - u8 txPowerRTL8812EU = *pData; - pData++; - u8 txPowerAtheros = *pData; - pData++; - u8 txMaxPowerRTL8812AU = *pData; - pData++; - u8 txMaxPowerRTL8812EU = *pData; - pData++; - u8 txMaxPowerAtheros = *pData; - pData++; - u8 cardIndex = *pData; - pData++; - u8 cardPower = *pData; - pData++; - bool bUpdated = false; bool bUpdatedWiFi = false; - if ( iParamsLength >= 11 ) { + pData += 8; if ( ( *pData == 0x81 ) && ( *(pData+2) == 0x81 ) ) { int iSiKPower = *(pData+1); @@ -3333,13 +3363,47 @@ bool process_command(u8* pBuffer, int length) g_pCurrentModel->radioInterfacesParams.txPowerSiK = iSiKPower; bUpdated = true; } + sendCommandReply(COMMAND_RESPONSE_FLAGS_OK, 0, 0); } else + { log_softerror_and_alarm("Received invalid power levels message (for SiK radio interfaces). Ignoring it."); + sendCommandReply(COMMAND_RESPONSE_FLAGS_FAILED, 0, 0); + } + if ( bUpdated ) + { + saveCurrentModel(); + signalReloadModel(MODEL_CHANGED_RADIO_POWERS, 0); + } + return true; } - else - log_line("Received radio power levels: 8812AU: %d, 8812EU: %d, Atheros: %d, Max: %d, %d, %d", txPowerRTL8812AU, txPowerRTL8812EU, txPowerAtheros, txMaxPowerRTL8812AU, txMaxPowerRTL8812EU, txMaxPowerAtheros); + + u8 txPowerRTL8812AU = *pData; + pData++; + u8 txPowerRTL8812EU = *pData; + pData++; + u8 txPowerAtheros = *pData; + pData++; + u8 txMaxPowerRTL8812AU = *pData; + pData++; + u8 txMaxPowerRTL8812EU = *pData; + pData++; + u8 txMaxPowerAtheros = *pData; + pData++; + u8 cardIndex = *pData; + pData++; + u8 cardPower = *pData; + pData++; + + log_line("Received radio power levels: 8812AU: %d, 8812EU: %d, Atheros: %d, Max: %d, %d, %d", txPowerRTL8812AU, txPowerRTL8812EU, txPowerAtheros, txMaxPowerRTL8812AU, txMaxPowerRTL8812EU, txMaxPowerAtheros); + log_line("Current radio power levels: 8812AU: %d, 8812EU: %d, Atheros: %d, Max: %d, %d, %d", g_pCurrentModel->radioInterfacesParams.txPowerRTL8812AU, g_pCurrentModel->radioInterfacesParams.txPowerRTL8812EU, g_pCurrentModel->radioInterfacesParams.txPowerAtheros, + g_pCurrentModel->radioInterfacesParams.txMaxPowerRTL8812AU, g_pCurrentModel->radioInterfacesParams.txMaxPowerRTL8812EU, g_pCurrentModel->radioInterfacesParams.txMaxPowerAtheros); + log_line("Current radio interfaces count: RTL8812AU: %d, RTL8812EU: %d, Atheros: %d", + hardware_radio_has_rtl8812au_cards(), hardware_radio_has_rtl8812eu_cards(), hardware_radio_has_atheros_cards()); + + sendCommandReply(COMMAND_RESPONSE_FLAGS_OK, 0, 0); + if ( (txMaxPowerRTL8812AU != 0xFF) && (txMaxPowerRTL8812AU > 0) && (g_pCurrentModel->radioInterfacesParams.txMaxPowerRTL8812AU != txMaxPowerRTL8812AU) ) { g_pCurrentModel->radioInterfacesParams.txMaxPowerRTL8812AU = txMaxPowerRTL8812AU; diff --git a/code/r_vehicle/shared_vars.cpp b/code/r_vehicle/shared_vars.cpp index dce08106..048cbad0 100644 --- a/code/r_vehicle/shared_vars.cpp +++ b/code/r_vehicle/shared_vars.cpp @@ -51,6 +51,7 @@ bool g_bReceivedPairingRequest = false; bool g_bHasLinkToController = false; bool g_bHadEverLinkToController = false; bool g_bHasSentVehicleSettingsAtLeastOnce = false; +bool g_bNegociatingRadioLinks = false; u32 g_uControllerId = 0; diff --git a/code/r_vehicle/shared_vars.h b/code/r_vehicle/shared_vars.h index 5402ba87..f54dffad 100644 --- a/code/r_vehicle/shared_vars.h +++ b/code/r_vehicle/shared_vars.h @@ -79,6 +79,7 @@ extern bool g_bReceivedPairingRequest; extern bool g_bHasLinkToController; extern bool g_bHadEverLinkToController; extern bool g_bHasSentVehicleSettingsAtLeastOnce; +extern bool g_bNegociatingRadioLinks; extern u32 g_uControllerId; diff --git a/code/r_vehicle/telemetry.cpp b/code/r_vehicle/telemetry.cpp index 96b59f0c..171cf42e 100644 --- a/code/r_vehicle/telemetry.cpp +++ b/code/r_vehicle/telemetry.cpp @@ -265,6 +265,10 @@ int telemetry_open_serial_port() telemetry_ltm_on_open_port(s_iTelemetrySerialPortFile); if ( g_pCurrentModel->telemetry_params.fc_telemetry_type == TELEMETRY_TYPE_MSP ) telemetry_msp_on_open_port(s_iTelemetrySerialPortFile); + + if ( g_pCurrentModel->processesPriorities.uProcessesFlags & PROCESSES_FLAGS_BALANCE_INT_CORES ) + hardware_balance_interupts(); + return s_iTelemetrySerialPortFile; } diff --git a/code/r_vehicle/test_link_params.cpp b/code/r_vehicle/test_link_params.cpp index 3b7ddf70..4487b9b3 100644 --- a/code/r_vehicle/test_link_params.cpp +++ b/code/r_vehicle/test_link_params.cpp @@ -85,6 +85,13 @@ void _test_link_end_and_notify() g_pCurrentModel->logVehicleRadioLinkDifferences(&s_RadioLinksParamsOriginal, &s_RadioLinksParamsToTest); memcpy(&(g_pCurrentModel->radioLinksParams), &s_RadioLinksParamsToTest, sizeof(type_radio_links_parameters)); + + log_line("Current radio power levels: 8812AU: %d, 8812EU: %d, Atheros: %d, Max: %d, %d, %d", g_pCurrentModel->radioInterfacesParams.txPowerRTL8812AU, g_pCurrentModel->radioInterfacesParams.txPowerRTL8812EU, g_pCurrentModel->radioInterfacesParams.txPowerAtheros, + g_pCurrentModel->radioInterfacesParams.txMaxPowerRTL8812AU, g_pCurrentModel->radioInterfacesParams.txMaxPowerRTL8812EU, g_pCurrentModel->radioInterfacesParams.txMaxPowerAtheros); + + log_line("Current radio interfaces count: RTL8812AU: %d, RTL8812EU: %d, Atheros: %d", + hardware_radio_has_rtl8812au_cards(), hardware_radio_has_rtl8812eu_cards(), hardware_radio_has_atheros_cards()); + saveCurrentModel(); t_packet_header PH; diff --git a/code/r_vehicle/timers.cpp b/code/r_vehicle/timers.cpp index e161d692..7a5d596e 100644 --- a/code/r_vehicle/timers.cpp +++ b/code/r_vehicle/timers.cpp @@ -54,3 +54,4 @@ u32 g_uTimeLastCommandSowftwareUpload = 0; u32 g_uTimeLastVideoTxOverload = 0; u32 g_uTimeToSaveCameraParams = 0; +u32 g_uTimeStartNegociatingRadioLinks = 0; diff --git a/code/r_vehicle/timers.h b/code/r_vehicle/timers.h index 388df579..a630dcc5 100644 --- a/code/r_vehicle/timers.h +++ b/code/r_vehicle/timers.h @@ -58,3 +58,4 @@ extern u32 g_uTimeLastCommandSowftwareUpload; extern u32 g_uTimeLastVideoTxOverload; extern u32 g_uTimeToSaveCameraParams; +extern u32 g_uTimeStartNegociatingRadioLinks; diff --git a/code/r_vehicle/video_link_auto_keyframe.cpp b/code/r_vehicle/video_link_auto_keyframe.cpp deleted file mode 100644 index 1fa7d94b..00000000 --- a/code/r_vehicle/video_link_auto_keyframe.cpp +++ /dev/null @@ -1,273 +0,0 @@ -/* - Ruby Licence - Copyright (c) 2024 Petru Soroaga petrusoroaga@yahoo.com - All rights reserved. - - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Copyright info and developer info must be preserved as is in the user - interface, additions could be made to that info. - * Neither the name of the organization nor the - names of its contributors may be used to endorse or promote products - derived from this software without specific prior written permission. - * Military use is not permited. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - DISCLAIMED. IN NO EVENT SHALL Julien Verneuil BE LIABLE FOR ANY - DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*/ - -#include "../base/base.h" -#include "../base/config.h" -#include "../base/models.h" -#include "../common/string_utils.h" -#include "../common/relay_utils.h" - -#include "processor_tx_video.h" -#include "utils_vehicle.h" -#include "ruby_rt_vehicle.h" -#include "packets_utils.h" -#include "shared_vars.h" -#include "timers.h" - -#include "video_link_auto_keyframe.h" - -int s_iLastRXQualityMinimumForKeyFrame = -1; -int s_iLastRXQualityMaximumForKeyFrame = -1; - -u32 s_uLastTimeKeyFrameMovedDown = 0; -u32 s_uLastTimeKeyFrameMovedUp = 0; - -void video_link_auto_keyframe_set_controller_requested_value(int iVideoStreamIndex, int iKeyframeMs) -{ - log_line("[Keyframe] Set requested keyframe interval from controller to %d ms. Previous requested was %d ms", iKeyframeMs, (int)g_SM_VideoLinkStats.overwrites.uCurrentControllerRequestedKeyframeMs); - - g_SM_VideoLinkStats.overwrites.uCurrentControllerRequestedKeyframeMs = (u16)iKeyframeMs; - g_SM_VideoLinkStats.overwrites.uLastTimeControllerRequestedAKeyframe = g_TimeNow; -} - -void video_link_auto_keyframe_set_local_requested_value(int iVideoStreamIndex, int iKeyframeMs, const char* szReason) -{ - if ( g_SM_VideoLinkStats.overwrites.uCurrentLocalRequestedKeyframeMs == (u16)iKeyframeMs ) - if ( g_SM_VideoLinkStats.overwrites.uCurrentActiveKeyframeMs == (u16)iKeyframeMs ) - if ( (0 == g_SM_VideoLinkStats.overwrites.uCurrentPendingKeyframeMs ) || (g_SM_VideoLinkStats.overwrites.uCurrentPendingKeyframeMs == (u16)iKeyframeMs) ) - return; - if ( NULL != szReason ) - log_line("[Keyframe] Set requested keyframe interval from local to %d ms. Previous requested was %d ms (reason: %s)", iKeyframeMs, (int)g_SM_VideoLinkStats.overwrites.uCurrentLocalRequestedKeyframeMs, szReason); - else - log_line("[Keyframe] Set requested keyframe interval from local to %d ms. Previous requested was %d ms", iKeyframeMs, (int)g_SM_VideoLinkStats.overwrites.uCurrentLocalRequestedKeyframeMs); - log_line("[Keyframe] Current active keyframe: %d ms, current pending to set keyframe: %d ms", - (int)g_SM_VideoLinkStats.overwrites.uCurrentActiveKeyframeMs, (int)g_SM_VideoLinkStats.overwrites.uCurrentPendingKeyframeMs); - g_SM_VideoLinkStats.overwrites.uCurrentLocalRequestedKeyframeMs = (u16)iKeyframeMs; - g_SM_VideoLinkStats.overwrites.uLastTimeLocalRequestedAKeyframe = g_TimeNow; -} - -void video_link_auto_keyframe_init() -{ - // Sets the default as when video capture starts with a default keyframe interval. - g_SM_VideoLinkStats.overwrites.uCurrentPendingKeyframeMs = 0; - g_SM_VideoLinkStats.overwrites.uCurrentActiveKeyframeMs = 0; - - g_SM_VideoLinkStats.overwrites.uCurrentLocalRequestedKeyframeMs = 0; - g_SM_VideoLinkStats.overwrites.uCurrentControllerRequestedKeyframeMs = 0; - g_SM_VideoLinkStats.overwrites.uLastTimeLocalRequestedAKeyframe = 0; - g_SM_VideoLinkStats.overwrites.uLastTimeControllerRequestedAKeyframe = 0; - - log_line("[Keyframe] Auto keyframe module init complete."); -} - -void video_link_auto_keyframe_do_adjustments() -{ - // Compute highest RX quality link - - int iLowestRXQuality = 1000; - int iHighestRXQuality = 0; - for( int i=0; iradioInterfacesParams.interfaces_count; i++ ) - { - radio_hw_info_t* pRadioInfo = hardware_get_radio_info(i); - - if ( (NULL != pRadioInfo) && (pRadioInfo->isHighCapacityInterface) ) - if ( ! (g_pCurrentModel->radioInterfacesParams.interface_capabilities_flags[i] & RADIO_HW_CAPABILITY_FLAG_DISABLED) ) - if ( g_pCurrentModel->radioInterfacesParams.interface_capabilities_flags[i] & RADIO_HW_CAPABILITY_FLAG_CAN_RX ) - if ( g_pCurrentModel->radioInterfacesParams.interface_link_id[i] != -1 ) - { - if ( g_SM_RadioStats.radio_interfaces[i].rxQuality < iLowestRXQuality ) - iLowestRXQuality = g_SM_RadioStats.radio_interfaces[i].rxQuality; - if ( g_SM_RadioStats.radio_interfaces[i].rxQuality > iHighestRXQuality ) - iHighestRXQuality = g_SM_RadioStats.radio_interfaces[i].rxQuality; - } - } - - // Have recent link from controller ok? Then do nothing. - - int iThresholdControllerLinkMs = 2000; - if ( g_TimeNow > g_TimeStart + 5000 ) - if ( g_TimeNow > get_video_capture_start_program_time() + 3000 ) - if ( g_TimeLastReceivedRadioPacketFromController + iThresholdControllerLinkMs > g_TimeNow ) - if ( iHighestRXQuality >= 20 ) - return; - - - // Adaptive: need to go to lowest level? - - bool bGoToLowestLevel = false; - - if ( (! g_bReceivedPairingRequest) && (!g_bHadEverLinkToController) ) - bGoToLowestLevel = true; - if ( g_TimeLastReceivedRadioPacketFromController + iThresholdControllerLinkMs < g_TimeNow ) - bGoToLowestLevel = true; - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile == VIDEO_PROFILE_LQ ) - bGoToLowestLevel = true; - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile == VIDEO_PROFILE_MQ ) - if ( g_SM_VideoLinkStats.overwrites.currentProfileShiftLevel >= g_pCurrentModel->get_video_profile_total_levels(VIDEO_PROFILE_MQ) ) - bGoToLowestLevel = true; - - if ( bGoToLowestLevel ) - { - video_link_auto_keyframe_set_local_requested_value(0, DEFAULT_VIDEO_MIN_AUTO_KEYFRAME_INTERVAL, "go lowest level"); - return; - } - - // Adaptive keyframe interval - - int iNewKeyframeIntervalMs = 0; - - if ( iHighestRXQuality < 20 ) - iNewKeyframeIntervalMs = DEFAULT_VIDEO_MIN_AUTO_KEYFRAME_INTERVAL; - - // RX Quality going down ? - - //if ( iHighestRXQuality < s_iLastRXQualityMaximumForKeyFrame ) - { - if ( iHighestRXQuality < 70 ) - if ( g_TimeNow > s_uLastTimeKeyFrameMovedDown + 300 ) - { - iNewKeyframeIntervalMs = g_SM_VideoLinkStats.overwrites.uCurrentActiveKeyframeMs/2; - if ( iNewKeyframeIntervalMs < 2 * DEFAULT_VIDEO_MIN_AUTO_KEYFRAME_INTERVAL ) - iNewKeyframeIntervalMs = 2 * DEFAULT_VIDEO_MIN_AUTO_KEYFRAME_INTERVAL; - s_uLastTimeKeyFrameMovedDown = g_TimeNow; - } - - if ( iHighestRXQuality < 50 ) - if ( g_TimeNow > s_uLastTimeKeyFrameMovedDown + 300 ) - { - iNewKeyframeIntervalMs = DEFAULT_VIDEO_MIN_AUTO_KEYFRAME_INTERVAL; - s_uLastTimeKeyFrameMovedDown = g_TimeNow; - } - } - - // RX Quality going up ? - - //if ( iHighestRXQuality > s_iLastRXQualityMaximumForKeyFrame ) - { - if ( iHighestRXQuality > 70 ) - if ( g_TimeNow > s_uLastTimeKeyFrameMovedDown + 400 ) - if ( g_TimeNow > s_uLastTimeKeyFrameMovedUp + 400 ) - { - iNewKeyframeIntervalMs = g_SM_VideoLinkStats.overwrites.uCurrentActiveKeyframeMs*2; - s_uLastTimeKeyFrameMovedUp = g_TimeNow; - } - - if ( iHighestRXQuality > 90 ) - if ( g_TimeNow > s_uLastTimeKeyFrameMovedDown + 200 ) - if ( g_TimeNow > s_uLastTimeKeyFrameMovedUp + 200 ) - { - iNewKeyframeIntervalMs = g_SM_VideoLinkStats.overwrites.uCurrentActiveKeyframeMs*4; - s_uLastTimeKeyFrameMovedUp = g_TimeNow; - } - } - - s_iLastRXQualityMinimumForKeyFrame = iLowestRXQuality; - s_iLastRXQualityMaximumForKeyFrame = iHighestRXQuality; - - // Apply the new value, if it changed - if ( iNewKeyframeIntervalMs != 0 ) - video_link_auto_keyframe_set_local_requested_value(0, iNewKeyframeIntervalMs, "adaptive"); -} - -void video_link_auto_keyframe_periodic_loop() -{ - if ( (g_pCurrentModel == NULL) || (! g_pCurrentModel->hasCamera()) ) - return; - - if ( (g_TimeNow < g_TimeStart + 5000) || - (g_TimeNow < get_video_capture_start_program_time() + 3000) ) - return; - - static u32 s_uTimeLastVideoAutoKeyFrameCheck = 0; - if ( g_TimeNow < s_uTimeLastVideoAutoKeyFrameCheck+50 ) - return; - s_uTimeLastVideoAutoKeyFrameCheck = g_TimeNow; - - // Is relaying a vehicle and no own video is needed now? - // Then go to a fast keyframe interval so that relay video switching happens fast - - if ( ! relay_vehicle_must_forward_own_video(g_pCurrentModel) ) - { - if ( (g_SM_VideoLinkStats.overwrites.uCurrentActiveKeyframeMs != DEFAULT_VIDEO_KEYFRAME_INTERVAL_WHEN_RELAYING) || - (g_SM_VideoLinkStats.overwrites.uCurrentPendingKeyframeMs != DEFAULT_VIDEO_KEYFRAME_INTERVAL_WHEN_RELAYING) ) - { - video_link_auto_keyframe_set_local_requested_value(0, DEFAULT_VIDEO_KEYFRAME_INTERVAL_WHEN_RELAYING, "is relaying"); - log_line("[KeyFrame] Set default relay keframe level (%u ms) due to relaying other vehicle (VID %u)", DEFAULT_VIDEO_KEYFRAME_INTERVAL_WHEN_RELAYING, g_pCurrentModel->relay_params.uRelayedVehicleId); - } - } - - // One way link? Just make sure it's the current one - else if ( (NULL != g_pCurrentModel) && g_pCurrentModel->isVideoLinkFixedOneWay() && (g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile >= 0) && (g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile < MAX_VIDEO_LINK_PROFILES) ) - { - int iKeyframeMs = g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].keyframe_ms; - if ( iKeyframeMs < 0 ) - iKeyframeMs = -iKeyframeMs; - video_link_auto_keyframe_set_local_requested_value(0, iKeyframeMs, "fixed by user or one way"); - } - - // Fixed keyframe interval, set by user? (no adaptive keyframe) Just make sure it's the current one - else if ( (NULL != g_pCurrentModel) && (!(g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].uProfileEncodingFlags & VIDEO_PROFILE_ENCODING_FLAG_ENABLE_ADAPTIVE_VIDEO_KEYFRAME)) && (g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile >= 0) && (g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile < MAX_VIDEO_LINK_PROFILES) ) - { - int iKeyframeMs = g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].keyframe_ms; - if ( iKeyframeMs < 0 ) - iKeyframeMs = -iKeyframeMs; - video_link_auto_keyframe_set_local_requested_value(0, iKeyframeMs, "fixed by user or one way"); - } - - // do Auto adjustments - else - video_link_auto_keyframe_do_adjustments(); - - // ---------------------------------------------- - // Apply the new value, if it changed, whether initial changed, manual changed or auto adjusted - - if ( g_SM_VideoLinkStats.overwrites.uCurrentControllerRequestedKeyframeMs == g_SM_VideoLinkStats.overwrites.uCurrentActiveKeyframeMs ) - if ( g_SM_VideoLinkStats.overwrites.uCurrentLocalRequestedKeyframeMs == g_SM_VideoLinkStats.overwrites.uCurrentActiveKeyframeMs ) - return; - - u32 uTimeNewest = g_SM_VideoLinkStats.overwrites.uLastTimeLocalRequestedAKeyframe; - u16 uNewKeyframeMs = g_SM_VideoLinkStats.overwrites.uCurrentLocalRequestedKeyframeMs; - if ( g_SM_VideoLinkStats.overwrites.uLastTimeControllerRequestedAKeyframe > uTimeNewest ) - if ( 0 != g_SM_VideoLinkStats.overwrites.uCurrentControllerRequestedKeyframeMs ) - { - uTimeNewest = g_SM_VideoLinkStats.overwrites.uLastTimeControllerRequestedAKeyframe; - uNewKeyframeMs = g_SM_VideoLinkStats.overwrites.uCurrentControllerRequestedKeyframeMs; - } - if ( (0 != uNewKeyframeMs) && ( 0 != uTimeNewest) ) - if ( uNewKeyframeMs != g_SM_VideoLinkStats.overwrites.uCurrentPendingKeyframeMs ) - { - g_SM_VideoLinkStats.overwrites.uCurrentPendingKeyframeMs = uNewKeyframeMs; - if ( g_SM_VideoLinkStats.overwrites.uCurrentPendingKeyframeMs < DEFAULT_VIDEO_MIN_AUTO_KEYFRAME_INTERVAL ) - g_SM_VideoLinkStats.overwrites.uCurrentPendingKeyframeMs = DEFAULT_VIDEO_MIN_AUTO_KEYFRAME_INTERVAL; - if ( g_SM_VideoLinkStats.overwrites.uCurrentPendingKeyframeMs > 10000 ) - g_SM_VideoLinkStats.overwrites.uCurrentPendingKeyframeMs = 10000; - } -} diff --git a/code/r_vehicle/video_link_auto_keyframe.h b/code/r_vehicle/video_link_auto_keyframe.h deleted file mode 100644 index a78f26b8..00000000 --- a/code/r_vehicle/video_link_auto_keyframe.h +++ /dev/null @@ -1,6 +0,0 @@ -#pragma once - -void video_link_auto_keyframe_init(); -void video_link_auto_keyframe_periodic_loop(); -void video_link_auto_keyframe_set_controller_requested_value(int iVideoStreamIndex, int iKeyframeMs); -void video_link_auto_keyframe_set_local_requested_value(int iVideoStreamIndex, int iKeyframeMs, const char* szReason); diff --git a/code/r_vehicle/video_link_check_bitrate.cpp b/code/r_vehicle/video_link_check_bitrate.cpp deleted file mode 100644 index 001a8f0a..00000000 --- a/code/r_vehicle/video_link_check_bitrate.cpp +++ /dev/null @@ -1,435 +0,0 @@ -/* - Ruby Licence - Copyright (c) 2024 Petru Soroaga petrusoroaga@yahoo.com - All rights reserved. - - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Copyright info and developer info must be preserved as is in the user - interface, additions could be made to that info. - * Neither the name of the organization nor the - names of its contributors may be used to endorse or promote products - derived from this software without specific prior written permission. - * Military use is not permited. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - DISCLAIMED. IN NO EVENT SHALL Julien Verneuil BE LIABLE FOR ANY - DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*/ - -#include "../base/base.h" -#include "../base/config.h" -#include "../base/shared_mem.h" -#include "../common/string_utils.h" - -#include "video_link_stats_overwrites.h" -#include "video_link_check_bitrate.h" -#include "processor_tx_video.h" -#include "ruby_rt_vehicle.h" -#include "utils_vehicle.h" -#include "packets_utils.h" -#include "video_source_csi.h" -#include "timers.h" -#include "shared_vars.h" - -u32 s_uTimeLastH264QuantizationCheck =0; -u32 s_uTimeLastQuantizationChangeTime = 0; -u8 s_uLastQuantizationSet = 0; -u8 s_uQuantizationOverflowValue = 0; - -void video_link_set_last_quantization_set(u8 uQuantValue) -{ - s_uLastQuantizationSet = uQuantValue; - s_uTimeLastQuantizationChangeTime = g_TimeNow; -} - -void video_link_set_fixed_quantization_values(u8 uQuantValue) -{ - g_SM_VideoLinkStats.overwrites.currentH264QUantization = uQuantValue; - video_link_set_last_quantization_set(g_SM_VideoLinkStats.overwrites.currentH264QUantization); - if ( g_pCurrentModel->hasCamera() ) - if ( g_pCurrentModel->isActiveCameraCSICompatible() || g_pCurrentModel->isActiveCameraVeye() ) - { - video_source_csi_send_control_message( RASPIVID_COMMAND_ID_QUANTIZATION_INIT, g_SM_VideoLinkStats.overwrites.currentH264QUantization ); - video_source_csi_send_control_message( RASPIVID_COMMAND_ID_QUANTIZATION_MIN, g_SM_VideoLinkStats.overwrites.currentH264QUantization ); - } - s_uLastQuantizationSet = uQuantValue; - s_uTimeLastQuantizationChangeTime = g_TimeNow; -} - -u8 video_link_get_oveflow_quantization_value() -{ - return s_uQuantizationOverflowValue; -} - -void video_link_reset_overflow_quantization_value() -{ - s_uQuantizationOverflowValue = 0; -} - -int video_link_get_default_quantization_for_videobitrate(u32 uVideoBitRate) -{ - if ( uVideoBitRate < 2000000 ) - return 40; - else if ( uVideoBitRate < 5000000 ) - return 30; - else - return 20; -} - -void video_link_quantization_shift(int iDelta) -{ - if (!((g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].uProfileEncodingFlags) & VIDEO_PROFILE_ENCODING_FLAG_ENABLE_VIDEO_ADAPTIVE_H264_QUANTIZATION) ) - { - if ( 0 != g_SM_VideoLinkStats.overwrites.currentH264QUantization ) - video_link_set_fixed_quantization_values(0); - return; - } - - if ( 0 == g_SM_VideoLinkStats.overwrites.currentH264QUantization ) - g_SM_VideoLinkStats.overwrites.currentH264QUantization = video_link_get_default_quantization_for_videobitrate(g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate); - - g_SM_VideoLinkStats.overwrites.currentH264QUantization += iDelta; - - if ( g_SM_VideoLinkStats.overwrites.currentH264QUantization > 44 ) - g_SM_VideoLinkStats.overwrites.currentH264QUantization = 44; - - if ( g_SM_VideoLinkStats.overwrites.currentH264QUantization < 4 ) - g_SM_VideoLinkStats.overwrites.currentH264QUantization = 4; - - if ( g_SM_VideoLinkStats.overwrites.currentH264QUantization == s_uLastQuantizationSet ) - return; - - s_uLastQuantizationSet = g_SM_VideoLinkStats.overwrites.currentH264QUantization; - s_uTimeLastQuantizationChangeTime = g_TimeNow; - if ( g_pCurrentModel->hasCamera() ) - if ( g_pCurrentModel->isActiveCameraCSICompatible() || g_pCurrentModel->isActiveCameraVeye() ) - video_source_csi_send_control_message(RASPIVID_COMMAND_ID_QUANTIZATION_MIN, g_SM_VideoLinkStats.overwrites.currentH264QUantization); -} - - -void video_link_check_adjust_quantization_for_overload_periodic_loop() -{ - if ( (g_pCurrentModel == NULL) || (! g_pCurrentModel->hasCamera()) || g_pCurrentModel->isVideoLinkFixedOneWay() ) - return; - if ( g_bVideoPaused ) - return; - - // Auto quantization is turned off ? - if (!((g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].uProfileEncodingFlags) & VIDEO_PROFILE_ENCODING_FLAG_ENABLE_VIDEO_ADAPTIVE_H264_QUANTIZATION) ) - { - if ( 0 != g_SM_VideoLinkStats.overwrites.currentH264QUantization ) - video_link_set_fixed_quantization_values(0); - return; - } - - // To fix : add autoquantization to sigmastar boards - if ( hardware_board_is_openipc(hardware_getBoardType()) ) - return; - - if ( hardware_board_is_goke(hardware_getBoardType()) ) - return; - - if ( (0 == get_video_capture_start_program_time()) || (g_TimeNow < get_video_capture_start_program_time() + 4000) ) - return; - - if ( g_TimeNow < s_uTimeLastQuantizationChangeTime + 200 ) - return; - - if ( g_TimeNow < g_TimeLastVideoProfileChanged + 500 ) - return; - - if ( g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].uProfileEncodingFlags & VIDEO_PROFILE_ENCODING_FLAG_VIDEO_ADAPTIVE_QUANTIZATION_STRENGTH_HIGH ) - if ( g_TimeNow < s_uTimeLastH264QuantizationCheck + 100 ) - return; - - if ( ! (g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].uProfileEncodingFlags & VIDEO_PROFILE_ENCODING_FLAG_VIDEO_ADAPTIVE_QUANTIZATION_STRENGTH_HIGH ) ) - if ( g_TimeNow < s_uTimeLastH264QuantizationCheck + 200 ) - return; - - s_uTimeLastH264QuantizationCheck = g_TimeNow; - - u32 uTotalVideoBitRateAvgFast = g_pProcessorTxVideo->getCurrentTotalVideoBitrateAverageLastMs(250); - u32 uTotalVideoBitRateAvgSlow = g_pProcessorTxVideo->getCurrentTotalVideoBitrateAverageLastMs(1000); - - u32 uVideoBitRateAvgFast = g_pProcessorTxVideo->getCurrentVideoBitrateAverageLastMs(250); - u32 uVideoBitRateAvgSlow = g_pProcessorTxVideo->getCurrentVideoBitrateAverageLastMs(1000); - - // This in Mbps. 1 = 1 Mbps - u32 uMinVideoMbBitrateTxUsed = (u32) get_last_tx_minimum_video_radio_datarate_bps()/1000/1000; - if ( uMinVideoMbBitrateTxUsed < 2 ) - { - log_softerror_and_alarm("VideoAdaptiveBitrate: Invalid minimum tx video radio datarate: %d Mbps. Reseting to 2 Mbps.", uMinVideoMbBitrateTxUsed); - uMinVideoMbBitrateTxUsed = 2; - send_alarm_to_controller(ALARM_ID_VEHICLE_VIDEO_TX_BITRATE_TOO_LOW, 0, 0, 2); - } - - u32 uMaxVideoBitrateThreshold = (uMinVideoMbBitrateTxUsed * 1000 * 10) * DEFAULT_VIDEO_LINK_MAX_LOAD_PERCENT; - - /* - log_line(DEBUG bit rate avg500/tot500: %.1f / %.1f avg1000/tot1000: %.1f / %.1f, min/max: %u / %.1f, set: %.1f", - (float)uVideoBitRateAvgFast/1000.0/1000.0, (float)uTotalVideoBitRateAvgFast/1000.0/1000.0, - (float)uVideoBitRateAvgSlow/1000.0/1000.0, (float)uTotalVideoBitRateAvgSlow/1000.0/1000.0, - uMinVideoMbBitrateTxUsed, (float)uMaxVideoBitrateThreshold/1000.0/1000.0, - (float)g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate/1000.0/1000.0 ); - */ - - // Adaptive video is turned off - if (!((g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].uProfileEncodingFlags) & VIDEO_PROFILE_ENCODING_FLAG_ENABLE_ADAPTIVE_VIDEO_LINK) ) - { - int dTime = 1000; - bool bOverflow = false; - - if ( g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].uProfileEncodingFlags & VIDEO_PROFILE_ENCODING_FLAG_VIDEO_ADAPTIVE_QUANTIZATION_STRENGTH_HIGH ) - { - dTime = 250; - if ( uVideoBitRateAvgFast > g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate * 1.1 ) - bOverflow = true; - if ( uTotalVideoBitRateAvgFast > uMaxVideoBitrateThreshold*1.05 ) - bOverflow = true; - } - else - { - dTime = 500; - if ( uVideoBitRateAvgSlow > g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate * 1.1 ) - bOverflow = true; - if ( uTotalVideoBitRateAvgSlow > uMaxVideoBitrateThreshold*1.1 ) - bOverflow = true; - } - - if ( bOverflow ) - if ( g_TimeNow >= s_uTimeLastQuantizationChangeTime + dTime ) - { - if ( g_SM_VideoLinkStats.overwrites.currentH264QUantization != 0 ) - if ( g_SM_VideoLinkStats.overwrites.currentH264QUantization != 0xFF ) - if ( g_SM_VideoLinkStats.overwrites.currentH264QUantization > s_uQuantizationOverflowValue ) - s_uQuantizationOverflowValue = g_SM_VideoLinkStats.overwrites.currentH264QUantization; - video_link_quantization_shift(1); - return; - } - - if ( g_TimeNow >= s_uTimeLastQuantizationChangeTime + dTime*4 ) - { - if ( uVideoBitRateAvgSlow < g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate*0.9 ) - //if ( g_SM_VideoLinkStats.overwrites.currentH264QUantization > s_uQuantizationOverflowValue+1 ) - video_link_quantization_shift(-1); - } - return; - } - - // Check first if we must quickly shift bitrate down right now fast - - bool bOverflow = false; - int dTime = 400; - if ( g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].uProfileEncodingFlags & VIDEO_PROFILE_ENCODING_FLAG_VIDEO_ADAPTIVE_QUANTIZATION_STRENGTH_HIGH ) - { - dTime = 250; - if ( uVideoBitRateAvgFast >= g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate * 1.1 ) - bOverflow = true; - if ( uTotalVideoBitRateAvgFast > uMaxVideoBitrateThreshold*1.1 ) - bOverflow = true; - } - else - { - dTime = 500; - if ( uVideoBitRateAvgSlow >= g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate * 1.1 ) - bOverflow = true; - if ( uTotalVideoBitRateAvgSlow > uMaxVideoBitrateThreshold*1.1 ) - bOverflow = true; - } - - if ( bOverflow ) - if ( g_TimeNow >= s_uTimeLastQuantizationChangeTime + dTime ) - { - video_link_quantization_shift(2); - //if ( g_pCurrentModel->bDeveloperMode ) - // log_line("[DEV] Auto bitrate: Must qickly shift video bitrate down. Current video bitrate: %.1f Mbps, total video data: %.1f Mbps. Current set video bitrate: %.1f Mbps. Current Q: %d.", uVideoBitRate/1000.0/1000.0,uTotalSentVideoBitRate/1000.0/1000.0, g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate/1000.0/1000.0, g_SM_VideoLinkStats.overwrites.currentH264QUantization); - return; - } - - // Must shift bitrate down slower to target - - bOverflow = false; - if ( g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].uProfileEncodingFlags & VIDEO_PROFILE_ENCODING_FLAG_VIDEO_ADAPTIVE_QUANTIZATION_STRENGTH_HIGH ) - { - dTime = 250; - if ( uVideoBitRateAvgFast >= g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate * 1.05 ) - bOverflow = true; - if ( uTotalVideoBitRateAvgFast >= uMaxVideoBitrateThreshold * 1.05) - bOverflow = true; - } - else - { - dTime = 500; - if ( uVideoBitRateAvgSlow >= g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate * 1.05 ) - bOverflow = true; - if ( uTotalVideoBitRateAvgSlow >= uMaxVideoBitrateThreshold * 1.05) - bOverflow = true; - } - - if ( bOverflow ) - if ( g_TimeNow >= s_uTimeLastQuantizationChangeTime + dTime ) - { - video_link_quantization_shift(1); - //if ( g_pCurrentModel->bDeveloperMode ) - // log_line("[DEV] Auto bitrate: Must shift video bitrate down. Current video bitrate: %.1f Mbps, total video data: %.1f Mbps. Current set video bitrate: %.1f Mbps. Current Q: %d.", uVideoBitRate/1000.0/1000.0,uTotalSentVideoBitRate/1000.0/1000.0, g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate/1000.0/1000.0, g_SM_VideoLinkStats.overwrites.currentH264QUantization); - return; - } - - // Must shift up to target - - bool bUndeflow = false; - if ( g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].uProfileEncodingFlags & VIDEO_PROFILE_ENCODING_FLAG_VIDEO_ADAPTIVE_QUANTIZATION_STRENGTH_HIGH ) - { - dTime = 500; - if ( uVideoBitRateAvgFast < g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate*0.9 ) - bUndeflow = true; - } - else - { - dTime = 1000; - if ( uVideoBitRateAvgSlow < g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate*0.9 ) - bUndeflow = true; - } - - if ( bUndeflow ) - if ( g_TimeNow >= s_uTimeLastQuantizationChangeTime + dTime ) - { - video_link_quantization_shift(-1); - //if ( g_pCurrentModel->bDeveloperMode ) - // log_line("[DEV] Auto bitrate: Must shift video bitrate up. Current video bitrate: %.1f Mbps, total video data: %.1f Mbps. Current set video bitrate: %.1f Mbps. Current Q: %d.", uVideoBitRate/1000.0/1000.0,uTotalSentVideoBitRate/1000.0/1000.0, g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate/1000.0/1000.0, g_SM_VideoLinkStats.overwrites.currentH264QUantization); - return; - } -} - -void video_link_check_adjust_bitrate_for_overload() -{ - if ( (0 == get_video_capture_start_program_time()) || (g_TimeNow < get_video_capture_start_program_time() + 5000) ) - return; - if ( hardware_board_is_goke(hardware_getBoardType()) ) - return; - - if ( g_pCurrentModel->uDeveloperFlags & DEVELOPER_FLAGS_BIT_DISABLE_VIDEO_OVERLOAD_CHECK ) - return; - - static u32 sl_uTimeLastVideoOverloadCheck = 0; - - if ( g_TimeNow < sl_uTimeLastVideoOverloadCheck+100 ) - return; - - sl_uTimeLastVideoOverloadCheck = g_TimeNow; - - u32 uTotalSentVideoBitRateAverage = g_pProcessorTxVideo->getCurrentTotalVideoBitrateAverage(); - u32 uTotalSentVideoBitRateFast = g_pProcessorTxVideo->getCurrentTotalVideoBitrateAverageLastMs(250); - u32 uMaxTxTime = DEFAULT_TX_TIME_OVERLOAD; - if ( ((g_pCurrentModel->hwCapabilities.uBoardType & BOARD_TYPE_MASK) == BOARD_TYPE_PIZERO) || ( (g_pCurrentModel->hwCapabilities.uBoardType & BOARD_TYPE_MASK) == BOARD_TYPE_PIZEROW) ) - uMaxTxTime += 200; - uMaxTxTime += (uTotalSentVideoBitRateFast/1000/1000)*20; - - if ( g_pCurrentModel->video_params.uVideoExtraFlags & VIDEO_FLAG_IGNORE_TX_SPIKES ) - uMaxTxTime = 900; - - // This is in Mbps: 1 = 1 Mbps - int iMinVideoRadioTxMbBitrateUsed = get_last_tx_minimum_video_radio_datarate_bps()/1000/1000; - if ( iMinVideoRadioTxMbBitrateUsed < 2 ) - { - log_softerror_and_alarm("VideoAdaptiveBitrate: Invalid minimum tx video radio datarate: %d Mbps. Reseting to 2 Mbps.", iMinVideoRadioTxMbBitrateUsed); - iMinVideoRadioTxMbBitrateUsed = 2; - send_alarm_to_controller(ALARM_ID_VEHICLE_VIDEO_TX_BITRATE_TOO_LOW, 0, 0, 2); - } - - int iMaxAllowedThresholdAlarm = (iMinVideoRadioTxMbBitrateUsed * 1000 * 10) * DEFAULT_VIDEO_LINK_MAX_LOAD_PERCENT; - int iMaxAllowedThreshold = (iMinVideoRadioTxMbBitrateUsed * 1000 * 10) * DEFAULT_VIDEO_LINK_LOAD_PERCENT; - - // Check for TX time overload or data rate overload and lower video bitrate if needed - - bool bIsDataOverloadCondition = false; - bool bIsTxOverloadCondition = false; - bool bIsLocalOverloadCondition = false; - bool bIsDataRateOverloadCondition = false; - - // To do / fix: use now and averages per radio link, not total - - if ( g_RadioTxTimers.uComputedVideoTxTimeMilisecPerSecondNow > uMaxTxTime ) - { - bIsDataOverloadCondition = true; - bIsTxOverloadCondition = true; - } - - if ( (uTotalSentVideoBitRateFast > (u32)iMaxAllowedThreshold) || (uTotalSentVideoBitRateAverage > (u32)iMaxAllowedThreshold) ) - { - bIsDataOverloadCondition = true; - bIsDataRateOverloadCondition = true; - } - - if ( g_uTimeLastVideoTxOverload + 4000 > g_TimeNow ) - { - bIsDataOverloadCondition = true; - bIsLocalOverloadCondition = true; - } - - if ( ! bIsDataOverloadCondition ) - { - // There is no current overload condition - // Increase the video bitrate back to video profile target rate - - if ( g_TimeNow > g_TimeLastOverwriteBitrateDownOnTxOverload + 1000 ) - if ( g_TimeNow > g_TimeLastOverwriteBitrateUpOnTxOverload + 1000 ) - if ( (g_RadioTxTimers.uComputedTotalTxTimeMilisecPerSecondAverage < uMaxTxTime/3) && (uTotalSentVideoBitRateAverage < (u32)iMaxAllowedThreshold) ) - if ( video_stats_overwrites_decrease_videobitrate_overwrite() ) - g_TimeLastOverwriteBitrateUpOnTxOverload = g_TimeNow; - - return; - } - - // Is in overload condition. Decrease target video profile bitrate - - if ( g_TimeNow >= g_SM_VideoLinkStats.timeLastAdaptiveParamsChangeDown + 200 ) - if ( g_TimeNow >= g_TimeLastOverwriteBitrateDownOnTxOverload + 250 ) - if ( g_TimeNow >= g_TimeLastOverwriteBitrateUpOnTxOverload + 250 ) - if ( video_stats_overwrites_increase_videobitrate_overwrite(uTotalSentVideoBitRateFast) ) - { - //log_line("DBG in overload condition (local: %d), decrease bitrate. total sent fast: %u, sent: %u, max allowed: %u", (int)bIsLocalOverloadCondition, uTotalSentVideoBitRateFast, uTotalSentVideoBitRateAverage, (u32)iMaxAllowedThreshold); - //log_line("DBG video fast bitrate: %u, avg bitrate: %u", g_pProcessorTxVideo->getCurrentVideoBitrateAverageLastMs(250), g_pProcessorTxVideo->getCurrentVideoBitrateAverage() ); - //log_line("DBG current EC scheme: %d/%d, current level shift: %d", (int)g_SM_VideoLinkStats.overwrites.currentDataBlocks, (int) g_SM_VideoLinkStats.overwrites.currentECBlocks, (int)g_SM_VideoLinkStats.overwrites.currentProfileShiftLevel); - g_uTimeLastVideoTxOverload = 0; - g_TimeLastOverwriteBitrateDownOnTxOverload = g_TimeNow; - - u32 uParam = 0; - if ( bIsDataRateOverloadCondition ) - if ( (uTotalSentVideoBitRateAverage > (u32)iMaxAllowedThresholdAlarm) ) - { - u32 bitrateTotal = uTotalSentVideoBitRateAverage; - bitrateTotal /= 1000; - u32 bitrateBar = iMaxAllowedThreshold; - bitrateBar /= 1000; - uParam = (bitrateTotal & 0xFFFF) | (bitrateBar << 16); - send_alarm_to_controller(ALARM_ID_VEHICLE_VIDEO_DATA_OVERLOAD, uParam, 0, 2); - } - if ( bIsTxOverloadCondition ) - { - uParam = (g_RadioTxTimers.uComputedTotalTxTimeMilisecPerSecondNow & 0xFFFF) | ((uMaxTxTime & 0xFFFF) << 16); - send_alarm_to_controller(ALARM_ID_VEHICLE_VIDEO_TX_OVERLOAD, uParam, 0, 2); - } - if ( bIsLocalOverloadCondition ) - { - uParam = 0; - if ( video_stats_overwrites_get_time_last_shift_down() != 0 ) - if ( g_TimeNow > video_stats_overwrites_get_time_last_shift_down() + 1500 ) - send_alarm_to_controller(ALARM_ID_VEHICLE_VIDEO_TX_OVERLOAD, uParam, 1, 2); - } - } -} - diff --git a/code/r_vehicle/video_link_check_bitrate.h b/code/r_vehicle/video_link_check_bitrate.h deleted file mode 100644 index 9fdbc8e5..00000000 --- a/code/r_vehicle/video_link_check_bitrate.h +++ /dev/null @@ -1,13 +0,0 @@ -#pragma once -#include "../base/base.h" - -int video_link_get_default_quantization_for_videobitrate(u32 uVideoBitRate); -void video_link_check_adjust_quantization_for_overload_periodic_loop(); -void video_link_check_adjust_bitrate_for_overload(); - -void video_link_set_last_quantization_set(u8 uQuantValue); -void video_link_set_fixed_quantization_values(u8 uQuantValue); -void video_link_quantization_shift(int iDelta); -u8 video_link_get_oveflow_quantization_value(); -void video_link_reset_overflow_quantization_value(); - diff --git a/code/r_vehicle/video_link_stats_overwrites.cpp b/code/r_vehicle/video_link_stats_overwrites.cpp deleted file mode 100644 index 74eb5ca0..00000000 --- a/code/r_vehicle/video_link_stats_overwrites.cpp +++ /dev/null @@ -1,1271 +0,0 @@ -/* - Ruby Licence - Copyright (c) 2024 Petru Soroaga petrusoroaga@yahoo.com - All rights reserved. - - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Copyright info and developer info must be preserved as is in the user - interface, additions could be made to that info. - * Neither the name of the organization nor the - names of its contributors may be used to endorse or promote products - derived from this software without specific prior written permission. - * Military use is not permited. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - DISCLAIMED. IN NO EVENT SHALL Julien Verneuil BE LIABLE FOR ANY - DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*/ - -#include "../base/base.h" -#include "../base/config.h" -#include "../base/shared_mem.h" -#include "../base/ruby_ipc.h" -#include "../base/utils.h" -#include "../common/string_utils.h" - -#include "video_link_stats_overwrites.h" -#include "video_link_check_bitrate.h" -#include "video_link_auto_keyframe.h" -#include "processor_tx_video.h" -#include "ruby_rt_vehicle.h" -#include "utils_vehicle.h" -#include "packets_utils.h" -#include "video_source_csi.h" -#include "video_source_majestic.h" -#include "events.h" -#include "timers.h" -#include "shared_vars.h" - - -bool s_bUseControllerInfo = false; -int s_iTargetShiftLevel = 0; -int s_iTargetProfile = 0; -int s_iMaxLevelShiftForCurrentProfile = 0; -float s_fParamsChangeStrength = 0.0; // 0...1 - -u32 s_TimeLastHistorySwitchUpdate = 0; -u32 s_uTimeLastShiftLevelDown = 0; -int s_iLastTotalLevelsShift = 0; -int s_iLastTargetVideoFPS = 0; - -u32 s_uTimeStartGoodIntervalForProfileShiftUp = 0; - - -void _video_overwrites_set_capture_video_bitrate(u32 uBitrateBPS, bool bIsInitialValue, int iReason) -{ - if ( g_pCurrentModel->hasCamera() ) - if ( g_pCurrentModel->isActiveCameraCSICompatible() || g_pCurrentModel->isActiveCameraVeye() ) - video_source_csi_send_control_message(RASPIVID_COMMAND_ID_VIDEO_BITRATE, uBitrateBPS/100000); - - if ( g_pCurrentModel->hasCamera() ) - if ( g_pCurrentModel->isActiveCameraOpenIPC() ) - video_source_majestic_set_videobitrate_value(uBitrateBPS); - - if ( NULL != g_pProcessorTxVideo ) - g_pProcessorTxVideo->setLastSetCaptureVideoBitrate(uBitrateBPS, bIsInitialValue, 1); -} - - -// Needs to be sent to other processes when we change the currently selected video profile - -void _video_stats_overwrites_signal_changes() -{ - //log_line("[Video Link Overwrites]: signal others that currently active video profile changed, to reload model"); - - t_packet_header PH; - radio_packet_init(&PH, PACKET_COMPONENT_LOCAL_CONTROL, PACKET_TYPE_LOCAL_CONTROL_UPDATED_VIDEO_LINK_OVERWRITES, STREAM_ID_DATA); - PH.vehicle_id_src = PACKET_COMPONENT_RUBY; - PH.total_length = sizeof(t_packet_header) + sizeof(shared_mem_video_link_overwrites); - - u8 buffer[MAX_PACKET_TOTAL_SIZE]; - memcpy(buffer, (u8*)&PH, sizeof(t_packet_header)); - memcpy(buffer + sizeof(t_packet_header), (u8*)&(g_SM_VideoLinkStats.overwrites), sizeof(shared_mem_video_link_overwrites)); - - ruby_ipc_channel_send_message(s_fIPCRouterToCommands, buffer, PH.total_length); - - if ( NULL != g_pProcessStats ) - { - g_pProcessStats->lastIPCOutgoingTime = g_TimeNow; - g_pProcessStats->lastActiveTime = get_current_timestamp_ms(); - } -} - -void _video_stats_overwrites_signal_video_prifle_changed() -{ - t_packet_header PH; - radio_packet_init(&PH, PACKET_COMPONENT_LOCAL_CONTROL, PACKET_TYPE_LOCAL_CONTROL_VEHICLE_VIDEO_PROFILE_SWITCHED, STREAM_ID_DATA); - PH.vehicle_id_src = PACKET_COMPONENT_RUBY; - PH.vehicle_id_dest = (u32)g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile; - PH.total_length = sizeof(t_packet_header); - - if ( 0 != s_fIPCRouterToTelemetry ) - ruby_ipc_channel_send_message(s_fIPCRouterToTelemetry, (u8*)&PH, PH.total_length); - - if ( NULL != g_pProcessStats ) - g_pProcessStats->lastIPCOutgoingTime = g_TimeNow; - if ( NULL != g_pProcessStats ) - g_pProcessStats->lastActiveTime = get_current_timestamp_ms(); -} - -void video_stats_overwrites_init() -{ - log_line("[Video Link Overwrites]: Init and reset structures."); - memset((u8*)&g_SM_VideoLinkStats, 0, sizeof(shared_mem_video_link_stats_and_overwrites)); - memset((u8*)&g_SM_VideoLinkGraphs, 0, sizeof(shared_mem_video_link_graphs)); - - if ( NULL == g_pCurrentModel ) - return; - - g_pCurrentModel->video_link_profiles[VIDEO_PROFILE_MQ].width = g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].width; - g_pCurrentModel->video_link_profiles[VIDEO_PROFILE_MQ].height = g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].height; - g_pCurrentModel->video_link_profiles[VIDEO_PROFILE_MQ].fps = g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].fps; - g_pCurrentModel->video_link_profiles[VIDEO_PROFILE_LQ].width = g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].width; - g_pCurrentModel->video_link_profiles[VIDEO_PROFILE_LQ].height = g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].height; - - video_overwrites_init(&(g_SM_VideoLinkStats.overwrites), g_pCurrentModel); - onEventBeforeRuntimeCurrentVideoProfileChanged(g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile, g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile); - - video_link_reset_overflow_quantization_value(); - - g_SM_VideoLinkStats.backIntervalsToLookForDownStep = DEFAULT_MINIMUM_INTERVALS_FOR_VIDEO_LINK_ADJUSTMENT; - g_SM_VideoLinkStats.backIntervalsToLookForUpStep = 2*DEFAULT_MINIMUM_INTERVALS_FOR_VIDEO_LINK_ADJUSTMENT; - - g_SM_VideoLinkStats.timeLastStatsCheck = 0; - g_SM_VideoLinkStats.timeLastAdaptiveParamsChangeDown = 0; - g_SM_VideoLinkStats.timeLastAdaptiveParamsChangeUp = 0; - g_SM_VideoLinkStats.timeLastProfileChangeDown = 0; - g_SM_VideoLinkStats.timeLastProfileChangeUp = 0; - g_SM_VideoLinkStats.timeLastReceivedControllerLinkInfo = 0; - - g_SM_VideoLinkStats.usedControllerInfo = 0; - g_SM_VideoLinkStats.computed_min_interval_for_change_up = 500; - g_SM_VideoLinkStats.computed_min_interval_for_change_down = 200; - g_SM_VideoLinkStats.computed_rx_quality_vehicle = 0; - g_SM_VideoLinkStats.computed_rx_quality_controller = 0; - - for( int i=0; i uOverwriteDown ) - uTargetVideoBitrate -= uOverwriteDown; - else - uTargetVideoBitrate = 0; - - if ( uTargetVideoBitrate < 250000 ) - uTargetVideoBitrate = 250000; - - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile != VIDEO_PROFILE_USER ) - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile != VIDEO_PROFILE_BEST_PERF ) - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile != VIDEO_PROFILE_HIGH_QUALITY ) - if ( uTargetVideoBitrate < g_pCurrentModel->video_params.lowestAllowedAdaptiveVideoBitrate ) - uTargetVideoBitrate = g_pCurrentModel->video_params.lowestAllowedAdaptiveVideoBitrate; - - // Minimum for MQ profile, for 12 Mb datarate is at least 2Mb, do not go below that - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile == VIDEO_PROFILE_MQ ) - if ( g_pCurrentModel->video_link_profiles[VIDEO_PROFILE_MQ].radio_datarate_video_bps == 0 ) - if ( getRealDataRateFromRadioDataRate(g_pCurrentModel->video_link_profiles[g_SM_VideoLinkStats.overwrites.userVideoLinkProfile].radio_datarate_video_bps, 0) >= 12000000 ) - if ( uTargetVideoBitrate < 2000000 ) - uTargetVideoBitrate = 2000000; - - return uTargetVideoBitrate; -} - -void video_stats_overwrites_switch_to_profile_and_level(int iTotalLevelsShift, int iVideoProfile, int iLevelShift) -{ - onEventBeforeRuntimeCurrentVideoProfileChanged(g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile, iVideoProfile); - - if ( iTotalLevelsShift > s_iLastTotalLevelsShift ) - s_uTimeLastShiftLevelDown = g_TimeNow; - s_iLastTotalLevelsShift = iTotalLevelsShift; - - bool bVideoProfileChanged = false; - - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile != iVideoProfile ) - bVideoProfileChanged = true; - - g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile = iVideoProfile; - g_SM_VideoLinkStats.overwrites.uTimeSetCurrentVideoLinkProfile = g_TimeNow; - g_SM_VideoLinkStats.overwrites.currentProfileShiftLevel = iLevelShift; - g_SM_VideoLinkStats.overwrites.currentProfileMaxVideoBitrate = utils_get_max_allowed_video_bitrate_for_profile_or_user_video_bitrate( g_pCurrentModel, g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile); - - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile == VIDEO_PROFILE_LQ ) - g_SM_VideoLinkStats.overwrites.hasEverSwitchedToLQMode = 1; - - if ( 0 == g_SM_VideoLinkStats.overwrites.currentProfileMaxVideoBitrate ) - { - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile == VIDEO_PROFILE_LQ ) - g_SM_VideoLinkStats.overwrites.currentProfileMaxVideoBitrate = g_pCurrentModel->video_link_profiles[VIDEO_PROFILE_MQ].bitrate_fixed_bps; - } - if ( 0 == g_SM_VideoLinkStats.overwrites.currentProfileMaxVideoBitrate ) - { - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile == VIDEO_PROFILE_LQ || - g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile == VIDEO_PROFILE_MQ ) - g_SM_VideoLinkStats.overwrites.currentProfileMaxVideoBitrate = g_pCurrentModel->video_link_profiles[g_SM_VideoLinkStats.overwrites.userVideoLinkProfile].bitrate_fixed_bps; - } - g_SM_VideoLinkStats.overwrites.currentProfileAndLevelDefaultBitrate = utils_get_max_allowed_video_bitrate_for_profile_and_level(g_pCurrentModel, g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile, (int)g_SM_VideoLinkStats.overwrites.currentProfileShiftLevel); - - int iData = 0; - int iEC = 0; - g_pCurrentModel->get_level_shift_ec_scheme(iTotalLevelsShift, &iData, &iEC); - - g_SM_VideoLinkStats.overwrites.currentDataBlocks = iData; - g_SM_VideoLinkStats.overwrites.currentECBlocks = iEC; - - g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate = _get_bitrate_for_current_level_and_profile_including_overwrites(); - - g_pCurrentModel->video_link_profiles[g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile].width = g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].width; - g_pCurrentModel->video_link_profiles[g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile].height = g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].height; - - // Do not use more video bitrate than the user set originaly in the user profile - - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile == VIDEO_PROFILE_MQ || g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile == VIDEO_PROFILE_LQ ) - if ( g_SM_VideoLinkStats.overwrites.currentProfileMaxVideoBitrate > g_pCurrentModel->video_link_profiles[g_SM_VideoLinkStats.overwrites.userVideoLinkProfile].bitrate_fixed_bps ) - { - g_SM_VideoLinkStats.overwrites.currentProfileMaxVideoBitrate = g_pCurrentModel->video_link_profiles[g_SM_VideoLinkStats.overwrites.userVideoLinkProfile].bitrate_fixed_bps; - g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate = _get_bitrate_for_current_level_and_profile_including_overwrites(); - } - - if ( g_SM_VideoLinkStats.overwrites.currentECBlocks > MAX_FECS_PACKETS_IN_BLOCK ) - g_SM_VideoLinkStats.overwrites.currentECBlocks = MAX_FECS_PACKETS_IN_BLOCK; - - g_SM_VideoLinkStats.historySwitches[0] = g_SM_VideoLinkStats.overwrites.currentProfileShiftLevel | (g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile<<4); - g_SM_VideoLinkStats.totalSwitches++; - - g_SM_VideoLinkStats.timeLastAdaptiveParamsChangeDown = g_TimeNow; - g_SM_VideoLinkStats.timeLastProfileChangeDown = g_TimeNow; - g_SM_VideoLinkStats.timeLastAdaptiveParamsChangeUp = g_TimeNow; - g_SM_VideoLinkStats.timeLastProfileChangeUp = g_TimeNow; - - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile != VIDEO_PROFILE_USER ) - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile != VIDEO_PROFILE_HIGH_QUALITY ) - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile != VIDEO_PROFILE_BEST_PERF ) - if ( g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate < g_pCurrentModel->video_params.lowestAllowedAdaptiveVideoBitrate ) - g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate = g_pCurrentModel->video_params.lowestAllowedAdaptiveVideoBitrate; - - if ( g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate < 200000 ) - g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate = 200000; - - // If video profile changed (so did the target video bitrate): - // Send default video quantization param to raspivid based on target video bitrate - if ( bVideoProfileChanged ) - if ((g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].uProfileEncodingFlags) & VIDEO_PROFILE_ENCODING_FLAG_ENABLE_VIDEO_ADAPTIVE_H264_QUANTIZATION ) - { - g_SM_VideoLinkStats.overwrites.currentH264QUantization = 0; - video_link_quantization_shift(2); - } - - _video_overwrites_set_capture_video_bitrate(g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate, false, 1); - - // Prevent video bitrate changes due to overload or underload right after a profile change; - // (A profile change usually sets a new, different video bitrate, so ignore this delta for a while) - - g_TimeLastOverwriteBitrateDownOnTxOverload = g_TimeNow; - g_TimeLastOverwriteBitrateUpOnTxOverload = g_TimeNow; - - // Signal other components (rx_command) to refresh their copy of current video overwrites structure - - _video_stats_overwrites_signal_changes(); - _video_stats_overwrites_signal_video_prifle_changed(); - - // Signal tx video that encoding scheme changed - - process_data_tx_video_signal_encoding_changed(); - -} - -void _video_stats_overwrites_apply_profile_changes(bool bDownDirection) -{ - onEventBeforeRuntimeCurrentVideoProfileChanged(g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile, s_iTargetProfile); - - g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile = s_iTargetProfile; - g_SM_VideoLinkStats.overwrites.uTimeSetCurrentVideoLinkProfile = g_TimeNow; - g_SM_VideoLinkStats.overwrites.currentProfileShiftLevel = s_iTargetShiftLevel; - g_SM_VideoLinkStats.overwrites.currentProfileMaxVideoBitrate = utils_get_max_allowed_video_bitrate_for_profile_or_user_video_bitrate(g_pCurrentModel, g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile); - - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile == VIDEO_PROFILE_LQ ) - g_SM_VideoLinkStats.overwrites.hasEverSwitchedToLQMode = 1; - - if ( 0 == g_SM_VideoLinkStats.overwrites.currentProfileMaxVideoBitrate ) - { - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile == VIDEO_PROFILE_LQ ) - g_SM_VideoLinkStats.overwrites.currentProfileMaxVideoBitrate = g_pCurrentModel->video_link_profiles[VIDEO_PROFILE_MQ].bitrate_fixed_bps; - } - if ( 0 == g_SM_VideoLinkStats.overwrites.currentProfileMaxVideoBitrate ) - { - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile == VIDEO_PROFILE_LQ || - g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile == VIDEO_PROFILE_MQ ) - g_SM_VideoLinkStats.overwrites.currentProfileMaxVideoBitrate = g_pCurrentModel->video_link_profiles[g_SM_VideoLinkStats.overwrites.userVideoLinkProfile].bitrate_fixed_bps; - } - g_SM_VideoLinkStats.overwrites.currentProfileAndLevelDefaultBitrate = utils_get_max_allowed_video_bitrate_for_profile_and_level(g_pCurrentModel, g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile, (int)g_SM_VideoLinkStats.overwrites.currentProfileShiftLevel); - - g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate = _get_bitrate_for_current_level_and_profile_including_overwrites(); - g_SM_VideoLinkStats.overwrites.currentDataBlocks = g_pCurrentModel->video_link_profiles[g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile].block_packets; - g_SM_VideoLinkStats.overwrites.currentECBlocks = g_pCurrentModel->video_link_profiles[g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile].block_fecs; - - - g_pCurrentModel->video_link_profiles[g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile].width = g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].width; - g_pCurrentModel->video_link_profiles[g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile].height = g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].height; - - // Do not use more video bitrate than the user set originaly in the user profile - - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile == VIDEO_PROFILE_MQ || g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile == VIDEO_PROFILE_LQ ) - if ( g_SM_VideoLinkStats.overwrites.currentProfileMaxVideoBitrate > g_pCurrentModel->video_link_profiles[g_SM_VideoLinkStats.overwrites.userVideoLinkProfile].bitrate_fixed_bps ) - { - g_SM_VideoLinkStats.overwrites.currentProfileMaxVideoBitrate = g_pCurrentModel->video_link_profiles[g_SM_VideoLinkStats.overwrites.userVideoLinkProfile].bitrate_fixed_bps; - g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate = _get_bitrate_for_current_level_and_profile_including_overwrites(); - } - - if ( g_SM_VideoLinkStats.overwrites.currentECBlocks > MAX_FECS_PACKETS_IN_BLOCK ) - g_SM_VideoLinkStats.overwrites.currentECBlocks = MAX_FECS_PACKETS_IN_BLOCK; - - g_SM_VideoLinkStats.historySwitches[0] = g_SM_VideoLinkStats.overwrites.currentProfileShiftLevel | (g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile<<4); - g_SM_VideoLinkStats.totalSwitches++; - - if ( bDownDirection ) - { - g_SM_VideoLinkStats.timeLastAdaptiveParamsChangeDown = g_TimeNow; - g_SM_VideoLinkStats.timeLastProfileChangeDown = g_TimeNow; - } - else - { - g_SM_VideoLinkStats.timeLastAdaptiveParamsChangeUp = g_TimeNow; - g_SM_VideoLinkStats.timeLastProfileChangeUp = g_TimeNow; - } - - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile != VIDEO_PROFILE_USER ) - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile != VIDEO_PROFILE_HIGH_QUALITY ) - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile != VIDEO_PROFILE_BEST_PERF ) - if ( g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate < g_pCurrentModel->video_params.lowestAllowedAdaptiveVideoBitrate ) - g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate = g_pCurrentModel->video_params.lowestAllowedAdaptiveVideoBitrate; - - if ( g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate < 200000 ) - g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate = 200000; - - _video_overwrites_set_capture_video_bitrate(g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate, false, 2); - - int nTargetFPS = g_pCurrentModel->video_link_profiles[g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile].fps; - if ( 0 == nTargetFPS ) - nTargetFPS = g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].fps; - - if ( s_iLastTargetVideoFPS != nTargetFPS ) - { - s_iLastTargetVideoFPS = nTargetFPS; - - log_line("[Video Link Overwrites]: Setting the video capture FPS rate to: %d", nTargetFPS); - if ( g_pCurrentModel->hasCamera() ) - if ( g_pCurrentModel->isActiveCameraCSICompatible() || g_pCurrentModel->isActiveCameraVeye() ) - video_source_csi_send_control_message(RASPIVID_COMMAND_ID_FPS, nTargetFPS); - } - - // Prevent video bitrate changes due to overload or underload right after a profile change; - // (A profile change usually sets a new, different video bitrate, so ignore this delta for a while) - - g_TimeLastOverwriteBitrateDownOnTxOverload = g_TimeNow; - g_TimeLastOverwriteBitrateUpOnTxOverload = g_TimeNow; - - // Signal other components (rx_command) to refresh their copy of current video overwrites structure - - _video_stats_overwrites_signal_changes(); - _video_stats_overwrites_signal_video_prifle_changed(); - - // Signal tx video that encoding scheme changed - - process_data_tx_video_signal_encoding_changed(); -} - -void _video_stats_overwrites_apply_ec_bitrate_changes(bool bDownDirection) -{ - if ( bDownDirection ) - g_SM_VideoLinkStats.timeLastAdaptiveParamsChangeDown = g_TimeNow; - else - g_SM_VideoLinkStats.timeLastAdaptiveParamsChangeUp = g_TimeNow; - - g_SM_VideoLinkStats.historySwitches[0] = g_SM_VideoLinkStats.overwrites.currentProfileShiftLevel | (g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile<<4); - g_SM_VideoLinkStats.totalSwitches++; - - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile != VIDEO_PROFILE_USER ) - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile != VIDEO_PROFILE_HIGH_QUALITY ) - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile != VIDEO_PROFILE_BEST_PERF ) - if ( g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate < g_pCurrentModel->video_params.lowestAllowedAdaptiveVideoBitrate ) - g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate = g_pCurrentModel->video_params.lowestAllowedAdaptiveVideoBitrate; - - if ( g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate < 200000 ) - g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate = 200000; - - _video_overwrites_set_capture_video_bitrate(g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate, false, 3); - - // Signal other components (rx_command) to refresh their copy of current video overwrites structure - - _video_stats_overwrites_signal_changes(); - _video_stats_overwrites_signal_video_prifle_changed(); - - // Signal tx video that encoding scheme changed - - process_data_tx_video_signal_encoding_changed(); -} - -void video_stats_overwrites_reset_to_highest_level() -{ - log_line("[Video Link Overwrites]: Reset to highest level (user profile, no adjustment)."); - - s_iTargetProfile = g_pCurrentModel->video_params.user_selected_video_link_profile; - s_iTargetShiftLevel = 0; - - g_SM_VideoLinkStats.overwrites.profilesTopVideoBitrateOverwritesDownward[g_pCurrentModel->video_params.user_selected_video_link_profile] = 0; - for( int i=0; i (int)g_SM_VideoLinkStats.overwrites.currentProfileShiftLevel ) - { - bTriedAnyShift = true; - s_uTimeStartGoodIntervalForProfileShiftUp = 0; - - if ( (g_iForcedVideoProfile != -1) && s_iTargetShiftLevel > s_iMaxLevelShiftForCurrentProfile ) - s_iTargetShiftLevel = s_iMaxLevelShiftForCurrentProfile; - - if ( s_iTargetShiftLevel > s_iMaxLevelShiftForCurrentProfile ) - { - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile == g_SM_VideoLinkStats.overwrites.userVideoLinkProfile ) - { - s_iTargetProfile = VIDEO_PROFILE_MQ; - s_iTargetShiftLevel = 0; - } - else if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile == VIDEO_PROFILE_MQ ) - { - s_iTargetProfile = VIDEO_PROFILE_LQ; - s_iTargetShiftLevel = 0; - } - else - { - // Reached bottom profile, can't go lower - s_iTargetShiftLevel = s_iMaxLevelShiftForCurrentProfile; - } - } - - if ( s_iTargetProfile != g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile ) - { - _video_stats_overwrites_apply_profile_changes(true); - } - else if ( s_iTargetShiftLevel != (int)g_SM_VideoLinkStats.overwrites.currentProfileShiftLevel ) - { - g_SM_VideoLinkStats.overwrites.currentProfileShiftLevel = s_iTargetShiftLevel; - g_SM_VideoLinkStats.overwrites.currentECBlocks = g_pCurrentModel->video_link_profiles[g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile].block_fecs; - g_SM_VideoLinkStats.overwrites.currentECBlocks += g_SM_VideoLinkStats.overwrites.currentProfileShiftLevel; - - if ( g_SM_VideoLinkStats.overwrites.currentECBlocks > MAX_FECS_PACKETS_IN_BLOCK ) - g_SM_VideoLinkStats.overwrites.currentECBlocks = MAX_FECS_PACKETS_IN_BLOCK; - if ( g_SM_VideoLinkStats.overwrites.currentECBlocks < g_pCurrentModel->video_link_profiles[g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile].block_fecs ) - g_SM_VideoLinkStats.overwrites.currentECBlocks = g_pCurrentModel->video_link_profiles[g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile].block_fecs; - if ( g_SM_VideoLinkStats.overwrites.currentECBlocks > 3*g_pCurrentModel->video_link_profiles[g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile].block_packets ) - g_SM_VideoLinkStats.overwrites.currentECBlocks = 3*g_pCurrentModel->video_link_profiles[g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile].block_packets; - - g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate = _get_bitrate_for_current_level_and_profile_including_overwrites(); - - _video_stats_overwrites_apply_ec_bitrate_changes(true); - } - return; - } - - if ( s_iTargetProfile == g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile ) - if ( s_iTargetShiftLevel < (int)g_SM_VideoLinkStats.overwrites.currentProfileShiftLevel ) - { - bTriedAnyShift = true; - - if ( (g_iForcedVideoProfile != -1) && s_iTargetShiftLevel < 0 ) - s_iTargetShiftLevel = 0; - - if ( s_iTargetShiftLevel < 0 ) - { - if ( 0 == s_uTimeStartGoodIntervalForProfileShiftUp ) - s_uTimeStartGoodIntervalForProfileShiftUp = g_TimeNow; - else if ( g_TimeNow >= s_uTimeStartGoodIntervalForProfileShiftUp + DEFAULT_MINIMUM_OK_INTERVAL_MS_TO_SWITCH_VIDEO_PROFILE_UP ) - { - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile == VIDEO_PROFILE_LQ ) - { - s_iTargetProfile = VIDEO_PROFILE_MQ; - s_iTargetShiftLevel = g_pCurrentModel->video_link_profiles[VIDEO_PROFILE_MQ].block_packets - g_pCurrentModel->video_link_profiles[VIDEO_PROFILE_MQ].block_fecs; - } - else if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile == VIDEO_PROFILE_MQ ) - { - s_iTargetProfile = g_SM_VideoLinkStats.overwrites.userVideoLinkProfile; - s_iTargetShiftLevel = g_pCurrentModel->video_link_profiles[g_SM_VideoLinkStats.overwrites.userVideoLinkProfile].block_packets - g_pCurrentModel->video_link_profiles[g_SM_VideoLinkStats.overwrites.userVideoLinkProfile].block_fecs; - - } - else - { - // Reached top profile, can't go higher - s_iTargetShiftLevel = 0; - } - s_uTimeStartGoodIntervalForProfileShiftUp = 0; - } - } - - if ( s_iTargetProfile != g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile ) - { - _video_stats_overwrites_apply_profile_changes(false); - } - else if ( s_iTargetShiftLevel != (int)g_SM_VideoLinkStats.overwrites.currentProfileShiftLevel ) - { - if ( s_iTargetShiftLevel < 0 ) - s_iTargetShiftLevel = 0; - - g_SM_VideoLinkStats.overwrites.currentProfileShiftLevel = s_iTargetShiftLevel; - g_SM_VideoLinkStats.overwrites.currentECBlocks = g_pCurrentModel->video_link_profiles[g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile].block_fecs; - g_SM_VideoLinkStats.overwrites.currentECBlocks += g_SM_VideoLinkStats.overwrites.currentProfileShiftLevel; - - if ( g_SM_VideoLinkStats.overwrites.currentECBlocks > MAX_FECS_PACKETS_IN_BLOCK ) - g_SM_VideoLinkStats.overwrites.currentECBlocks = MAX_FECS_PACKETS_IN_BLOCK; - if ( g_SM_VideoLinkStats.overwrites.currentECBlocks < g_pCurrentModel->video_link_profiles[g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile].block_fecs ) - g_SM_VideoLinkStats.overwrites.currentECBlocks = g_pCurrentModel->video_link_profiles[g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile].block_fecs; - - g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate = _get_bitrate_for_current_level_and_profile_including_overwrites(); - - _video_stats_overwrites_apply_ec_bitrate_changes(false); - } - return; - } - - if ( ! bTriedAnyShift ) - s_uTimeStartGoodIntervalForProfileShiftUp = 0; -} - -void _check_video_link_params_using_vehicle_info(bool bDownDirection) -{ - int vehicleMinRXQuality = 255; - int vehicleAvgRXQuality = 0; - int iCountRetransmissions = 0; - int iCountPacketsRetried = 0; - int iCountIntervalsWithRetires = 0; - - int iIntervals = g_SM_VideoLinkStats.backIntervalsToLookForUpStep; - if ( bDownDirection ) - iIntervals = g_SM_VideoLinkStats.backIntervalsToLookForDownStep; - if ( iIntervals > MAX_INTERVALS_VIDEO_LINK_STATS ) - iIntervals = MAX_INTERVALS_VIDEO_LINK_STATS; - - for( int i=0; i 0 ) - { - iCountPacketsRetried += g_SM_VideoLinkGraphs.vehicleReceivedRetransmissionsRequestsPacketsRetried[i]; - iCountIntervalsWithRetires++; - } - } - - if ( vehicleMinRXQuality == 255 ) - vehicleMinRXQuality = 0; - - vehicleAvgRXQuality /= g_SM_VideoLinkStats.backIntervalsToLookForDownStep; - vehicleMinRXQuality = vehicleAvgRXQuality; - - g_SM_VideoLinkStats.computed_rx_quality_vehicle = vehicleMinRXQuality; - - int rxQualityPerLevel = (1.0-s_fParamsChangeStrength) * 50; - - if ( bDownDirection ) - { - if ( g_SM_VideoLinkStats.computed_rx_quality_vehicle < 90 ) - { - int iTargetLevel = (90-g_SM_VideoLinkStats.computed_rx_quality_vehicle)/rxQualityPerLevel; - s_iTargetShiftLevel = iTargetLevel; - } - - if ( (0 == s_iTargetShiftLevel) && ( (iCountRetransmissions != 0) || (iCountIntervalsWithRetires != 0) ) ) - s_iTargetShiftLevel = 1; - else - { - if ( iCountRetransmissions >= 0.3*iIntervals ) - { - int iLevel = g_SM_VideoLinkStats.overwrites.currentProfileShiftLevel+1; - if ( iLevel > s_iTargetShiftLevel ) - s_iTargetShiftLevel = iLevel; - } - - if ( iCountIntervalsWithRetires > 0.2 * iIntervals ) - { - // Go down to next profile - s_iTargetShiftLevel = s_iMaxLevelShiftForCurrentProfile + 1; - } - } - } - else - { - int iTargetLevelFromRx = (100-g_SM_VideoLinkStats.computed_rx_quality_vehicle)/rxQualityPerLevel; - if ( iTargetLevelFromRx < 0 ) - { - iTargetLevelFromRx = 0; - } - s_iTargetShiftLevel = iTargetLevelFromRx; - - // Do only one level shift, not more than one - if ( s_iTargetShiftLevel < ((int)g_SM_VideoLinkStats.overwrites.currentProfileShiftLevel)-1 ) - s_iTargetShiftLevel = ((int)g_SM_VideoLinkStats.overwrites.currentProfileShiftLevel)-1; - - if ( iCountRetransmissions == 0 && iCountIntervalsWithRetires == 0 ) - s_iTargetShiftLevel--; - else if ( iCountRetransmissions < 0.1*iIntervals ) - { - int iLevelFromR = ((int)g_SM_VideoLinkStats.overwrites.currentProfileShiftLevel)-1; - if ( iLevelFromR < s_iTargetShiftLevel ) - s_iTargetShiftLevel = iLevelFromR; - } - else if ( iCountRetransmissions < 0.3*iIntervals && - 0 == iCountIntervalsWithRetires && (g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile != g_SM_VideoLinkStats.overwrites.userVideoLinkProfile) ) - { - s_iTargetShiftLevel--; - } - } -} - -void _check_video_link_params_using_controller_info(bool bDownDirection) -{ - int iIntervals = g_SM_VideoLinkStats.backIntervalsToLookForUpStep; - if ( bDownDirection ) - iIntervals = g_SM_VideoLinkStats.backIntervalsToLookForDownStep; - if ( iIntervals > MAX_INTERVALS_VIDEO_LINK_STATS ) - iIntervals = MAX_INTERVALS_VIDEO_LINK_STATS; - - int maxECPacketsUsed = 0; - int intervalsWithReconstructedBlocks = 0; - int intervalsWithReconstructedBlocksAtECLimit = 0; - int intervalsWithMissingData = 0; - int intervalsWithRetransmissions = 0; - int intervalsWithBadOutputBlocks = 0; - - for( int i=0; i maxECPacketsUsed ) - maxECPacketsUsed = g_SM_VideoLinkGraphs.controller_received_video_streams_blocks_max_ec_packets_used[0][i]; - - if ( ( g_SM_VideoLinkGraphs.controller_received_video_streams_blocks_reconstructed[0][i] == 0xFF ) - || ( g_SM_VideoLinkGraphs.controller_received_video_streams_blocks_reconstructed[0][i] == 0 ) ) - if ( ( g_SM_VideoLinkGraphs.controller_received_video_streams_blocks_clean[0][i] == 0xFF ) - || ( g_SM_VideoLinkGraphs.controller_received_video_streams_blocks_clean[0][i] == 0 ) ) - intervalsWithBadOutputBlocks++; - - if ( g_SM_VideoLinkGraphs.controller_received_video_streams_blocks_reconstructed[0][i] != 0xFF ) - if ( g_SM_VideoLinkGraphs.controller_received_video_streams_blocks_reconstructed[0][i] != 0 ) - intervalsWithReconstructedBlocks++; - - if ( g_SM_VideoLinkGraphs.controller_received_video_streams_blocks_max_ec_packets_used[0][i] != 0xFF ) - if ( ((int)g_SM_VideoLinkGraphs.controller_received_video_streams_blocks_max_ec_packets_used[0][i]) > ((int)g_SM_VideoLinkStats.overwrites.currentECBlocks)-2 ) - intervalsWithReconstructedBlocksAtECLimit++; - - if ( g_SM_VideoLinkGraphs.controller_received_video_streams_requested_retransmission_packets[0][i] != 0xFF ) - if ( g_SM_VideoLinkGraphs.controller_received_video_streams_requested_retransmission_packets[0][i] != 0 ) - intervalsWithRetransmissions++; - } - - if ( bDownDirection ) - { - int nTargetShift = 0; - - //if ( intervalsWithMissingData > iIntervals * 0.5 + 1 ) - // nTargetShift = 1; - - //if ( intervalsWithReconstructedBlocks > iIntervals * (0.7*(1.0-s_fParamsChangeStrength)+0.1) ) - // nTargetShift = 1; - - if ( intervalsWithReconstructedBlocksAtECLimit > iIntervals * (0.7*(1.0-s_fParamsChangeStrength)+0.1) ) - nTargetShift = 1; - - if ( intervalsWithRetransmissions > iIntervals * (0.5*(1.0-s_fParamsChangeStrength)+0.2 ) ) - nTargetShift = 1; - - // Switch to lower level on high retransmissions count or on many bad blocks outputs or discarded segments - - if ( intervalsWithBadOutputBlocks > iIntervals * 0.5 + 1 ) - nTargetShift += s_iMaxLevelShiftForCurrentProfile+1; - - if ( intervalsWithRetransmissions > iIntervals * (0.9*(1.0-s_fParamsChangeStrength)+0.1 ) ) - nTargetShift += s_iMaxLevelShiftForCurrentProfile+1; - - if ( nTargetShift != 0 ) - { - s_iTargetShiftLevel += nTargetShift; - } - } - else if ( intervalsWithMissingData < iIntervals * 0.5 ) - { - int nTargetShift = 0; - - //if ( intervalsWithReconstructedBlocks < iIntervals * (0.3*(1.0-s_fParamsChangeStrength)+0.05)+1 ) - // nTargetShift = -1; - - if ( intervalsWithReconstructedBlocksAtECLimit < iIntervals * (0.3*(1.0-s_fParamsChangeStrength)+0.05)+1 ) - nTargetShift = -1; - - // Switch to higher level on very low retransmissions count - - if ( intervalsWithRetransmissions < iIntervals * (0.2*(1.0-s_fParamsChangeStrength)+0.1) + 1 ) - nTargetShift -= s_iMaxLevelShiftForCurrentProfile+1; - - if ( nTargetShift != 0 ) - { - s_iTargetShiftLevel += nTargetShift; - } - } -} - -void _video_stats_overwrites_update_graphs() -{ - for( int i=MAX_INTERVALS_VIDEO_LINK_STATS-1; i>0; i-- ) - { - g_SM_VideoLinkGraphs.vehicleRXQuality[i] = g_SM_VideoLinkGraphs.vehicleRXQuality[i-1]; - g_SM_VideoLinkGraphs.vehicleRXMaxTimeGap[i] = g_SM_VideoLinkGraphs.vehicleRXMaxTimeGap[i-1]; - g_SM_VideoLinkGraphs.vehileReceivedRetransmissionsRequestsCount[i] = g_SM_VideoLinkGraphs.vehileReceivedRetransmissionsRequestsCount[i-1]; - g_SM_VideoLinkGraphs.vehicleReceivedRetransmissionsRequestsPackets[i] = g_SM_VideoLinkGraphs.vehicleReceivedRetransmissionsRequestsPackets[i-1]; - g_SM_VideoLinkGraphs.vehicleReceivedRetransmissionsRequestsPacketsRetried[i] = g_SM_VideoLinkGraphs.vehicleReceivedRetransmissionsRequestsPacketsRetried[i-1]; - - for( int k=0; kget_video_profile_total_levels(g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile); - s_bUseControllerInfo = (g_pCurrentModel->video_link_profiles[g_SM_VideoLinkStats.overwrites.userVideoLinkProfile].uProfileEncodingFlags & VIDEO_PROFILE_ENCODING_FLAG_ADAPTIVE_VIDEO_LINK_USE_CONTROLLER_INFO_TOO)?true:false; - - s_iTargetProfile = g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile; - s_iTargetShiftLevel = g_SM_VideoLinkStats.overwrites.currentProfileShiftLevel; - - s_fParamsChangeStrength = 0.8*(float)g_pCurrentModel->video_params.videoAdjustmentStrength / 10.0; - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile == g_SM_VideoLinkStats.overwrites.userVideoLinkProfile ) - s_fParamsChangeStrength = 0.8*(float)(g_pCurrentModel->video_params.videoAdjustmentStrength+2) / 10.0; - if ( s_fParamsChangeStrength > 1.0 ) - s_fParamsChangeStrength = 1.0; - - g_SM_VideoLinkStats.backIntervalsToLookForDownStep = DEFAULT_MINIMUM_INTERVALS_FOR_VIDEO_LINK_ADJUSTMENT + (1.0 - s_fParamsChangeStrength) * 0.5 * MAX_INTERVALS_VIDEO_LINK_STATS; - if ( g_SM_VideoLinkStats.backIntervalsToLookForDownStep > MAX_INTERVALS_VIDEO_LINK_STATS ) - g_SM_VideoLinkStats.backIntervalsToLookForDownStep = MAX_INTERVALS_VIDEO_LINK_STATS; - - g_SM_VideoLinkStats.backIntervalsToLookForUpStep = 6 + s_fParamsChangeStrength * MAX_INTERVALS_VIDEO_LINK_STATS; - if ( g_SM_VideoLinkStats.backIntervalsToLookForUpStep > MAX_INTERVALS_VIDEO_LINK_STATS ) - g_SM_VideoLinkStats.backIntervalsToLookForUpStep = MAX_INTERVALS_VIDEO_LINK_STATS; - - g_SM_VideoLinkStats.computed_min_interval_for_change_down = 50 + (1.0 - s_fParamsChangeStrength)*500; - g_SM_VideoLinkStats.computed_min_interval_for_change_up = 500 + (1.0 - s_fParamsChangeStrength)*1500; - - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile == VIDEO_PROFILE_MQ ) - { - s_fParamsChangeStrength -= 0.1; - if ( s_fParamsChangeStrength < 0.0 ) - s_fParamsChangeStrength = 0.0; - - g_SM_VideoLinkStats.backIntervalsToLookForDownStep = DEFAULT_MINIMUM_INTERVALS_FOR_VIDEO_LINK_ADJUSTMENT*2 + (1.0 - s_fParamsChangeStrength) * 0.6 * MAX_INTERVALS_VIDEO_LINK_STATS; - if ( g_SM_VideoLinkStats.backIntervalsToLookForDownStep > MAX_INTERVALS_VIDEO_LINK_STATS ) - g_SM_VideoLinkStats.backIntervalsToLookForDownStep = MAX_INTERVALS_VIDEO_LINK_STATS; - g_SM_VideoLinkStats.computed_min_interval_for_change_down = 200 + (1.0 - s_fParamsChangeStrength)*1000; - } - //log_line("usr profile %d-%d, curent profile %d, level %d", g_pCurrentModel->video_params.user_selected_video_link_profile, g_SM_VideoLinkStats.overwrites.userVideoLinkProfile, g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile, g_SM_VideoLinkStats.overwrites.currentProfileShiftLevel); - - - int countIntervalsReceivedOkFromControllerForDownShift = 0; - int countIntervalsReceivedOkFromControllerForUpShift = 0; - for( int i=0; i= g_SM_VideoLinkStats.backIntervalsToLookForDownStep/2 + 1 ) - { - g_SM_VideoLinkStats.usedControllerInfo = 1; - bUseControllerDown = true; - } - if ( countIntervalsOkUpFromController >= g_SM_VideoLinkStats.backIntervalsToLookForUpStep/2 + 1 ) - { - g_SM_VideoLinkStats.usedControllerInfo = 1; - bUseControllerUp = true; - } - */ - - if ( g_iForcedVideoProfile != -1 ) - if ( g_iForcedVideoProfile != g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile ) - { - s_iTargetProfile = g_iForcedVideoProfile; - s_iTargetShiftLevel = 0; - _video_stats_overwrites_apply_profile_changes(true); - - g_SM_VideoLinkStats.timeLastAdaptiveParamsChangeDown = g_TimeNow; - g_SM_VideoLinkStats.timeLastAdaptiveParamsChangeUp = g_TimeNow; - g_SM_VideoLinkStats.timeLastProfileChangeDown = g_TimeNow; - g_SM_VideoLinkStats.timeLastProfileChangeUp = g_TimeNow; - - return; - } - - // Check for actual video bitrate outputed to be in overload or underload condition - - video_link_check_adjust_bitrate_for_overload(); - - // Check for shift down - - if ( g_TimeNow >= g_SM_VideoLinkStats.timeLastAdaptiveParamsChangeDown + g_SM_VideoLinkStats.computed_min_interval_for_change_down ) - if ( g_TimeNow >= g_SM_VideoLinkStats.timeLastAdaptiveParamsChangeUp + g_SM_VideoLinkStats.computed_min_interval_for_change_down ) - { - if ( (! s_bUseControllerInfo) || (!bUseControllerDown) ) - _check_video_link_params_using_vehicle_info(true); - else if ( bUseControllerDown ) - _check_video_link_params_using_controller_info(true); - - if ( g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].uProfileEncodingFlags & VIDEO_PROFILE_ENCODING_FLAG_ENABLE_ADAPTIVE_VIDEO_LINK ) - if ( g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].uProfileEncodingFlags & VIDEO_PROFILE_ENCODING_FLAG_ADAPTIVE_VIDEO_LINK_GO_LOWER_ON_LINK_LOST ) - if ( g_bHadEverLinkToController ) - if ( (! g_bHasLinkToController) || ( g_SM_RadioStats.iMaxRxQuality< 30) ) - { - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile == g_SM_VideoLinkStats.overwrites.userVideoLinkProfile ) - { - s_iTargetProfile = VIDEO_PROFILE_MQ; - s_iTargetShiftLevel = 0; - } - else if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile == VIDEO_PROFILE_MQ ) - { - s_iTargetProfile = VIDEO_PROFILE_LQ; - s_iTargetShiftLevel = 0; - } - } - - if ( (s_iTargetShiftLevel != g_SM_VideoLinkStats.overwrites.currentProfileShiftLevel) || (s_iTargetProfile != g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile) ) - { - _video_stats_overwrites_apply_changes(); - return; - } - else - s_uTimeStartGoodIntervalForProfileShiftUp = 0; - } - - // Check for shift up - - if ( g_TimeNow >= g_SM_VideoLinkStats.timeLastAdaptiveParamsChangeDown + g_SM_VideoLinkStats.computed_min_interval_for_change_up ) - if ( g_TimeNow >= g_SM_VideoLinkStats.timeLastAdaptiveParamsChangeUp + g_SM_VideoLinkStats.computed_min_interval_for_change_up ) - { - if ( (! s_bUseControllerInfo) || (!bUseControllerUp) ) - _check_video_link_params_using_vehicle_info(false); - else if ( bUseControllerUp ) - _check_video_link_params_using_controller_info(false); - - if ( (s_iTargetShiftLevel != g_SM_VideoLinkStats.overwrites.currentProfileShiftLevel) || (s_iTargetProfile != g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile) ) - { - _video_stats_overwrites_apply_changes(); - return; - } - else - s_uTimeStartGoodIntervalForProfileShiftUp = 0; - } -} - -void video_stats_overwrites_periodic_loop() -{ - if ( (g_pCurrentModel == NULL) || (! g_pCurrentModel->hasCamera()) ) - return; - if ( g_bVideoPaused ) - return; - if ( hardware_board_is_goke(hardware_getBoardType()) ) - return; - if ( g_TimeNow >= g_SM_VideoLinkGraphs.timeLastStatsUpdate + VIDEO_LINK_STATS_REFRESH_INTERVAL_MS ) - { - g_SM_VideoLinkGraphs.timeLastStatsUpdate = g_TimeNow; - _video_stats_overwrites_update_graphs(); - } - - if ( g_TimeNow >= s_TimeLastHistorySwitchUpdate + 100 ) - { - s_TimeLastHistorySwitchUpdate = g_TimeNow; - for( int i=MAX_INTERVALS_VIDEO_LINK_SWITCHES-1; i>0; i-- ) - g_SM_VideoLinkStats.historySwitches[i] = g_SM_VideoLinkStats.historySwitches[i-1]; - g_SM_VideoLinkStats.historySwitches[0] = g_SM_VideoLinkStats.overwrites.currentProfileShiftLevel | (g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile<<4); - - #ifdef FEATURE_VEHICLE_COMPUTES_ADAPTIVE_VIDEO - #else - g_SM_VideoLinkStats.timeLastStatsCheck = g_TimeNow; - #endif - } - - // Update adaptive video rate for tx radio: - - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile == g_SM_VideoLinkStats.overwrites.userVideoLinkProfile ) - packet_utils_set_adaptive_video_datarate(0); - else - { - int nRateTxVideo = DEFAULT_RADIO_DATARATE_VIDEO; - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile == VIDEO_PROFILE_MQ ) - nRateTxVideo = utils_get_video_profile_mq_radio_datarate(g_pCurrentModel); - - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile == VIDEO_PROFILE_LQ ) - nRateTxVideo = utils_get_video_profile_lq_radio_datarate(g_pCurrentModel); - - // If it's increasing, set it right away - if ( (0 != packet_utils_get_last_set_adaptive_video_datarate()) && - (getRealDataRateFromRadioDataRate(nRateTxVideo, 0) >= getRealDataRateFromRadioDataRate(packet_utils_get_last_set_adaptive_video_datarate(), 0)) ) - packet_utils_set_adaptive_video_datarate(nRateTxVideo); - // It's decreasing, set it after a short period - else if ( g_TimeNow > g_SM_VideoLinkStats.overwrites.uTimeSetCurrentVideoLinkProfile + DEFAULT_LOWER_VIDEO_RADIO_DATARATE_AFTER_MS ) - packet_utils_set_adaptive_video_datarate(nRateTxVideo); - } - - video_link_check_adjust_quantization_for_overload_periodic_loop(); - - // On link from controller lost, switch to lower profile and keyframe - - int iThresholdControllerLinkMs = 500; - - // Change on link to controller lost - - float fParamsChangeStrength = (float)g_pCurrentModel->video_params.videoAdjustmentStrength / 10.0; - iThresholdControllerLinkMs = 1000 + (1.0 - fParamsChangeStrength)*2000.0; - - if ( ! g_pCurrentModel->isVideoLinkFixedOneWay() ) - if ( g_TimeNow > g_TimeStart + 5000 ) - if ( g_TimeNow > get_video_capture_start_program_time() + 3000 ) - if ((g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].uProfileEncodingFlags) & VIDEO_PROFILE_ENCODING_FLAG_ENABLE_ADAPTIVE_VIDEO_LINK ) - if ( g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].uProfileEncodingFlags & VIDEO_PROFILE_ENCODING_FLAG_ADAPTIVE_VIDEO_LINK_GO_LOWER_ON_LINK_LOST ) - if ( (g_TimeNow > g_TimeLastReceivedRadioPacketFromController + iThresholdControllerLinkMs) || - (g_SM_RadioStats.iMaxRxQuality< 30) ) - if ( g_TimeNow > g_SM_VideoLinkStats.timeLastProfileChangeDown + iThresholdControllerLinkMs*0.7 ) - if ( g_TimeNow > g_SM_VideoLinkStats.timeLastProfileChangeUp + iThresholdControllerLinkMs*0.7 ) - { - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile == g_SM_VideoLinkStats.overwrites.userVideoLinkProfile ) - { - log_line("[Video] Switch to MQ profile due to controller link lost and adaptive video is on."); - video_stats_overwrites_switch_to_profile_and_level(g_pCurrentModel->get_video_profile_total_levels(g_pCurrentModel->video_params.user_selected_video_link_profile), VIDEO_PROFILE_MQ,0); - } - else if ( ! (g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].uProfileEncodingFlags & VIDEO_PROFILE_ENCODING_FLAG_USE_MEDIUM_ADAPTIVE_VIDEO) ) - { - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile == VIDEO_PROFILE_MQ ) - { - log_line("[Video] Switch to LQ profile due to controller link lost and adaptive video is on (full)."); - video_stats_overwrites_switch_to_profile_and_level(g_pCurrentModel->get_video_profile_total_levels(g_pCurrentModel->video_params.user_selected_video_link_profile) + g_pCurrentModel->get_video_profile_total_levels(VIDEO_PROFILE_MQ), VIDEO_PROFILE_LQ,0); - } - } - } - - if ( ((g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].uProfileEncodingFlags) & VIDEO_PROFILE_ENCODING_FLAG_ENABLE_ADAPTIVE_VIDEO_LINK) || - ((g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].uProfileEncodingFlags) & VIDEO_PROFILE_ENCODING_FLAG_KEEP_CONSTANT_BITRATE) ) - video_link_check_adjust_bitrate_for_overload(); - - - if (!((g_pCurrentModel->video_link_profiles[g_pCurrentModel->video_params.user_selected_video_link_profile].uProfileEncodingFlags) & VIDEO_PROFILE_ENCODING_FLAG_ENABLE_ADAPTIVE_VIDEO_LINK) ) - return; - - #ifdef FEATURE_VEHICLE_COMPUTES_ADAPTIVE_VIDEO - if ( ! g_pCurrentModel->isVideoLinkFixedOneWay() ) - _video_stats_overwrites_check_update_params(); - #endif -} - -// Returns true if it increased - -bool video_stats_overwrites_increase_videobitrate_overwrite(u32 uCurrentTotalBitrate) -{ - int iCurrentVideoProfile = g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile; - int iCurrentOverwriteDown = g_SM_VideoLinkStats.overwrites.profilesTopVideoBitrateOverwritesDownward[iCurrentVideoProfile]; - - u32 uTopVideoBitrate = g_SM_VideoLinkStats.overwrites.currentProfileMaxVideoBitrate; - uTopVideoBitrate -= iCurrentOverwriteDown; - - // Can't go lower - if ( uTopVideoBitrate <= g_pCurrentModel->video_params.lowestAllowedAdaptiveVideoBitrate ) - return false; - - if ( g_pCurrentModel->video_link_profiles[g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile].bitrate_fixed_bps > 0 ) - if ( (u32)iCurrentOverwriteDown >= g_pCurrentModel->video_link_profiles[g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile].bitrate_fixed_bps/2 ) - return false; - - if ( iCurrentVideoProfile == VIDEO_PROFILE_LQ ) - g_SM_VideoLinkStats.overwrites.profilesTopVideoBitrateOverwritesDownward[iCurrentVideoProfile] += 250000; - else - { - u32 uShift = uCurrentTotalBitrate / 10; - if ( uShift < 100000 ) - uShift = 100000; - if ( uShift > g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate/4 ) - uShift = g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate/4; - g_SM_VideoLinkStats.overwrites.profilesTopVideoBitrateOverwritesDownward[iCurrentVideoProfile] += uShift; - } - - if ( g_SM_VideoLinkStats.overwrites.profilesTopVideoBitrateOverwritesDownward[iCurrentVideoProfile] > (uTopVideoBitrate*2)/3 ) - g_SM_VideoLinkStats.overwrites.profilesTopVideoBitrateOverwritesDownward[iCurrentVideoProfile] = (uTopVideoBitrate*2)/3; - - g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate = _get_bitrate_for_current_level_and_profile_including_overwrites(); - - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile != VIDEO_PROFILE_USER ) - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile != VIDEO_PROFILE_HIGH_QUALITY ) - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile != VIDEO_PROFILE_BEST_PERF ) - if ( g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate < g_pCurrentModel->video_params.lowestAllowedAdaptiveVideoBitrate ) - g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate = g_pCurrentModel->video_params.lowestAllowedAdaptiveVideoBitrate; - - if ( g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate < 200000 ) - g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate = 200000; - - _video_overwrites_set_capture_video_bitrate(g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate, false, 4); - - return true; -} - -// Returns true if it decreased -bool video_stats_overwrites_decrease_videobitrate_overwrite() -{ - int iCurrentVideoProfile = g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile; - int iCurrentOverwriteDown = g_SM_VideoLinkStats.overwrites.profilesTopVideoBitrateOverwritesDownward[iCurrentVideoProfile]; - if ( iCurrentOverwriteDown == 0 ) - return false; - - if ( iCurrentVideoProfile == VIDEO_PROFILE_LQ ) - { - if ( iCurrentOverwriteDown >= 250000 ) - g_SM_VideoLinkStats.overwrites.profilesTopVideoBitrateOverwritesDownward[iCurrentVideoProfile] -= 250000; - else - g_SM_VideoLinkStats.overwrites.profilesTopVideoBitrateOverwritesDownward[iCurrentVideoProfile] = 0; - } - else - { - if ( iCurrentOverwriteDown >= 500000 ) - g_SM_VideoLinkStats.overwrites.profilesTopVideoBitrateOverwritesDownward[iCurrentVideoProfile] -= 500000; - else - g_SM_VideoLinkStats.overwrites.profilesTopVideoBitrateOverwritesDownward[iCurrentVideoProfile] = 0; - } - - g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate = _get_bitrate_for_current_level_and_profile_including_overwrites(); - - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile != VIDEO_PROFILE_USER ) - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile != VIDEO_PROFILE_HIGH_QUALITY ) - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile != VIDEO_PROFILE_BEST_PERF ) - if ( g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate < g_pCurrentModel->video_params.lowestAllowedAdaptiveVideoBitrate ) - g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate = g_pCurrentModel->video_params.lowestAllowedAdaptiveVideoBitrate; - - if ( g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate < 200000 ) - g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate = 200000; - - _video_overwrites_set_capture_video_bitrate(g_SM_VideoLinkStats.overwrites.currentSetVideoBitrate, false, 5); - - return true; -} - -int _video_stats_overwrites_get_lower_datarate_value(int iDataRateBPS, int iLevelsDown ) -{ - if ( iDataRateBPS < 0 ) - { - for( int i=0; i 0) ) - if ( getRealDataRateFromRadioDataRate(getDataRatesBPS()[iCurrentIndex], 0) > 6000000) - iCurrentIndex--; - } - - if ( iCurrentIndex >= 0 ) - iDataRateBPS = getDataRatesBPS()[iCurrentIndex]; - return iDataRateBPS; -} - -int video_stats_overwrites_get_current_radio_datarate_video(int iRadioLink, int iRadioInterface) -{ - int nRateTx = g_pCurrentModel->radioLinksParams.link_datarate_video_bps[iRadioLink]; - bool bUsesHT40 = false; - if ( g_pCurrentModel->radioLinksParams.link_radio_flags[iRadioLink] & RADIO_FLAG_HT40_VEHICLE ) - bUsesHT40 = true; - - int nRateUserVideoProfile = g_pCurrentModel->video_link_profiles[g_SM_VideoLinkStats.overwrites.userVideoLinkProfile].radio_datarate_video_bps; - if ( 0 != nRateUserVideoProfile ) - if ( getRealDataRateFromRadioDataRate(nRateUserVideoProfile, bUsesHT40) < getRealDataRateFromRadioDataRate(nRateTx, bUsesHT40) ) - nRateTx = nRateUserVideoProfile; - - // nRateTx is now the top radio rate (highest one, set by user) - - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile == g_SM_VideoLinkStats.overwrites.userVideoLinkProfile ) - return nRateTx; - - - int nRateCurrentVideoProfile = g_pCurrentModel->video_link_profiles[g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile].radio_datarate_video_bps; - if ( (0 != nRateCurrentVideoProfile) && ( getRealDataRateFromRadioDataRate(nRateCurrentVideoProfile, bUsesHT40) < getRealDataRateFromRadioDataRate(nRateTx, bUsesHT40) ) ) - nRateTx = nRateCurrentVideoProfile; - - // Decrease nRateTx for MQ, LQ profiles - - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile == VIDEO_PROFILE_MQ ) - nRateTx = utils_get_video_profile_mq_radio_datarate(g_pCurrentModel); - - if ( g_SM_VideoLinkStats.overwrites.currentVideoLinkProfile == VIDEO_PROFILE_LQ ) - nRateTx = utils_get_video_profile_lq_radio_datarate(g_pCurrentModel); - - return nRateTx; -} - -int video_stats_overwrites_get_next_level_down_radio_datarate_video(int iRadioLink, int iRadioInterface) -{ - int nRateTx = video_stats_overwrites_get_current_radio_datarate_video(iRadioLink, iRadioInterface); - - nRateTx = _video_stats_overwrites_get_lower_datarate_value(nRateTx, 1); - return nRateTx; -} - -u32 video_stats_overwrites_get_time_last_shift_down() -{ - return s_uTimeLastShiftLevelDown; -} \ No newline at end of file diff --git a/code/r_vehicle/video_link_stats_overwrites.h b/code/r_vehicle/video_link_stats_overwrites.h deleted file mode 100644 index aa8bd452..00000000 --- a/code/r_vehicle/video_link_stats_overwrites.h +++ /dev/null @@ -1,16 +0,0 @@ -#pragma once - -void video_stats_overwrites_init(); -void video_stats_overwrites_switch_to_profile_and_level(int iTotalLevelsShift, int iVideoProfile, int iLevelShift); -void video_stats_overwrites_reset_to_highest_level(); -void video_stats_overwrites_reset_to_forced_profile(); -void video_stats_overwrites_periodic_loop(); - -bool video_stats_overwrites_increase_videobitrate_overwrite(u32 uCurrentTotalBitrate); -bool video_stats_overwrites_decrease_videobitrate_overwrite(); - -// Returns in bps or negative for MCS rates -int video_stats_overwrites_get_current_radio_datarate_video(int iRadioLink, int iRadioInterface); -int video_stats_overwrites_get_next_level_down_radio_datarate_video(int iRadioLink, int iRadioInterface); - -u32 video_stats_overwrites_get_time_last_shift_down(); diff --git a/code/r_vehicle/video_source_csi.cpp b/code/r_vehicle/video_source_csi.cpp index cdc48307..a27dbe30 100644 --- a/code/r_vehicle/video_source_csi.cpp +++ b/code/r_vehicle/video_source_csi.cpp @@ -53,6 +53,7 @@ #include "events.h" #include "timers.h" #include "shared_vars.h" +#include "adaptive_video.h" #ifdef HW_PLATFORM_RASPBERRY @@ -322,7 +323,7 @@ u8* video_source_csi_read(int* piReadSize, bool* pbIsInsideIFrame) timePipeInput.tv_sec = 0; if ( s_bLastCameraReadTimedOut ) - timePipeInput.tv_usec = 500; // 0.5 miliseconds timeout + timePipeInput.tv_usec = 50; // 0.05 miliseconds timeout else timePipeInput.tv_usec = 10; // 0.01 miliseconds timeout @@ -413,6 +414,7 @@ void video_source_csi_start_program() s_bHasThreadWatchDogVideoCapture = true; log_line("[VideoSourceCSI] Created thread for watchdog."); } + adaptive_video_on_capture_restarted(); s_uRaspiVidStartTimeMs = g_TimeNow; s_bDidSentRaspividBitrateRefresh = false; } diff --git a/code/r_vehicle/video_source_majestic.cpp b/code/r_vehicle/video_source_majestic.cpp index 5ca35b0d..69732139 100644 --- a/code/r_vehicle/video_source_majestic.cpp +++ b/code/r_vehicle/video_source_majestic.cpp @@ -53,12 +53,14 @@ #include "shared_vars.h" #include "launchers_vehicle.h" #include "packets_utils.h" +#include "adaptive_video.h" //To fix extern ParserH264 s_ParserH264CameraOutput; int s_fInputVideoStreamUDPSocket = -1; int s_iInputVideoStreamUDPPort = 5600; u32 s_uTimeStartVideoInput = 0; +bool s_bLogStartOfInputVideoData = true; u8 s_uInputVideoUDPBuffer[MAX_PACKET_TOTAL_SIZE]; u8 s_uOutputUDPNALFrameSegment[MAX_PACKET_TOTAL_SIZE]; @@ -81,6 +83,7 @@ u32 s_uTimeLastCheckMajestic = 0; int s_iCountMajestigProcessNotRunning = 0; void video_source_majestic_start_capture_program(); +void video_source_majestic_stop_capture_program(int iSignal); void video_source_majestic_init_all_params() { @@ -88,6 +91,31 @@ void video_source_majestic_init_all_params() hardware_set_oipc_gpu_boost(g_pCurrentModel->processesPriorities.iFreqGPU); + // Stop default majestic + char szPID[256]; + + video_source_majestic_stop_capture_program(-1); + hardware_sleep_ms(100); + video_source_majestic_stop_capture_program(-9); + hardware_sleep_ms(50); + hw_execute_bash_command_raw("pidof majestic", szPID); + + log_line("[VideoSourceUDP] Init: stopping majestic: PID after: (%s)", szPID); + int iRetry = 5; + while ( (iRetry > 0) && (0 < strlen(szPID)) ) + { + iRetry--; + hardware_sleep_ms(50); + hw_execute_bash_command_raw("killall -1 majestic", NULL); + hardware_sleep_ms(100); + hw_execute_bash_command_raw("pidof majestic", szPID); + } + log_line("[VideoSourceUDP] Init: stopping majestic (2): PID after: (%s)", szPID); + + hardware_camera_apply_all_majestic_settings(g_pCurrentModel, &(g_pCurrentModel->camera_params[g_pCurrentModel->iCurrentCamera].profiles[g_pCurrentModel->camera_params[g_pCurrentModel->iCurrentCamera].iCurrentProfile]), + g_pCurrentModel->video_params.user_selected_video_link_profile, + &(g_pCurrentModel->video_params)); + // Start majestic if not running int iRepeatCount = 2; @@ -125,9 +153,6 @@ void video_source_majestic_init_all_params() log_line("[VideoSourceUDP] Majestic is still not running, try to start it again..."); } - hardware_camera_apply_all_majestic_settings(g_pCurrentModel, &(g_pCurrentModel->camera_params[g_pCurrentModel->iCurrentCamera].profiles[g_pCurrentModel->camera_params[g_pCurrentModel->iCurrentCamera].iCurrentProfile]), - g_pCurrentModel->video_params.user_selected_video_link_profile, - &(g_pCurrentModel->video_params)); // To fix // g_SM_VideoLinkStats.overwrites.uCurrentPendingKeyframeMs = g_pCurrentModel->getInitialKeyframeIntervalMs(g_pCurrentModel->video_params.user_selected_video_link_profile); // g_SM_VideoLinkStats.overwrites.uCurrentActiveKeyframeMs = g_SM_VideoLinkStats.overwrites.uCurrentPendingKeyframeMs; @@ -147,7 +172,6 @@ void video_source_majestic_init_all_params() else log_softerror_and_alarm("[VideoSourceUDP] Can't find the PID of majestic"); } - } void video_source_majestic_close() @@ -217,12 +241,15 @@ u32 video_source_majestic_get_program_start_time() void video_source_majestic_start_capture_program() { hardware_set_oipc_gpu_boost(g_pCurrentModel->processesPriorities.iFreqGPU); + + if ( g_pCurrentModel->processesPriorities.uProcessesFlags & PROCESSES_FLAGS_BALANCE_INT_CORES ) + hardware_balance_interupts(); hw_execute_bash_command("/usr/bin/majestic -s 2>&1 1>/dev/null &", NULL); - s_uTimeLastCheckMajestic = g_TimeNow; hardware_sleep_ms(50); + int iPID = hw_process_exists("majestic"); + log_line("[VideoSourceUDP] New majestic PID: (%d)", iPID); if ( g_pCurrentModel->processesPriorities.iNiceVideo < 0 ) { - int iPID = hw_process_exists("majestic"); if ( iPID > 1 ) { log_line("[VideoSourceUDP] Adjust majestic nice priority to %d", g_pCurrentModel->processesPriorities.iNiceVideo); @@ -233,12 +260,20 @@ void video_source_majestic_start_capture_program() else log_softerror_and_alarm("[VideoSourceUDP] Can't find the PID of majestic"); } + adaptive_video_on_capture_restarted(); + s_uTimeLastCheckMajestic = g_TimeNow-3000; + s_iCountMajestigProcessNotRunning = 0; + s_uTimeStartVideoInput = g_TimeNow; + s_bLogStartOfInputVideoData = true; } -void video_source_majestic_stop_capture_program() +void video_source_majestic_stop_capture_program(int iSignal) { - hw_execute_bash_command_raw("pidof majestic | xargs kill -9 2>/dev/null ", NULL); - hardware_set_oipc_gpu_boost(g_pCurrentModel->processesPriorities.iFreqGPU); + hw_kill_process("majestic", iSignal); + char szOutput[256]; + szOutput[0] = 0; + hw_execute_bash_command_raw("pidof majestic", szOutput); + log_line("[VideoSourceUDP] Majestic PID after stop command: (%s)", szOutput); } void video_source_majestic_request_update_program(u32 uChangeReason) @@ -278,7 +313,7 @@ void video_source_majestic_set_keyframe_value(float fGOP) void video_source_majestic_set_videobitrate_value(u32 uBitrate) { char szComm[128]; - char szOutput[256]; + //char szOutput[256]; sprintf(szComm, "curl localhost/api/v1/set?video0.bitrate=%u", uBitrate/1000); hw_execute_bash_command_raw(szComm, NULL); sprintf(szComm, "cli -s .video0.bitrate %u", uBitrate/1000); @@ -571,6 +606,11 @@ u8* video_source_majestic_read(int* piReadSize, bool bAsync) if ( iRecvBytes <= 0 ) return NULL; + if ( s_bLogStartOfInputVideoData ) + { + log_line("[VideoSourceUDP] Start receiving data (H264/H265 stream) from camera"); + s_bLogStartOfInputVideoData = false; + } s_uDebugUDPInputBytes += iRecvBytes; s_uDebugUDPInputReads++; @@ -609,6 +649,113 @@ bool video_source_majestic_las_read_is_picture_frame() return s_bIsInsidePictureFrameNAL; } +void _video_source_majestic_check_params_update() +{ + char szComm[128]; + char szOutput[256]; + + camera_profile_parameters_t* pCameraParams = &(g_pCurrentModel->camera_params[g_pCurrentModel->iCurrentCamera].profiles[g_pCurrentModel->camera_params[g_pCurrentModel->iCurrentCamera].iCurrentProfile]); + video_parameters_t* pVideoParams = &(g_pCurrentModel->video_params); + u32 uParam = (s_uRequestedVideoMajesticCaptureUpdateReason>>16); + + bool bUpdatedImageParams = false; + if ( (s_uRequestedVideoMajesticCaptureUpdateReason & 0xFF) == MODEL_CHANGED_CAMERA_BRIGHTNESS ) + { + sprintf(szComm, "curl localhost/api/v1/set?image.luminance=%u", uParam); + hw_execute_bash_command_raw(szComm, szOutput); + //hw_execute_bash_command_raw("curl localhost/api/v1/reload", szOutput); + bUpdatedImageParams = true; + } + else if ( (s_uRequestedVideoMajesticCaptureUpdateReason & 0xFF) == MODEL_CHANGED_CAMERA_CONTRAST ) + { + sprintf(szComm, "curl localhost/api/v1/set?image.contrast=%u", uParam); + hw_execute_bash_command_raw(szComm, szOutput); + //hw_execute_bash_command_raw("curl localhost/api/v1/reload", szOutput); + bUpdatedImageParams = true; + } + else if ( (s_uRequestedVideoMajesticCaptureUpdateReason & 0xFF) == MODEL_CHANGED_CAMERA_SATURATION ) + { + sprintf(szComm, "curl localhost/api/v1/set?image.saturation=%u", uParam/2); + hw_execute_bash_command_raw(szComm, szOutput); + //hw_execute_bash_command_raw("curl localhost/api/v1/reload", szOutput); + bUpdatedImageParams = true; + } + else if ( (s_uRequestedVideoMajesticCaptureUpdateReason & 0xFF) == MODEL_CHANGED_CAMERA_HUE ) + { + sprintf(szComm, "curl localhost/api/v1/set?image.hue=%u", uParam); + hw_execute_bash_command_raw(szComm, szOutput); + //hw_execute_bash_command_raw("curl localhost/api/v1/reload", szOutput); + bUpdatedImageParams = true; + } + + if ( bUpdatedImageParams ) + { + s_bRequestedVideoMajesticCaptureUpdate = false; + s_bHasPendingMajesticRealTimeChanges = true; + s_uTimeLastMajesticImageRealTimeUpdate = g_TimeNow; + s_uRequestedVideoMajesticCaptureUpdateReason = 0; + return; + } + + // Stop majestic, apply settings, restart majestic + log_line("[VideoSourceUDP] Has pending changes and restart of majestic..."); + + char szPIDBefore[256]; + char szPIDAfter[256]; + szPIDBefore[0] = 0; + szPIDAfter[0] = 0; + hw_execute_bash_command_raw("pidof majestic", szPIDBefore); + hardware_sleep_ms(50); + video_source_majestic_stop_capture_program(-1); + hardware_sleep_ms(100); + video_source_majestic_stop_capture_program(-9); + hardware_sleep_ms(50); + hw_execute_bash_command_raw("pidof majestic", szPIDAfter); + + log_line("[VideoSourceUDP] Stopping majestic: PID before: (%s), PID after: (%s)", szPIDBefore, szPIDAfter); + int iRetry = 5; + while ( (iRetry > 0) && (0 < strlen(szPIDAfter)) ) + { + iRetry--; + hardware_sleep_ms(50); + hw_execute_bash_command_raw("killall -1 majestic", NULL); + hardware_sleep_ms(100); + hw_execute_bash_command_raw("pidof majestic", szPIDAfter); + } + + if ( 0 < strlen(szPIDAfter) ) + { + send_alarm_to_controller_now(ALARM_ID_VIDEO_CAPTURE_MALFUNCTION, 1,0, 20); + hardware_reboot(); + } + + if ( (s_uRequestedVideoMajesticCaptureUpdateReason & 0xFF) == MODEL_CHANGED_CAMERA_PARAMS ) + { + log_line("[VideoSourceUDP] Periodic loop: apply all majestic camera only settings."); + hardware_camera_apply_all_majestic_camera_settings(pCameraParams); + } + else + { + log_line("[VideoSourceUDP] Periodic loop: signaled to apply all majestic settings."); + hardware_camera_apply_all_majestic_settings(g_pCurrentModel, pCameraParams, g_pCurrentModel->video_params.user_selected_video_link_profile, pVideoParams); + // To fix + // g_SM_VideoLinkStats.overwrites.uCurrentPendingKeyframeMs = g_pCurrentModel->getInitialKeyframeIntervalMs(g_pCurrentModel->video_params.user_selected_video_link_profile); + // g_SM_VideoLinkStats.overwrites.uCurrentActiveKeyframeMs = g_SM_VideoLinkStats.overwrites.uCurrentPendingKeyframeMs; + } + + if ( ((s_uRequestedVideoMajesticCaptureUpdateReason & 0xFF) == MODEL_CHANGED_VIDEO_RESOLUTION) || + ((s_uRequestedVideoMajesticCaptureUpdateReason & 0xFF) == MODEL_CHANGED_VIDEO_CODEC) ) + { + log_line("[VideoSourceUDP] Periodic loop: signaled changed video resolution or codec."); + } + + hardware_set_oipc_gpu_boost(g_pCurrentModel->processesPriorities.iFreqGPU); + video_source_majestic_start_capture_program(); + vehicle_check_update_processes_affinities(false, false); + s_bRequestedVideoMajesticCaptureUpdate = false; + s_uRequestedVideoMajesticCaptureUpdateReason = 0; +} + void video_source_majestic_periodic_checks() { if ( g_TimeNow >= s_uDebugTimeLastUDPVideoInputCheck+10000 ) @@ -630,7 +777,7 @@ void video_source_majestic_periodic_checks() char szOutput[128]; szOutput[0] = 0; hw_execute_bash_command_silent("pidof majestic", szOutput); - if ( (strlen(szOutput) < 3) || (strlen(szOutput) > 5) ) + if ( (strlen(szOutput) < 3) || (strlen(szOutput) > 6) ) { s_iCountMajestigProcessNotRunning++; if ( s_iCountMajestigProcessNotRunning >= 2 ) @@ -652,76 +799,11 @@ void video_source_majestic_periodic_checks() s_bHasPendingMajesticRealTimeChanges = false; s_uTimeLastMajesticImageRealTimeUpdate = g_TimeNow; camera_profile_parameters_t* pCameraParams = &(g_pCurrentModel->camera_params[g_pCurrentModel->iCurrentCamera].profiles[g_pCurrentModel->camera_params[g_pCurrentModel->iCurrentCamera].iCurrentProfile]); - // Save image settings to yaml file, but do not restart majestic - hardware_camera_apply_all_majestic_camera_settings(pCameraParams, false); + // Save image settings to yaml file + hardware_camera_apply_all_majestic_camera_settings(pCameraParams); } if ( s_bRequestedVideoMajesticCaptureUpdate ) - { - char szComm[128]; - char szOutput[256]; - s_bRequestedVideoMajesticCaptureUpdate = false; - camera_profile_parameters_t* pCameraParams = &(g_pCurrentModel->camera_params[g_pCurrentModel->iCurrentCamera].profiles[g_pCurrentModel->camera_params[g_pCurrentModel->iCurrentCamera].iCurrentProfile]); - video_parameters_t* pVideoParams = &(g_pCurrentModel->video_params); - u32 uParam = (s_uRequestedVideoMajesticCaptureUpdateReason>>16); + _video_source_majestic_check_params_update(); - bool bUpdatedImageParams = false; - - if ( (s_uRequestedVideoMajesticCaptureUpdateReason & 0xFF) == MODEL_CHANGED_CAMERA_BRIGHTNESS ) - { - sprintf(szComm, "curl localhost/api/v1/set?image.luminance=%u", uParam); - hw_execute_bash_command_raw(szComm, szOutput); - //hw_execute_bash_command_raw("curl localhost/api/v1/reload", szOutput); - bUpdatedImageParams = true; - } - else if ( (s_uRequestedVideoMajesticCaptureUpdateReason & 0xFF) == MODEL_CHANGED_CAMERA_CONTRAST ) - { - sprintf(szComm, "curl localhost/api/v1/set?image.contrast=%u", uParam); - hw_execute_bash_command_raw(szComm, szOutput); - //hw_execute_bash_command_raw("curl localhost/api/v1/reload", szOutput); - bUpdatedImageParams = true; - } - else if ( (s_uRequestedVideoMajesticCaptureUpdateReason & 0xFF) == MODEL_CHANGED_CAMERA_SATURATION ) - { - sprintf(szComm, "curl localhost/api/v1/set?image.saturation=%u", uParam/2); - hw_execute_bash_command_raw(szComm, szOutput); - //hw_execute_bash_command_raw("curl localhost/api/v1/reload", szOutput); - bUpdatedImageParams = true; - } - else if ( (s_uRequestedVideoMajesticCaptureUpdateReason & 0xFF) == MODEL_CHANGED_CAMERA_HUE ) - { - sprintf(szComm, "curl localhost/api/v1/set?image.hue=%u", uParam); - hw_execute_bash_command_raw(szComm, szOutput); - //hw_execute_bash_command_raw("curl localhost/api/v1/reload", szOutput); - bUpdatedImageParams = true; - } - else if ( (s_uRequestedVideoMajesticCaptureUpdateReason & 0xFF) == MODEL_CHANGED_CAMERA_PARAMS ) - hardware_camera_apply_all_majestic_camera_settings(pCameraParams, true); - else - { - hardware_camera_apply_all_majestic_settings(g_pCurrentModel, pCameraParams, g_pCurrentModel->video_params.user_selected_video_link_profile, pVideoParams); -// To fix -// g_SM_VideoLinkStats.overwrites.uCurrentPendingKeyframeMs = g_pCurrentModel->getInitialKeyframeIntervalMs(g_pCurrentModel->video_params.user_selected_video_link_profile); -// g_SM_VideoLinkStats.overwrites.uCurrentActiveKeyframeMs = g_SM_VideoLinkStats.overwrites.uCurrentPendingKeyframeMs; - } - - if ( bUpdatedImageParams ) - { - s_bHasPendingMajesticRealTimeChanges = true; - s_uTimeLastMajesticImageRealTimeUpdate = g_TimeNow; - } - - if ( ((s_uRequestedVideoMajesticCaptureUpdateReason & 0xFF) == MODEL_CHANGED_VIDEO_RESOLUTION) || - ((s_uRequestedVideoMajesticCaptureUpdateReason & 0xFF) == MODEL_CHANGED_VIDEO_CODEC) ) - { - hardware_sleep_ms(50); - video_source_majestic_stop_capture_program(); - hardware_sleep_ms(50); - video_source_majestic_stop_capture_program(); - hardware_sleep_ms(50); - video_source_majestic_start_capture_program(); - vehicle_check_update_processes_affinities(false, false); - } - s_uRequestedVideoMajesticCaptureUpdateReason = 0; - } } \ No newline at end of file diff --git a/code/r_vehicle/video_source_majestic.h b/code/r_vehicle/video_source_majestic.h index 3273c844..0304c194 100644 --- a/code/r_vehicle/video_source_majestic.h +++ b/code/r_vehicle/video_source_majestic.h @@ -8,7 +8,7 @@ int video_source_majestic_open(int iUDPPort); u32 video_source_majestic_get_program_start_time(); void video_source_majestic_start_capture_program(); -void video_source_majestic_stop_capture_program(); +void video_source_majestic_stop_capture_program(int iSignal); void video_source_majestic_request_update_program(u32 uChangeReason); void video_source_majestic_set_keyframe_value(float fGOP); void video_source_majestic_set_videobitrate_value(u32 uBitrate); diff --git a/code/r_vehicle/video_tx_buffers.cpp b/code/r_vehicle/video_tx_buffers.cpp index fea43971..681fb236 100644 --- a/code/r_vehicle/video_tx_buffers.cpp +++ b/code/r_vehicle/video_tx_buffers.cpp @@ -469,7 +469,7 @@ void VideoTxPacketsBuffer::_sendPacket(int iBufferIndex, int iPacketIndex, u32 u t_packet_header* pCurrentPacketHeader = m_VideoPackets[iBufferIndex][iPacketIndex].pPH; t_packet_header_video_full_98* pCurrentVideoPacketHeader = m_VideoPackets[iBufferIndex][iPacketIndex].pPHVF; - t_packet_header_video_full_98* pPHVF = (t_packet_header_video_full_98*) (m_VideoPackets[iBufferIndex][iPacketIndex].pRawData+sizeof(t_packet_header)); + //t_packet_header_video_full_98* pPHVF = (t_packet_header_video_full_98*) (m_VideoPackets[iBufferIndex][iPacketIndex].pRawData+sizeof(t_packet_header)); // stream_packet_idx: high 4 bits: stream id (0..15), lower 28 bits: stream packet index pCurrentPacketHeader->stream_packet_idx = m_uRadioStreamPacketIndex; @@ -504,10 +504,10 @@ void VideoTxPacketsBuffer::_sendPacket(int iBufferIndex, int iPacketIndex, u32 u if ( g_bVideoPaused || (! relay_current_vehicle_must_send_own_video_feeds()) ) return; - t_packet_header_video_full_98_debug_info* pPHVFDebugInfo = (t_packet_header_video_full_98_debug_info*) m_VideoPackets[iBufferIndex][iPacketIndex].pVideoData; - u8* pVideoData = m_VideoPackets[iBufferIndex][iPacketIndex].pVideoData; - pVideoData += sizeof(t_packet_header_video_full_98_debug_info); - u32 crc = base_compute_crc32(pVideoData, pCurrentVideoPacketHeader->uCurrentBlockPacketSize); + //t_packet_header_video_full_98_debug_info* pPHVFDebugInfo = (t_packet_header_video_full_98_debug_info*) m_VideoPackets[iBufferIndex][iPacketIndex].pVideoData; + //u8* pVideoData = m_VideoPackets[iBufferIndex][iPacketIndex].pVideoData; + //pVideoData += sizeof(t_packet_header_video_full_98_debug_info); + //u32 crc = base_compute_crc32(pVideoData, pCurrentVideoPacketHeader->uCurrentBlockPacketSize); send_packet_to_radio_interfaces((u8*)pCurrentPacketHeader, pCurrentPacketHeader->total_length, -1); } @@ -517,14 +517,17 @@ int VideoTxPacketsBuffer::hasPendingPacketsToSend() return m_iCountReadyToSend; } -void VideoTxPacketsBuffer::sendAvailablePackets() +int VideoTxPacketsBuffer::sendAvailablePackets(int iMaxCountToSend) { if ( m_iCountReadyToSend <= 0 ) - return; + return 0; int iToSend = m_iCountReadyToSend; if ( iToSend > MAX_PACKETS_TO_SEND_IN_ONE_SLICE ) iToSend = MAX_PACKETS_TO_SEND_IN_ONE_SLICE; + if ( iMaxCountToSend > 0 ) + if ( iToSend > iMaxCountToSend ) + iToSend = iMaxCountToSend; int iCountSent = 0; for( int i=0; i #include "../common/radio_stats.h" #include "../common/string_utils.h" @@ -46,14 +47,14 @@ typedef struct u32 uLastReceivedPacketIndex; u32 uLastTimeReceivedPacket; u32 packetsHashIndexes[PACKETS_INDEX_HASH_SIZE]; -} __attribute__((packed)) t_stream_history_packets_indexes; +} ALIGN_STRUCT_SPEC_INFO t_stream_history_packets_indexes; typedef struct { u32 uVehicleId; t_stream_history_packets_indexes streamsPacketsHistory[MAX_RADIO_STREAMS]; int iRestartDetected; -} __attribute__((packed)) t_vehicle_history_packets_indexes; +} ALIGN_STRUCT_SPEC_INFO t_vehicle_history_packets_indexes; t_vehicle_history_packets_indexes s_ListHistoryRxPacketsVehicles[MAX_CONCURENT_VEHICLES]; diff --git a/code/radio/radio_rx.c b/code/radio/radio_rx.c index 0b0cf22f..acbcfc9d 100644 --- a/code/radio/radio_rx.c +++ b/code/radio/radio_rx.c @@ -33,7 +33,6 @@ #include "../base/encr.h" #include "../base/config_hw.h" #include "../base/hw_procs.h" -#include #include "../common/radio_stats.h" #include "../common/string_utils.h" #include "radio_rx.h" @@ -50,9 +49,8 @@ int s_iCustomRxThreadPriority = DEFAULT_PRIORITY_THREAD_RADIO_RX; int s_iLastSetCustomRxThreadPriority = DEFAULT_PRIORITY_THREAD_RADIO_RX; t_radio_rx_state s_RadioRxState; - pthread_t s_pThreadRadioRx; -pthread_mutex_t s_pThreadRadioRxMutex; + shared_mem_radio_stats* s_pSMRadioStats = NULL; shared_mem_radio_stats_interfaces_rx_graph* s_pSMRadioRxGraphs = NULL; int s_iSearchMode = 0; @@ -75,6 +73,8 @@ u32 s_uLastRxShortPacketsVehicleIds[MAX_RADIO_INTERFACES]; // Pointers to array of int-s (max radio cards, for each card) u8* s_pPacketsCounterOutputVideo = NULL; +u8* s_pPacketsCounterOutputECVideo = NULL; +u8* s_pPacketsCounterOutputRetrVideo = NULL; u8* s_pPacketsCounterOutputData = NULL; u8* s_pPacketsCounterOutputMissing = NULL; u8* s_pPacketsCounterOutputMissingMaxGap = NULL; @@ -100,93 +100,88 @@ int _radio_rx_update_local_stats_on_new_radio_packet(int iInterface, int iIsShor //---------------------------------------------- // Begin: Compute index of stats to use - int iStatsIndex = -1; + t_radio_rx_state_vehicle* pStatsVehicle = NULL; if ( (s_RadioRxState.vehicles[0].uVehicleId == 0) || (s_RadioRxState.vehicles[0].uVehicleId == uVehicleId) ) - iStatsIndex = 0; - - if ( -1 == iStatsIndex ) + pStatsVehicle = &(s_RadioRxState.vehicles[0]); + + if ( NULL == pStatsVehicle ) { for( int i=0; iuVehicleId = uVehicleId; if ( iDataIsOk ) { - s_RadioRxState.vehicles[iStatsIndex].uTotalRxPackets++; - s_RadioRxState.vehicles[iStatsIndex].uTmpRxPackets++; + pStatsVehicle->uTotalRxPackets++; + pStatsVehicle->uTmpRxPackets++; } else { - s_RadioRxState.vehicles[iStatsIndex].uTotalRxPacketsBad++; - s_RadioRxState.vehicles[iStatsIndex].uTmpRxPacketsBad++; + pStatsVehicle->uTotalRxPacketsBad++; + pStatsVehicle->uTmpRxPacketsBad++; } if ( iIsShortPacket ) { t_packet_header_short* pPHS = (t_packet_header_short*)pPacket; - u32 uNext = ((s_RadioRxState.vehicles[iStatsIndex].uLastRxRadioLinkPacketIndex[iInterface]+1) & 0xFF); + u32 uNext = ((pStatsVehicle->uLastRxRadioLinkPacketIndex[iInterface]+1) & 0xFF); if ( pPHS->packet_id != uNext ) { u32 lost = pPHS->packet_id - uNext; if ( pPHS->packet_id < uNext ) lost = pPHS->packet_id + 255 - uNext; - s_RadioRxState.vehicles[iStatsIndex].uTotalRxPacketsLost += lost; - s_RadioRxState.vehicles[iStatsIndex].uTmpRxPacketsLost += lost; + pStatsVehicle->uTotalRxPacketsLost += lost; + pStatsVehicle->uTmpRxPacketsLost += lost; nReturnLost = lost; } - s_RadioRxState.vehicles[iStatsIndex].uLastRxRadioLinkPacketIndex[iInterface] = pPHS->packet_id; + pStatsVehicle->uLastRxRadioLinkPacketIndex[iInterface] = pPHS->packet_id; } else { t_packet_header* pPH = (t_packet_header*)pPacket; - if ( s_RadioRxState.vehicles[iStatsIndex].uLastRxRadioLinkPacketIndex[iInterface] > 0 ) - if ( pPH->radio_link_packet_index > s_RadioRxState.vehicles[iStatsIndex].uLastRxRadioLinkPacketIndex[iInterface]+1 ) + if ( pStatsVehicle->uLastRxRadioLinkPacketIndex[iInterface] > 0 ) + if ( pPH->radio_link_packet_index > pStatsVehicle->uLastRxRadioLinkPacketIndex[iInterface]+1 ) { - u32 lost = pPH->radio_link_packet_index - (s_RadioRxState.vehicles[iStatsIndex].uLastRxRadioLinkPacketIndex[iInterface] + 1); - s_RadioRxState.vehicles[iStatsIndex].uTotalRxPacketsLost += lost; - s_RadioRxState.vehicles[iStatsIndex].uTmpRxPacketsLost += lost; + u32 lost = pPH->radio_link_packet_index - (pStatsVehicle->uLastRxRadioLinkPacketIndex[iInterface] + 1); + pStatsVehicle->uTotalRxPacketsLost += lost; + pStatsVehicle->uTmpRxPacketsLost += lost; nReturnLost = lost; } - if ( s_RadioRxState.vehicles[iStatsIndex].uLastRxRadioLinkPacketIndex[iInterface] > 0 ) - if ( s_RadioRxState.vehicles[iStatsIndex].uLastRxRadioLinkPacketIndex[iInterface] < 0xFA00 ) + if ( pStatsVehicle->uLastRxRadioLinkPacketIndex[iInterface] > 0 ) + if ( pStatsVehicle->uLastRxRadioLinkPacketIndex[iInterface] < 0xFA00 ) if ( pPH->radio_link_packet_index > 0x0500 ) - if ( pPH->radio_link_packet_index < s_RadioRxState.vehicles[iStatsIndex].uLastRxRadioLinkPacketIndex[iInterface] ) + if ( pPH->radio_link_packet_index < pStatsVehicle->uLastRxRadioLinkPacketIndex[iInterface] ) radio_dup_detection_set_vehicle_restarted_flag(pPH->vehicle_id_src); - s_RadioRxState.vehicles[iStatsIndex].uLastRxRadioLinkPacketIndex[iInterface] = pPH->radio_link_packet_index; + pStatsVehicle->uLastRxRadioLinkPacketIndex[iInterface] = pPH->radio_link_packet_index; } return nReturnLost; } @@ -226,56 +221,52 @@ void _radio_rx_add_packet_to_rx_queue(u8* pPacket, int iLength, int iRadioInterf if ( (NULL == pPacket) || (iLength <= 0) || s_iRadioRxMarkedForQuit ) return; - // Add the packet to the queue - s_RadioRxState.iPacketsRxInterface[s_RadioRxState.iCurrentRxPacketIndex] = iRadioInterface; - s_RadioRxState.iPacketsAreShort[s_RadioRxState.iCurrentRxPacketIndex] = 0; - s_RadioRxState.iPacketsLengths[s_RadioRxState.iCurrentRxPacketIndex] = iLength; - memcpy(s_RadioRxState.pPacketsBuffers[s_RadioRxState.iCurrentRxPacketIndex], pPacket, iLength); - + t_radio_rx_state_packets_queue* pQueue = &s_RadioRxState.queue_reg_priority; + t_packet_header* pPH = (t_packet_header*)pPacket; + if ( radio_packet_type_is_high_priority( pPH->packet_type ) ) + pQueue = &s_RadioRxState.queue_high_priority; - int iPacketsInQueue = 0; - if ( s_RadioRxState.iCurrentRxPacketIndex > s_RadioRxState.iCurrentRxPacketToConsume ) - iPacketsInQueue = s_RadioRxState.iCurrentRxPacketIndex - s_RadioRxState.iCurrentRxPacketToConsume; - else if ( s_RadioRxState.iCurrentRxPacketIndex < s_RadioRxState.iCurrentRxPacketToConsume ) - iPacketsInQueue = MAX_RX_PACKETS_QUEUE - s_RadioRxState.iCurrentRxPacketToConsume + s_RadioRxState.iCurrentRxPacketIndex; + // Add the packet to the queue + pQueue->uPacketsRxInterface[pQueue->iCurrentRxPacketIndex] = iRadioInterface; + pQueue->uPacketsAreShort[pQueue->iCurrentRxPacketIndex] = 0; + pQueue->iPacketsLengths[pQueue->iCurrentRxPacketIndex] = iLength; + memcpy(pQueue->pPacketsBuffers[pQueue->iCurrentRxPacketIndex], pPacket, iLength); - if ( iPacketsInQueue > s_RadioRxState.iMaxPacketsInQueueLastMinute ) - s_RadioRxState.iMaxPacketsInQueueLastMinute = iPacketsInQueue; - if ( iPacketsInQueue > s_RadioRxState.iMaxPacketsInQueue ) - s_RadioRxState.iMaxPacketsInQueue = iPacketsInQueue; - u32 uTimeStartTryLock = get_current_timestamp_ms(); int iLock = -1; while ( (iLock != 0 ) && (get_current_timestamp_ms() <= uTimeStartTryLock+5) ) { - iLock = pthread_mutex_trylock(&s_pThreadRadioRxMutex); + iLock = pthread_mutex_trylock(&(pQueue->mutex)); hardware_sleep_micros(100); } if ( iLock != 0 ) log_softerror_and_alarm("[RadioRxThread] Doing unguarded rx buffers manipulation"); - s_RadioRxState.iCurrentRxPacketIndex++; - if ( s_RadioRxState.iCurrentRxPacketIndex >= MAX_RX_PACKETS_QUEUE ) - s_RadioRxState.iCurrentRxPacketIndex = 0; - - if ( s_RadioRxState.iCurrentRxPacketIndex == s_RadioRxState.iCurrentRxPacketToConsume ) + // No more room? Discard it + if ( pQueue->iCurrentRxPacketsInQueue >= MAX_RX_PACKETS_QUEUE ) { - // No more room. Must skip few pending processing packets - log_softerror_and_alarm("[RadioRxThread] No more room in rx buffers. Discarding some unprocessed messages. Max messages in queue: %d, last 10 sec: %d.", s_RadioRxState.iMaxPacketsInQueue, s_RadioRxState.iMaxPacketsInQueueLastMinute); - if ( iLock != 0 ) - { - iLock = pthread_mutex_lock(&s_pThreadRadioRxMutex); - } - for( int i=0; i<3; i++ ) - { - s_RadioRxState.iCurrentRxPacketToConsume++; - if ( s_RadioRxState.iCurrentRxPacketToConsume >= MAX_RX_PACKETS_QUEUE ) - s_RadioRxState.iCurrentRxPacketToConsume = 0; - } + if ( 0 == iLock ) + pthread_mutex_unlock(&(pQueue->mutex)); + s_uRadioRxLastTimeQueue += get_current_timestamp_ms() - s_uRadioRxTimeNow; + return; } + + pQueue->iCurrentRxPacketsInQueue++; + pQueue->iCurrentRxPacketIndex++; + if ( pQueue->iCurrentRxPacketIndex >= MAX_RX_PACKETS_QUEUE ) + pQueue->iCurrentRxPacketIndex = 0; + + if ( pQueue->iCurrentRxPacketsInQueue > pQueue->iStatsMaxPacketsInQueueLastMinute ) + pQueue->iStatsMaxPacketsInQueueLastMinute = pQueue->iCurrentRxPacketsInQueue; + if ( pQueue->iCurrentRxPacketsInQueue > pQueue->iStatsMaxPacketsInQueue ) + pQueue->iStatsMaxPacketsInQueue = pQueue->iCurrentRxPacketsInQueue; + + if ( NULL != pQueue->pSemaphore ) + sem_post(pQueue->pSemaphore); + if ( 0 == iLock ) - pthread_mutex_unlock(&s_pThreadRadioRxMutex); + pthread_mutex_unlock(&(pQueue->mutex)); s_uRadioRxLastTimeQueue += get_current_timestamp_ms() - s_uRadioRxTimeNow; } @@ -528,9 +519,28 @@ int _radio_rx_parse_received_wifi_radio_data(int iInterfaceIndex) iIsVideoData = 1; if ( (pPH->packet_flags & PACKET_FLAGS_MASK_MODULE) == PACKET_COMPONENT_VIDEO ) - if ( NULL != s_pPacketsCounterOutputVideo ) - s_pPacketsCounterOutputVideo[iInterfaceIndex] = s_pPacketsCounterOutputVideo[iInterfaceIndex]+1; - + { + t_packet_header_video_full_98* pPHVF = (t_packet_header_video_full_98*) (pData+sizeof(t_packet_header)); + + if ( pPH->packet_flags & PACKET_FLAGS_BIT_RETRANSMITED ) + { + if ( NULL != s_pPacketsCounterOutputRetrVideo ) + s_pPacketsCounterOutputRetrVideo[iInterfaceIndex] = s_pPacketsCounterOutputRetrVideo[iInterfaceIndex]+1; + } + else + { + if ( pPHVF->uCurrentBlockPacketIndex >= pPHVF->uCurrentBlockDataPackets ) + { + if ( NULL != s_pPacketsCounterOutputECVideo ) + s_pPacketsCounterOutputECVideo[iInterfaceIndex] = s_pPacketsCounterOutputECVideo[iInterfaceIndex]+1; + } + else + { + if ( NULL != s_pPacketsCounterOutputVideo ) + s_pPacketsCounterOutputVideo[iInterfaceIndex] = s_pPacketsCounterOutputVideo[iInterfaceIndex]+1; + } + } + } if ( (pPH->packet_flags & PACKET_FLAGS_MASK_MODULE) != PACKET_COMPONENT_VIDEO ) if ( NULL != s_pPacketsCounterOutputData ) s_pPacketsCounterOutputData[iInterfaceIndex] = s_pPacketsCounterOutputData[iInterfaceIndex]+1; @@ -660,20 +670,25 @@ void _radio_rx_update_stats(u32 uTimeNow) if ( 0 == iAnyRxPackets ) log_line("[RadioRxThread] No packets received in the last seconds"); } - if ( uTimeNow >= s_RadioRxState.uTimeLastMinute + 1000 * 5 ) + if ( uTimeNow >= s_RadioRxState.uTimeLastMinuteStatsUpdate + 1000 * 5 ) { - s_RadioRxState.uTimeLastMinute = uTimeNow; + s_RadioRxState.uTimeLastMinuteStatsUpdate = uTimeNow; s_iCounterRadioRxStatsUpdate2++; - log_line("[RadioRxThread] Max packets in queue: %d. Max packets in queue in last 10 sec: %d.", - s_RadioRxState.iMaxPacketsInQueue, s_RadioRxState.iMaxPacketsInQueueLastMinute); - s_RadioRxState.iMaxPacketsInQueueLastMinute = 0; + log_line("[RadioRxThread] Max packets in queues (high/reg prio): %d/%d. Max packets in queue in last 10 sec: %d/%d", + s_RadioRxState.queue_high_priority.iStatsMaxPacketsInQueue, + s_RadioRxState.queue_reg_priority.iStatsMaxPacketsInQueue, + s_RadioRxState.queue_high_priority.iStatsMaxPacketsInQueueLastMinute, + s_RadioRxState.queue_reg_priority.iStatsMaxPacketsInQueueLastMinute); + s_RadioRxState.queue_high_priority.iStatsMaxPacketsInQueueLastMinute = 0; + s_RadioRxState.queue_reg_priority.iStatsMaxPacketsInQueueLastMinute = 0; radio_duplicate_detection_log_info(); if ( (s_iCounterRadioRxStatsUpdate2 % 10) == 0 ) { log_line("[RadioRxThread] Reset max stats."); - s_RadioRxState.iMaxPacketsInQueue = 0; + s_RadioRxState.queue_high_priority.iStatsMaxPacketsInQueue = 0; + s_RadioRxState.queue_reg_priority.iStatsMaxPacketsInQueue = 0; } } } @@ -842,7 +857,6 @@ int radio_rx_start_rx_thread(shared_mem_radio_stats* pSMRadioStats, shared_mem_r s_iSearchMode = iSearchMode; s_iRadioRxSingalStop = 0; s_RadioRxState.uAcceptedFirmwareType = uAcceptedFirmwareType; - radio_rx_reset_interfaces_broken_state(); for( int i=0; i 0 ) { s_iRadioRxPausedInterfaces[iInterfaceIndex]--; if ( s_iRadioRxPausedInterfaces[iInterfaceIndex] == 0 ) s_iRadioRxAllInterfacesPaused = 0; } - pthread_mutex_unlock(&s_pThreadRadioRxMutex); + pthread_mutex_unlock(&(s_RadioRxState.queue_reg_priority.mutex)); + pthread_mutex_unlock(&(s_RadioRxState.queue_high_priority.mutex)); _radio_rx_update_fd_sets(); s_bHasPendingOperation = 0; s_bCanDoOperations = 0; @@ -1038,9 +1102,11 @@ void radio_rx_set_dev_mode() } // Pointers to array of int-s (max radio cards, for each card) -void radio_rx_set_packet_counter_output(u8* pCounterOutputVideo, u8* pCounterOutputData, u8* pCounterMissingPackets, u8* pCounterMissingPacketsMaxGap) +void radio_rx_set_packet_counter_output(u8* pCounterOutputVideo, u8* pCounterOutputECVideo, u8* pCounterOutputRetrVideo, u8* pCounterOutputData, u8* pCounterMissingPackets, u8* pCounterMissingPacketsMaxGap) { s_pPacketsCounterOutputVideo = pCounterOutputVideo; + s_pPacketsCounterOutputECVideo = pCounterOutputECVideo; + s_pPacketsCounterOutputRetrVideo = pCounterOutputRetrVideo; s_pPacketsCounterOutputData = pCounterOutputData; s_pPacketsCounterOutputMissing = pCounterMissingPackets; s_pPacketsCounterOutputMissingMaxGap = pCounterMissingPacketsMaxGap; @@ -1076,7 +1142,6 @@ int radio_rx_detect_firmware_type_from_packet(u8* pPacketBuffer, int nPacketLeng return 0; } - int radio_rx_any_interface_broken() { for( int i=0; ipacket_flags & PACKET_FLAGS_MASK_MODULE) == PACKET_COMPONENT_VIDEO ) - if ( (pPH->packet_type == PACKET_TYPE_VIDEO_REQ_MULTIPLE_PACKETS) ) - iCount++; - iBottom++; - if ( iBottom >= MAX_RX_PACKETS_QUEUE ) - iBottom++; - } - return iCount; + u32 u = s_uRadioRxLastTimeRead; + s_uRadioRxLastTimeRead = 0; + return u; } -int radio_rx_has_packets_to_consume() +u32 radio_rx_get_and_reset_max_loop_time_queue() { - if ( 0 == s_iRadioRxInitialized ) - return 0; - if ( s_RadioRxState.iCurrentRxPacketIndex == s_RadioRxState.iCurrentRxPacketToConsume ) - return 0; - - int iCount = 0; - int iBottom = 0; - int iTop = 0; - //pthread_mutex_lock(&s_pThreadRadioRxMutex); - iBottom = s_RadioRxState.iCurrentRxPacketToConsume; - iTop = s_RadioRxState.iCurrentRxPacketIndex; - //pthread_mutex_unlock(&s_pThreadRadioRxMutex); - - if ( iBottom < iTop ) - iCount = iTop - iBottom; - else - iCount = MAX_RX_PACKETS_QUEUE - iBottom + iTop; - - return iCount; + u32 u = s_uRadioRxLastTimeQueue; + s_uRadioRxLastTimeQueue = 0; + return u; } -u8* radio_rx_get_next_received_packet(int* pLength, int* pIsShortPacket, int* pRadioInterfaceIndex) + +u8* _radio_rx_wait_get_queue_packet(t_radio_rx_state_packets_queue* pQueue, u32 uTimeoutMicroSec, int* pLength, int* pIsShortPacket, int* pRadioInterfaceIndex) { - if ( NULL != pLength ) - *pLength = 0; - if ( NULL != pIsShortPacket ) - *pIsShortPacket = 0; - if ( NULL != pRadioInterfaceIndex ) - *pRadioInterfaceIndex = 0; + int iRes = -1; - if ( 0 == s_iRadioRxInitialized ) - return NULL; - if ( s_RadioRxState.iCurrentRxPacketIndex == s_RadioRxState.iCurrentRxPacketToConsume ) + if ( 0 == uTimeoutMicroSec ) + { + iRes = sem_trywait(pQueue->pSemaphore); + } + else + { + struct timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + ts.tv_nsec += 1000*uTimeoutMicroSec; + iRes = sem_timedwait(pQueue->pSemaphore, &ts); + } + if ( 0 != iRes ) return NULL; - pthread_mutex_lock(&s_pThreadRadioRxMutex); + int iIndexToCopy = 0; + pthread_mutex_lock(&(pQueue->mutex)); + iIndexToCopy = pQueue->iCurrentRxPacketToConsume; if ( NULL != pLength ) - *pLength = s_RadioRxState.iPacketsLengths[s_RadioRxState.iCurrentRxPacketToConsume]; + *pLength = pQueue->iPacketsLengths[pQueue->iCurrentRxPacketToConsume]; if ( NULL != pIsShortPacket ) - *pIsShortPacket = s_RadioRxState.iPacketsAreShort[s_RadioRxState.iCurrentRxPacketToConsume]; + *pIsShortPacket = pQueue->uPacketsAreShort[pQueue->iCurrentRxPacketToConsume]; if ( NULL != pRadioInterfaceIndex ) - *pRadioInterfaceIndex = s_RadioRxState.iPacketsRxInterface[s_RadioRxState.iCurrentRxPacketToConsume]; + *pRadioInterfaceIndex = pQueue->uPacketsRxInterface[pQueue->iCurrentRxPacketToConsume]; - memcpy( s_tmpLastProcessedRadioRxPacket, s_RadioRxState.pPacketsBuffers[s_RadioRxState.iCurrentRxPacketToConsume], s_RadioRxState.iPacketsLengths[s_RadioRxState.iCurrentRxPacketToConsume]); + // Defer copy after mutex unlock + //memcpy(s_tmpLastProcessedRadioRxPacket, pQueue->pPacketsBuffers[pQueue->iCurrentRxPacketToConsume], pQueue->iPacketsLengths[pQueue->iCurrentRxPacketToConsume]); - s_RadioRxState.iCurrentRxPacketToConsume++; - if ( s_RadioRxState.iCurrentRxPacketToConsume >= MAX_RX_PACKETS_QUEUE ) - s_RadioRxState.iCurrentRxPacketToConsume = 0; + pQueue->iCurrentRxPacketToConsume++; + if ( pQueue->iCurrentRxPacketToConsume >= MAX_RX_PACKETS_QUEUE ) + pQueue->iCurrentRxPacketToConsume = 0; + pQueue->iCurrentRxPacketsInQueue--; + pthread_mutex_unlock(&(pQueue->mutex)); - pthread_mutex_unlock(&s_pThreadRadioRxMutex); + memcpy(s_tmpLastProcessedRadioRxPacket, pQueue->pPacketsBuffers[iIndexToCopy], pQueue->iPacketsLengths[iIndexToCopy]); return s_tmpLastProcessedRadioRxPacket; } -int radio_rx_get_received_packets(int iCount, type_received_radio_packet* pOutputArray) +u8* radio_rx_wait_get_next_received_high_prio_packet(u32 uTimeoutMicroSec, int* pLength, int* pIsShortPacket, int* pRadioInterfaceIndex) { - if ( (iCount <= 0) || (NULL == pOutputArray) ) - return 0; - + if ( NULL != pLength ) + *pLength = 0; + if ( NULL != pIsShortPacket ) + *pIsShortPacket = 0; + if ( NULL != pRadioInterfaceIndex ) + *pRadioInterfaceIndex = 0; if ( 0 == s_iRadioRxInitialized ) - return 0; - if ( s_RadioRxState.iCurrentRxPacketIndex == s_RadioRxState.iCurrentRxPacketToConsume ) - return 0; - - int iRead = 0; - - pthread_mutex_lock(&s_pThreadRadioRxMutex); - - for( int i=0; i= MAX_RX_PACKETS_QUEUE ) - s_RadioRxState.iCurrentRxPacketToConsume = 0; - - iRead++; - - if ( s_RadioRxState.iCurrentRxPacketIndex == s_RadioRxState.iCurrentRxPacketToConsume ) - break; - } + return NULL; - pthread_mutex_unlock(&s_pThreadRadioRxMutex); + if ( NULL == s_RadioRxState.queue_high_priority.pSemaphore ) + if ( s_RadioRxState.queue_high_priority.iCurrentRxPacketIndex == s_RadioRxState.queue_high_priority.iCurrentRxPacketToConsume ) + return 0; - return iRead; + return _radio_rx_wait_get_queue_packet(&(s_RadioRxState.queue_high_priority), uTimeoutMicroSec, pLength, pIsShortPacket, pRadioInterfaceIndex); } -u32 radio_rx_get_and_reset_max_loop_time() +u8* radio_rx_wait_get_next_received_reg_prio_packet(u32 uTimeoutMicroSec, int* pLength, int* pIsShortPacket, int* pRadioInterfaceIndex) { - u32 u = s_RadioRxState.uMaxLoopTime; - s_RadioRxState.uMaxLoopTime = 0; - return u; -} + if ( NULL != pLength ) + *pLength = 0; + if ( NULL != pIsShortPacket ) + *pIsShortPacket = 0; + if ( NULL != pRadioInterfaceIndex ) + *pRadioInterfaceIndex = 0; + if ( 0 == s_iRadioRxInitialized ) + return NULL; -u32 radio_rx_get_and_reset_max_loop_time_read() -{ - u32 u = s_uRadioRxLastTimeRead; - s_uRadioRxLastTimeRead = 0; - return u; -} + if ( NULL == s_RadioRxState.queue_reg_priority.pSemaphore ) + if ( s_RadioRxState.queue_reg_priority.iCurrentRxPacketIndex == s_RadioRxState.queue_reg_priority.iCurrentRxPacketToConsume ) + return 0; -u32 radio_rx_get_and_reset_max_loop_time_queue() -{ - u32 u = s_uRadioRxLastTimeQueue; - s_uRadioRxLastTimeQueue = 0; - return u; + return _radio_rx_wait_get_queue_packet(&(s_RadioRxState.queue_reg_priority), uTimeoutMicroSec, pLength, pIsShortPacket, pRadioInterfaceIndex); } - diff --git a/code/radio/radio_rx.h b/code/radio/radio_rx.h index 9e395423..5ba45ff5 100644 --- a/code/radio/radio_rx.h +++ b/code/radio/radio_rx.h @@ -3,11 +3,13 @@ #include "../base/base.h" #include "../base/config.h" #include "../base/hardware.h" +#include +#include #if defined (HW_PLATFORM_RASPBERRY) || defined (HW_PLATFORM_RADXA_ZERO3) -#define MAX_RX_PACKETS_QUEUE 500 +#define MAX_RX_PACKETS_QUEUE 300 #else -#define MAX_RX_PACKETS_QUEUE 50 +#define MAX_RX_PACKETS_QUEUE 100 #endif typedef struct { @@ -23,30 +25,39 @@ typedef struct int iMinRxPacketsPerSec; u32 uLastRxRadioLinkPacketIndex[MAX_RADIO_INTERFACES]; // per radio interface -} __attribute__((packed)) t_radio_rx_state_vehicle; +} ALIGN_STRUCT_SPEC_INFO t_radio_rx_state_vehicle; typedef struct { u8* pPacketsBuffers[MAX_RX_PACKETS_QUEUE]; int iPacketsLengths[MAX_RX_PACKETS_QUEUE]; - int iPacketsAreShort[MAX_RX_PACKETS_QUEUE]; - int iPacketsRxInterface[MAX_RX_PACKETS_QUEUE]; + u8 uPacketsAreShort[MAX_RX_PACKETS_QUEUE]; + u8 uPacketsRxInterface[MAX_RX_PACKETS_QUEUE]; int iCurrentRxPacketIndex; // Where next packet will be added int iCurrentRxPacketToConsume; // Where the first packet to read/consume is + int iCurrentRxPacketsInQueue; + int iStatsMaxPacketsInQueue; + int iStatsMaxPacketsInQueueLastMinute; + pthread_mutex_t mutex; + sem_t* pSemaphore; +} ALIGN_STRUCT_SPEC_INFO t_radio_rx_state_packets_queue; + +typedef struct +{ int iRadioInterfacesBroken[MAX_RADIO_INTERFACES]; int iRadioInterfacesRxTimeouts[MAX_RADIO_INTERFACES]; int iRadioInterfacesRxBadPackets[MAX_RADIO_INTERFACES]; - u32 uTimeLastStatsUpdate; t_radio_rx_state_vehicle vehicles[MAX_CONCURENT_VEHICLES]; + t_radio_rx_state_packets_queue queue_high_priority; + t_radio_rx_state_packets_queue queue_reg_priority; u32 uMaxLoopTime; u32 uAcceptedFirmwareType; - u32 uTimeLastMinute; - int iMaxPacketsInQueue; - int iMaxPacketsInQueueLastMinute; -} __attribute__((packed)) t_radio_rx_state; + u32 uTimeLastMinuteStatsUpdate; + u32 uTimeLastStatsUpdate; +} ALIGN_STRUCT_SPEC_INFO t_radio_rx_state; typedef struct { @@ -54,7 +65,7 @@ typedef struct int iPacketLength; int iPacketIsShort; int iPacketRxInterface; -} __attribute__((packed)) type_received_radio_packet; +} ALIGN_STRUCT_SPEC_INFO type_received_radio_packet; #ifdef __cplusplus extern "C" { @@ -70,7 +81,7 @@ void radio_rx_pause_interface(int iInterfaceIndex, const char* szReason); void radio_rx_resume_interface(int iInterfaceIndex); void radio_rx_mark_quit(); void radio_rx_set_dev_mode(); -void radio_rx_set_packet_counter_output(u8* pCounterOutputVideo, u8* pCounterOutputData, u8* pCounterMissingPackets, u8* pCounterMissingPacketsMaxGap); +void radio_rx_set_packet_counter_output(u8* pCounterOutputVideo, u8* pCounterOutputECVideo, u8* pCounterOutputRetrVideo, u8* pCounterOutputData, u8* pCounterMissingPackets, u8* pCounterMissingPacketsMaxGap); int radio_rx_detect_firmware_type_from_packet(u8* pPacketBuffer, int nPacketLength); @@ -80,17 +91,14 @@ int radio_rx_get_interface_bad_packets_error_and_reset(int iInterfaceIndex); int radio_rx_any_rx_timeouts(); int radio_rx_get_timeout_count_and_reset(int iInterfaceIndex); void radio_rx_reset_interfaces_broken_state(); -t_radio_rx_state* radio_rx_get_state(); - -int radio_rx_has_retransmissions_requests_to_consume(); -int radio_rx_has_packets_to_consume(); -u8* radio_rx_get_next_received_packet(int* pLength, int* pIsShortPacket, int* pRadioInterfaceIndex); -int radio_rx_get_received_packets(int iCount, type_received_radio_packet* pOutputArray); u32 radio_rx_get_and_reset_max_loop_time(); u32 radio_rx_get_and_reset_max_loop_time_read(); u32 radio_rx_get_and_reset_max_loop_time_queue(); +u8* radio_rx_wait_get_next_received_high_prio_packet(u32 uTimeoutMicroSec, int* pLength, int* pIsShortPacket, int* pRadioInterfaceIndex); +u8* radio_rx_wait_get_next_received_reg_prio_packet(u32 uTimeoutMicroSec, int* pLength, int* pIsShortPacket, int* pRadioInterfaceIndex); + #ifdef __cplusplus } #endif diff --git a/code/radio/radiolink.c b/code/radio/radiolink.c index e671f666..a6781bc5 100644 --- a/code/radio/radiolink.c +++ b/code/radio/radiolink.c @@ -1340,16 +1340,6 @@ int radio_write_raw_packet(int interfaceIndex, u8* pData, int dataLength) return 0; } - // To fix to remove - t_packet_header* pPH = (t_packet_header*)&s_uLastPacketBuilt[0]; - if ( pPH->packet_type == PACKET_TYPE_VIDEO_DATA_98 ) - { - t_packet_header_video_full_98* pPHVF = (t_packet_header_video_full_98*)&s_uLastPacketBuilt[sizeof(t_packet_header)]; - t_packet_header_video_full_98_debug_info* pPHVFDebugInfo = (t_packet_header_video_full_98_debug_info*) &s_uLastPacketBuilt[sizeof(t_packet_header)+sizeof(t_packet_header_video_full_98)]; - u8* pVideoData = &s_uLastPacketBuilt[sizeof(t_packet_header)+sizeof(t_packet_header_video_full_98) + sizeof(t_packet_header_video_full_98_debug_info)]; - u32 crc = base_compute_crc32(pVideoData, pPHVF->uCurrentBlockPacketSize); - } - if ( s_bRadioDebugFlag ) { t_packet_header* pPH = (t_packet_header*)&s_uLastPacketBuilt[0]; diff --git a/code/radio/radiopackets2.h b/code/radio/radiopackets2.h index 32d75a49..aeea87bb 100644 --- a/code/radio/radiopackets2.h +++ b/code/radio/radiopackets2.h @@ -190,7 +190,9 @@ typedef struct typedef struct { - u8 uVideoStreamIndexAndType; // bits 0...3: video stream index, bits 4...7: video stream type: H264, H265, IP, etc + u8 uVideoStreamIndexAndType; + // bits 0...3: video stream index + // bits 4...7: video stream type: H264, H265, IP, etc u32 uVideoStatusFlags2; // byte 0: current h264 quantization value // byte 1: @@ -814,6 +816,7 @@ byte 4: command type: #define PACKET_TYPE_VIDEO_SWITCH_VIDEO_KEYFRAME_TO_VALUE_ACK 65 // From vehicle to controller. Contains the acknowledge keyframe milisec value as an u32 #define PACKET_TYPE_SIK_CONFIG 70 +// // Can be send locally or to vehicle // u8: vehicle radio link id // u8: command id: @@ -822,7 +825,25 @@ byte 4: command type: // 2 - resume SiK interface // u8+: data response +#define PACKET_TYPE_NEGOCIATE_RADIO_LINKS 72 +// +// u8: uType +// 0: send to vehicle +// 1: confirmation from vehicle +// +// u8: uCommand: +// 1: change video datarate (uParam is an (int) datarate bps or negative mcs) +// 254: end and update (uParam is an (int) datarate bps or negative mcs) +// 255: end no change (revert to original) +// u32: uParam +#define NEGOCIATE_RADIO_STEP_DATA_RATE 1 +#define NEGOCIATE_RADIO_STEP_END 254 +#define NEGOCIATE_RADIO_STEP_CANCEL 255 + + + #define PACKET_TYPE_OTA_UPDATE_STATUS 75 +// // From vehicle to controller, during OTA update // u8 status // u32 counter @@ -831,6 +852,8 @@ byte 4: command type: #define OTA_UPDATE_STATUS_UPDATING 3 #define OTA_UPDATE_STATUS_POST_UPDATING 4 #define OTA_UPDATE_STATUS_COMPLETED 5 +#define OTA_UPDATE_STATUS_FAILED_DISK_SPACE 250 +#define OTA_UPDATE_STATUS_FAILED 255 #define PACKET_TYPE_DEBUG_VEHICLE_RT_INFO 110 diff --git a/code/radio/radiopackets_rc.h b/code/radio/radiopackets_rc.h index 238dbb23..14a60770 100644 --- a/code/radio/radiopackets_rc.h +++ b/code/radio/radiopackets_rc.h @@ -21,7 +21,7 @@ typedef struct u8 extra_info1; // not used, for future use u8 extra_info2; // not used, for future use u8 extra_info3; // not used, for future use -} __attribute__((packed)) t_packet_header_rc_full_frame_upstream; +} ALIGN_STRUCT_SPEC_INFO t_packet_header_rc_full_frame_upstream; #define RC_INFO_HISTORY_SIZE 50 // every 50ms @@ -40,7 +40,7 @@ typedef struct u8 last_history_slice; u8 rc_rssi; u32 extra_flags; // not used now. for future use -} __attribute__((packed)) t_packet_header_rc_info_downstream; +} ALIGN_STRUCT_SPEC_INFO t_packet_header_rc_info_downstream; diff --git a/code/radio/radiopacketsqueue.h b/code/radio/radiopacketsqueue.h index eeb22aa7..f4d5e42b 100644 --- a/code/radio/radiopacketsqueue.h +++ b/code/radio/radiopacketsqueue.h @@ -8,7 +8,7 @@ typedef struct u8 has_radio_header; u8 packet_buffer[MAX_PACKET_TOTAL_SIZE]; u16 packet_length; -} __attribute__((packed)) t_packet_queue_item; +} ALIGN_STRUCT_SPEC_INFO t_packet_queue_item; typedef struct { @@ -16,7 +16,7 @@ typedef struct int queue_start_pos; // position of first element in queue int queue_end_pos; // position of first free element in queue (after the last one) u32 timeFirstPacket; -} __attribute__((packed)) t_packet_queue; +} ALIGN_STRUCT_SPEC_INFO t_packet_queue; #ifdef __cplusplus extern "C" { diff --git a/code/renderer/fbgraphics.c b/code/renderer/fbgraphics.c index 043e8796..4f80a12a 100644 --- a/code/renderer/fbgraphics.c +++ b/code/renderer/fbgraphics.c @@ -1076,6 +1076,13 @@ void fbg_flip(struct _fbg *fbg) { //fbg_computeFramerate(fbg, 1); } +int fbg_getcomponents(struct _fbg* fbg) +{ + if ( NULL != fbg ) + return fbg->components; + return 4; +} + void fbg_clear(struct _fbg *fbg, unsigned char color) { memset(fbg->back_buffer, color, fbg->size); } diff --git a/code/renderer/fbgraphics.h b/code/renderer/fbgraphics.h index 322ffc0c..7c456768 100644 --- a/code/renderer/fbgraphics.h +++ b/code/renderer/fbgraphics.h @@ -591,6 +591,7 @@ extern "C" { */ extern void fbg_flip(struct _fbg *fbg); + extern int fbg_getcomponents(struct _fbg* fbg); //! create an empty image /*! \param fbg pointer to a FBG context / data structure diff --git a/code/renderer/render_engine.cpp b/code/renderer/render_engine.cpp index 3f6a9dbb..3cee316c 100644 --- a/code/renderer/render_engine.cpp +++ b/code/renderer/render_engine.cpp @@ -592,6 +592,11 @@ void RenderEngine::freeRawFont(u32 idFont) log_line("Unloaded font id %u, remaining fonts: %d", idFont, m_iCountRawFonts); } +void RenderEngine::setFontOutlineColor(u32 idFont, u8 r, u8 g, u8 b, u8 a) +{ + +} + u32 RenderEngine::loadImage(const char* szFile) { return 0; diff --git a/code/renderer/render_engine.h b/code/renderer/render_engine.h index 4c14dbfb..a1c7f8e3 100644 --- a/code/renderer/render_engine.h +++ b/code/renderer/render_engine.h @@ -86,6 +86,7 @@ class RenderEngine virtual int loadRawFont(const char* szFontFile); virtual void freeRawFont(u32 idFont); + virtual void setFontOutlineColor(u32 idFont, u8 r, u8 g, u8 b, u8 a); virtual u32 loadImage(const char* szFile); virtual void freeImage(u32 idImage); virtual u32 loadIcon(const char* szFile); diff --git a/code/renderer/render_engine_cairo.cpp b/code/renderer/render_engine_cairo.cpp index b72534c6..bf9a45f0 100644 --- a/code/renderer/render_engine_cairo.cpp +++ b/code/renderer/render_engine_cairo.cpp @@ -191,6 +191,11 @@ void RenderEngineCairo::_freeRawFontImageObject(void* pImageObject) log_line("[RendererCairo] Destroyed image object %X", pImageObject); } +void RenderEngineCairo::setFontOutlineColor(u32 idFont, u8 r, u8 g, u8 b, u8 a) +{ + +} + u32 RenderEngineCairo::loadImage(const char* szFile) { if ( m_iCountImages > MAX_RAW_IMAGES ) diff --git a/code/renderer/render_engine_cairo.h b/code/renderer/render_engine_cairo.h index fdbb57c0..9dcbb444 100644 --- a/code/renderer/render_engine_cairo.h +++ b/code/renderer/render_engine_cairo.h @@ -13,6 +13,7 @@ class RenderEngineCairo: public RenderEngine virtual void setStroke(double* color, float fStrokeSize); virtual void setStrokeSize(float fStrokeSize); + virtual void setFontOutlineColor(u32 idFont, u8 r, u8 g, u8 b, u8 a); virtual u32 loadImage(const char* szFile); virtual void freeImage(u32 idImage); virtual u32 loadIcon(const char* szFile); diff --git a/code/renderer/render_engine_raw.cpp b/code/renderer/render_engine_raw.cpp index edd8e1fc..5aabd24c 100644 --- a/code/renderer/render_engine_raw.cpp +++ b/code/renderer/render_engine_raw.cpp @@ -80,6 +80,44 @@ void RenderEngineRaw::_freeRawFontImageObject(void* pImageObject) } +void RenderEngineRaw::setFontOutlineColor(u32 idFont, u8 r, u8 g, u8 b, u8 a) +{ + int indexFont = _getRawFontIndexFromId(idFont); + if ( -1 == indexFont ) + { + log_softerror_and_alarm("Tried to update invalid raw font id %u, not in the list (%d raw fonts)", idFont, m_iCountRawFonts); + return; + } + + if ( (r<30) && (g<30) && (b<30) ) + return; + RenderEngineRawFont* pFont = m_pRawFonts[indexFont]; + struct _fbg_img* pImg = (struct _fbg_img*) pFont->pImageObject; + unsigned char *img_data_pointer_row = (unsigned char *)(pImg->data); + u8 r0,g0,b0,a0; + + for( int y=0; y<(int)pImg->height; y++ ) + { + unsigned char *img_data_pointer = img_data_pointer_row; + for( int x=0; x<(int)pImg->width; x++ ) + { + r0 = *img_data_pointer; + g0 = *(img_data_pointer+1); + b0 = *(img_data_pointer+2); + a0 = *(img_data_pointer+3); + if ( (r0 < 50) && (g0 < 50) && (b0 < 50) && (a0 > 0) ) + { + *img_data_pointer = r; + *(img_data_pointer+1) = g; + *(img_data_pointer+2) = b; + *(img_data_pointer+3) = a0; + } + img_data_pointer += fbg_getcomponents(m_pFBG); + } + img_data_pointer_row += fbg_getcomponents(m_pFBG) * pImg->width; + } +} + u32 RenderEngineRaw::loadImage(const char* szFile) { if ( m_iCountImages > MAX_RAW_IMAGES ) diff --git a/code/renderer/render_engine_raw.h b/code/renderer/render_engine_raw.h index 4b40f163..aeb06e71 100644 --- a/code/renderer/render_engine_raw.h +++ b/code/renderer/render_engine_raw.h @@ -8,6 +8,7 @@ class RenderEngineRaw: public RenderEngine RenderEngineRaw(); virtual ~RenderEngineRaw(); + virtual void setFontOutlineColor(u32 idFont, u8 r, u8 g, u8 b, u8 a); virtual u32 loadImage(const char* szFile); virtual void freeImage(u32 idImage); virtual u32 loadIcon(const char* szFile); diff --git a/res/bg_img6.jpg b/res/bg_img6.jpg new file mode 100644 index 00000000..08f8d8c1 Binary files /dev/null and b/res/bg_img6.jpg differ diff --git a/res/bg_img7.jpg b/res/bg_img7.jpg new file mode 100644 index 00000000..88a8c3bb Binary files /dev/null and b/res/bg_img7.jpg differ diff --git a/res/board_radxa_3w.png b/res/board_radxa_3w.png new file mode 100644 index 00000000..c0ec50d4 Binary files /dev/null and b/res/board_radxa_3w.png differ diff --git a/res/favorite.png b/res/favorite.png index a7672c06..64704eff 100644 Binary files a/res/favorite.png and b/res/favorite.png differ diff --git a/res/icon_alarm.png b/res/icon_alarm.png index 804b5dcc..00fe129d 100644 Binary files a/res/icon_alarm.png and b/res/icon_alarm.png differ diff --git a/res/icon_arrow_down.png b/res/icon_arrow_down.png index 0298a385..23d2066d 100644 Binary files a/res/icon_arrow_down.png and b/res/icon_arrow_down.png differ diff --git a/res/icon_arrow_up.png b/res/icon_arrow_up.png index 735a0036..63ac4c5d 100644 Binary files a/res/icon_arrow_up.png and b/res/icon_arrow_up.png differ diff --git a/res/icon_camera.png b/res/icon_camera.png index ab949c73..8e66cfc1 100644 Binary files a/res/icon_camera.png and b/res/icon_camera.png differ diff --git a/res/icon_checkok.png b/res/icon_checkok.png index e41420f1..41b87f13 100644 Binary files a/res/icon_checkok.png and b/res/icon_checkok.png differ diff --git a/res/icon_clock.png b/res/icon_clock.png index 71ccc76d..54bf1675 100644 Binary files a/res/icon_clock.png and b/res/icon_clock.png differ diff --git a/res/icon_cpu.png b/res/icon_cpu.png index b04b328a..ccb64cae 100644 Binary files a/res/icon_cpu.png and b/res/icon_cpu.png differ diff --git a/res/icon_error.png b/res/icon_error.png index 683f86de..aeb43df9 100644 Binary files a/res/icon_error.png and b/res/icon_error.png differ diff --git a/res/icon_exclamation.png b/res/icon_exclamation.png index 683f86de..aeb43df9 100644 Binary files a/res/icon_exclamation.png and b/res/icon_exclamation.png differ diff --git a/res/icon_gauge.png b/res/icon_gauge.png index cf304004..0400d79a 100644 Binary files a/res/icon_gauge.png and b/res/icon_gauge.png differ diff --git a/res/icon_heading.png b/res/icon_heading.png index 2249f6b9..5b9752bd 100644 Binary files a/res/icon_heading.png and b/res/icon_heading.png differ diff --git a/res/icon_home.png b/res/icon_home.png index 13e51b9f..2259aae7 100644 Binary files a/res/icon_home.png and b/res/icon_home.png differ diff --git a/res/icon_info.png b/res/icon_info.png index 5ee21d2f..bf4d19d7 100644 Binary files a/res/icon_info.png and b/res/icon_info.png differ diff --git a/res/icon_radio.png b/res/icon_radio.png index 89b7df7d..f9f85b63 100644 Binary files a/res/icon_radio.png and b/res/icon_radio.png differ diff --git a/res/icon_shield.png b/res/icon_shield.png index 1016f50a..021fb131 100644 Binary files a/res/icon_shield.png and b/res/icon_shield.png differ diff --git a/res/icon_uplink.png b/res/icon_uplink.png index 65dd3508..11d1465d 100644 Binary files a/res/icon_uplink.png and b/res/icon_uplink.png differ diff --git a/res/icon_uplink2.png b/res/icon_uplink2.png index 2c99e820..d8e47021 100644 Binary files a/res/icon_uplink2.png and b/res/icon_uplink2.png differ diff --git a/res/icon_v_car.png b/res/icon_v_car.png index ecb5d6cb..6f0afb06 100644 Binary files a/res/icon_v_car.png and b/res/icon_v_car.png differ diff --git a/res/icon_v_drone.png b/res/icon_v_drone.png index fae7eaf3..577ce77a 100644 Binary files a/res/icon_v_drone.png and b/res/icon_v_drone.png differ diff --git a/res/icon_v_plane.png b/res/icon_v_plane.png index 9fb9d713..ef0354f6 100644 Binary files a/res/icon_v_plane.png and b/res/icon_v_plane.png differ diff --git a/res/icon_warning.png b/res/icon_warning.png index bbb7a6a3..c9ebe78f 100644 Binary files a/res/icon_warning.png and b/res/icon_warning.png differ diff --git a/res/icon_x.png b/res/icon_x.png index 3d8a9a2f..a88c9496 100644 Binary files a/res/icon_x.png and b/res/icon_x.png differ diff --git a/res/licenses/license.txt b/res/licenses/license.txt new file mode 100644 index 00000000..b8b3ee7c --- /dev/null +++ b/res/licenses/license.txt @@ -0,0 +1,38 @@ +Ruby written by Petru Soroaga (petrusoroaga@yahoo.com) +Contributors: Tree Orbit, Piotr Kujawski (aka bitkuna) + +More info about Ruby here: +https://rubyfpv.com +https://www.rcgroups.com/forums/showthread.php?3880253-Ruby-Digital-FPV-Video-Link-%28v-2-3%29 + + + Ruby Licence + Copyright (c) 2024 Petru Soroaga petrusoroaga@yahoo.com + All rights reserved. + + Redistribution and use in source and/or binary forms, with or without + modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + Copyright info and developer info must be preserved as is in the user + interface, additions could be made to that info. + * Neither the name of the organization nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + * Military use is not permited. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + DISCLAIMED. IN NO EVENT SHALL THE AUTHOR (PETRU SOROAGA) BE LIABLE FOR ANY + DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +See the res/licences folder for additional details. diff --git a/res/licenses/license_fbg.txt b/res/licenses/license_fbg.txt new file mode 100644 index 00000000..cb2205b7 --- /dev/null +++ b/res/licenses/license_fbg.txt @@ -0,0 +1,30 @@ +Uses code derived from FBG library: +BSD 3-Clause License + +Copyright (c) 2018, Julien Verneuil +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. \ No newline at end of file diff --git a/res/licenses/license_fec.txt b/res/licenses/license_fec.txt new file mode 100644 index 00000000..744f810e --- /dev/null +++ b/res/licenses/license_fec.txt @@ -0,0 +1,34 @@ +/* + * fec.c -- forward error correction based on Vandermonde matrices + * 980624 + * (C) 1997-98 Luigi Rizzo (luigi@iet.unipi.it) + * (C) 2001 Alain Knaff (alain@knaff.lu) + * + * Portions derived from code by Phil Karn (karn@ka9q.ampr.org), + * Robert Morelos-Zaragoza (robert@spectra.eng.hawaii.edu) and Hari + * Thirumoorthy (harit@spectra.eng.hawaii.edu), Aug 1995 + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials + * provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE AUTHORS ``AS IS'' AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A + * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS + * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, + * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, + * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR + * TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT + * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY + * OF SUCH DAMAGE. + */ diff --git a/res/licenses/license_openipc.txt b/res/licenses/license_openipc.txt new file mode 100644 index 00000000..1414dfe9 --- /dev/null +++ b/res/licenses/license_openipc.txt @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2021 OpenIPC + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/res/licenses/license_qualcomm.txt b/res/licenses/license_qualcomm.txt new file mode 100644 index 00000000..f3203b1b --- /dev/null +++ b/res/licenses/license_qualcomm.txt @@ -0,0 +1,40 @@ +Files with a Qualcomm Atheros / Atheros licence fall under the following +licence. + +---- + +Copyright (c) 2013 Qualcomm Atheros, Inc. + +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted (subject to the limitations in the +disclaimer below) provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the + distribution. + + * Neither the name of Qualcomm Atheros nor the names of its + contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE +GRANTED BY THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT +HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR +BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE +OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN +IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +---- \ No newline at end of file diff --git a/res/licenses/license_raspberry.txt b/res/licenses/license_raspberry.txt new file mode 100644 index 00000000..784eb006 --- /dev/null +++ b/res/licenses/license_raspberry.txt @@ -0,0 +1,28 @@ +/* +Copyright (c) 2018, Raspberry Pi (Trading) Ltd. +Copyright (c) 2013, Broadcom Europe Ltd. +Copyright (c) 2013, James Hughes +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the copyright holder nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ diff --git a/res/licenses/license_rtl8812.txt b/res/licenses/license_rtl8812.txt new file mode 100644 index 00000000..ecbc0593 --- /dev/null +++ b/res/licenses/license_rtl8812.txt @@ -0,0 +1,339 @@ + GNU GENERAL PUBLIC LICENSE + Version 2, June 1991 + + Copyright (C) 1989, 1991 Free Software Foundation, Inc., + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The licenses for most software are designed to take away your +freedom to share and change it. By contrast, the GNU General Public +License is intended to guarantee your freedom to share and change free +software--to make sure the software is free for all its users. This +General Public License applies to most of the Free Software +Foundation's software and to any other program whose authors commit to +using it. (Some other Free Software Foundation software is covered by +the GNU Lesser General Public License instead.) You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +this service if you wish), that you receive source code or can get it +if you want it, that you can change the software or use pieces of it +in new free programs; and that you know you can do these things. + + To protect your rights, we need to make restrictions that forbid +anyone to deny you these rights or to ask you to surrender the rights. +These restrictions translate to certain responsibilities for you if you +distribute copies of the software, or if you modify it. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must give the recipients all the rights that +you have. You must make sure that they, too, receive or can get the +source code. And you must show them these terms so they know their +rights. + + We protect your rights with two steps: (1) copyright the software, and +(2) offer you this license which gives you legal permission to copy, +distribute and/or modify the software. + + Also, for each author's protection and ours, we want to make certain +that everyone understands that there is no warranty for this free +software. If the software is modified by someone else and passed on, we +want its recipients to know that what they have is not the original, so +that any problems introduced by others will not reflect on the original +authors' reputations. + + Finally, any free program is threatened constantly by software +patents. We wish to avoid the danger that redistributors of a free +program will individually obtain patent licenses, in effect making the +program proprietary. To prevent this, we have made it clear that any +patent must be licensed for everyone's free use or not licensed at all. + + The precise terms and conditions for copying, distribution and +modification follow. + + GNU GENERAL PUBLIC LICENSE + TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION + + 0. This License applies to any program or other work which contains +a notice placed by the copyright holder saying it may be distributed +under the terms of this General Public License. The "Program", below, +refers to any such program or work, and a "work based on the Program" +means either the Program or any derivative work under copyright law: +that is to say, a work containing the Program or a portion of it, +either verbatim or with modifications and/or translated into another +language. (Hereinafter, translation is included without limitation in +the term "modification".) Each licensee is addressed as "you". + +Activities other than copying, distribution and modification are not +covered by this License; they are outside its scope. The act of +running the Program is not restricted, and the output from the Program +is covered only if its contents constitute a work based on the +Program (independent of having been made by running the Program). +Whether that is true depends on what the Program does. + + 1. You may copy and distribute verbatim copies of the Program's +source code as you receive it, in any medium, provided that you +conspicuously and appropriately publish on each copy an appropriate +copyright notice and disclaimer of warranty; keep intact all the +notices that refer to this License and to the absence of any warranty; +and give any other recipients of the Program a copy of this License +along with the Program. + +You may charge a fee for the physical act of transferring a copy, and +you may at your option offer warranty protection in exchange for a fee. + + 2. You may modify your copy or copies of the Program or any portion +of it, thus forming a work based on the Program, and copy and +distribute such modifications or work under the terms of Section 1 +above, provided that you also meet all of these conditions: + + a) You must cause the modified files to carry prominent notices + stating that you changed the files and the date of any change. + + b) You must cause any work that you distribute or publish, that in + whole or in part contains or is derived from the Program or any + part thereof, to be licensed as a whole at no charge to all third + parties under the terms of this License. + + c) If the modified program normally reads commands interactively + when run, you must cause it, when started running for such + interactive use in the most ordinary way, to print or display an + announcement including an appropriate copyright notice and a + notice that there is no warranty (or else, saying that you provide + a warranty) and that users may redistribute the program under + these conditions, and telling the user how to view a copy of this + License. (Exception: if the Program itself is interactive but + does not normally print such an announcement, your work based on + the Program is not required to print an announcement.) + +These requirements apply to the modified work as a whole. If +identifiable sections of that work are not derived from the Program, +and can be reasonably considered independent and separate works in +themselves, then this License, and its terms, do not apply to those +sections when you distribute them as separate works. But when you +distribute the same sections as part of a whole which is a work based +on the Program, the distribution of the whole must be on the terms of +this License, whose permissions for other licensees extend to the +entire whole, and thus to each and every part regardless of who wrote it. + +Thus, it is not the intent of this section to claim rights or contest +your rights to work written entirely by you; rather, the intent is to +exercise the right to control the distribution of derivative or +collective works based on the Program. + +In addition, mere aggregation of another work not based on the Program +with the Program (or with a work based on the Program) on a volume of +a storage or distribution medium does not bring the other work under +the scope of this License. + + 3. You may copy and distribute the Program (or a work based on it, +under Section 2) in object code or executable form under the terms of +Sections 1 and 2 above provided that you also do one of the following: + + a) Accompany it with the complete corresponding machine-readable + source code, which must be distributed under the terms of Sections + 1 and 2 above on a medium customarily used for software interchange; or, + + b) Accompany it with a written offer, valid for at least three + years, to give any third party, for a charge no more than your + cost of physically performing source distribution, a complete + machine-readable copy of the corresponding source code, to be + distributed under the terms of Sections 1 and 2 above on a medium + customarily used for software interchange; or, + + c) Accompany it with the information you received as to the offer + to distribute corresponding source code. (This alternative is + allowed only for noncommercial distribution and only if you + received the program in object code or executable form with such + an offer, in accord with Subsection b above.) + +The source code for a work means the preferred form of the work for +making modifications to it. For an executable work, complete source +code means all the source code for all modules it contains, plus any +associated interface definition files, plus the scripts used to +control compilation and installation of the executable. However, as a +special exception, the source code distributed need not include +anything that is normally distributed (in either source or binary +form) with the major components (compiler, kernel, and so on) of the +operating system on which the executable runs, unless that component +itself accompanies the executable. + +If distribution of executable or object code is made by offering +access to copy from a designated place, then offering equivalent +access to copy the source code from the same place counts as +distribution of the source code, even though third parties are not +compelled to copy the source along with the object code. + + 4. You may not copy, modify, sublicense, or distribute the Program +except as expressly provided under this License. Any attempt +otherwise to copy, modify, sublicense or distribute the Program is +void, and will automatically terminate your rights under this License. +However, parties who have received copies, or rights, from you under +this License will not have their licenses terminated so long as such +parties remain in full compliance. + + 5. You are not required to accept this License, since you have not +signed it. However, nothing else grants you permission to modify or +distribute the Program or its derivative works. These actions are +prohibited by law if you do not accept this License. Therefore, by +modifying or distributing the Program (or any work based on the +Program), you indicate your acceptance of this License to do so, and +all its terms and conditions for copying, distributing or modifying +the Program or works based on it. + + 6. Each time you redistribute the Program (or any work based on the +Program), the recipient automatically receives a license from the +original licensor to copy, distribute or modify the Program subject to +these terms and conditions. You may not impose any further +restrictions on the recipients' exercise of the rights granted herein. +You are not responsible for enforcing compliance by third parties to +this License. + + 7. If, as a consequence of a court judgment or allegation of patent +infringement or for any other reason (not limited to patent issues), +conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot +distribute so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you +may not distribute the Program at all. For example, if a patent +license would not permit royalty-free redistribution of the Program by +all those who receive copies directly or indirectly through you, then +the only way you could satisfy both it and this License would be to +refrain entirely from distribution of the Program. + +If any portion of this section is held invalid or unenforceable under +any particular circumstance, the balance of the section is intended to +apply and the section as a whole is intended to apply in other +circumstances. + +It is not the purpose of this section to induce you to infringe any +patents or other property right claims or to contest validity of any +such claims; this section has the sole purpose of protecting the +integrity of the free software distribution system, which is +implemented by public license practices. Many people have made +generous contributions to the wide range of software distributed +through that system in reliance on consistent application of that +system; it is up to the author/donor to decide if he or she is willing +to distribute software through any other system and a licensee cannot +impose that choice. + +This section is intended to make thoroughly clear what is believed to +be a consequence of the rest of this License. + + 8. If the distribution and/or use of the Program is restricted in +certain countries either by patents or by copyrighted interfaces, the +original copyright holder who places the Program under this License +may add an explicit geographical distribution limitation excluding +those countries, so that distribution is permitted only in or among +countries not thus excluded. In such case, this License incorporates +the limitation as if written in the body of this License. + + 9. The Free Software Foundation may publish revised and/or new versions +of the General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + +Each version is given a distinguishing version number. If the Program +specifies a version number of this License which applies to it and "any +later version", you have the option of following the terms and conditions +either of that version or of any later version published by the Free +Software Foundation. If the Program does not specify a version number of +this License, you may choose any version ever published by the Free Software +Foundation. + + 10. If you wish to incorporate parts of the Program into other free +programs whose distribution conditions are different, write to the author +to ask for permission. For software which is copyrighted by the Free +Software Foundation, write to the Free Software Foundation; we sometimes +make exceptions for this. Our decision will be guided by the two goals +of preserving the free status of all derivatives of our free software and +of promoting the sharing and reuse of software generally. + + NO WARRANTY + + 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY +FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN +OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES +PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED +OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS +TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE +PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, +REPAIR OR CORRECTION. + + 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR +REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, +INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING +OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED +TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY +YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER +PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE +POSSIBILITY OF SUCH DAMAGES. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +convey the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This program 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 2 of the License, or + (at your option) any later version. + + This program 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, write to the Free Software Foundation, Inc., + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + +Also add information on how to contact you by electronic and paper mail. + +If the program is interactive, make it output a short notice like this +when it starts in an interactive mode: + + Gnomovision version 69, Copyright (C) year name of author + Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, the commands you use may +be called something other than `show w' and `show c'; they could even be +mouse-clicks or menu items--whatever suits your program. + +You should also get your employer (if you work as a programmer) or your +school, if any, to sign a "copyright disclaimer" for the program, if +necessary. Here is a sample; alter the names: + + Yoyodyne, Inc., hereby disclaims all copyright interest in the program + `Gnomovision' (which makes passes at compilers) written by James Hacker. + + , 1 April 1989 + Ty Coon, President of Vice + +This General Public License does not permit incorporating your program into +proprietary programs. If your program is a subroutine library, you may +consider it more useful to permit linking proprietary applications with the +library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. \ No newline at end of file diff --git a/res/licenses/license_ruby.txt b/res/licenses/license_ruby.txt new file mode 100644 index 00000000..b8b3ee7c --- /dev/null +++ b/res/licenses/license_ruby.txt @@ -0,0 +1,38 @@ +Ruby written by Petru Soroaga (petrusoroaga@yahoo.com) +Contributors: Tree Orbit, Piotr Kujawski (aka bitkuna) + +More info about Ruby here: +https://rubyfpv.com +https://www.rcgroups.com/forums/showthread.php?3880253-Ruby-Digital-FPV-Video-Link-%28v-2-3%29 + + + Ruby Licence + Copyright (c) 2024 Petru Soroaga petrusoroaga@yahoo.com + All rights reserved. + + Redistribution and use in source and/or binary forms, with or without + modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + Copyright info and developer info must be preserved as is in the user + interface, additions could be made to that info. + * Neither the name of the organization nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + * Military use is not permited. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + DISCLAIMED. IN NO EVENT SHALL THE AUTHOR (PETRU SOROAGA) BE LIABLE FOR ANY + DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +See the res/licences folder for additional details. diff --git a/res/msp_osd_ardu.png b/res/msp_osd_ardu.png new file mode 100644 index 00000000..7f85b839 Binary files /dev/null and b/res/msp_osd_ardu.png differ diff --git a/res/msp_osd_betaflight.png b/res/msp_osd_betaflight.png new file mode 100644 index 00000000..2be0ec54 Binary files /dev/null and b/res/msp_osd_betaflight.png differ diff --git a/res/msp_osd_inav.png b/res/msp_osd_inav.png new file mode 100644 index 00000000..cae667ea Binary files /dev/null and b/res/msp_osd_inav.png differ diff --git a/res/ruby_bg4.png b/res/ruby_bg4.png new file mode 100644 index 00000000..db1ee2c9 Binary files /dev/null and b/res/ruby_bg4.png differ diff --git a/res/ruby_bg5.png b/res/ruby_bg5.png new file mode 100644 index 00000000..cecd95aa Binary files /dev/null and b/res/ruby_bg5.png differ