Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: Allow GPS's to run functions on arming (used by SBF) 

Supports starting/stopping GPS logging
This commit is contained in:
Michael du Breuil 2017-10-19 18:15:11 -07:00 committed by Francisco Ferreira
parent 5843086760
commit c856cc6f33
5 changed files with 97 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_GPS/AP_GPS.cpp
View File

@ -132,8 +132,8 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @Param: RAW_DATA
// @DisplayName: Raw data logging
// @Description: Enable logging of RXM raw data from uBlox which includes carrier phase and pseudo range information. This allows for post processing of dataflash logs for more precise positioning. Note that this requires a raw capable uBlox such as the 6P or 6T.
// @Values: 0:Disabled,1:log every sample,5:log every 5 samples
// @Description: Handles logging raw data; on uBlox chips that support raw data this will log RXM messages into dataflash log; on Septentrio this will log on the equipment's SD card and when set to 2, the autopilot will try to stop logging after disarming and restart after arming
// @Values: 0:Ignore,1:Always log,2:Stop logging when disarmed (SBF only),5:Only log every five samples (uBlox only)
// @RebootRequired: True
// @User: Advanced
AP_GROUPINFO("RAW_DATA", 9, AP_GPS, _raw_data, 0),
@ -1502,3 +1502,13 @@ bool AP_GPS::is_healthy(uint8_t instance) const {
last_message_delta_time_ms(instance) < GPS_MAX_DELTA_MS &&
drivers[instance]->is_healthy();
}
bool AP_GPS::prepare_for_arming(void) {
bool all_passed = true;
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (drivers[i] != nullptr) {
all_passed &= drivers[i]->prepare_for_arming();
}
}
return all_passed;
}

3
libraries/AP_GPS/AP_GPS.h
View File

@ -413,6 +413,9 @@ public:
bool is_healthy(uint8_t instance) const;
bool is_healthy(void) const { return is_healthy(primary_instance); }
// returns true if all GPS instances have passed all final arming checks/state changes
bool prepare_for_arming(void);
protected:
// configuration parameters

109
libraries/AP_GPS/AP_GPS_SBF.cpp
View File

