2015-05-13 00:16:45 -03:00
# include "Rover.h"
2016-05-05 19:10:08 -03:00
# include "version.h"
2015-05-13 00:16:45 -03:00
2012-04-30 04:17:14 -03:00
# if LOGGING_ENABLED == ENABLED
2015-03-13 08:40:06 -03:00
# if CLI_ENABLED == ENABLED
2012-04-30 04:17:14 -03:00
// Code to interact with the user to dump or erase logs
// Creates a constant array of structs representing menu options
// and stores them in Flash memory, not RAM.
// User enters the string in the console to call the functions on the right.
// See class Menu in AP_Coommon for implementation details
2015-10-25 14:03:46 -03:00
static const struct Menu : : command log_menu_commands [ ] = {
2016-12-20 09:28:58 -04:00
{ " dump " , MENU_FUNC ( dump_log ) } ,
{ " erase " , MENU_FUNC ( erase_logs ) } ,
{ " enable " , MENU_FUNC ( select_logs ) } ,
{ " disable " , MENU_FUNC ( select_logs ) }
2012-04-30 04:17:14 -03:00
} ;
// A Macro to create the Menu
2015-05-24 20:24:11 -03:00
MENU2 ( log_menu , " Log " , log_menu_commands , FUNCTOR_BIND ( & rover , & Rover : : print_log_menu , bool ) ) ;
2012-04-30 04:17:14 -03:00
2015-05-12 04:00:25 -03:00
bool Rover : : print_log_menu ( void )
2012-04-30 04:17:14 -03:00
{
2017-01-21 00:33:27 -04:00
cliSerial - > printf ( " logs enabled: " ) ;
2016-12-20 09:28:58 -04:00
if ( 0 = = g . log_bitmask ) {
2017-01-21 00:33:27 -04:00
cliSerial - > printf ( " none " ) ;
2016-12-20 09:28:58 -04:00
} else {
// Macro to make the following code a bit easier on the eye.
// Pass it the capitalised name of the log option, as defined
// in defines.h but without the LOG_ prefix. It will check for
// the bit being set and print the name of the log option to suit.
# define PLOG(_s) if (g.log_bitmask & MASK_LOG_ ## _s) cliSerial->printf(" %s", #_s)
PLOG ( ATTITUDE_FAST ) ;
PLOG ( ATTITUDE_MED ) ;
PLOG ( GPS ) ;
PLOG ( PM ) ;
PLOG ( CTUN ) ;
PLOG ( NTUN ) ;
PLOG ( MODE ) ;
PLOG ( IMU ) ;
PLOG ( CMD ) ;
PLOG ( CURRENT ) ;
PLOG ( SONAR ) ;
PLOG ( COMPASS ) ;
PLOG ( CAMERA ) ;
PLOG ( STEERING ) ;
# undef PLOG
}
2017-01-21 00:33:27 -04:00
cliSerial - > printf ( " \n " ) ;
2012-04-30 04:17:14 -03:00
2013-04-17 08:34:20 -03:00
DataFlash . ListAvailableLogs ( cliSerial ) ;
2016-12-20 09:28:58 -04:00
return ( true ) ;
2012-04-30 04:17:14 -03:00
}
2015-05-12 04:00:25 -03:00
int8_t Rover : : dump_log ( uint8_t argc , const Menu : : arg * argv )
2012-04-30 04:17:14 -03:00
{
2015-05-12 04:00:25 -03:00
int16_t dump_log_num ;
2013-04-19 18:29:57 -03:00
uint16_t dump_log_start ;
uint16_t dump_log_end ;
// check that the requested log number can be read
2015-05-12 04:00:25 -03:00
dump_log_num = argv [ 1 ] . i ;
2013-04-19 18:29:57 -03:00
2015-05-12 04:00:25 -03:00
if ( dump_log_num = = - 2 ) {
2013-02-22 19:14:35 -04:00
DataFlash . DumpPageInfo ( cliSerial ) ;
2013-04-19 18:29:57 -03:00
return ( - 1 ) ;
2015-05-12 04:00:25 -03:00
} else if ( dump_log_num < = 0 ) {
2017-01-21 00:33:27 -04:00
cliSerial - > printf ( " dumping all \n " ) ;
2013-04-19 18:29:57 -03:00
Log_Read ( 0 , 1 , 0 ) ;
return ( - 1 ) ;
2017-03-30 11:07:24 -03:00
} else if ( ( argc ! = 2 ) | | ( static_cast < uint16_t > ( dump_log_num ) > DataFlash . get_num_logs ( ) ) ) {
2017-01-21 00:33:27 -04:00
cliSerial - > printf ( " bad log number \n " ) ;
2013-04-19 18:29:57 -03:00
return ( - 1 ) ;
}
2012-04-30 04:17:14 -03:00
2015-05-12 04:00:25 -03:00
DataFlash . get_log_boundaries ( dump_log_num , dump_log_start , dump_log_end ) ;
2017-03-30 11:07:24 -03:00
Log_Read ( static_cast < uint16_t > ( dump_log_num ) , dump_log_start , dump_log_end ) ;
2012-04-30 04:17:14 -03:00
return 0 ;
}
2015-05-12 04:00:25 -03:00
int8_t Rover : : erase_logs ( uint8_t argc , const Menu : : arg * argv )
2012-04-30 04:17:14 -03:00
{
in_mavlink_delay = true ;
do_erase_logs ( ) ;
in_mavlink_delay = false ;
return 0 ;
}
2015-05-12 04:00:25 -03:00
int8_t Rover : : select_logs ( uint8_t argc , const Menu : : arg * argv )
2012-04-30 04:17:14 -03:00
{
2017-01-31 06:12:26 -04:00
uint16_t bits = 0 ;
2016-12-20 09:28:58 -04:00
if ( argc ! = 2 ) {
2017-01-21 00:33:27 -04:00
cliSerial - > printf ( " missing log type \n " ) ;
2016-12-20 09:28:58 -04:00
return ( - 1 ) ;
}
// Macro to make the following code a bit easier on the eye.
// Pass it the capitalised name of the log option, as defined
// in defines.h but without the LOG_ prefix. It will check for
// that name as the argument to the command, and set the bit in
// bits accordingly.
//
if ( ! strcasecmp ( argv [ 1 ] . str , " all " ) ) {
bits = ~ 0 ;
} else {
# define TARG(_s) if (!strcasecmp(argv[1].str, #_s)) bits |= MASK_LOG_ ## _s
TARG ( ATTITUDE_FAST ) ;
TARG ( ATTITUDE_MED ) ;
TARG ( GPS ) ;
TARG ( PM ) ;
TARG ( CTUN ) ;
TARG ( NTUN ) ;
TARG ( MODE ) ;
TARG ( IMU ) ;
TARG ( CMD ) ;
TARG ( CURRENT ) ;
TARG ( SONAR ) ;
TARG ( COMPASS ) ;
TARG ( CAMERA ) ;
TARG ( STEERING ) ;
# undef TARG
}
if ( ! strcasecmp ( argv [ 0 ] . str , " enable " ) ) {
g . log_bitmask . set_and_save ( g . log_bitmask | bits ) ;
} else {
g . log_bitmask . set_and_save ( g . log_bitmask & ~ bits ) ;
}
return ( 0 ) ;
2012-04-30 04:17:14 -03:00
}
2015-05-12 04:00:25 -03:00
int8_t Rover : : process_logs ( uint8_t argc , const Menu : : arg * argv )
2012-04-30 04:17:14 -03:00
{
2016-12-20 09:28:58 -04:00
log_menu . run ( ) ;
return 0 ;
2012-04-30 04:17:14 -03:00
}
2016-12-20 09:28:58 -04:00
# endif // CLI_ENABLED == ENABLED
2015-03-13 08:40:06 -03:00
2015-05-12 02:03:23 -03:00
void Rover : : do_erase_logs ( void )
2015-03-13 08:40:06 -03:00
{
2016-12-20 09:28:58 -04:00
cliSerial - > printf ( " \n Erasing log... \n " ) ;
2015-03-13 08:40:06 -03:00
DataFlash . EraseAll ( ) ;
2016-12-20 09:28:58 -04:00
cliSerial - > printf ( " \n Log erased. \n " ) ;
2015-03-13 08:40:06 -03:00
}
2013-04-19 04:53:07 -03:00
struct PACKED log_Performance {
2013-01-26 05:46:16 -04:00
LOG_PACKET_HEADER ;
2015-04-29 08:11:24 -03:00
uint64_t time_us ;
2013-01-26 05:46:16 -04:00
uint32_t loop_time ;
uint16_t main_loop_count ;
2013-11-01 20:40:29 -03:00
uint32_t g_dt_max ;
2013-01-26 05:46:16 -04:00
int16_t gyro_drift_x ;
int16_t gyro_drift_y ;
int16_t gyro_drift_z ;
2013-04-26 10:40:09 -03:00
uint8_t i2c_lockup_count ;
2013-11-18 00:08:53 -04:00
uint16_t ins_error_count ;
2017-03-26 18:12:11 -03:00
uint32_t mem_avail ;
2013-01-26 05:46:16 -04:00
} ;
2012-04-30 04:17:14 -03:00
// Write a performance monitoring packet. Total length : 19 bytes
2015-05-12 02:03:23 -03:00
void Rover : : Log_Write_Performance ( )
2012-04-30 04:17:14 -03:00
{
2013-01-26 05:46:16 -04:00
struct log_Performance pkt = {
LOG_PACKET_HEADER_INIT ( LOG_PERFORMANCE_MSG ) ,
2015-11-19 23:04:16 -04:00
time_us : AP_HAL : : micros64 ( ) ,
2013-01-26 05:46:16 -04:00
loop_time : millis ( ) - perf_mon_timer ,
main_loop_count : mainLoop_count ,
g_dt_max : G_Dt_max ,
gyro_drift_x : ( int16_t ) ( ahrs . get_gyro_drift ( ) . x * 1000 ) ,
gyro_drift_y : ( int16_t ) ( ahrs . get_gyro_drift ( ) . y * 1000 ) ,
gyro_drift_z : ( int16_t ) ( ahrs . get_gyro_drift ( ) . z * 1000 ) ,
2016-07-22 14:36:26 -03:00
i2c_lockup_count : 0 ,
2017-03-26 18:12:11 -03:00
ins_error_count : ins . error_count ( ) ,
hal . util - > available_memory ( )
2013-01-26 05:46:16 -04:00
} ;
DataFlash . WriteBlock ( & pkt , sizeof ( pkt ) ) ;
2012-04-30 04:17:14 -03:00
}
2013-12-15 20:14:58 -04:00
struct PACKED log_Steering {
LOG_PACKET_HEADER ;
2015-04-29 08:11:24 -03:00
uint64_t time_us ;
2013-12-15 20:14:58 -04:00
float demanded_accel ;
float achieved_accel ;
} ;
// Write a steering packet
2015-05-12 02:03:23 -03:00
void Rover : : Log_Write_Steering ( )
2013-12-15 20:14:58 -04:00
{
struct log_Steering pkt = {
LOG_PACKET_HEADER_INIT ( LOG_STEERING_MSG ) ,
2015-11-19 23:04:16 -04:00
time_us : AP_HAL : : micros64 ( ) ,
2013-12-15 20:14:58 -04:00
demanded_accel : lateral_acceleration ,
2016-05-31 08:23:01 -03:00
achieved_accel : ahrs . groundspeed ( ) * ins . get_gyro ( ) . z ,
2013-12-15 20:14:58 -04:00
} ;
DataFlash . WriteBlock ( & pkt , sizeof ( pkt ) ) ;
}
2017-04-03 17:46:12 -03:00
// Write beacon position and distances
void Rover : : Log_Write_Beacon ( )
{
// exit immediately if feature is disabled
if ( ! g2 . beacon . enabled ( ) ) {
return ;
}
DataFlash . Log_Write_Beacon ( g2 . beacon ) ;
}
2013-04-19 04:53:07 -03:00
struct PACKED log_Startup {
2013-01-26 05:46:16 -04:00
LOG_PACKET_HEADER ;
2015-04-29 08:11:24 -03:00
uint64_t time_us ;
2013-01-26 05:46:16 -04:00
uint8_t startup_type ;
2014-03-10 05:45:49 -03:00
uint16_t command_total ;
2013-01-26 05:46:16 -04:00
} ;
2012-04-30 04:17:14 -03:00
2015-11-09 18:40:10 -04:00
void Rover : : Log_Write_Startup ( uint8_t type )
2012-04-30 04:17:14 -03:00
{
2013-01-26 05:46:16 -04:00
struct log_Startup pkt = {
LOG_PACKET_HEADER_INIT ( LOG_STARTUP_MSG ) ,
2015-11-19 23:04:16 -04:00
time_us : AP_HAL : : micros64 ( ) ,
2013-01-26 05:46:16 -04:00
startup_type : type ,
2014-03-10 05:45:49 -03:00
command_total : mission . num_commands ( )
2013-01-26 05:46:16 -04:00
} ;
2015-11-09 18:40:10 -04:00
DataFlash . WriteBlock ( & pkt , sizeof ( pkt ) ) ;
2012-04-30 04:17:14 -03:00
}
2013-04-19 04:53:07 -03:00
struct PACKED log_Control_Tuning {
2013-01-26 05:46:16 -04:00
LOG_PACKET_HEADER ;
2015-04-29 08:11:24 -03:00
uint64_t time_us ;
2013-02-07 18:21:22 -04:00
int16_t steer_out ;
2013-01-26 05:46:16 -04:00
int16_t roll ;
int16_t pitch ;
int16_t throttle_out ;
2013-04-19 04:53:07 -03:00
float accel_y ;
2013-01-26 05:46:16 -04:00
} ;
2012-04-30 04:17:14 -03:00
2013-01-26 05:46:16 -04:00
// Write a control tuning packet. Total length : 22 bytes
2015-05-12 02:03:23 -03:00
void Rover : : Log_Write_Control_Tuning ( )
2012-04-30 04:17:14 -03:00
{
2017-01-31 06:12:26 -04:00
const Vector3f accel = ins . get_accel ( ) ;
2013-01-26 05:46:16 -04:00
struct log_Control_Tuning pkt = {
2013-04-19 04:53:07 -03:00
LOG_PACKET_HEADER_INIT ( LOG_CTUN_MSG ) ,
2015-11-19 23:04:16 -04:00
time_us : AP_HAL : : micros64 ( ) ,
2017-01-06 06:31:10 -04:00
steer_out : ( int16_t ) SRV_Channels : : get_output_scaled ( SRV_Channel : : k_steering ) ,
2013-01-26 05:46:16 -04:00
roll : ( int16_t ) ahrs . roll_sensor ,
pitch : ( int16_t ) ahrs . pitch_sensor ,
2017-01-06 06:31:10 -04:00
throttle_out : ( int16_t ) SRV_Channels : : get_output_scaled ( SRV_Channel : : k_throttle ) ,
2013-04-19 04:53:07 -03:00
accel_y : accel . y
2013-01-26 05:46:16 -04:00
} ;
DataFlash . WriteBlock ( & pkt , sizeof ( pkt ) ) ;
2012-04-30 04:17:14 -03:00
}
2013-04-19 04:53:07 -03:00
struct PACKED log_Nav_Tuning {
2013-01-26 05:46:16 -04:00
LOG_PACKET_HEADER ;
2015-04-29 08:11:24 -03:00
uint64_t time_us ;
2013-01-26 05:46:16 -04:00
uint16_t yaw ;
2013-03-01 07:32:57 -04:00
float wp_distance ;
2013-01-26 05:46:16 -04:00
uint16_t target_bearing_cd ;
uint16_t nav_bearing_cd ;
2013-04-18 21:23:57 -03:00
int8_t throttle ;
2016-01-05 20:18:29 -04:00
float xtrack_error ;
2013-01-26 05:46:16 -04:00
} ;
2012-04-30 04:17:14 -03:00
2016-01-05 20:18:29 -04:00
// Write a navigation tuning packet
2015-05-12 02:03:23 -03:00
void Rover : : Log_Write_Nav_Tuning ( )
2012-04-30 04:17:14 -03:00
{
2013-01-26 05:46:16 -04:00
struct log_Nav_Tuning pkt = {
2013-04-19 04:53:07 -03:00
LOG_PACKET_HEADER_INIT ( LOG_NTUN_MSG ) ,
2015-11-19 23:04:16 -04:00
time_us : AP_HAL : : micros64 ( ) ,
2017-02-20 09:26:00 -04:00
yaw : static_cast < uint16_t > ( ahrs . yaw_sensor ) ,
2013-03-01 07:32:57 -04:00
wp_distance : wp_distance ,
2017-02-20 09:26:00 -04:00
target_bearing_cd : static_cast < uint16_t > ( fabsf ( nav_controller - > target_bearing_cd ( ) ) ) ,
nav_bearing_cd : static_cast < uint16_t > ( fabsf ( nav_controller - > nav_bearing_cd ( ) ) ) ,
throttle : static_cast < int8_t > ( 100 * SRV_Channels : : get_output_norm ( SRV_Channel : : k_throttle ) ) ,
2016-01-05 20:18:29 -04:00
xtrack_error : nav_controller - > crosstrack_error ( )
2013-01-26 05:46:16 -04:00
} ;
DataFlash . WriteBlock ( & pkt , sizeof ( pkt ) ) ;
2012-04-30 04:17:14 -03:00
}
2013-04-19 04:53:07 -03:00
// Write an attitude packet
2015-05-12 02:03:23 -03:00
void Rover : : Log_Write_Attitude ( )
2012-04-30 04:17:14 -03:00
{
2017-01-31 06:12:26 -04:00
const Vector3f targets ( 0.0f , 0.0f , 0.0f ) ; // Rover does not have attitude targets, use place-holder for commonality with Dataflash Log_Write_Attitude message
2015-01-16 16:29:09 -04:00
DataFlash . Log_Write_Attitude ( ahrs , targets ) ;
2014-03-02 18:03:48 -04:00
# if AP_AHRS_NAVEKF_AVAILABLE
2016-12-20 09:28:58 -04:00
# if defined(OPTFLOW) and (OPTFLOW == ENABLED)
DataFlash . Log_Write_EKF ( ahrs , optflow . enabled ( ) ) ;
# else
DataFlash . Log_Write_EKF ( ahrs , false ) ;
# endif
2014-03-02 18:03:48 -04:00
DataFlash . Log_Write_AHRS2 ( ahrs ) ;
# endif
2015-05-15 01:24:55 -03:00
DataFlash . Log_Write_POS ( ahrs ) ;
2015-06-18 04:34:58 -03:00
2016-05-29 20:44:23 -03:00
DataFlash . Log_Write_PID ( LOG_PIDS_MSG , steerController . get_pid_info ( ) ) ;
DataFlash . Log_Write_PID ( LOG_PIDA_MSG , g . pidSpeedThrottle . get_pid_info ( ) ) ;
2012-04-30 04:17:14 -03:00
}
2013-04-19 04:53:07 -03:00
struct PACKED log_Sonar {
2013-04-18 21:23:57 -03:00
LOG_PACKET_HEADER ;
2015-04-29 08:11:24 -03:00
uint64_t time_us ;
2013-06-16 20:50:53 -03:00
float lateral_accel ;
2013-04-18 21:23:57 -03:00
uint16_t sonar1_distance ;
uint16_t sonar2_distance ;
uint16_t detected_count ;
int8_t turn_angle ;
uint16_t turn_time ;
uint16_t ground_speed ;
int8_t throttle ;
} ;
// Write a sonar packet
2015-05-12 02:03:23 -03:00
void Rover : : Log_Write_Sonar ( )
2013-04-18 21:23:57 -03:00
{
uint16_t turn_time = 0 ;
2015-05-04 23:34:02 -03:00
if ( ! is_zero ( obstacle . turn_angle ) ) {
2015-11-19 23:04:16 -04:00
turn_time = AP_HAL : : millis ( ) - obstacle . detected_time_ms ;
2013-04-18 21:23:57 -03:00
}
struct log_Sonar pkt = {
LOG_PACKET_HEADER_INIT ( LOG_SONAR_MSG ) ,
2015-11-19 23:04:16 -04:00
time_us : AP_HAL : : micros64 ( ) ,
2013-06-16 20:50:53 -03:00
lateral_accel : lateral_acceleration ,
2017-03-30 11:07:24 -03:00
sonar1_distance : sonar . distance_cm ( 0 ) ,
sonar2_distance : sonar . distance_cm ( 1 ) ,
2013-04-18 21:23:57 -03:00
detected_count : obstacle . detected_count ,
2017-02-20 09:26:00 -04:00
turn_angle : static_cast < int8_t > ( obstacle . turn_angle ) ,
2013-04-18 21:23:57 -03:00
turn_time : turn_time ,
2017-02-20 09:26:00 -04:00
ground_speed : static_cast < uint16_t > ( fabsf ( ground_speed * 100 ) ) ,
throttle : static_cast < int8_t > ( 100 * SRV_Channels : : get_output_norm ( SRV_Channel : : k_throttle ) )
2013-04-18 21:23:57 -03:00
} ;
DataFlash . WriteBlock ( & pkt , sizeof ( pkt ) ) ;
}
2015-05-12 02:03:23 -03:00
void Rover : : Log_Write_Current ( )
2012-04-30 04:17:14 -03:00
{
2016-06-02 17:59:41 -03:00
DataFlash . Log_Write_Current ( battery ) ;
2014-02-13 07:10:11 -04:00
// also write power status
DataFlash . Log_Write_Power ( ) ;
2012-04-30 04:17:14 -03:00
}
2015-10-30 02:56:41 -03:00
struct PACKED log_Arm_Disarm {
LOG_PACKET_HEADER ;
uint64_t time_us ;
uint8_t arm_state ;
uint16_t arm_checks ;
} ;
void Rover : : Log_Arm_Disarm ( ) {
struct log_Arm_Disarm pkt = {
LOG_PACKET_HEADER_INIT ( LOG_ARM_DISARM_MSG ) ,
2015-11-19 23:04:16 -04:00
time_us : AP_HAL : : micros64 ( ) ,
2015-10-30 02:56:41 -03:00
arm_state : arming . is_armed ( ) ,
arm_checks : arming . get_enabled_checks ( )
} ;
DataFlash . WriteBlock ( & pkt , sizeof ( pkt ) ) ;
}
2015-05-12 02:03:23 -03:00
void Rover : : Log_Write_RC ( void )
2013-12-29 19:24:01 -04:00
{
DataFlash . Log_Write_RCIN ( ) ;
DataFlash . Log_Write_RCOUT ( ) ;
2015-09-04 12:51:24 -03:00
if ( rssi . enabled ( ) ) {
DataFlash . Log_Write_RSSI ( rssi ) ;
}
2013-12-29 19:24:01 -04:00
}
2016-11-21 12:08:24 -04:00
struct PACKED log_Error {
LOG_PACKET_HEADER ;
uint64_t time_us ;
uint8_t sub_system ;
uint8_t error_code ;
} ;
// Write an error packet
void Rover : : Log_Write_Error ( uint8_t sub_system , uint8_t error_code )
{
struct log_Error pkt = {
LOG_PACKET_HEADER_INIT ( LOG_ERROR_MSG ) ,
time_us : AP_HAL : : micros64 ( ) ,
sub_system : sub_system ,
error_code : error_code ,
} ;
DataFlash . WriteBlock ( & pkt , sizeof ( pkt ) ) ;
}
2015-05-12 02:03:23 -03:00
void Rover : : Log_Write_Baro ( void )
2014-02-23 18:25:50 -04:00
{
DataFlash . Log_Write_Baro ( barometer ) ;
}
2015-07-06 01:17:05 -03:00
// log ahrs home and EKF origin to dataflash
void Rover : : Log_Write_Home_And_Origin ( )
{
# if AP_AHRS_NAVEKF_AVAILABLE
// log ekf origin if set
Location ekf_orig ;
2016-03-03 02:57:16 -04:00
if ( ahrs . get_origin ( ekf_orig ) ) {
2015-07-06 01:17:05 -03:00
DataFlash . Log_Write_Origin ( LogOriginType : : ekf_origin , ekf_orig ) ;
}
# endif
// log ahrs home if set
2015-10-30 02:41:06 -03:00
if ( home_is_set ! = HOME_UNSET ) {
2015-07-06 01:17:05 -03:00
DataFlash . Log_Write_Origin ( LogOriginType : : ahrs_home , ahrs . get_home ( ) ) ;
}
}
2016-11-24 06:29:50 -04:00
// guided mode logging
struct PACKED log_GuidedTarget {
LOG_PACKET_HEADER ;
uint64_t time_us ;
uint8_t type ;
float pos_target_x ;
float pos_target_y ;
float pos_target_z ;
float vel_target_x ;
float vel_target_y ;
float vel_target_z ;
} ;
// Write a Guided mode target
void Rover : : Log_Write_GuidedTarget ( uint8_t target_type , const Vector3f & pos_target , const Vector3f & vel_target )
{
struct log_GuidedTarget pkt = {
LOG_PACKET_HEADER_INIT ( LOG_GUIDEDTARGET_MSG ) ,
time_us : AP_HAL : : micros64 ( ) ,
type : target_type ,
pos_target_x : pos_target . x ,
pos_target_y : pos_target . y ,
pos_target_z : pos_target . z ,
vel_target_x : vel_target . x ,
vel_target_y : vel_target . y ,
vel_target_z : vel_target . z
} ;
DataFlash . WriteBlock ( & pkt , sizeof ( pkt ) ) ;
}
2015-10-25 14:03:46 -03:00
const LogStructure Rover : : log_structure [ ] = {
2013-04-19 04:53:07 -03:00
LOG_COMMON_STRUCTURES ,
2016-12-20 09:28:58 -04:00
{ LOG_PERFORMANCE_MSG , sizeof ( log_Performance ) ,
2017-03-26 18:12:11 -03:00
" PM " , " QIHIhhhBHI " , " TimeUS,LTime,MLC,gDt,GDx,GDy,GDz,I2CErr,INSErr,Mem " } ,
2016-12-20 09:28:58 -04:00
{ LOG_STARTUP_MSG , sizeof ( log_Startup ) ,
2015-04-29 08:11:24 -03:00
" STRT " , " QBH " , " TimeUS,SType,CTot " } ,
2016-12-20 09:28:58 -04:00
{ LOG_CTUN_MSG , sizeof ( log_Control_Tuning ) ,
2015-04-29 08:11:24 -03:00
" CTUN " , " Qhcchf " , " TimeUS,Steer,Roll,Pitch,ThrOut,AccY " } ,
2016-12-20 09:28:58 -04:00
{ LOG_NTUN_MSG , sizeof ( log_Nav_Tuning ) ,
2016-01-05 20:18:29 -04:00
" NTUN " , " QHfHHbf " , " TimeUS,Yaw,WpDist,TargBrg,NavBrg,Thr,XT " } ,
2016-12-20 09:28:58 -04:00
{ LOG_SONAR_MSG , sizeof ( log_Sonar ) ,
2015-04-29 08:11:24 -03:00
" SONR " , " QfHHHbHCb " , " TimeUS,LatAcc,S1Dist,S2Dist,DCnt,TAng,TTim,Spd,Thr " } ,
2015-10-30 02:56:41 -03:00
{ LOG_ARM_DISARM_MSG , sizeof ( log_Arm_Disarm ) ,
" ARM " , " QBH " , " TimeUS,ArmState,ArmChecks " } ,
2016-12-20 09:28:58 -04:00
{ LOG_STEERING_MSG , sizeof ( log_Steering ) ,
2015-04-29 08:11:24 -03:00
" STER " , " Qff " , " TimeUS,Demanded,Achieved " } ,
2016-11-24 06:29:50 -04:00
{ LOG_GUIDEDTARGET_MSG , sizeof ( log_GuidedTarget ) ,
" GUID " , " QBffffff " , " TimeUS,Type,pX,pY,pZ,vX,vY,vZ " } ,
2016-11-21 12:08:24 -04:00
{ LOG_ERROR_MSG , sizeof ( log_Error ) ,
" ERR " , " QBB " , " TimeUS,Subsys,ECode " } ,
2013-04-19 04:53:07 -03:00
} ;
2015-05-13 00:16:45 -03:00
void Rover : : log_init ( void )
{
2016-12-20 09:28:58 -04:00
DataFlash . Init ( log_structure , ARRAY_SIZE ( log_structure ) ) ;
2015-05-13 00:16:45 -03:00
if ( ! DataFlash . CardInserted ( ) ) {
2015-10-24 19:36:35 -03:00
gcs_send_text ( MAV_SEVERITY_WARNING , " No dataflash card inserted " ) ;
2015-08-08 03:13:54 -03:00
} else if ( DataFlash . NeedPrep ( ) ) {
2015-11-03 01:19:03 -04:00
gcs_send_text ( MAV_SEVERITY_INFO , " Preparing log system " ) ;
2015-08-08 03:13:54 -03:00
DataFlash . Prep ( ) ;
2015-11-03 01:19:03 -04:00
gcs_send_text ( MAV_SEVERITY_INFO , " Prepared log system " ) ;
2016-12-20 09:28:58 -04:00
for ( uint8_t i = 0 ; i < num_gcs ; i + + ) {
2016-06-16 22:34:37 -03:00
gcs_chan [ i ] . reset_cli_timeout ( ) ;
2015-08-08 03:13:54 -03:00
}
2015-05-13 00:16:45 -03:00
}
2015-08-08 03:13:54 -03:00
2016-12-20 09:28:58 -04:00
if ( g . log_bitmask ! = 0 ) {
start_logging ( ) ;
}
2015-05-13 00:16:45 -03:00
}
2012-04-30 04:17:14 -03:00
2015-03-13 08:40:06 -03:00
# if CLI_ENABLED == ENABLED
2012-04-30 04:17:14 -03:00
// Read the DataFlash log memory : Packet Parser
2015-10-20 07:36:48 -03:00
void Rover : : Log_Read ( uint16_t list_entry , uint16_t start_page , uint16_t end_page )
2012-04-30 04:17:14 -03:00
{
2015-10-25 17:10:41 -03:00
cliSerial - > printf ( " \n " FIRMWARE_STRING
2015-10-24 18:45:41 -03:00
" \n Free RAM: %u \n " ,
2017-03-30 11:07:24 -03:00
static_cast < uint32_t > ( hal . util - > available_memory ( ) ) ) ;
2013-04-18 21:23:57 -03:00
2017-01-21 00:33:27 -04:00
cliSerial - > printf ( " %s \n " , HAL_BOARD_NAME ) ;
2013-04-18 21:23:57 -03:00
2016-12-20 09:28:58 -04:00
DataFlash . LogReadProcess ( list_entry , start_page , end_page ,
2015-05-24 20:24:11 -03:00
FUNCTOR_BIND_MEMBER ( & Rover : : print_mode , void , AP_HAL : : BetterStream * , uint8_t ) ,
2013-04-20 02:18:42 -03:00
cliSerial ) ;
2012-04-30 04:17:14 -03:00
}
2016-12-20 09:28:58 -04:00
# endif // CLI_ENABLED
2012-04-30 04:17:14 -03:00
2015-08-06 09:50:52 -03:00
void Rover : : Log_Write_Vehicle_Startup_Messages ( )
{
// only 200(?) bytes are guaranteed by DataFlash
Log_Write_Startup ( TYPE_GROUNDSTART_MSG ) ;
2015-09-17 07:14:35 -03:00
DataFlash . Log_Write_Mode ( control_mode ) ;
2017-03-13 19:43:16 -03:00
Log_Write_Home_And_Origin ( ) ;
2016-08-01 09:39:54 -03:00
gps . Write_DataFlash_Log_Startup_messages ( ) ;
2015-08-06 09:50:52 -03:00
}
2013-04-19 10:19:37 -03:00
// start a new log
2016-12-20 09:28:58 -04:00
void Rover : : start_logging ( )
2013-04-19 10:19:37 -03:00
{
2014-01-13 23:40:10 -04:00
in_mavlink_delay = true ;
2017-06-09 02:42:11 -03:00
DataFlash . StartUnstartedLogging ( ) ;
2014-01-13 23:40:10 -04:00
in_mavlink_delay = false ;
2013-04-19 10:19:37 -03:00
}
2016-12-20 09:28:58 -04:00
# else // LOGGING_ENABLED
2012-04-30 04:17:14 -03:00
// dummy functions
2015-05-12 02:03:23 -03:00
void Rover : : Log_Write_Startup ( uint8_t type ) { }
void Rover : : Log_Write_Current ( ) { }
void Rover : : Log_Write_Nav_Tuning ( ) { }
void Rover : : Log_Write_Performance ( ) { }
int8_t Rover : : process_logs ( uint8_t argc , const Menu : : arg * argv ) { return 0 ; }
void Rover : : Log_Write_Control_Tuning ( ) { }
void Rover : : Log_Write_Sonar ( ) { }
void Rover : : Log_Write_Attitude ( ) { }
void Rover : : start_logging ( ) { }
void Rover : : Log_Write_RC ( void ) { }
2017-01-02 00:57:00 -04:00
void Rover : : Log_Write_GuidedTarget ( uint8_t target_type , const Vector3f & pos_target , const Vector3f & vel_target ) { }
void Rover : : Log_Write_Home_And_Origin ( ) { }
void Rover : : Log_Write_Baro ( void ) { }
void Rover : : Log_Arm_Disarm ( ) { }
void Rover : : Log_Write_Error ( uint8_t sub_system , uint8_t error_code ) { }
void Rover : : Log_Write_Steering ( ) { }
2012-04-30 04:17:14 -03:00
2016-12-20 09:28:58 -04:00
# endif // LOGGING_ENABLED