ardupilot/APMrover2/Log.cpp

482 lines
13 KiB
C++
Raw Normal View History

// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
2013-01-14 23:03:17 -04:00
2015-05-13 00:16:45 -03:00
#include "Rover.h"
#include "version.h"
2015-05-13 00:16:45 -03:00
#if LOGGING_ENABLED == ENABLED
#if CLI_ENABLED == ENABLED
// 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
static const struct Menu::command log_menu_commands[] = {
2015-05-12 04:00:25 -03:00
{"dump", MENU_FUNC(dump_log)},
{"erase", MENU_FUNC(erase_logs)},
{"enable", MENU_FUNC(select_logs)},
{"disable", MENU_FUNC(select_logs)}
};
// A Macro to create the Menu
MENU2(log_menu, "Log", log_menu_commands, FUNCTOR_BIND(&rover, &Rover::print_log_menu, bool));
2015-05-12 04:00:25 -03:00
bool Rover::print_log_menu(void)
{
cliSerial->printf("logs enabled: ");
if (0 == g.log_bitmask) {
cliSerial->printf("none");
}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);
2013-04-19 10:54:26 -03:00
PLOG(COMPASS);
PLOG(CAMERA);
PLOG(STEERING);
#undef PLOG
}
cliSerial->println();
2013-04-17 08:34:20 -03:00
DataFlash.ListAvailableLogs(cliSerial);
return(true);
}
2015-05-12 04:00:25 -03:00
int8_t Rover::dump_log(uint8_t argc, const Menu::arg *argv)
{
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) {
cliSerial->printf("dumping all\n");
2013-04-19 18:29:57 -03:00
Log_Read(0, 1, 0);
return(-1);
} else if ((argc != 2) || ((uint16_t)dump_log_num > DataFlash.get_num_logs())) {
cliSerial->printf("bad log number\n");
2013-04-19 18:29:57 -03:00
return(-1);
}
2015-05-12 04:00:25 -03:00
DataFlash.get_log_boundaries(dump_log_num, dump_log_start, dump_log_end);
Log_Read((uint16_t)dump_log_num, dump_log_start, dump_log_end);
return 0;
}
2015-05-12 04:00:25 -03:00
int8_t Rover::erase_logs(uint8_t argc, const Menu::arg *argv)
{
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)
{
uint16_t bits;
if (argc != 2) {
cliSerial->printf("missing log type\n");
return(-1);
}
bits = 0;
// 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);
2013-04-19 10:54:26 -03:00
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);
}
2015-05-12 04:00:25 -03:00
int8_t Rover::process_logs(uint8_t argc, const Menu::arg *argv)
{
log_menu.run();
return 0;
}
#endif // CLI_ENABLED == ENABLED
void Rover::do_erase_logs(void)
{
cliSerial->printf("\nErasing log...\n");
DataFlash.EraseAll();
cliSerial->printf("\nLog erased.\n");
}
2013-04-19 04:53:07 -03:00
struct PACKED log_Performance {
LOG_PACKET_HEADER;
uint64_t time_us;
uint32_t loop_time;
uint16_t main_loop_count;
2013-11-01 20:40:29 -03:00
uint32_t g_dt_max;
int16_t gyro_drift_x;
int16_t gyro_drift_y;
int16_t gyro_drift_z;
uint8_t i2c_lockup_count;
2013-11-18 00:08:53 -04:00
uint16_t ins_error_count;
};
// Write a performance monitoring packet. Total length : 19 bytes
void Rover::Log_Write_Performance()
{
struct log_Performance pkt = {
LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG),
time_us : AP_HAL::micros64(),
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),
i2c_lockup_count: 0,
2013-11-18 00:08:53 -04:00
ins_error_count : ins.error_count()
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
struct PACKED log_Steering {
LOG_PACKET_HEADER;
uint64_t time_us;
float demanded_accel;
float achieved_accel;
};
// Write a steering packet
void Rover::Log_Write_Steering()
{
struct log_Steering pkt = {
LOG_PACKET_HEADER_INIT(LOG_STEERING_MSG),
time_us : AP_HAL::micros64(),
demanded_accel : lateral_acceleration,
achieved_accel : ahrs.groundspeed() * ins.get_gyro().z,
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
2013-04-19 04:53:07 -03:00
struct PACKED log_Startup {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t startup_type;
uint16_t command_total;
};
void Rover::Log_Write_Startup(uint8_t type)
{
struct log_Startup pkt = {
LOG_PACKET_HEADER_INIT(LOG_STARTUP_MSG),
time_us : AP_HAL::micros64(),
startup_type : type,
command_total : mission.num_commands()
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
2013-04-19 04:53:07 -03:00
struct PACKED log_Control_Tuning {
LOG_PACKET_HEADER;
uint64_t time_us;
int16_t steer_out;
int16_t roll;
int16_t pitch;
int16_t throttle_out;
2013-04-19 04:53:07 -03:00
float accel_y;
};
// Write a control tuning packet. Total length : 22 bytes
void Rover::Log_Write_Control_Tuning()
{
Vector3f accel = ins.get_accel();
struct log_Control_Tuning pkt = {
2013-04-19 04:53:07 -03:00
LOG_PACKET_HEADER_INIT(LOG_CTUN_MSG),
time_us : AP_HAL::micros64(),
APMRover2: Fix up after refactoring RC_Channel class Further to refactor of RC_Channel class which included adding get_xx set_xx methods, change reads and writes to the public members to calls to get and set functionsss old public member(int16_t) get function -> int16_t set function (int16_t) (expression where c is an object of type RC_Channel) c.radio_in c.get_radio_in() c.set_radio_in(v) c.control_in c.get_control_in() c.set_control_in(v) c.servo_out c.get_servo_out() c.set_servo_out(v) c.pwm_out c.get_pwm_out() // use existing c.radio_out c.get_radio_out() c.set_radio_out(v) c.radio_max c.get_radio_max() c.set_radio_max(v) c.radio_min c.get_radio_min() c.set_radio_min(v) c.radio_trim c.get_radio_trim() c.set_radio_trim(v); c.min_max_configured() // return true if min and max are configured Because data members of RC_Channels are now private and so cannot be written directly some overloads are provided in the Plane classes to provide the old functionality new overload Plane::stick_mix_channel(RC_Channel *channel) which forwards to the previously existing void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const which forwards to (uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const; Rename functions RC_Channel_aux::set_radio_trim(Aux_servo_function_t function) to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function) RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value) to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value) Rationale: RC_Channel is a complicated class, which combines several functionalities dealing with stick inputs in pwm and logical units, logical and actual actuator outputs, unit conversion etc, etc The intent of this PR is to clarify existing use of the class. At the basic level it should now be possible to grep all places where private variable is set by searching for the set_xx function. (The wider purpose is to provide a more generic and logically simpler method of output mixing. This is a small step)
2016-05-08 05:49:39 -03:00
steer_out : (int16_t)channel_steer->get_servo_out(),
roll : (int16_t)ahrs.roll_sensor,
pitch : (int16_t)ahrs.pitch_sensor,
APMRover2: Fix up after refactoring RC_Channel class Further to refactor of RC_Channel class which included adding get_xx set_xx methods, change reads and writes to the public members to calls to get and set functionsss old public member(int16_t) get function -> int16_t set function (int16_t) (expression where c is an object of type RC_Channel) c.radio_in c.get_radio_in() c.set_radio_in(v) c.control_in c.get_control_in() c.set_control_in(v) c.servo_out c.get_servo_out() c.set_servo_out(v) c.pwm_out c.get_pwm_out() // use existing c.radio_out c.get_radio_out() c.set_radio_out(v) c.radio_max c.get_radio_max() c.set_radio_max(v) c.radio_min c.get_radio_min() c.set_radio_min(v) c.radio_trim c.get_radio_trim() c.set_radio_trim(v); c.min_max_configured() // return true if min and max are configured Because data members of RC_Channels are now private and so cannot be written directly some overloads are provided in the Plane classes to provide the old functionality new overload Plane::stick_mix_channel(RC_Channel *channel) which forwards to the previously existing void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const which forwards to (uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const; Rename functions RC_Channel_aux::set_radio_trim(Aux_servo_function_t function) to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function) RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value) to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value) Rationale: RC_Channel is a complicated class, which combines several functionalities dealing with stick inputs in pwm and logical units, logical and actual actuator outputs, unit conversion etc, etc The intent of this PR is to clarify existing use of the class. At the basic level it should now be possible to grep all places where private variable is set by searching for the set_xx function. (The wider purpose is to provide a more generic and logically simpler method of output mixing. This is a small step)
2016-05-08 05:49:39 -03:00
throttle_out : (int16_t)channel_throttle->get_servo_out(),
2013-04-19 04:53:07 -03:00
accel_y : accel.y
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
2013-04-19 04:53:07 -03:00
struct PACKED log_Nav_Tuning {
LOG_PACKET_HEADER;
uint64_t time_us;
uint16_t yaw;
float wp_distance;
uint16_t target_bearing_cd;
uint16_t nav_bearing_cd;
int8_t throttle;
2016-01-05 20:18:29 -04:00
float xtrack_error;
};
2016-01-05 20:18:29 -04:00
// Write a navigation tuning packet
void Rover::Log_Write_Nav_Tuning()
{
struct log_Nav_Tuning pkt = {
2013-04-19 04:53:07 -03:00
LOG_PACKET_HEADER_INIT(LOG_NTUN_MSG),
time_us : AP_HAL::micros64(),
yaw : (uint16_t)ahrs.yaw_sensor,
wp_distance : wp_distance,
target_bearing_cd : (uint16_t)nav_controller->target_bearing_cd(),
nav_bearing_cd : (uint16_t)nav_controller->nav_bearing_cd(),
2016-01-05 20:18:29 -04:00
throttle : (int8_t)(100 * channel_throttle->norm_output()),
xtrack_error : nav_controller->crosstrack_error()
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
2013-04-19 04:53:07 -03:00
// Write an attitude packet
void Rover::Log_Write_Attitude()
{
Vector3f targets(0,0,0); // Rover does not have attitude targets, use place-holder for commonality with Dataflash Log_Write_Attitude message
DataFlash.Log_Write_Attitude(ahrs, targets);
2014-03-02 18:03:48 -04:00
#if AP_AHRS_NAVEKF_AVAILABLE
#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);
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());
}
2013-04-19 04:53:07 -03:00
struct PACKED log_Sonar {
LOG_PACKET_HEADER;
uint64_t time_us;
float lateral_accel;
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
void Rover::Log_Write_Sonar()
{
uint16_t turn_time = 0;
2015-05-04 23:34:02 -03:00
if (!is_zero(obstacle.turn_angle)) {
turn_time = AP_HAL::millis() - obstacle.detected_time_ms;
}
struct log_Sonar pkt = {
LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG),
time_us : AP_HAL::micros64(),
lateral_accel : lateral_acceleration,
sonar1_distance : (uint16_t)sonar.distance_cm(0),
sonar2_distance : (uint16_t)sonar.distance_cm(1),
detected_count : obstacle.detected_count,
turn_angle : (int8_t)obstacle.turn_angle,
turn_time : turn_time,
ground_speed : (uint16_t)(ground_speed*100),
throttle : (int8_t)(100 * channel_throttle->norm_output())
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
void Rover::Log_Write_Current()
{
DataFlash.Log_Write_Current(battery);
2014-02-13 07:10:11 -04:00
// also write power status
DataFlash.Log_Write_Power();
}
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),
time_us : AP_HAL::micros64(),
arm_state : arming.is_armed(),
arm_checks : arming.get_enabled_checks()
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
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
}
void Rover::Log_Write_Baro(void)
{
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;
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
if (home_is_set != HOME_UNSET) {
2015-07-06 01:17:05 -03:00
DataFlash.Log_Write_Origin(LogOriginType::ahrs_home, ahrs.get_home());
}
}
const LogStructure Rover::log_structure[] = {
2013-04-19 04:53:07 -03:00
LOG_COMMON_STRUCTURES,
{ LOG_PERFORMANCE_MSG, sizeof(log_Performance),
"PM", "QIHIhhhBH", "TimeUS,LTime,MLC,gDt,GDx,GDy,GDz,I2CErr,INSErr" },
2013-04-19 04:53:07 -03:00
{ LOG_STARTUP_MSG, sizeof(log_Startup),
"STRT", "QBH", "TimeUS,SType,CTot" },
2013-04-19 04:53:07 -03:00
{ LOG_CTUN_MSG, sizeof(log_Control_Tuning),
"CTUN", "Qhcchf", "TimeUS,Steer,Roll,Pitch,ThrOut,AccY" },
2013-04-19 04:53:07 -03: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" },
2013-04-19 04:53:07 -03:00
{ LOG_SONAR_MSG, sizeof(log_Sonar),
"SONR", "QfHHHbHCb", "TimeUS,LatAcc,S1Dist,S2Dist,DCnt,TAng,TTim,Spd,Thr" },
{ LOG_ARM_DISARM_MSG, sizeof(log_Arm_Disarm),
"ARM", "QBH", "TimeUS,ArmState,ArmChecks" },
{ LOG_STEERING_MSG, sizeof(log_Steering),
"STER", "Qff", "TimeUS,Demanded,Achieved" },
2013-04-19 04:53:07 -03:00
};
2015-05-13 00:16:45 -03:00
void Rover::log_init(void)
{
2015-07-06 12:30:40 -03:00
DataFlash.Init(log_structure, ARRAY_SIZE(log_structure));
2015-05-13 00:16:45 -03:00
if (!DataFlash.CardInserted()) {
gcs_send_text(MAV_SEVERITY_WARNING, "No dataflash card inserted");
2015-05-13 00:16:45 -03:00
g.log_bitmask.set(0);
} else if (DataFlash.NeedPrep()) {
gcs_send_text(MAV_SEVERITY_INFO, "Preparing log system");
DataFlash.Prep();
gcs_send_text(MAV_SEVERITY_INFO, "Prepared log system");
for (uint8_t i=0; i<num_gcs; i++) {
gcs[i].reset_cli_timeout();
}
2015-05-13 00:16:45 -03:00
}
2015-05-13 00:16:45 -03:00
if (g.log_bitmask != 0) {
start_logging();
}
arming.set_logging_available(DataFlash.CardInserted());
2015-05-13 00:16:45 -03:00
}
#if CLI_ENABLED == ENABLED
// Read the DataFlash log memory : Packet Parser
void Rover::Log_Read(uint16_t list_entry, uint16_t start_page, uint16_t end_page)
{
cliSerial->printf("\n" FIRMWARE_STRING
"\nFree RAM: %u\n",
2013-12-28 01:02:45 -04:00
(unsigned)hal.util->available_memory());
cliSerial->println(HAL_BOARD_NAME);
DataFlash.LogReadProcess(list_entry, start_page, end_page,
FUNCTOR_BIND_MEMBER(&Rover::print_mode, void, AP_HAL::BetterStream *, uint8_t),
2013-04-20 02:18:42 -03:00
cliSerial);
}
#endif // CLI_ENABLED
void Rover::Log_Write_Vehicle_Startup_Messages()
{
// only 200(?) bytes are guaranteed by DataFlash
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
DataFlash.Log_Write_Mode(control_mode);
}
2013-04-19 10:19:37 -03:00
// start a new log
void Rover::start_logging()
2013-04-19 10:19:37 -03:00
{
in_mavlink_delay = true;
DataFlash.set_mission(&mission);
DataFlash.setVehicle_Startup_Log_Writer(
FUNCTOR_BIND(&rover, &Rover::Log_Write_Vehicle_Startup_Messages, void)
);
DataFlash.StartNewLog();
in_mavlink_delay = false;
2013-04-19 10:19:37 -03:00
}
#else // LOGGING_ENABLED
// dummy functions
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) {}
#endif // LOGGING_ENABLED