mirror of https://github.com/ArduPilot/ardupilot
Plane: DFMessageWriter; ability to trickle messages out to DF
This commit is contained in:
parent
6e3687f8af
commit
c2d61391ef
|
@ -80,6 +80,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] PROGMEM = {
|
|||
#endif
|
||||
SCHED_TASK(terrain_update, 5, 500),
|
||||
SCHED_TASK(update_is_flying_5Hz, 10, 100),
|
||||
SCHED_TASK(dataflash_periodic, 1, 300),
|
||||
};
|
||||
|
||||
void Plane::setup()
|
||||
|
@ -368,6 +369,11 @@ void Plane::terrain_update(void)
|
|||
#endif
|
||||
}
|
||||
|
||||
void Plane::dataflash_periodic(void)
|
||||
{
|
||||
DataFlash.periodic_tasks();
|
||||
}
|
||||
|
||||
/*
|
||||
once a second update the airspeed calibration ratio
|
||||
*/
|
||||
|
|
|
@ -228,7 +228,8 @@ struct PACKED log_Startup {
|
|||
uint16_t command_total;
|
||||
};
|
||||
|
||||
void Plane::Log_Write_Startup(uint8_t type)
|
||||
// do not add any extra log writes to this function; see LogStartup.cpp
|
||||
bool Plane::Log_Write_Startup(uint8_t type)
|
||||
{
|
||||
struct log_Startup pkt = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_STARTUP_MSG),
|
||||
|
@ -236,12 +237,7 @@ void Plane::Log_Write_Startup(uint8_t type)
|
|||
startup_type : type,
|
||||
command_total : mission.num_commands()
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
|
||||
// write all commands to the dataflash as well
|
||||
if (should_log(MASK_LOG_CMD)) {
|
||||
DataFlash.Log_Write_EntireMission(mission);
|
||||
}
|
||||
return DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
struct PACKED log_Control_Tuning {
|
||||
|
@ -517,14 +513,21 @@ void Plane::Log_Read(uint16_t log_num, int16_t start_page, int16_t end_page)
|
|||
}
|
||||
#endif // CLI_ENABLED
|
||||
|
||||
void Plane::Log_Write_Vehicle_Startup_Messages()
|
||||
{
|
||||
// only 200(?) bytes are guaranteed by DataFlash
|
||||
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
|
||||
}
|
||||
|
||||
// start a new log
|
||||
void Plane::start_logging()
|
||||
{
|
||||
DataFlash.set_mission(&mission);
|
||||
DataFlash.setVehicle_Startup_Log_Writer(
|
||||
FUNCTOR_BIND(&plane, &Plane::Log_Write_Vehicle_Startup_Messages, void)
|
||||
);
|
||||
|
||||
DataFlash.StartNewLog();
|
||||
|
||||
DataFlash.Log_Write_SysInfo(PSTR(FIRMWARE_STRING));
|
||||
|
||||
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
|
@ -1,6 +1,7 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
#ifndef _PLANE_H_
|
||||
#define _PLANE_H_
|
||||
|
||||
#ifndef _PLANE_H
|
||||
#define _PLANE_H
|
||||
|
||||
#define THISFIRMWARE "ArduPlane V3.4.0beta1"
|
||||
#define FIRMWARE_VERSION 3,4,0,FIRMWARE_VERSION_TYPE_BETA
|
||||
|
@ -163,7 +164,7 @@ private:
|
|||
// notification object for LEDs, buzzers etc (parameter set to false disables external leds)
|
||||
AP_Notify notify;
|
||||
|
||||
DataFlash_Class DataFlash;
|
||||
DataFlash_Class DataFlash{FIRMWARE_STRING};
|
||||
|
||||
// has a log download started?
|
||||
bool in_log_download;
|
||||
|
@ -698,10 +699,11 @@ private:
|
|||
void gcs_send_text_P(MAV_SEVERITY severity, const prog_char_t *str);
|
||||
void gcs_send_airspeed_calibration(const Vector3f &vg);
|
||||
void gcs_retry_deferred(void);
|
||||
|
||||
void do_erase_logs(void);
|
||||
void Log_Write_Attitude(void);
|
||||
void Log_Write_Performance();
|
||||
void Log_Write_Startup(uint8_t type);
|
||||
bool Log_Write_Startup(uint8_t type);
|
||||
void Log_Write_Control_Tuning();
|
||||
void Log_Write_TECS_Tuning(void);
|
||||
void Log_Write_Nav_Tuning();
|
||||
|
@ -716,8 +718,10 @@ private:
|
|||
void Log_Write_Baro(void);
|
||||
void Log_Write_Airspeed(void);
|
||||
void Log_Write_Home_And_Origin();
|
||||
void Log_Write_Vehicle_Startup_Messages();
|
||||
void Log_Read(uint16_t log_num, int16_t start_page, int16_t end_page);
|
||||
void start_logging();
|
||||
|
||||
void load_parameters(void);
|
||||
void adjust_altitude_target();
|
||||
void setup_glide_slope(void);
|
||||
|
@ -943,6 +947,7 @@ private:
|
|||
uint32_t millis() const;
|
||||
uint32_t micros() const;
|
||||
void init_capabilities(void);
|
||||
void dataflash_periodic(void);
|
||||
|
||||
public:
|
||||
void mavlink_delay_cb();
|
||||
|
|
|
@ -242,7 +242,6 @@ void Plane::init_ardupilot()
|
|||
init_capabilities();
|
||||
|
||||
startup_ground();
|
||||
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
|
||||
|
||||
// choose the nav controller
|
||||
set_nav_controller();
|
||||
|
@ -718,6 +717,7 @@ void Plane::servo_write(uint8_t ch, uint16_t pwm)
|
|||
*/
|
||||
bool Plane::should_log(uint32_t mask)
|
||||
{
|
||||
#if LOGGING_ENABLED == ENABLED
|
||||
if (!(mask & g.log_bitmask) || in_mavlink_delay) {
|
||||
return false;
|
||||
}
|
||||
|
@ -725,13 +725,12 @@ bool Plane::should_log(uint32_t mask)
|
|||
if (ret && !DataFlash.logging_started() && !in_log_download) {
|
||||
// we have to set in_mavlink_delay to prevent logging while
|
||||
// writing headers
|
||||
in_mavlink_delay = true;
|
||||
#if LOGGING_ENABLED == ENABLED
|
||||
start_logging();
|
||||
#endif
|
||||
in_mavlink_delay = false;
|
||||
}
|
||||
return ret;
|
||||
#else
|
||||
return false;
|
||||
#endif
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
Loading…
Reference in New Issue