2015-05-13 03:09:36 -03:00
|
|
|
#include "Plane.h"
|
2016-05-05 19:10:08 -03:00
|
|
|
#include "version.h"
|
2015-05-13 03:09:36 -03:00
|
|
|
|
2011-09-08 22:29:39 -03:00
|
|
|
#if LOGGING_ENABLED == ENABLED
|
|
|
|
|
2015-03-13 08:33:48 -03:00
|
|
|
#if CLI_ENABLED == ENABLED
|
2012-12-08 00:35:49 -04:00
|
|
|
// Code to Write and Read packets from DataFlash.log memory
|
2011-09-08 22:29:39 -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[] = {
|
2015-05-13 03:09:36 -03:00
|
|
|
{"dump", MENU_FUNC(dump_log)},
|
|
|
|
{"erase", MENU_FUNC(erase_logs)},
|
|
|
|
{"enable", MENU_FUNC(select_logs)},
|
|
|
|
{"disable", MENU_FUNC(select_logs)}
|
2011-09-08 22:29:39 -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(&plane, &Plane::print_log_menu, bool));
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
bool Plane::print_log_menu(void)
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2015-10-25 16:50:51 -03:00
|
|
|
cliSerial->println("logs enabled: ");
|
2012-08-21 23:19:50 -03:00
|
|
|
|
|
|
|
if (0 == g.log_bitmask) {
|
2015-10-25 16:50:51 -03:00
|
|
|
cliSerial->println("none");
|
2012-08-21 23:19:50 -03: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.
|
2015-10-26 10:08:19 -03:00
|
|
|
#define PLOG(_s) if (g.log_bitmask & MASK_LOG_ ## _s) cliSerial->printf(" %s", # _s)
|
2012-08-21 23:19:50 -03:00
|
|
|
PLOG(ATTITUDE_FAST);
|
|
|
|
PLOG(ATTITUDE_MED);
|
|
|
|
PLOG(GPS);
|
|
|
|
PLOG(PM);
|
|
|
|
PLOG(CTUN);
|
|
|
|
PLOG(NTUN);
|
|
|
|
PLOG(MODE);
|
2013-01-26 04:29:29 -04:00
|
|
|
PLOG(IMU);
|
2012-08-21 23:19:50 -03:00
|
|
|
PLOG(CMD);
|
2013-01-26 04:19:42 -04:00
|
|
|
PLOG(CURRENT);
|
2013-04-19 10:54:40 -03:00
|
|
|
PLOG(COMPASS);
|
2013-06-26 05:36:53 -03:00
|
|
|
PLOG(TECS);
|
2013-07-09 23:23:36 -03:00
|
|
|
PLOG(CAMERA);
|
2013-11-25 17:44:17 -04:00
|
|
|
PLOG(RC);
|
2013-10-30 19:23:21 -03:00
|
|
|
PLOG(SONAR);
|
2012-08-21 23:19:50 -03:00
|
|
|
#undef PLOG
|
|
|
|
}
|
|
|
|
|
2012-11-21 01:19:16 -04:00
|
|
|
cliSerial->println();
|
2012-08-21 23:19:50 -03:00
|
|
|
|
2013-04-17 08:34:56 -03:00
|
|
|
DataFlash.ListAvailableLogs(cliSerial);
|
2012-08-21 23:19:50 -03:00
|
|
|
return(true);
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
int8_t Plane::dump_log(uint8_t argc, const Menu::arg *argv)
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2015-05-13 03:09:36 -03:00
|
|
|
int16_t dump_log_num;
|
2013-02-22 19:15:15 -04:00
|
|
|
uint16_t dump_log_start;
|
|
|
|
uint16_t dump_log_end;
|
2012-08-21 23:19:50 -03:00
|
|
|
|
|
|
|
// check that the requested log number can be read
|
2015-05-13 03:09:36 -03:00
|
|
|
dump_log_num = argv[1].i;
|
2012-08-21 23:19:50 -03:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
if (dump_log_num == -2) {
|
2013-02-22 19:15:15 -04:00
|
|
|
DataFlash.DumpPageInfo(cliSerial);
|
2012-08-21 23:19:50 -03:00
|
|
|
return(-1);
|
2015-05-13 03:09:36 -03:00
|
|
|
} else if (dump_log_num <= 0) {
|
2016-11-25 19:11:36 -04:00
|
|
|
cliSerial->println("dumping all");
|
2013-02-28 16:16:32 -04:00
|
|
|
Log_Read(0, 1, 0);
|
2012-08-21 23:19:50 -03:00
|
|
|
return(-1);
|
2015-10-20 07:36:42 -03:00
|
|
|
} else if ((argc != 2) || ((uint16_t)dump_log_num > DataFlash.get_num_logs())) {
|
2016-11-25 19:11:36 -04:00
|
|
|
cliSerial->println("bad log number");
|
2012-08-21 23:19:50 -03:00
|
|
|
return(-1);
|
|
|
|
}
|
|
|
|
|
2015-05-13 03:09:36 -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);
|
2011-09-08 22:29:39 -03:00
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
int8_t Plane::erase_logs(uint8_t argc, const Menu::arg *argv)
|
2011-12-17 19:19:52 -04:00
|
|
|
{
|
2011-12-28 00:53:14 -04:00
|
|
|
in_mavlink_delay = true;
|
|
|
|
do_erase_logs();
|
|
|
|
in_mavlink_delay = false;
|
2011-09-08 22:29:39 -03:00
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
int8_t Plane::select_logs(uint8_t argc, const Menu::arg *argv)
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2014-01-13 21:52:22 -04:00
|
|
|
uint32_t bits;
|
2012-08-21 23:19:50 -03:00
|
|
|
|
|
|
|
if (argc != 2) {
|
2016-11-25 19:11:36 -04:00
|
|
|
cliSerial->println("missing log type");
|
2012-08-21 23:19:50 -03:00
|
|
|
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.
|
|
|
|
//
|
2015-10-25 13:22:16 -03:00
|
|
|
if (!strcasecmp(argv[1].str, "all")) {
|
2014-01-13 21:52:22 -04:00
|
|
|
bits = 0xFFFFFFFFUL;
|
2012-08-21 23:19:50 -03:00
|
|
|
} else {
|
2015-10-25 13:22:16 -03:00
|
|
|
#define TARG(_s) if (!strcasecmp(argv[1].str, # _s)) bits |= MASK_LOG_ ## _s
|
2012-08-21 23:19:50 -03:00
|
|
|
TARG(ATTITUDE_FAST);
|
|
|
|
TARG(ATTITUDE_MED);
|
|
|
|
TARG(GPS);
|
|
|
|
TARG(PM);
|
|
|
|
TARG(CTUN);
|
|
|
|
TARG(NTUN);
|
|
|
|
TARG(MODE);
|
2013-01-26 04:29:29 -04:00
|
|
|
TARG(IMU);
|
2012-08-21 23:19:50 -03:00
|
|
|
TARG(CMD);
|
2013-01-26 04:19:42 -04:00
|
|
|
TARG(CURRENT);
|
2013-04-19 10:54:40 -03:00
|
|
|
TARG(COMPASS);
|
2013-07-09 23:23:36 -03:00
|
|
|
TARG(TECS);
|
|
|
|
TARG(CAMERA);
|
2013-11-25 17:44:17 -04:00
|
|
|
TARG(RC);
|
2013-10-30 19:23:21 -03:00
|
|
|
TARG(SONAR);
|
2012-08-21 23:19:50 -03:00
|
|
|
#undef TARG
|
|
|
|
}
|
|
|
|
|
2015-10-25 13:22:16 -03:00
|
|
|
if (!strcasecmp(argv[0].str, "enable")) {
|
2012-08-21 23:19:50 -03:00
|
|
|
g.log_bitmask.set_and_save(g.log_bitmask | bits);
|
|
|
|
}else{
|
|
|
|
g.log_bitmask.set_and_save(g.log_bitmask & ~bits);
|
|
|
|
}
|
|
|
|
return(0);
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
int8_t Plane::process_logs(uint8_t argc, const Menu::arg *argv)
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2012-08-21 23:19:50 -03:00
|
|
|
log_menu.run();
|
|
|
|
return 0;
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
2015-03-13 08:33:48 -03:00
|
|
|
#endif // CLI_ENABLED == ENABLED
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::do_erase_logs(void)
|
2015-03-13 08:33:48 -03:00
|
|
|
{
|
2015-11-18 15:17:50 -04:00
|
|
|
gcs_send_text(MAV_SEVERITY_INFO, "Erasing logs");
|
2015-03-13 08:33:48 -03:00
|
|
|
DataFlash.EraseAll();
|
2015-11-18 15:17:50 -04:00
|
|
|
gcs_send_text(MAV_SEVERITY_INFO, "Log erase complete");
|
2015-03-13 08:33:48 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
|
2013-12-30 07:24:36 -04:00
|
|
|
// Write an attitude packet
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::Log_Write_Attitude(void)
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2015-01-16 12:01:33 -04:00
|
|
|
Vector3f targets; // Package up the targets into a vector for commonality with Copter usage of Log_Wrote_Attitude
|
|
|
|
targets.x = nav_roll_cd;
|
|
|
|
targets.y = nav_pitch_cd;
|
|
|
|
targets.z = 0; //Plane does not have the concept of navyaw. This is a placeholder.
|
|
|
|
|
|
|
|
DataFlash.Log_Write_Attitude(ahrs, targets);
|
2016-02-20 05:20:27 -04:00
|
|
|
if (quadplane.in_vtol_mode()) {
|
2016-02-15 04:45:56 -04:00
|
|
|
DataFlash.Log_Write_PID(LOG_PIDR_MSG, quadplane.attitude_control->get_rate_roll_pid().get_pid_info());
|
|
|
|
DataFlash.Log_Write_PID(LOG_PIDP_MSG, quadplane.attitude_control->get_rate_pitch_pid().get_pid_info());
|
|
|
|
DataFlash.Log_Write_PID(LOG_PIDY_MSG, quadplane.attitude_control->get_rate_yaw_pid().get_pid_info());
|
2016-02-20 05:20:27 -04:00
|
|
|
DataFlash.Log_Write_PID(LOG_PIDA_MSG, quadplane.pid_accel_z.get_pid_info() );
|
|
|
|
} else {
|
|
|
|
DataFlash.Log_Write_PID(LOG_PIDR_MSG, rollController.get_pid_info());
|
|
|
|
DataFlash.Log_Write_PID(LOG_PIDP_MSG, pitchController.get_pid_info());
|
|
|
|
DataFlash.Log_Write_PID(LOG_PIDY_MSG, yawController.get_pid_info());
|
|
|
|
DataFlash.Log_Write_PID(LOG_PIDS_MSG, steerController.get_pid_info());
|
|
|
|
}
|
2014-01-01 21:16:20 -04:00
|
|
|
|
|
|
|
#if AP_AHRS_NAVEKF_AVAILABLE
|
2014-12-31 02:21:18 -04:00
|
|
|
#if OPTFLOW == ENABLED
|
2016-12-17 21:11:12 -04:00
|
|
|
DataFlash.Log_Write_EKF(ahrs,optflow.enabled());
|
2014-12-31 02:21:18 -04:00
|
|
|
#else
|
2016-12-17 21:11:12 -04:00
|
|
|
DataFlash.Log_Write_EKF(ahrs,false);
|
2014-12-31 02:21:18 -04:00
|
|
|
#endif
|
2014-01-03 20:51:36 -04:00
|
|
|
DataFlash.Log_Write_AHRS2(ahrs);
|
|
|
|
#endif
|
2015-05-04 03:18:55 -03:00
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
2015-11-16 00:10:42 -04:00
|
|
|
sitl.Log_Write_SIMSTATE(&DataFlash);
|
2013-12-30 19:24:38 -04:00
|
|
|
#endif
|
2015-05-15 01:24:46 -03:00
|
|
|
DataFlash.Log_Write_POS(ahrs);
|
2013-12-30 07:24:36 -04:00
|
|
|
}
|
|
|
|
|
2016-05-01 19:51:13 -03:00
|
|
|
// do logging at loop rate
|
|
|
|
void Plane::Log_Write_Fast(void)
|
|
|
|
{
|
|
|
|
if (should_log(MASK_LOG_ATTITUDE_FAST)) {
|
|
|
|
Log_Write_Attitude();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2014-01-01 21:16:20 -04:00
|
|
|
|
2013-04-19 10:37:21 -03:00
|
|
|
struct PACKED log_Performance {
|
2013-01-26 04:07:21 -04:00
|
|
|
LOG_PACKET_HEADER;
|
2015-04-30 00:40:51 -03:00
|
|
|
uint64_t time_us;
|
2016-04-21 01:51:29 -03:00
|
|
|
uint16_t num_long;
|
2013-01-26 04:07:21 -04:00
|
|
|
uint16_t main_loop_count;
|
2013-10-11 23:30:27 -03:00
|
|
|
uint32_t g_dt_max;
|
2016-04-21 01:51:29 -03:00
|
|
|
uint32_t g_dt_min;
|
2016-04-21 03:11:48 -03:00
|
|
|
uint32_t log_dropped;
|
2013-01-26 04:07:21 -04:00
|
|
|
};
|
2013-01-12 07:18:58 -04:00
|
|
|
|
2011-09-08 22:29:39 -03:00
|
|
|
// Write a performance monitoring packet. Total length : 19 bytes
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::Log_Write_Performance()
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2013-01-26 04:07:21 -04:00
|
|
|
struct log_Performance pkt = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG),
|
2015-11-19 23:04:52 -04:00
|
|
|
time_us : AP_HAL::micros64(),
|
2016-04-21 01:51:29 -03:00
|
|
|
num_long : perf.num_long,
|
2016-04-20 22:50:13 -03:00
|
|
|
main_loop_count : perf.mainLoop_count,
|
|
|
|
g_dt_max : perf.G_Dt_max,
|
2016-04-21 03:11:48 -03:00
|
|
|
g_dt_min : perf.G_Dt_min,
|
|
|
|
log_dropped : DataFlash.num_dropped() - perf.last_log_dropped
|
2013-01-26 04:07:21 -04:00
|
|
|
};
|
2016-04-21 03:11:48 -03:00
|
|
|
DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
|
2012-08-21 23:19:50 -03:00
|
|
|
}
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2013-04-19 10:37:21 -03:00
|
|
|
struct PACKED log_Startup {
|
2013-01-26 04:07:21 -04:00
|
|
|
LOG_PACKET_HEADER;
|
2015-04-30 00:40:51 -03:00
|
|
|
uint64_t time_us;
|
2013-01-26 04:07:21 -04:00
|
|
|
uint8_t startup_type;
|
2014-03-10 05:20:08 -03:00
|
|
|
uint16_t command_total;
|
2013-01-26 04:07:21 -04:00
|
|
|
};
|
|
|
|
|
2015-11-09 18:40:34 -04:00
|
|
|
void Plane::Log_Write_Startup(uint8_t type)
|
2013-01-26 04:07:21 -04:00
|
|
|
{
|
|
|
|
struct log_Startup pkt = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_STARTUP_MSG),
|
2015-11-19 23:04:52 -04:00
|
|
|
time_us : AP_HAL::micros64(),
|
2013-01-26 04:07:21 -04:00
|
|
|
startup_type : type,
|
2014-03-03 09:48:49 -04:00
|
|
|
command_total : mission.num_commands()
|
2013-01-26 04:07:21 -04:00
|
|
|
};
|
2016-04-21 03:11:48 -03:00
|
|
|
DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
2013-04-19 10:37:21 -03:00
|
|
|
struct PACKED log_Control_Tuning {
|
2013-01-26 04:07:21 -04:00
|
|
|
LOG_PACKET_HEADER;
|
2015-04-30 00:40:51 -03:00
|
|
|
uint64_t time_us;
|
2013-01-26 04:07:21 -04:00
|
|
|
int16_t nav_roll_cd;
|
|
|
|
int16_t roll;
|
|
|
|
int16_t nav_pitch_cd;
|
|
|
|
int16_t pitch;
|
|
|
|
int16_t throttle_out;
|
|
|
|
int16_t rudder_out;
|
2016-01-30 02:38:31 -04:00
|
|
|
int16_t throttle_dem;
|
2013-01-26 04:07:21 -04:00
|
|
|
};
|
2011-09-08 22:29:39 -03:00
|
|
|
|
|
|
|
// Write a control tuning packet. Total length : 22 bytes
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::Log_Write_Control_Tuning()
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2013-01-26 04:07:21 -04:00
|
|
|
struct log_Control_Tuning pkt = {
|
2013-04-19 10:37:21 -03:00
|
|
|
LOG_PACKET_HEADER_INIT(LOG_CTUN_MSG),
|
2015-11-19 23:04:52 -04:00
|
|
|
time_us : AP_HAL::micros64(),
|
2013-01-26 04:07:21 -04:00
|
|
|
nav_roll_cd : (int16_t)nav_roll_cd,
|
|
|
|
roll : (int16_t)ahrs.roll_sensor,
|
|
|
|
nav_pitch_cd : (int16_t)nav_pitch_cd,
|
|
|
|
pitch : (int16_t)ahrs.pitch_sensor,
|
ArduPlane: 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:33:02 -03:00
|
|
|
throttle_out : (int16_t)channel_throttle->get_servo_out(),
|
|
|
|
rudder_out : (int16_t)channel_rudder->get_servo_out(),
|
2016-01-30 02:38:31 -04:00
|
|
|
throttle_dem : (int16_t)SpdHgt_Controller->get_throttle_demand()
|
2013-01-26 04:07:21 -04:00
|
|
|
};
|
|
|
|
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
|
|
|
}
|
|
|
|
|
2013-04-19 10:37:21 -03:00
|
|
|
struct PACKED log_Nav_Tuning {
|
2013-01-26 04:07:21 -04:00
|
|
|
LOG_PACKET_HEADER;
|
2015-04-30 00:40:51 -03:00
|
|
|
uint64_t time_us;
|
2015-01-01 00:17:45 -04:00
|
|
|
float wp_distance;
|
2014-05-21 07:14:00 -03:00
|
|
|
int16_t target_bearing_cd;
|
|
|
|
int16_t nav_bearing_cd;
|
2013-01-26 04:07:21 -04:00
|
|
|
int16_t altitude_error_cm;
|
2016-01-05 19:33:49 -04:00
|
|
|
float xtrack_error;
|
2016-04-20 19:35:03 -03:00
|
|
|
float xtrack_error_i;
|
2016-05-10 16:51:36 -03:00
|
|
|
float airspeed_error;
|
2013-01-26 04:07:21 -04:00
|
|
|
};
|
|
|
|
|
2016-01-05 19:33:49 -04:00
|
|
|
// Write a navigation tuning packet
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::Log_Write_Nav_Tuning()
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2013-01-26 04:07:21 -04:00
|
|
|
struct log_Nav_Tuning pkt = {
|
2013-04-19 10:37:21 -03:00
|
|
|
LOG_PACKET_HEADER_INIT(LOG_NTUN_MSG),
|
2015-11-19 23:04:52 -04:00
|
|
|
time_us : AP_HAL::micros64(),
|
2015-01-01 00:17:45 -04:00
|
|
|
wp_distance : auto_state.wp_distance,
|
2014-05-21 07:14:00 -03:00
|
|
|
target_bearing_cd : (int16_t)nav_controller->target_bearing_cd(),
|
|
|
|
nav_bearing_cd : (int16_t)nav_controller->nav_bearing_cd(),
|
2013-01-26 04:07:21 -04:00
|
|
|
altitude_error_cm : (int16_t)altitude_error_cm,
|
2016-04-20 19:35:03 -03:00
|
|
|
xtrack_error : nav_controller->crosstrack_error(),
|
2016-05-10 16:51:36 -03:00
|
|
|
xtrack_error_i : nav_controller->crosstrack_error_integrator(),
|
|
|
|
airspeed_error : airspeed_error
|
2013-01-26 04:07:21 -04:00
|
|
|
};
|
|
|
|
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
2015-04-09 22:42:48 -03:00
|
|
|
struct PACKED log_Status {
|
|
|
|
LOG_PACKET_HEADER;
|
2015-04-30 00:40:51 -03:00
|
|
|
uint64_t time_us;
|
2015-04-09 22:42:48 -03:00
|
|
|
uint8_t is_flying;
|
|
|
|
float is_flying_probability;
|
2015-04-29 22:15:08 -03:00
|
|
|
uint8_t armed;
|
|
|
|
uint8_t safety;
|
2015-06-03 16:22:24 -03:00
|
|
|
bool is_crashed;
|
|
|
|
bool is_still;
|
2015-09-25 22:30:26 -03:00
|
|
|
uint8_t stage;
|
2015-11-17 20:11:21 -04:00
|
|
|
bool impact;
|
2015-04-09 22:42:48 -03:00
|
|
|
};
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::Log_Write_Status()
|
2015-04-09 22:42:48 -03:00
|
|
|
{
|
|
|
|
struct log_Status pkt = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_STATUS_MSG)
|
2015-11-19 23:04:52 -04:00
|
|
|
,time_us : AP_HAL::micros64()
|
2015-04-09 22:42:48 -03:00
|
|
|
,is_flying : is_flying()
|
|
|
|
,is_flying_probability : isFlyingProbability
|
2015-04-29 22:15:08 -03:00
|
|
|
,armed : hal.util->get_soft_armed()
|
2016-02-26 01:12:34 -04:00
|
|
|
,safety : static_cast<uint8_t>(hal.util->safety_switch_state())
|
2015-08-20 17:44:32 -03:00
|
|
|
,is_crashed : crash_state.is_crashed
|
2015-06-03 16:22:24 -03:00
|
|
|
,is_still : plane.ins.is_still()
|
2016-02-26 01:12:34 -04:00
|
|
|
,stage : static_cast<uint8_t>(flight_stage)
|
2015-11-17 20:11:21 -04:00
|
|
|
,impact : crash_state.impact_detected
|
2015-06-03 16:22:24 -03:00
|
|
|
};
|
2015-04-09 22:42:48 -03:00
|
|
|
|
|
|
|
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
|
|
|
}
|
|
|
|
|
2013-10-30 19:23:21 -03:00
|
|
|
struct PACKED log_Sonar {
|
|
|
|
LOG_PACKET_HEADER;
|
2015-04-30 00:40:51 -03:00
|
|
|
uint64_t time_us;
|
2016-05-17 22:34:39 -03:00
|
|
|
float distance;
|
2013-10-30 19:23:21 -03:00
|
|
|
float voltage;
|
2014-08-26 08:17:47 -03:00
|
|
|
uint8_t count;
|
2014-08-27 05:25:17 -03:00
|
|
|
float correction;
|
2013-10-30 19:23:21 -03:00
|
|
|
};
|
|
|
|
|
|
|
|
// Write a sonar packet
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::Log_Write_Sonar()
|
2013-10-30 19:23:21 -03:00
|
|
|
{
|
2015-05-27 20:51:29 -03:00
|
|
|
uint16_t distance = 0;
|
|
|
|
if (rangefinder.status() == RangeFinder::RangeFinder_Good) {
|
|
|
|
distance = rangefinder.distance_cm();
|
|
|
|
}
|
|
|
|
|
2013-10-30 19:23:21 -03:00
|
|
|
struct log_Sonar pkt = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG),
|
2015-11-19 23:04:52 -04:00
|
|
|
time_us : AP_HAL::micros64(),
|
2016-05-17 22:34:39 -03:00
|
|
|
distance : (float)distance*0.01f,
|
2014-08-26 08:17:47 -03:00
|
|
|
voltage : rangefinder.voltage_mv()*0.001f,
|
|
|
|
count : rangefinder_state.in_range_count,
|
2014-08-27 05:25:17 -03:00
|
|
|
correction : rangefinder_state.correction
|
2013-10-30 19:23:21 -03:00
|
|
|
};
|
|
|
|
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
2015-09-10 07:26:48 -03:00
|
|
|
|
|
|
|
DataFlash.Log_Write_RFND(rangefinder);
|
2013-10-30 19:23:21 -03:00
|
|
|
}
|
|
|
|
|
2014-11-02 02:26:05 -04:00
|
|
|
struct PACKED log_Optflow {
|
|
|
|
LOG_PACKET_HEADER;
|
2015-04-30 00:40:51 -03:00
|
|
|
uint64_t time_us;
|
2014-11-02 02:26:05 -04:00
|
|
|
uint8_t surface_quality;
|
|
|
|
float flow_x;
|
|
|
|
float flow_y;
|
|
|
|
float body_x;
|
|
|
|
float body_y;
|
|
|
|
};
|
|
|
|
|
|
|
|
#if OPTFLOW == ENABLED
|
|
|
|
// Write an optical flow packet
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::Log_Write_Optflow()
|
2014-11-02 02:26:05 -04:00
|
|
|
{
|
|
|
|
// exit immediately if not enabled
|
|
|
|
if (!optflow.enabled()) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
const Vector2f &flowRate = optflow.flowRate();
|
|
|
|
const Vector2f &bodyRate = optflow.bodyRate();
|
|
|
|
struct log_Optflow pkt = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_OPTFLOW_MSG),
|
2015-11-19 23:04:52 -04:00
|
|
|
time_us : AP_HAL::micros64(),
|
2014-11-02 02:26:05 -04:00
|
|
|
surface_quality : optflow.quality(),
|
|
|
|
flow_x : flowRate.x,
|
|
|
|
flow_y : flowRate.y,
|
|
|
|
body_x : bodyRate.x,
|
|
|
|
body_y : bodyRate.y
|
|
|
|
};
|
|
|
|
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
2013-11-27 22:19:34 -04:00
|
|
|
struct PACKED log_Arm_Disarm {
|
|
|
|
LOG_PACKET_HEADER;
|
2015-04-30 00:40:51 -03:00
|
|
|
uint64_t time_us;
|
2013-11-27 22:19:34 -04:00
|
|
|
uint8_t arm_state;
|
|
|
|
uint16_t arm_checks;
|
|
|
|
};
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::Log_Write_Current()
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2016-06-02 18:00:01 -03:00
|
|
|
DataFlash.Log_Write_Current(battery);
|
2014-02-13 07:08:25 -04:00
|
|
|
|
|
|
|
// also write power status
|
|
|
|
DataFlash.Log_Write_Power();
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::Log_Arm_Disarm() {
|
2013-11-27 22:19:34 -04:00
|
|
|
struct log_Arm_Disarm pkt = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_ARM_DISARM_MSG),
|
2015-11-19 23:04:52 -04:00
|
|
|
time_us : AP_HAL::micros64(),
|
2013-11-27 22:19:34 -04:00
|
|
|
arm_state : arming.is_armed(),
|
|
|
|
arm_checks : arming.get_enabled_checks()
|
|
|
|
};
|
2016-04-21 03:11:48 -03:00
|
|
|
DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
|
2013-11-27 22:19:34 -04:00
|
|
|
}
|
2013-04-19 10:54:40 -03:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::Log_Write_GPS(uint8_t instance)
|
2013-04-19 18:24:22 -03:00
|
|
|
{
|
2016-05-04 05:32:02 -03:00
|
|
|
if (!ahrs.have_ekf_logging()) {
|
|
|
|
DataFlash.Log_Write_GPS(gps, instance);
|
|
|
|
}
|
2013-04-19 18:24:22 -03:00
|
|
|
}
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::Log_Write_IMU()
|
2013-04-19 18:24:22 -03:00
|
|
|
{
|
2013-11-03 23:37:32 -04:00
|
|
|
DataFlash.Log_Write_IMU(ins);
|
2013-04-19 18:24:22 -03:00
|
|
|
}
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::Log_Write_RC(void)
|
2013-11-25 17:44:17 -04:00
|
|
|
{
|
|
|
|
DataFlash.Log_Write_RCIN();
|
2013-11-27 07:14:57 -04:00
|
|
|
DataFlash.Log_Write_RCOUT();
|
2015-09-04 12:51:03 -03:00
|
|
|
if (rssi.enabled()) {
|
|
|
|
DataFlash.Log_Write_RSSI(rssi);
|
|
|
|
}
|
2013-11-25 17:44:17 -04:00
|
|
|
}
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::Log_Write_Baro(void)
|
2014-01-27 19:35:59 -04:00
|
|
|
{
|
2016-05-04 05:32:02 -03:00
|
|
|
if (!ahrs.have_ekf_logging()) {
|
|
|
|
DataFlash.Log_Write_Baro(barometer);
|
|
|
|
}
|
2014-01-27 19:35:59 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// Write a AIRSPEED packet
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::Log_Write_Airspeed(void)
|
2014-01-27 19:35:59 -04:00
|
|
|
{
|
2015-01-19 18:10:57 -04:00
|
|
|
DataFlash.Log_Write_Airspeed(airspeed);
|
2014-01-27 19:35:59 -04:00
|
|
|
}
|
|
|
|
|
2015-07-05 23:01:17 -03:00
|
|
|
// log ahrs home and EKF origin to dataflash
|
|
|
|
void Plane::Log_Write_Home_And_Origin()
|
|
|
|
{
|
|
|
|
#if AP_AHRS_NAVEKF_AVAILABLE
|
|
|
|
// log ekf origin if set
|
|
|
|
Location ekf_orig;
|
2016-03-03 03:00:10 -04:00
|
|
|
if (ahrs.get_origin(ekf_orig)) {
|
2015-07-05 23:01:17 -03:00
|
|
|
DataFlash.Log_Write_Origin(LogOriginType::ekf_origin, ekf_orig);
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
|
|
|
// log ahrs home if set
|
|
|
|
if (home_is_set != HOME_UNSET) {
|
|
|
|
DataFlash.Log_Write_Origin(LogOriginType::ahrs_home, ahrs.get_home());
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2016-04-16 07:26:43 -03:00
|
|
|
const struct LogStructure Plane::log_structure[] = {
|
2013-04-19 10:37:21 -03:00
|
|
|
LOG_COMMON_STRUCTURES,
|
|
|
|
{ LOG_PERFORMANCE_MSG, sizeof(log_Performance),
|
2016-04-21 03:11:48 -03:00
|
|
|
"PM", "QHHIII", "TimeUS,NLon,NLoop,MaxT,MinT,LogDrop" },
|
2013-04-19 10:37:21 -03:00
|
|
|
{ LOG_STARTUP_MSG, sizeof(log_Startup),
|
2015-04-30 00:40:51 -03:00
|
|
|
"STRT", "QBH", "TimeUS,SType,CTot" },
|
2013-04-19 10:37:21 -03:00
|
|
|
{ LOG_CTUN_MSG, sizeof(log_Control_Tuning),
|
2016-01-30 02:38:31 -04:00
|
|
|
"CTUN", "Qcccchhh", "TimeUS,NavRoll,Roll,NavPitch,Pitch,ThrOut,RdrOut,ThrDem" },
|
2013-04-19 10:37:21 -03:00
|
|
|
{ LOG_NTUN_MSG, sizeof(log_Nav_Tuning),
|
2016-05-10 16:51:36 -03:00
|
|
|
"NTUN", "Qfcccfff", "TimeUS,WpDist,TargBrg,NavBrg,AltErr,XT,XTi,ArspdErr" },
|
2013-10-30 19:23:21 -03:00
|
|
|
{ LOG_SONAR_MSG, sizeof(log_Sonar),
|
2016-05-17 22:34:39 -03:00
|
|
|
"SONR", "QffBf", "TimeUS,Dist,Volt,Cnt,Corr" },
|
2013-11-27 22:19:34 -04:00
|
|
|
{ LOG_ARM_DISARM_MSG, sizeof(log_Arm_Disarm),
|
2015-07-19 20:46:19 -03:00
|
|
|
"ARM", "QBH", "TimeUS,ArmState,ArmChecks" },
|
2014-04-30 08:22:16 -03:00
|
|
|
{ LOG_ATRP_MSG, sizeof(AP_AutoTune::log_ATRP),
|
2015-04-30 00:40:51 -03:00
|
|
|
"ATRP", "QBBcfff", "TimeUS,Type,State,Servo,Demanded,Achieved,P" },
|
2015-04-09 22:42:48 -03:00
|
|
|
{ LOG_STATUS_MSG, sizeof(log_Status),
|
2015-11-17 20:11:21 -04:00
|
|
|
"STAT", "QBfBBBBBB", "TimeUS,isFlying,isFlyProb,Armed,Safety,Crash,Still,Stage,Hit" },
|
2016-03-24 22:33:19 -03:00
|
|
|
{ LOG_QTUN_MSG, sizeof(QuadPlane::log_QControl_Tuning),
|
2016-04-10 07:36:20 -03:00
|
|
|
"QTUN", "Qffffehhffff", "TimeUS,AngBst,ThrOut,DAlt,Alt,BarAlt,DCRt,CRt,DVx,DVy,DAx,DAy" },
|
2014-11-02 02:26:05 -04:00
|
|
|
#if OPTFLOW == ENABLED
|
|
|
|
{ LOG_OPTFLOW_MSG, sizeof(log_Optflow),
|
2015-04-30 00:40:51 -03:00
|
|
|
"OF", "QBffff", "TimeUS,Qual,flowX,flowY,bodyX,bodyY" },
|
2014-11-02 02:26:05 -04:00
|
|
|
#endif
|
2013-04-19 10:37:21 -03:00
|
|
|
};
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2015-03-13 08:33:48 -03:00
|
|
|
#if CLI_ENABLED == ENABLED
|
2012-12-08 00:35:49 -04:00
|
|
|
// Read the DataFlash.log memory : Packet Parser
|
2015-10-20 07:36:42 -03:00
|
|
|
void Plane::Log_Read(uint16_t list_entry, int16_t start_page, int16_t end_page)
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2015-10-25 17:10:41 -03:00
|
|
|
cliSerial->printf("\n" FIRMWARE_STRING
|
2015-10-24 18:45:41 -03:00
|
|
|
"\nFree RAM: %u\n",
|
2013-12-27 23:51:37 -04:00
|
|
|
(unsigned)hal.util->available_memory());
|
2011-11-19 18:02:45 -04:00
|
|
|
|
2015-10-25 16:50:51 -03:00
|
|
|
cliSerial->println(HAL_BOARD_NAME);
|
2013-04-18 21:24:20 -03:00
|
|
|
|
2015-10-20 07:36:42 -03:00
|
|
|
DataFlash.LogReadProcess(list_entry, start_page, end_page,
|
2015-05-24 20:24:11 -03:00
|
|
|
FUNCTOR_BIND_MEMBER(&Plane::print_flight_mode, void, AP_HAL::BetterStream *, uint8_t),
|
2013-04-20 02:18:10 -03:00
|
|
|
cliSerial);
|
2011-11-16 11:29:40 -04:00
|
|
|
}
|
2015-03-13 08:33:48 -03:00
|
|
|
#endif // CLI_ENABLED
|
2011-11-16 11:29:40 -04:00
|
|
|
|
2015-08-06 10:25:22 -03:00
|
|
|
void Plane::Log_Write_Vehicle_Startup_Messages()
|
|
|
|
{
|
|
|
|
// only 200(?) bytes are guaranteed by DataFlash
|
|
|
|
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
|
2015-09-17 07:15:26 -03:00
|
|
|
DataFlash.Log_Write_Mode(control_mode);
|
2016-07-02 03:55:01 -03:00
|
|
|
DataFlash.Log_Write_Rally(rally);
|
2015-08-06 10:25:22 -03:00
|
|
|
}
|
|
|
|
|
2013-04-19 10:37:21 -03:00
|
|
|
// start a new log
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::start_logging()
|
2011-11-16 11:29:40 -04:00
|
|
|
{
|
2015-08-06 10:25:22 -03:00
|
|
|
DataFlash.set_mission(&mission);
|
|
|
|
DataFlash.setVehicle_Startup_Log_Writer(
|
|
|
|
FUNCTOR_BIND(&plane, &Plane::Log_Write_Vehicle_Startup_Messages, void)
|
|
|
|
);
|
2015-06-12 00:11:06 -03:00
|
|
|
|
2015-08-06 10:25:22 -03:00
|
|
|
DataFlash.StartNewLog();
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
2013-01-14 23:03:08 -04:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
/*
|
|
|
|
initialise logging subsystem
|
|
|
|
*/
|
|
|
|
void Plane::log_init(void)
|
|
|
|
{
|
2015-07-06 12:30:40 -03:00
|
|
|
DataFlash.Init(log_structure, ARRAY_SIZE(log_structure));
|
2015-05-13 03:09:36 -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:47 -03:00
|
|
|
} else if (DataFlash.NeedPrep()) {
|
2015-11-18 15:17:50 -04:00
|
|
|
gcs_send_text(MAV_SEVERITY_INFO, "Preparing log system");
|
2015-08-08 03:13:47 -03:00
|
|
|
DataFlash.Prep();
|
2015-11-18 15:17:50 -04:00
|
|
|
gcs_send_text(MAV_SEVERITY_INFO, "Prepared log system");
|
2015-05-13 03:09:36 -03:00
|
|
|
for (uint8_t i=0; i<num_gcs; i++) {
|
|
|
|
gcs[i].reset_cli_timeout();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
#else // LOGGING_ENABLED
|
2013-01-12 07:18:58 -04:00
|
|
|
|
2015-07-31 18:26:14 -03:00
|
|
|
#if CLI_ENABLED == ENABLED
|
|
|
|
bool Plane::print_log_menu(void) { return true; }
|
|
|
|
int8_t Plane::dump_log(uint8_t argc, const Menu::arg *argv) { return 0; }
|
|
|
|
int8_t Plane::erase_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
|
|
|
|
int8_t Plane::select_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
|
|
|
|
int8_t Plane::process_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
|
|
|
|
#endif // CLI_ENABLED == ENABLED
|
|
|
|
|
|
|
|
void Plane::do_erase_logs(void) {}
|
|
|
|
void Plane::Log_Write_Attitude(void) {}
|
|
|
|
void Plane::Log_Write_Performance() {}
|
|
|
|
void Plane::Log_Write_Startup(uint8_t type) {}
|
|
|
|
void Plane::Log_Write_Control_Tuning() {}
|
|
|
|
void Plane::Log_Write_Nav_Tuning() {}
|
|
|
|
void Plane::Log_Write_Status() {}
|
|
|
|
void Plane::Log_Write_Sonar() {}
|
|
|
|
|
|
|
|
#if OPTFLOW == ENABLED
|
|
|
|
void Plane::Log_Write_Optflow() {}
|
|
|
|
#endif
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2015-07-31 18:26:14 -03:00
|
|
|
void Plane::Log_Write_Current() {}
|
|
|
|
void Plane::Log_Arm_Disarm() {}
|
|
|
|
void Plane::Log_Write_GPS(uint8_t instance) {}
|
|
|
|
void Plane::Log_Write_IMU() {}
|
|
|
|
void Plane::Log_Write_RC(void) {}
|
|
|
|
void Plane::Log_Write_Baro(void) {}
|
|
|
|
void Plane::Log_Write_Airspeed(void) {}
|
|
|
|
void Plane::Log_Write_Home_And_Origin() {}
|
|
|
|
|
|
|
|
#if CLI_ENABLED == ENABLED
|
|
|
|
void Plane::Log_Read(uint16_t log_num, int16_t start_page, int16_t end_page) {}
|
|
|
|
#endif // CLI_ENABLED
|
|
|
|
|
|
|
|
void Plane::start_logging() {}
|
|
|
|
void Plane::log_init(void) {}
|
2011-09-08 22:29:39 -03:00
|
|
|
|
|
|
|
#endif // LOGGING_ENABLED
|