2015-05-13 00:16:45 -03:00
# include "Rover.h"
2017-08-08 02:58:52 -03:00
# include <AP_RangeFinder/RangeFinder_Backend.h>
2012-04-30 04:17:14 -03:00
# if LOGGING_ENABLED == ENABLED
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 ( ) ,
2017-07-18 23:19:08 -03:00
demanded_accel : control_mode - > 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-07-06 00:06:20 -03:00
steer_out : ( int16_t ) g2 . motors . get_steering ( ) ,
2013-01-26 05:46:16 -04:00
roll : ( int16_t ) ahrs . roll_sensor ,
pitch : ( int16_t ) ahrs . pitch_sensor ,
2017-07-06 00:06:20 -03:00
throttle_out : ( int16_t ) g2 . motors . get_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 ) ,
2017-08-01 20:14:49 -03:00
wp_distance : control_mode - > get_distance_to_destination ( ) ,
2017-08-16 05:35:36 -03:00
target_bearing_cd : static_cast < uint16_t > ( abs ( nav_controller - > target_bearing_cd ( ) ) ) ,
nav_bearing_cd : static_cast < uint16_t > ( abs ( nav_controller - > nav_bearing_cd ( ) ) ) ,
2017-06-20 03:18:38 -03:00
throttle : int8_t ( SRV_Channels : : get_output_scaled ( 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
2017-08-22 00:15:06 -03:00
DataFlash . Log_Write_EKF ( ahrs ) ;
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
2017-08-08 02:37:21 -03:00
// log steering rate controller
DataFlash . Log_Write_PID ( LOG_PIDS_MSG , g2 . attitude_control . get_steering_rate_pid ( ) . get_pid_info ( ) ) ;
2017-08-08 21:24:30 -03:00
DataFlash . Log_Write_PID ( LOG_PIDA_MSG , g2 . attitude_control . get_throttle_speed_pid ( ) . get_pid_info ( ) ) ;
2012-04-30 04:17:14 -03:00
}
2017-07-13 08:36:44 -03:00
struct PACKED log_Rangefinder {
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 ;
2017-07-13 08:36:44 -03:00
uint16_t rangefinder1_distance ;
uint16_t rangefinder2_distance ;
2013-04-18 21:23:57 -03:00
uint16_t detected_count ;
int8_t turn_angle ;
uint16_t turn_time ;
uint16_t ground_speed ;
int8_t throttle ;
} ;
2017-07-13 08:36:44 -03:00
// Write a rangefinder packet
void Rover : : Log_Write_Rangefinder ( )
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
}
2017-08-08 02:58:52 -03:00
AP_RangeFinder_Backend * s0 = rangefinder . get_backend ( 0 ) ;
AP_RangeFinder_Backend * s1 = rangefinder . get_backend ( 1 ) ;
2017-07-13 08:36:44 -03:00
struct log_Rangefinder pkt = {
LOG_PACKET_HEADER_INIT ( LOG_RANGEFINDER_MSG ) ,
time_us : AP_HAL : : micros64 ( ) ,
2017-07-18 07:34:59 -03:00
lateral_accel : control_mode - > lateral_acceleration ,
2017-08-08 02:58:52 -03:00
rangefinder1_distance : s0 ? s0 - > distance_cm ( ) : ( uint16_t ) 0 ,
rangefinder2_distance : s1 ? s1 - > distance_cm ( ) : ( uint16_t ) 0 ,
2017-07-13 08:36:44 -03:00
detected_count : obstacle . detected_count ,
turn_angle : static_cast < int8_t > ( obstacle . turn_angle ) ,
turn_time : turn_time ,
2017-07-18 07:34:59 -03:00
ground_speed : static_cast < uint16_t > ( fabsf ( ground_speed * 100.0f ) ) ,
2017-07-13 08:36:44 -03:00
throttle : int8_t ( SRV_Channels : : get_output_scaled ( 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 ) ) ;
}
2017-07-11 23:02:51 -03:00
// wheel encoder packet
struct PACKED log_WheelEncoder {
LOG_PACKET_HEADER ;
uint64_t time_us ;
float distance_0 ;
uint8_t quality_0 ;
float distance_1 ;
uint8_t quality_1 ;
} ;
// log wheel encoder information
void Rover : : Log_Write_WheelEncoder ( )
{
// return immediately if no wheel encoders are enabled
if ( ! g2 . wheel_encoder . enabled ( 0 ) & & ! g2 . wheel_encoder . enabled ( 1 ) ) {
return ;
}
struct log_WheelEncoder pkt = {
LOG_PACKET_HEADER_INIT ( LOG_WHEELENCODER_MSG ) ,
time_us : AP_HAL : : micros64 ( ) ,
distance_0 : g2 . wheel_encoder . get_distance ( 0 ) ,
2017-08-03 06:53:22 -03:00
quality_0 : ( uint8_t ) constrain_float ( g2 . wheel_encoder . get_signal_quality ( 0 ) , 0.0f , 100.0f ) ,
2017-07-11 23:02:51 -03:00
distance_1 : g2 . wheel_encoder . get_distance ( 1 ) ,
2017-08-03 06:53:22 -03:00
quality_1 : ( uint8_t ) constrain_float ( g2 . wheel_encoder . get_signal_quality ( 1 ) , 0.0f , 100.0f )
2017-07-11 23:02:51 -03:00
} ;
DataFlash . WriteBlock ( & pkt , sizeof ( pkt ) ) ;
}
2016-11-24 06:29:50 -04:00
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 " } ,
2017-07-13 08:36:44 -03:00
{ LOG_RANGEFINDER_MSG , sizeof ( log_Rangefinder ) ,
" RGFD " , " QfHHHbHCb " , " TimeUS,LatAcc,R1Dist,R2Dist,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 " } ,
2017-07-11 23:02:51 -03:00
{ LOG_WHEELENCODER_MSG , sizeof ( log_WheelEncoder ) ,
" WENC " , " Qfbfb " , " TimeUS,Dist0,Qual0,Dist1,Qual1 " } ,
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 ) ) ;
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 ) ;
2017-07-24 14:05:59 -03:00
DataFlash . Log_Write_Mode ( control_mode - > mode_number ( ) , control_mode_reason ) ;
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
}
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 ( ) { }
void Rover : : Log_Write_Control_Tuning ( ) { }
2017-07-13 08:36:44 -03:00
void Rover : : Log_Write_Rangefinder ( ) { }
2015-05-12 02:03:23 -03:00
void Rover : : Log_Write_Attitude ( ) { }
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 ( ) { }
2017-07-11 23:02:51 -03:00
void Rover : : Log_Write_WheelEncoder ( ) { }
2012-04-30 04:17:14 -03:00
2016-12-20 09:28:58 -04:00
# endif // LOGGING_ENABLED