@ -1,6 +1,6 @@
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright ( c ) 2012 - 2016 PX4 Development Team . All rights reserved .
* Copyright ( c ) 2012 - 2017 PX4 Development Team . All rights reserved .
*
* Redistribution and use in source and binary forms , with or without
* modification , are permitted provided that the following conditions
@ -33,7 +33,7 @@
@@ -33,7 +33,7 @@
/**
* @ file mavlink_messages . cpp
* MAVLink 1 .0 message formatters implementation .
* MAVLink 2 .0 message formatters implementation .
*
* @ author Lorenz Meier < lorenz @ px4 . io >
* @ author Anton Babushkin < anton . babushkin @ me . com >
@ -375,26 +375,11 @@ private:
@@ -375,26 +375,11 @@ private:
MavlinkStreamStatustext ( MavlinkStreamStatustext & ) ;
MavlinkStreamStatustext & operator = ( const MavlinkStreamStatustext & ) ;
unsigned _write_err_count = 0 ;
static const unsigned write_err_threshold = 5 ;
# if !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_EXCELSIOR)
FILE * _fp = nullptr ;
# endif
protected :
explicit MavlinkStreamStatustext ( Mavlink * mavlink ) : MavlinkStream ( mavlink )
{ }
~ MavlinkStreamStatustext ( )
{
# if !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_EXCELSIOR)
if ( _fp ! = nullptr ) {
fclose ( _fp ) ;
}
# endif
}
~ MavlinkStreamStatustext ( ) { }
void send ( const hrt_abstime t )
@ -405,78 +390,12 @@ protected:
@@ -405,78 +390,12 @@ protected:
if ( _mavlink - > get_logbuffer ( ) - > get ( & mavlink_log ) ) {
mavlink_statustext_t msg = { } ;
mavlink_statustext_t msg ;
msg . severity = mavlink_log . severity ;
strncpy ( msg . text , ( const char * ) mavlink_log . text , sizeof ( msg . text ) ) ;
msg . text [ sizeof ( msg . text ) - 1 ] = ' \0 ' ;
mavlink_msg_statustext_send_struct ( _mavlink - > get_channel ( ) , & msg ) ;
// TODO: the logging doesn't work on Snapdragon yet because of file paths.
# if !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_EXCELSIOR)
/* write log messages in first instance to disk
* timestamp each message with gps time
*/
timespec ts ;
px4_clock_gettime ( CLOCK_REALTIME , & ts ) ;
time_t gps_time_sec = ts . tv_sec + ( ts . tv_nsec / 1e9 ) ;
struct tm tt = { } ;
gmtime_r ( & gps_time_sec , & tt ) ;
char tstamp [ 22 ] ;
strftime ( tstamp , sizeof ( tstamp ) - 1 , " %Y_%m_%d_%H_%M_%S " , & tt ) ;
if ( _mavlink - > get_instance_id ( ) = = 0 /* && _mavlink->get_logging_enabled()*/ ) {
if ( _fp ! = nullptr ) {
fputs ( tstamp , _fp ) ;
fputs ( " : " , _fp ) ;
if ( EOF = = fputs ( msg . text , _fp ) ) {
_write_err_count + + ;
} else {
_write_err_count = 0 ;
}
if ( _write_err_count > = write_err_threshold ) {
( void ) fclose ( _fp ) ;
_fp = nullptr ;
PX4_WARN ( " mavlink logging disabled " ) ;
} else {
( void ) fputs ( " \n " , _fp ) ;
# ifdef __PX4_NUTTX
fsync ( fileno ( _fp ) ) ;
# endif
}
} else if ( _write_err_count < write_err_threshold ) {
/* string to hold the path to the log */
char log_file_path [ 128 ] ;
/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
/* store the log file in the root directory */
snprintf ( log_file_path , sizeof ( log_file_path ) - 1 , PX4_ROOTFSDIR " /fs/microsd/msgs_%s.txt " , tstamp ) ;
_fp = fopen ( log_file_path , " ab " ) ;
if ( _fp ! = nullptr ) {
/* write first message */
fputs ( tstamp , _fp ) ;
fputs ( " : " , _fp ) ;
fputs ( msg . text , _fp ) ;
fputs ( " \n " , _fp ) ;
# ifdef __PX4_NUTTX
fsync ( fileno ( _fp ) ) ;
# endif
} else {
PX4_WARN ( " Failed to open MAVLink log: %s " , log_file_path ) ;
_write_err_count = write_err_threshold ; //only try to open the file once
}
}
}
# endif
}
}
}
@ -531,7 +450,7 @@ protected:
@@ -531,7 +450,7 @@ protected:
void send ( const hrt_abstime t )
{
struct vehicle_command_s cmd = { } ;
struct vehicle_command_s cmd ;
if ( _cmd_sub - > update ( & _cmd_time , & cmd ) ) {
/* only send commands for other systems/components */