2012-04-30 04:17:14 -03:00
|
|
|
// -*- 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"
|
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[] = {
|
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)}
|
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
|
|
|
{
|
2015-10-25 17:10:41 -03:00
|
|
|
cliSerial->printf("logs enabled: ");
|
2012-04-30 04:17:14 -03:00
|
|
|
|
|
|
|
if (0 == g.log_bitmask) {
|
2015-10-25 17:10:41 -03:00
|
|
|
cliSerial->printf("none");
|
2012-04-30 04:17:14 -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-29 10:57:48 -03:00
|
|
|
#define PLOG(_s) if (g.log_bitmask & MASK_LOG_ ## _s) cliSerial->printf(" %s", #_s)
|
2012-04-30 04:17:14 -03:00
|
|
|
PLOG(ATTITUDE_FAST);
|
|
|
|
PLOG(ATTITUDE_MED);
|
|
|
|
PLOG(GPS);
|
|
|
|
PLOG(PM);
|
|
|
|
PLOG(CTUN);
|
|
|
|
PLOG(NTUN);
|
|
|
|
PLOG(MODE);
|
2013-01-26 04:35:18 -04:00
|
|
|
PLOG(IMU);
|
2012-04-30 04:17:14 -03:00
|
|
|
PLOG(CMD);
|
2013-01-26 04:41:38 -04:00
|
|
|
PLOG(CURRENT);
|
2013-04-18 21:23:57 -03:00
|
|
|
PLOG(SONAR);
|
2013-04-19 10:54:26 -03:00
|
|
|
PLOG(COMPASS);
|
2013-07-14 20:57:00 -03:00
|
|
|
PLOG(CAMERA);
|
2013-12-15 20:14:58 -04:00
|
|
|
PLOG(STEERING);
|
2012-04-30 04:17:14 -03:00
|
|
|
#undef PLOG
|
|
|
|
}
|
|
|
|
|
2012-11-21 02:25:11 -04:00
|
|
|
cliSerial->println();
|
2012-04-30 04:17:14 -03:00
|
|
|
|
2013-04-17 08:34:20 -03:00
|
|
|
DataFlash.ListAvailableLogs(cliSerial);
|
2012-04-30 04:17:14 -03:00
|
|
|
return(true);
|
|
|
|
}
|
|
|
|
|
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) {
|
2015-10-25 17:10:41 -03:00
|
|
|
cliSerial->printf("dumping all\n");
|
2013-04-19 18:29:57 -03:00
|
|
|
Log_Read(0, 1, 0);
|
|
|
|
return(-1);
|
2015-10-20 07:36:48 -03:00
|
|
|
} else if ((argc != 2) || ((uint16_t)dump_log_num > DataFlash.get_num_logs())) {
|
2015-10-25 17:10:41 -03: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);
|
|
|
|
Log_Read((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
|
|
|
{
|
|
|
|
uint16_t bits;
|
|
|
|
|
|
|
|
if (argc != 2) {
|
2015-10-25 17:10:41 -03:00
|
|
|
cliSerial->printf("missing log type\n");
|
2012-04-30 04:17:14 -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")) {
|
2012-04-30 04:17:14 -03:00
|
|
|
bits = ~0;
|
|
|
|
} else {
|
2015-10-25 13:22:16 -03:00
|
|
|
#define TARG(_s) if (!strcasecmp(argv[1].str, #_s)) bits |= MASK_LOG_ ## _s
|
2012-04-30 04:17:14 -03:00
|
|
|
TARG(ATTITUDE_FAST);
|
|
|
|
TARG(ATTITUDE_MED);
|
|
|
|
TARG(GPS);
|
|
|
|
TARG(PM);
|
|
|
|
TARG(CTUN);
|
|
|
|
TARG(NTUN);
|
|
|
|
TARG(MODE);
|
2013-01-26 04:35:18 -04:00
|
|
|
TARG(IMU);
|
2012-04-30 04:17:14 -03:00
|
|
|
TARG(CMD);
|
2013-01-26 04:41:38 -04:00
|
|
|
TARG(CURRENT);
|
2013-04-18 21:23:57 -03:00
|
|
|
TARG(SONAR);
|
2013-04-19 10:54:26 -03:00
|
|
|
TARG(COMPASS);
|
2013-07-14 20:57:00 -03:00
|
|
|
TARG(CAMERA);
|
2013-12-15 20:14:58 -04:00
|
|
|
TARG(STEERING);
|
2012-04-30 04:17:14 -03:00
|
|
|
#undef TARG
|
|
|
|
}
|
|
|
|
|
2015-10-25 13:22:16 -03:00
|
|
|
if (!strcasecmp(argv[0].str, "enable")) {
|
2012-04-30 04:17:14 -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);
|
|
|
|
}
|
|
|
|
|
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
|
|
|
{
|
|
|
|
log_menu.run();
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
2015-03-13 08:40:06 -03:00
|
|
|
#endif // CLI_ENABLED == ENABLED
|
|
|
|
|
2015-05-12 02:03:23 -03:00
|
|
|
void Rover::do_erase_logs(void)
|
2015-03-13 08:40:06 -03:00
|
|
|
{
|
2015-10-25 17:10:41 -03:00
|
|
|
cliSerial->printf("\nErasing log...\n");
|
2015-03-13 08:40:06 -03:00
|
|
|
DataFlash.EraseAll();
|
2015-10-25 17:10:41 -03:00
|
|
|
cliSerial->printf("\nLog 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;
|
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,
|
2013-11-18 00:08:53 -04:00
|
|
|
ins_error_count : ins.error_count()
|
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));
|
|
|
|
}
|
|
|
|
|
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
|
|
|
{
|
2013-01-26 05:46:16 -04:00
|
|
|
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),
|
2015-11-19 23:04:16 -04:00
|
|
|
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(),
|
2013-01-26 05:46:16 -04:00
|
|
|
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
|
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(),
|
2013-01-26 05:46:16 -04:00
|
|
|
yaw : (uint16_t)ahrs.yaw_sensor,
|
2013-03-01 07:32:57 -04:00
|
|
|
wp_distance : wp_distance,
|
2013-06-16 20:50:53 -03:00
|
|
|
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()
|
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
|
|
|
{
|
2015-01-16 16:29:09 -04:00
|
|
|
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
|
2015-02-05 02:23:49 -04:00
|
|
|
#if defined(OPTFLOW) and (OPTFLOW == ENABLED)
|
2014-12-30 23:24:01 -04:00
|
|
|
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,
|
2014-06-27 00:18:20 -03:00
|
|
|
sonar1_distance : (uint16_t)sonar.distance_cm(0),
|
|
|
|
sonar2_distance : (uint16_t)sonar.distance_cm(1),
|
2013-04-18 21:23:57 -03:00
|
|
|
detected_count : obstacle.detected_count,
|
|
|
|
turn_angle : (int8_t)obstacle.turn_angle,
|
|
|
|
turn_time : turn_time,
|
|
|
|
ground_speed : (uint16_t)(ground_speed*100),
|
2013-06-03 06:33:59 -03:00
|
|
|
throttle : (int8_t)(100 * channel_throttle->norm_output())
|
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
|
|
|
}
|
|
|
|
|
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());
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2015-10-25 14:03:46 -03:00
|
|
|
const LogStructure Rover::log_structure[] = {
|
2013-04-19 04:53:07 -03:00
|
|
|
LOG_COMMON_STRUCTURES,
|
|
|
|
{ LOG_PERFORMANCE_MSG, sizeof(log_Performance),
|
2015-04-29 08:11:24 -03:00
|
|
|
"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),
|
2015-04-29 08:11:24 -03:00
|
|
|
"STRT", "QBH", "TimeUS,SType,CTot" },
|
2013-04-19 04:53:07 -03:00
|
|
|
{ LOG_CTUN_MSG, sizeof(log_Control_Tuning),
|
2015-04-29 08:11:24 -03:00
|
|
|
"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),
|
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" },
|
2013-12-15 20:14:58 -04:00
|
|
|
{ LOG_STEERING_MSG, sizeof(log_Steering),
|
2015-04-29 08:11:24 -03:00
|
|
|
"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()) {
|
2015-10-24 19:36:35 -03:00
|
|
|
gcs_send_text(MAV_SEVERITY_WARNING, "No dataflash card inserted");
|
2015-05-13 00:16:45 -03:00
|
|
|
g.log_bitmask.set(0);
|
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");
|
2015-08-08 03:13:54 -03:00
|
|
|
for (uint8_t i=0; i<num_gcs; i++) {
|
|
|
|
gcs[i].reset_cli_timeout();
|
|
|
|
}
|
2015-05-13 00:16:45 -03:00
|
|
|
}
|
2015-08-08 03:13:54 -03:00
|
|
|
|
2015-05-13 00:16:45 -03:00
|
|
|
if (g.log_bitmask != 0) {
|
|
|
|
start_logging();
|
|
|
|
}
|
2015-10-30 02:56:41 -03:00
|
|
|
|
|
|
|
arming.set_logging_available(DataFlash.CardInserted());
|
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
|
|
|
"\nFree RAM: %u\n",
|
2013-12-28 01:02:45 -04:00
|
|
|
(unsigned)hal.util->available_memory());
|
2013-04-18 21:23:57 -03:00
|
|
|
|
2015-10-25 16:50:51 -03:00
|
|
|
cliSerial->println(HAL_BOARD_NAME);
|
2013-04-18 21:23:57 -03:00
|
|
|
|
2015-10-20 07:36:48 -03: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
|
|
|
}
|
2015-03-13 08:40:06 -03: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);
|
2015-08-06 09:50:52 -03:00
|
|
|
}
|
|
|
|
|
2013-04-19 10:19:37 -03:00
|
|
|
// start a new log
|
2015-05-12 02:03:23 -03: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;
|
2015-08-06 09:50:52 -03:00
|
|
|
DataFlash.set_mission(&mission);
|
|
|
|
DataFlash.setVehicle_Startup_Log_Writer(
|
|
|
|
FUNCTOR_BIND(&rover, &Rover::Log_Write_Vehicle_Startup_Messages, void)
|
|
|
|
);
|
2013-12-16 20:32:40 -04:00
|
|
|
DataFlash.StartNewLog();
|
2014-01-13 23:40:10 -04:00
|
|
|
in_mavlink_delay = false;
|
2013-04-19 10:19:37 -03:00
|
|
|
}
|
|
|
|
|
2012-04-30 04:17:14 -03:00
|
|
|
#else // LOGGING_ENABLED
|
|
|
|
|
|
|
|
// 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) {}
|
2012-04-30 04:17:14 -03:00
|
|
|
|
2016-01-19 01:26:14 -04:00
|
|
|
#endif // LOGGING_ENABLED
|