@ -64,32 +64,45 @@ AP_GPS_SBF::AP_GPS_SBF(AP_GPS &_gps, AP_GPS::GPS_State &_state,
bool
AP_GPS_SBF::read(void)
{
uint32_t now = AP_HAL::millis();
if (gps._auto_config != AP_GPS::GPS_AUTO_CONFIG_DISABLE &&
_init_blob_index < ARRAY_SIZE(_initialisation_blob)) {
const char *init_str = _initialisation_blob[_init_blob_index];
if (now > _init_blob_time) {
if (now > _config_last_ack_time + 2500) {
// try to enable input on the GPS port if we have not made progress on configuring it
Debug("SBF Sending port enable");
port->write((const uint8_t*)_port_enable, strlen(_port_enable));
_config_last_ack_time = now;
} else {
Debug("SBF sending init string: %s", init_str);
port->write((const uint8_t*)init_str, strlen(init_str));
}
_init_blob_time = now + 1000;
}
}
bool ret = false;
while (port->available() > 0) {
uint32_t available_bytes = port->available();
for (uint32_t i = 0; i < available_bytes; i++) {
uint8_t temp = port->read();
ret |= parse(temp);
}
if (gps._auto_config != AP_GPS::GPS_AUTO_CONFIG_DISABLE) {
if (_init_blob_index < ARRAY_SIZE(_initialisation_blob)) {
uint32_t now = AP_HAL::millis();
const char *init_str = _initialisation_blob[_init_blob_index];
if (now > _init_blob_time) {
if (now > _config_last_ack_time + 2500) {
// try to enable input on the GPS port if we have not made progress on configuring it
Debug("SBF Sending port enable");
port->write((const uint8_t*)_port_enable, strlen(_port_enable));
_config_last_ack_time = now;
} else {
Debug("SBF sending init string: %s", init_str);
port->write((const uint8_t*)init_str, strlen(init_str));
}
_init_blob_time = now + 1000;
}
} else if (gps._raw_data == 2) { // only manage disarm/rearms when the user opts into it
if (hal.util->get_soft_armed()) {
_has_been_armed = true;
} else if (_has_been_armed && (RxState & SBF_DISK_MOUNTED)) {
// since init is done at this point and unmounting should be rate limited,
// take over the _init_blob_time variable
uint32_t now = AP_HAL::millis();
if (now > _init_blob_time) {
unmount_disk();
_init_blob_time = now + 1000;
}
}
}
}
return ret;
}
@ -380,18 +393,6 @@ AP_GPS_SBF::process_message(void)
void AP_GPS_SBF::broadcast_configuration_failure_reason(void) const
{
if (gps._raw_data) {
if (!(RxState & SBF_DISK_MOUNTED)){
gcs().send_text(MAV_SEVERITY_INFO, "GPS %d: SBF disk is not mounted", state.instance + 1);
}
else if (RxState & SBF_DISK_FULL) {
gcs().send_text(MAV_SEVERITY_INFO, "GPS %d: SBF disk is full", state.instance + 1);
}
else if (!(RxState & SBF_DISK_ACTIVITY)) {
gcs().send_text(MAV_SEVERITY_INFO, "GPS %d: SBF is not currently logging", state.instance + 1);
}
}
if (gps._auto_config != AP_GPS::GPS_AUTO_CONFIG_DISABLE &&
_init_blob_index < ARRAY_SIZE(_initialisation_blob)) {
gcs().send_text(MAV_SEVERITY_INFO, "GPS %d: SBF is not fully configured (%d/%d)", state.instance + 1,
@ -400,11 +401,49 @@ void AP_GPS_SBF::broadcast_configuration_failure_reason(void) const
}
bool AP_GPS_SBF::is_configured (void) {
return (!gps._raw_data || (RxState & SBF_DISK_ACTIVITY)) &&
(gps._auto_config == AP_GPS::GPS_AUTO_CONFIG_DISABLE ||
return (gps._auto_config == AP_GPS::GPS_AUTO_CONFIG_DISABLE ||
_init_blob_index >= ARRAY_SIZE(_initialisation_blob));
}
bool AP_GPS_SBF::is_healthy (void) const {
return (RxError & RX_ERROR_MASK) == 0;
}
void AP_GPS_SBF::mount_disk (void) const {
const char* command = "emd, DSK1, Mount\n";
Debug("Mounting disk");
port->write((const uint8_t*)command, strlen(command));
}
void AP_GPS_SBF::unmount_disk (void) const {
const char* command = "emd, DSK1, Unmount\n";
Debug("Unmounting disk");
port->write((const uint8_t*)command, strlen(command));
}
bool AP_GPS_SBF::prepare_for_arming(void) {
bool is_logging = true; // assume that its logging until proven otherwise
if (gps._raw_data) {
if (!(RxState & SBF_DISK_MOUNTED)){
is_logging = false;
gcs().send_text(MAV_SEVERITY_INFO, "GPS %d: SBF disk is not mounted", state.instance + 1);
// simply attempt to mount the disk, no need to check if the command was
// ACK/NACK'd as we don't continously attempt to remount the disk
gcs().send_text(MAV_SEVERITY_INFO, "GPS %d: Attempting to mount disk", state.instance + 1);
mount_disk();
// reset the flag to indicate if we should be logging
_has_been_armed = false;
}
else if (RxState & SBF_DISK_FULL) {
is_logging = false;
gcs().send_text(MAV_SEVERITY_INFO, "GPS %d: SBF disk is full", state.instance + 1);
}
else if (!(RxState & SBF_DISK_ACTIVITY)) {
is_logging = false;
gcs().send_text(MAV_SEVERITY_INFO, "GPS %d: SBF is not currently logging", state.instance + 1);
}
}
return is_logging;
}

6
libraries/AP_GPS/AP_GPS_SBF.h
View File

@ -48,6 +48,8 @@ public:
bool is_healthy(void) const override;
bool prepare_for_arming(void) override;
private:
@ -74,6 +76,10 @@ private:
uint32_t RxState;
uint32_t RxError;
void mount_disk(void) const;
void unmount_disk(void) const;
bool _has_been_armed;
enum sbf_ids {
DOP = 4001,
PVTGeodetic = 4007,

2
libraries/AP_GPS/GPS_Backend.h
View File

@ -63,6 +63,8 @@ public:
void broadcast_gps_type() const;
virtual void Write_DataFlash_Log_Startup_messages() const;
virtual bool prepare_for_arming(void) { return true; }
protected:
AP_HAL::UARTDriver *port; ///< UART we are attached to
AP_GPS &gps; ///< access to frontend (for parameters)