mirror of https://github.com/ArduPilot/ardupilot
Plane: added sonar logging to dataflash
This commit is contained in:
parent
ee7c742863
commit
5a0cb5dea4
|
@ -307,6 +307,11 @@ static AP_HAL::AnalogSource *rssi_analog_source;
|
||||||
|
|
||||||
static AP_HAL::AnalogSource *vcc_pin;
|
static AP_HAL::AnalogSource *vcc_pin;
|
||||||
|
|
||||||
|
////////////////////////////////////////////////////////////////////////////////
|
||||||
|
// Sonar
|
||||||
|
////////////////////////////////////////////////////////////////////////////////
|
||||||
|
static AP_RangeFinder_analog sonar;
|
||||||
|
|
||||||
////////////////////////////////////////////////////////////////////////////////
|
////////////////////////////////////////////////////////////////////////////////
|
||||||
// Relay
|
// Relay
|
||||||
////////////////////////////////////////////////////////////////////////////////
|
////////////////////////////////////////////////////////////////////////////////
|
||||||
|
@ -728,6 +733,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
|
||||||
{ compass_accumulate, 1, 1500 },
|
{ compass_accumulate, 1, 1500 },
|
||||||
{ barometer_accumulate, 1, 900 },
|
{ barometer_accumulate, 1, 900 },
|
||||||
{ update_notify, 1, 300 },
|
{ update_notify, 1, 300 },
|
||||||
|
{ read_sonars, 1, 500 },
|
||||||
{ one_second_loop, 50, 3900 },
|
{ one_second_loop, 50, 3900 },
|
||||||
{ check_long_failsafe, 15, 1000 },
|
{ check_long_failsafe, 15, 1000 },
|
||||||
{ airspeed_ratio_update, 50, 1000 },
|
{ airspeed_ratio_update, 50, 1000 },
|
||||||
|
|
|
@ -52,6 +52,7 @@ print_log_menu(void)
|
||||||
PLOG(TECS);
|
PLOG(TECS);
|
||||||
PLOG(CAMERA);
|
PLOG(CAMERA);
|
||||||
PLOG(RC);
|
PLOG(RC);
|
||||||
|
PLOG(SONAR);
|
||||||
#undef PLOG
|
#undef PLOG
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -144,6 +145,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
|
||||||
TARG(TECS);
|
TARG(TECS);
|
||||||
TARG(CAMERA);
|
TARG(CAMERA);
|
||||||
TARG(RC);
|
TARG(RC);
|
||||||
|
TARG(SONAR);
|
||||||
#undef TARG
|
#undef TARG
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -389,6 +391,31 @@ static void Log_Write_Mode(uint8_t mode)
|
||||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
struct PACKED log_Sonar {
|
||||||
|
LOG_PACKET_HEADER;
|
||||||
|
uint32_t timestamp;
|
||||||
|
float distance;
|
||||||
|
float voltage;
|
||||||
|
float baro_alt;
|
||||||
|
float groundspeed;
|
||||||
|
uint8_t throttle;
|
||||||
|
};
|
||||||
|
|
||||||
|
// Write a sonar packet
|
||||||
|
static void Log_Write_Sonar()
|
||||||
|
{
|
||||||
|
struct log_Sonar pkt = {
|
||||||
|
LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG),
|
||||||
|
timestamp : hal.scheduler->millis(),
|
||||||
|
distance : sonar.distance_cm(),
|
||||||
|
voltage : sonar.voltage(),
|
||||||
|
baro_alt : barometer.get_altitude(),
|
||||||
|
groundspeed : (0.01f * g_gps->ground_speed_cm),
|
||||||
|
throttle : (uint8_t)(100 * channel_throttle->norm_output())
|
||||||
|
};
|
||||||
|
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||||
|
}
|
||||||
|
|
||||||
struct PACKED log_Current {
|
struct PACKED log_Current {
|
||||||
LOG_PACKET_HEADER;
|
LOG_PACKET_HEADER;
|
||||||
uint32_t time_ms;
|
uint32_t time_ms;
|
||||||
|
@ -474,6 +501,8 @@ static const struct LogStructure log_structure[] PROGMEM = {
|
||||||
"CTUN", "Icccchhf", "TimeMS,NavRoll,Roll,NavPitch,Pitch,ThrOut,RdrOut,AccY" },
|
"CTUN", "Icccchhf", "TimeMS,NavRoll,Roll,NavPitch,Pitch,ThrOut,RdrOut,AccY" },
|
||||||
{ LOG_NTUN_MSG, sizeof(log_Nav_Tuning),
|
{ LOG_NTUN_MSG, sizeof(log_Nav_Tuning),
|
||||||
"NTUN", "ICICCccfI", "TimeMS,Yaw,WpDist,TargBrg,NavBrg,AltErr,Arspd,Alt,GSpdCM" },
|
"NTUN", "ICICCccfI", "TimeMS,Yaw,WpDist,TargBrg,NavBrg,AltErr,Arspd,Alt,GSpdCM" },
|
||||||
|
{ LOG_SONAR_MSG, sizeof(log_Sonar),
|
||||||
|
"SONR", "IffffB", "TimeMS,DistCM,Volt,BaroAlt,GSpd,Thr" },
|
||||||
{ LOG_MODE_MSG, sizeof(log_Mode),
|
{ LOG_MODE_MSG, sizeof(log_Mode),
|
||||||
"MODE", "IMB", "TimeMS,Mode,ModeNum" },
|
"MODE", "IMB", "TimeMS,Mode,ModeNum" },
|
||||||
{ LOG_CURRENT_MSG, sizeof(log_Current),
|
{ LOG_CURRENT_MSG, sizeof(log_Current),
|
||||||
|
|
|
@ -98,6 +98,7 @@ public:
|
||||||
k_param_ground_steer_dps,
|
k_param_ground_steer_dps,
|
||||||
k_param_rally_limit_km,
|
k_param_rally_limit_km,
|
||||||
k_param_hil_err_limit,
|
k_param_hil_err_limit,
|
||||||
|
k_param_sonar,
|
||||||
|
|
||||||
// 110: Telemetry control
|
// 110: Telemetry control
|
||||||
//
|
//
|
||||||
|
|
|
@ -776,6 +776,10 @@ const AP_Param::Info var_info[] PROGMEM = {
|
||||||
// @Path: ../libraries/AP_Relay/AP_Relay.cpp
|
// @Path: ../libraries/AP_Relay/AP_Relay.cpp
|
||||||
GOBJECT(relay, "RELAY_", AP_Relay),
|
GOBJECT(relay, "RELAY_", AP_Relay),
|
||||||
|
|
||||||
|
// @Group: SONAR_
|
||||||
|
// @Path: ../libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp
|
||||||
|
GOBJECT(sonar, "SONAR_", AP_RangeFinder_analog),
|
||||||
|
|
||||||
// RC channel
|
// RC channel
|
||||||
//-----------
|
//-----------
|
||||||
// @Group: RC1_
|
// @Group: RC1_
|
||||||
|
|
|
@ -162,6 +162,7 @@ enum log_messages {
|
||||||
LOG_COMPASS_MSG,
|
LOG_COMPASS_MSG,
|
||||||
LOG_TECS_MSG,
|
LOG_TECS_MSG,
|
||||||
LOG_RC_MSG,
|
LOG_RC_MSG,
|
||||||
|
LOG_SONAR_MSG,
|
||||||
MAX_NUM_LOGS // always at the end
|
MAX_NUM_LOGS // always at the end
|
||||||
};
|
};
|
||||||
|
|
||||||
|
@ -179,6 +180,7 @@ enum log_messages {
|
||||||
#define MASK_LOG_TECS (1<<11)
|
#define MASK_LOG_TECS (1<<11)
|
||||||
#define MASK_LOG_CAMERA (1<<12)
|
#define MASK_LOG_CAMERA (1<<12)
|
||||||
#define MASK_LOG_RC (1<<13)
|
#define MASK_LOG_RC (1<<13)
|
||||||
|
#define MASK_LOG_SONAR (1<<14)
|
||||||
|
|
||||||
// Waypoint Modes
|
// Waypoint Modes
|
||||||
// ----------------
|
// ----------------
|
||||||
|
|
|
@ -23,6 +23,27 @@ static int32_t read_barometer(void)
|
||||||
return altitude_filter.apply(barometer.get_altitude() * 100.0);
|
return altitude_filter.apply(barometer.get_altitude() * 100.0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static void init_sonar(void)
|
||||||
|
{
|
||||||
|
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
|
||||||
|
sonar.Init(&adc);
|
||||||
|
#else
|
||||||
|
sonar.Init(NULL);
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
// read the sonars
|
||||||
|
static void read_sonars(void)
|
||||||
|
{
|
||||||
|
if (!sonar.enabled()) {
|
||||||
|
// this makes it possible to disable sonar at runtime
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (g.log_bitmask & MASK_LOG_SONAR)
|
||||||
|
Log_Write_Sonar();
|
||||||
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
ask airspeed sensor for a new value
|
ask airspeed sensor for a new value
|
||||||
*/
|
*/
|
||||||
|
@ -71,3 +92,4 @@ static int32_t adjusted_altitude_cm(void)
|
||||||
{
|
{
|
||||||
return current_loc.alt - (g.alt_offset*100);
|
return current_loc.alt - (g.alt_offset*100);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -104,6 +104,9 @@ static void init_ardupilot()
|
||||||
// init baro before we start the GCS, so that the CLI baro test works
|
// init baro before we start the GCS, so that the CLI baro test works
|
||||||
barometer.init();
|
barometer.init();
|
||||||
|
|
||||||
|
// initialise sonar
|
||||||
|
init_sonar();
|
||||||
|
|
||||||
// init the GCS
|
// init the GCS
|
||||||
gcs[0].init(hal.uartA);
|
gcs[0].init(hal.uartA);
|
||||||
// Register mavlink_delay_cb, which will run anytime you have
|
// Register mavlink_delay_cb, which will run anytime you have
|
||||||
|
|
Loading…
Reference in New Issue