Line data    Source code

       1             : #include "AP_Logger_config.h"
       2             : 
       3             : #if HAL_LOGGING_ENABLED
       4             : 
       5             : #include "AP_Common/AP_FWVersion.h"
       6             : #include "LoggerMessageWriter.h"
       7             : #include <AP_Scheduler/AP_Scheduler.h>
       8             : #include <AP_Vehicle/AP_Vehicle_Type.h>
       9             : #include "AP_Logger.h"
      10             : #include <AP_RCProtocol/AP_RCProtocol.h>
      11             : 
      12             : #if HAL_LOGGER_FENCE_ENABLED
      13             :     #include <AC_Fence/AC_Fence.h>
      14             : #endif
      15             : 
      16             : #if HAL_LOGGER_RALLY_ENABLED
      17             : #include <AP_Rally/AP_Rally.h>
      18             : #endif
      19             : 
      20             : #define FORCE_VERSION_H_INCLUDE
      21             : #include "ap_version.h"
      22             : #undef FORCE_VERSION_H_INCLUDE
      23             : 
      24             : // the message writers are mainly called at the end of periodic_fullrate() with a budget of 300us
      25             : // until they are finished calls to Write() will fail so we want to take up as much of
      26             : // the budget as possible in order that logging can begin in earnest as early as possible
      27             : #define MIN_LOOP_TIME_REMAINING_FOR_MESSAGE_WRITE_US 50
      28             : 
      29             : extern const AP_HAL::HAL& hal;
      30             : 
      31             : /* LogStartup - these are simple state machines which allow us to
      32             :  * trickle out messages to the log files
      33             :  */
      34             : 
      35       13565 : void LoggerMessageWriter::reset()
      36             : {
      37       13565 :     _finished = false;
      38       13565 : }
      39             : 
      40       39221 : bool LoggerMessageWriter::out_of_time_for_writing_messages() const
      41             : {
      42             : #if AP_SCHEDULER_ENABLED
      43       39221 :     return AP::scheduler().time_available_usec() < MIN_LOOP_TIME_REMAINING_FOR_MESSAGE_WRITE_US;
      44             : #else
      45             :     return false;
      46             : #endif
      47             : }
      48             : 
      49        2079 : void LoggerMessageWriter_DFLogStart::reset()
      50             : {
      51        2079 :     LoggerMessageWriter::reset();
      52             : 
      53        2079 :     _fmt_done = false;
      54        2079 :     _params_done = false;
      55        2079 :     _writesysinfo.reset();
      56             : #if AP_MISSION_ENABLED
      57        2079 :     _writeentiremission.reset();
      58             : #endif
      59             : #if HAL_LOGGER_RALLY_ENABLED
      60        2079 :     _writeallrallypoints.reset();
      61             : #endif
      62             : #if HAL_LOGGER_FENCE_ENABLED
      63        2079 :     _writeallpolyfence.reset();
      64             : #endif
      65             : 
      66        2079 :     stage = Stage::FORMATS;
      67        2079 :     next_format_to_send = 0;
      68        2079 :     _next_unit_to_send = 0;
      69        2079 :     _next_multiplier_to_send = 0;
      70        2079 :     _next_format_unit_to_send = 0;
      71        2079 :     param_default = AP_Logger::quiet_nanf();
      72        2079 :     ap = AP_Param::first(&token, &type, &param_default);
      73        2079 : }
      74             : 
      75     1729266 : bool LoggerMessageWriter_DFLogStart::out_of_time_for_writing_messages_df() const
      76             : {
      77     1729266 :     if (stage == Stage::FORMATS) {
      78             :         // write out the FMT messages as fast as we can
      79             : #if AP_SCHEDULER_ENABLED
      80     1702702 :         return AP::scheduler().time_available_usec() == 0;
      81             : #else
      82             :         return false;
      83             : #endif
      84             :     }
      85       26564 :     return LoggerMessageWriter::out_of_time_for_writing_messages();
      86             : }
      87             : 
      88             : /*
      89             :   check if we've taken too long in a process() stage
      90             :   return true if we should stop processing now as we are out of time
      91             :  */
      92     3760569 : bool LoggerMessageWriter_DFLogStart::check_process_limit(uint32_t start_us)
      93             : {
      94     3760569 :     const uint32_t limit_us = 1000U;
      95     3760569 :     if (AP_HAL::micros() - start_us > limit_us) {
      96           0 :         return true;
      97             :     }
      98     3760569 :     return false;
      99             : }
     100             : 
     101     1729266 : void LoggerMessageWriter_DFLogStart::process()
     102             : {
     103     1729266 :     if (out_of_time_for_writing_messages_df()) {
     104           0 :         return;
     105             :     }
     106             :     // allow any stage to run for max 1ms, to prevent a long loop on arming
     107     1729266 :     const uint32_t start_us = AP_HAL::micros();
     108             : 
     109     1729266 :     switch(stage) {
     110     1702702 :     case Stage::FORMATS:
     111             :         // write log formats so the log is self-describing
     112     2085404 :         while (next_format_to_send < _logger_backend->num_types()) {
     113     2083389 :             const auto &s { _logger_backend->structure(next_format_to_send) };
     114     2083389 :             if (_logger_backend->have_emitted_format_for_type((LogMessages)s->msg_type)) {
     115       13132 :                 next_format_to_send++;
     116       13132 :                 continue;
     117             :             }
     118     2070257 :             if (!_logger_backend->Write_Format(s)) {
     119     1700687 :                 return; // call me again!
     120             :             }
     121      369570 :             next_format_to_send++;
     122      369570 :             if (check_process_limit(start_us)) {
     123           0 :                 return; // call me again!
     124             :             }
     125             :         }
     126        2015 :         _fmt_done = true;
     127        2015 :         stage = Stage::PARMS;
     128             :         FALLTHROUGH;
     129             : 
     130        9580 :     case Stage::PARMS: {
     131     2911144 :         while (ap) {
     132     2909129 :             if (!_logger_backend->Write_Parameter(ap, token, type, param_default)) {
     133        7565 :                 return;
     134             :             }
     135     2901564 :             param_default = AP_Logger::quiet_nanf();
     136     2901564 :             ap = AP_Param::next_scalar(&token, &type, &param_default);
     137     2901564 :             if (check_process_limit(start_us)) {
     138           0 :                 return; // call me again!
     139             :             }
     140             :         }
     141             : 
     142        2015 :         _params_done = true;
     143        2015 :         stage = Stage::UNITS;
     144             :         }
     145             :         FALLTHROUGH;
     146             : 
     147       17172 :     case Stage::UNITS:
     148       93742 :         while (_next_unit_to_send < _logger_backend->num_units()) {
     149       78297 :             if (!_logger_backend->Write_Unit(_logger_backend->unit(_next_unit_to_send))) {
     150        1727 :                 return; // call me again!
     151             :             }
     152       76570 :             _next_unit_to_send++;
     153       76570 :             if (check_process_limit(start_us)) {
     154           0 :                 return; // call me again!
     155             :             }
     156             :         }
     157       15445 :         stage = Stage::MULTIPLIERS;
     158             :         FALLTHROUGH;
     159             : 
     160       16076 :     case Stage::MULTIPLIERS:
     161       46301 :         while (_next_multiplier_to_send < _logger_backend->num_multipliers()) {
     162       30856 :             if (!_logger_backend->Write_Multiplier(_logger_backend->multiplier(_next_multiplier_to_send))) {
     163         631 :                 return; // call me again!
     164             :             }
     165       30225 :             _next_multiplier_to_send++;
     166       30225 :             if (check_process_limit(start_us)) {
     167           0 :                 return; // call me again!
     168             :             }
     169             :         }
     170       15445 :         stage = Stage::UNITS;
     171             :         FALLTHROUGH;
     172             : 
     173       15445 :     case Stage::FORMAT_UNITS:
     174      398085 :         while (_next_format_unit_to_send < _logger_backend->num_types()) {
     175      396071 :             if (!_logger_backend->Write_Format_Units(_logger_backend->structure(_next_format_unit_to_send))) {
     176       13431 :                 return; // call me again!
     177             :             }
     178      382640 :             _next_format_unit_to_send++;
     179      382640 :             if (check_process_limit(start_us)) {
     180           0 :                 return; // call me again!
     181             :             }
     182             :         }
     183        2014 :         stage = Stage::RUNNING_SUBWRITERS;
     184             :         FALLTHROUGH;
     185             : 
     186        5225 :     case Stage::RUNNING_SUBWRITERS: {
     187             :         LoggerMessageWriter *subwriters[] {
     188             :             &_writesysinfo,
     189             : #if AP_MISSION_ENABLED
     190             :             &_writeentiremission,
     191             : #endif
     192             : #if HAL_LOGGER_RALLY_ENABLED
     193             :             &_writeallrallypoints,
     194             : #endif
     195             : #if HAL_LOGGER_FENCE_ENABLED
     196             :             &_writeallpolyfence,
     197             : #endif
     198        5225 :         };
     199       25990 :         for (auto *sw : subwriters) {
     200       20806 :             if (!sw->finished()) {
     201       11267 :                 sw->process();
     202       11267 :                 if (!sw->finished()) {
     203          41 :                     return;
     204             :                 }
     205             :             }
     206             :         }
     207        5184 :         stage = Stage::VEHICLE_MESSAGES;
     208             :         FALLTHROUGH;
     209             :     }
     210             :     case Stage::VEHICLE_MESSAGES:
     211             :         // we guarantee 200 bytes of space for the vehicle startup
     212             :         // messages.  This allows them to be simple functions rather
     213             :         // than e.g. LoggerMessageWriter-based state machines
     214        5184 :         if (_logger_backend->vehicle_message_writer()) {
     215        5184 :             if (_logger_backend->bufferspace_available() < 200) {
     216           0 :                 return;
     217             :             }
     218        5184 :             (_logger_backend->vehicle_message_writer())();
     219             :         }
     220        5184 :         stage = Stage::DONE;
     221             :         FALLTHROUGH;
     222             : 
     223        5184 :     case Stage::DONE:
     224        5184 :         break;
     225             :     }
     226             : 
     227        5184 :     _finished = true;
     228             : }
     229             : 
     230             : #if AP_MISSION_ENABLED
     231        1287 : bool LoggerMessageWriter_DFLogStart::writeentiremission()
     232             : {
     233        1287 :     if (stage != Stage::DONE) {
     234           4 :         return false;
     235             :     }
     236        1283 :     stage = Stage::RUNNING_SUBWRITERS;
     237        1283 :     _finished = false;
     238        1283 :     _writeentiremission.reset();
     239        1283 :     return true;
     240             : }
     241             : #endif
     242             : 
     243             : #if HAL_LOGGER_RALLY_ENABLED
     244         886 : bool LoggerMessageWriter_DFLogStart::writeallrallypoints()
     245             : {
     246         886 :     if (stage != Stage::DONE) {
     247           0 :         return false;
     248             :     }
     249         886 :     stage = Stage::RUNNING_SUBWRITERS;
     250         886 :     _finished = false;
     251         886 :     _writeallrallypoints.reset();
     252         886 :     return true;
     253             : }
     254             : #endif
     255             : 
     256             : #if HAL_LOGGER_FENCE_ENABLED
     257        1001 : bool LoggerMessageWriter_DFLogStart::writeallfence()
     258             : {
     259        1001 :     if (stage != Stage::DONE) {
     260           0 :         return false;
     261             :     }
     262        1001 :     stage = Stage::RUNNING_SUBWRITERS;
     263        1001 :     _finished = false;
     264        1001 :     _writeallpolyfence.reset();
     265        1001 :     return true;
     266             : }
     267             : #endif
     268             : 
     269        2079 : void LoggerMessageWriter_WriteSysInfo::reset()
     270             : {
     271        2079 :     LoggerMessageWriter::reset();
     272        2079 :     stage = Stage::FIRMWARE_STRING;
     273        2079 : }
     274             : 
     275        2038 : void LoggerMessageWriter_WriteSysInfo::process() {
     276        2038 :     const AP_FWVersion &fwver = AP::fwversion();
     277             : 
     278        2038 :     switch(stage) {
     279             : 
     280        2014 :     case Stage::FIRMWARE_STRING:
     281             : #ifdef AP_CUSTOM_FIRMWARE_STRING
     282             :         // also log original firmware string if different
     283             :         if (! _logger_backend->Write_MessageF("%s [%s]",
     284             :                                               fwver.fw_string,
     285             :                                               fwver.fw_string_original)) {
     286             :             return; // call me again
     287             :         }
     288             : #else
     289        2014 :         if (! _logger_backend->Write_Message(fwver.fw_string)) {
     290          24 :             return; // call me again
     291             :         }
     292             : #endif
     293        2014 :         stage = Stage::GIT_VERSIONS;
     294             :         FALLTHROUGH;
     295             : 
     296        2014 :     case Stage::GIT_VERSIONS:
     297        2014 :         if (fwver.middleware_name && fwver.os_name) {
     298           0 :             if (! _logger_backend->Write_MessageF("%s: %s %s: %s",
     299           0 :                                                         fwver.middleware_name,
     300           0 :                                                         fwver.middleware_hash_str,
     301           0 :                                                         fwver.os_name,
     302           0 :                                                         fwver.os_hash_str)) {
     303           0 :                 return; // call me again
     304             :             }
     305        2014 :         } else if (fwver.os_name) {
     306           0 :             if (! _logger_backend->Write_MessageF("%s: %s",
     307           0 :                                                         fwver.os_name,
     308           0 :                                                         fwver.os_hash_str)) {
     309           0 :                 return; // call me again
     310             :             }
     311             :         }
     312        2014 :         stage = Stage::VER;
     313             :         FALLTHROUGH;
     314             : 
     315        2021 :     case Stage::VER: {
     316        2021 :         if (!_logger_backend->Write_VER()) {
     317           7 :             return;
     318             :         }
     319        2014 :         stage = Stage::SYSTEM_ID;
     320             :         FALLTHROUGH;
     321             :     }
     322        2018 :     case Stage::SYSTEM_ID:
     323             :         char sysid[50];
     324        2018 :         if (hal.util->get_system_id(sysid)) {
     325        2018 :             if (! _logger_backend->Write_Message(sysid)) {
     326           4 :                 return; // call me again
     327             :             }
     328             :         }
     329        2014 :         stage = Stage::PARAM_SPACE_USED;
     330             :         FALLTHROUGH;
     331             : 
     332        2018 :     case Stage::PARAM_SPACE_USED:
     333        2018 :         if (! _logger_backend->Write_MessageF("Param space used: %u/%u", AP_Param::storage_used(), AP_Param::storage_size())) {
     334           4 :             return; // call me again
     335             :         }
     336        2014 :         stage = Stage::RC_PROTOCOL;
     337             :         FALLTHROUGH;
     338             : 
     339        2019 :     case Stage::RC_PROTOCOL: {
     340             : #if CONFIG_HAL_BOARD != HAL_BOARD_LINUX
     341             : #if AP_RCPROTOCOL_ENABLED
     342        2019 :         const char *prot = AP::RC().detected_protocol_name();
     343             : #else
     344             :         const char *prot = nullptr;
     345             : #endif
     346             : #else  // this is not hal-chibios
     347           0 :         const char *prot = hal.rcin->protocol();
     348             : #endif
     349        2019 :         if (prot == nullptr) {
     350          10 :             prot = "None";
     351             :         }
     352        2019 :         if (! _logger_backend->Write_MessageF("RC Protocol: %s", prot)) {
     353           5 :             return; // call me again
     354             :         }
     355        2014 :         stage = Stage::RC_OUTPUT;
     356             :         FALLTHROUGH;
     357             :     }
     358        2014 :     case Stage::RC_OUTPUT: {
     359             :         char banner_msg[50];
     360        2014 :         if (hal.rcout->get_output_mode_banner(banner_msg, sizeof(banner_msg))) {
     361           0 :             if (!_logger_backend->Write_Message(banner_msg)) {
     362           0 :                 return; // call me again
     363             :             }
     364             :         }
     365             : #if AP_RTC_LOGGING_ENABLED
     366        2014 :         stage = Stage::LOG_RTC_MSG;
     367             :         FALLTHROUGH;
     368             : #else
     369             :         break;
     370             : #endif  // AP_RTC_LOGGING_ENABLED
     371             :     }
     372             : #if AP_RTC_LOGGING_ENABLED
     373             :     case Stage::LOG_RTC_MSG:
     374        2018 :         if (! _logger_backend->Write_RTC()) {
     375           4 :             return;
     376             :         }
     377        2014 :         break;
     378             : #endif  // AP_RTC_LOGGING_ENABLED
     379             :     }
     380             : 
     381        2014 :     _finished = true;  // all done!
     382             : }
     383             : 
     384             : #if HAL_LOGGER_RALLY_ENABLED
     385        2902 : void LoggerMessageWriter_WriteAllRallyPoints::process()
     386             : {
     387        2902 :     const AP_Rally *_rally = AP::rally();
     388        2902 :     if (_rally == nullptr) {
     389          32 :         _finished = true;
     390          32 :         return;
     391             :     }
     392             : 
     393        2870 :     switch(stage) {
     394             : 
     395        2870 :     case Stage::WRITE_NEW_RALLY_MESSAGE:
     396        2870 :         if (! _logger_backend->Write_Message("New rally")) {
     397           2 :             return; // call me again
     398             :         }
     399        2868 :         stage = Stage::WRITE_ALL_RALLY_POINTS;
     400             :         FALLTHROUGH;
     401             : 
     402        2868 :     case Stage::WRITE_ALL_RALLY_POINTS:
     403        2879 :         while (_rally_number_to_send < _rally->get_rally_total()) {
     404          11 :             if (out_of_time_for_writing_messages()) {
     405           0 :                 return;
     406             :             }
     407             :             RallyLocation rallypoint;
     408          11 :             if (_rally->get_rally_point_with_index(_rally_number_to_send, rallypoint)) {
     409          11 :                 if (!_logger_backend->Write_RallyPoint(
     410          11 :                         _rally->get_rally_total(),
     411          11 :                         _rally_number_to_send,
     412             :                         rallypoint)) {
     413           0 :                     return; // call me again
     414             :                 }
     415             :             }
     416          11 :             _rally_number_to_send++;
     417             :         }
     418        2868 :         stage = Stage::DONE;
     419             :         FALLTHROUGH;
     420             : 
     421        2868 :     case Stage::DONE:
     422        2868 :         break;
     423             :     }
     424             : 
     425        2868 :     _finished = true;
     426             : }
     427             : 
     428        2965 : void LoggerMessageWriter_WriteAllRallyPoints::reset()
     429             : {
     430        2965 :     LoggerMessageWriter::reset();
     431        2965 :     stage = Stage::WRITE_NEW_RALLY_MESSAGE;
     432        2965 :     _rally_number_to_send = 0;
     433        2965 : }
     434             : #endif  // HAL_LOGGER_RALLY_ENABLED
     435             : 
     436        3307 : void LoggerMessageWriter_WriteEntireMission::process() {
     437        3307 :     const AP_Mission &_mission = AP::mission();
     438             : 
     439        3307 :     switch(stage) {
     440             : 
     441        3297 :     case Stage::WRITE_NEW_MISSION_MESSAGE:
     442        3297 :         if (! _logger_backend->Write_Message("New mission")) {
     443           0 :             return; // call me again
     444             :         }
     445        3297 :         stage = Stage::WRITE_MISSION_ITEMS;
     446             :         FALLTHROUGH;
     447             : 
     448        3307 :     case Stage::WRITE_MISSION_ITEMS: {
     449        3307 :         AP_Mission::Mission_Command cmd;
     450       14100 :         while (_mission_number_to_send < _mission.num_commands()) {
     451       10803 :             if (out_of_time_for_writing_messages()) {
     452          10 :                 return;
     453             :             }
     454             :             // upon failure to write the mission we will re-read from
     455             :             // storage; this could be improved.
     456       10803 :             if (_mission.read_cmd_from_storage(_mission_number_to_send,cmd)) {
     457       10803 :                 if (!_logger_backend->Write_Mission_Cmd(_mission, cmd, LOG_CMD_MSG)) {
     458          10 :                     return; // call me again
     459             :                 }
     460             :             }
     461       10793 :             _mission_number_to_send++;
     462             :         }
     463        3297 :         stage = Stage::DONE;
     464             :         FALLTHROUGH;
     465             :     }
     466             : 
     467             :     case Stage::DONE:
     468        3297 :         break;
     469             :     }
     470             : 
     471        3297 :     _finished = true;
     472             : }
     473             : 
     474        3362 : void LoggerMessageWriter_WriteEntireMission::reset()
     475             : {
     476        3362 :     LoggerMessageWriter::reset();
     477        3362 :     stage = Stage::WRITE_NEW_MISSION_MESSAGE;
     478        3362 :     _mission_number_to_send = 0;
     479        3362 : }
     480             : 
     481             : #if HAL_LOGGER_FENCE_ENABLED
     482             : #if APM_BUILD_TYPE(APM_BUILD_Replay)
     483             : // dummy methods to allow build with Replay
     484             : void LoggerMessageWriter_Write_Polyfence::process() { };
     485             : void LoggerMessageWriter_Write_Polyfence::reset() { };
     486             : #else
     487        3020 : void LoggerMessageWriter_Write_Polyfence::process() {
     488             : 
     489        3020 :     AC_Fence *fence = AP::fence();
     490        3020 :     if (fence == nullptr) {
     491          32 :         _finished = true;
     492          32 :         return;
     493             :     }
     494             : 
     495        2988 :     switch(stage) {
     496             : 
     497        2988 :     case Stage::WRITE_NEW_FENCE_MESSAGE:
     498        2988 :         if (!_logger_backend->Write_Message("New fence")) {
     499           5 :             return; // call me again
     500             :         }
     501        2983 :         stage = Stage::WRITE_FENCE_ITEMS;
     502             :         FALLTHROUGH;
     503             : 
     504        2983 :     case Stage::WRITE_FENCE_ITEMS: {
     505        4826 :         while (_fence_number_to_send < fence->polyfence().num_stored_items()) {
     506        1843 :             if (out_of_time_for_writing_messages()) {
     507           0 :                 return;
     508             :             }
     509             : 
     510             :             // upon failure to write the fence we will re-read from
     511             :             // storage; this could be improved.
     512        1843 :             AC_PolyFenceItem fenceitem {};
     513        1843 :             if (fence->polyfence().get_item(_fence_number_to_send, fenceitem)) {
     514        1843 :                 if (!_logger_backend->Write_FencePoint(fence->polyfence().num_stored_items(), _fence_number_to_send, fenceitem)) {
     515           0 :                     return; // call me again
     516             :                 }
     517             :             }
     518        1843 :             _fence_number_to_send++;
     519             :         }
     520        2983 :         stage = Stage::DONE;
     521             :         FALLTHROUGH;
     522             :     }
     523             : 
     524        2983 :     case Stage::DONE:
     525        2983 :         break;
     526             :     }
     527             : 
     528        2983 :     _finished = true;
     529             : }
     530             : 
     531        3080 : void LoggerMessageWriter_Write_Polyfence::reset()
     532             : {
     533        3080 :     LoggerMessageWriter::reset();
     534        3080 :     stage = Stage::WRITE_NEW_FENCE_MESSAGE;
     535        3080 :     _fence_number_to_send = 0;
     536        3080 : }
     537             : #endif // !APM_BUILD_TYPE(APM_BUILD_Replay)
     538             : #endif // HAL_LOGGER_FENCE_ENABLED
     539             : 
     540             : #endif  // HAL_LOGGING_ENABLED