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

Revert "AP_GPS: Move init logic from constructor to class" 

This reverts commit 5f84cd8f2b.
This commit is contained in:
Peter Barker 2023-11-28 16:02:37 +11:00 committed by Peter Barker
parent 35b52a4604
commit 8b7652d1db
2 changed files with 24 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
libraries/AP_GPS/AP_GPS_GSOF.cpp
View File

@ -59,7 +59,21 @@ AP_GPS_GSOF::AP_GPS_GSOF(AP_GPS &_gps, AP_GPS::GPS_State &_state,
{
// https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_Overview.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257COverview%257C_____0
static_assert(ARRAY_SIZE(gsofmsgreq) <= 10, "The maximum number of outputs allowed with GSOF is 10.");
static_assert(sizeof(gsofmsgreq) != 0, "gsofmsgreq is not empty");
msg.state = Msg_Parser::State::STARTTX;
constexpr uint8_t default_com_port = static_cast<uint8_t>(HW_Port::COM2);
gps._com_port[state.instance].set_default(default_com_port);
const auto com_port = gps._com_port[state.instance].get();
if (!validate_com_port(com_port)) {
// The user parameter for COM port is not a valid GSOF port
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "GSOF instance %d has invalid COM port setting of %d", state.instance, com_port);
return;
}
const HW_Baud baud = HW_Baud::BAUD115K;
// Start by disabling output during config
[[maybe_unused]] const auto output_disabled = disableOutput(static_cast<HW_Port>(com_port));
is_baud_configured = requestBaud(static_cast<HW_Port>(com_port), baud);
}
// Process all bytes available from the stream
@ -67,48 +81,22 @@ AP_GPS_GSOF::AP_GPS_GSOF(AP_GPS &_gps, AP_GPS::GPS_State &_state,
bool
AP_GPS_GSOF::read(void)
{
if (!is_baud_configured) {
// Debug("Baud not configured, not ready to read()");
return false;
}
constexpr uint8_t default_com_port = static_cast<uint8_t>(HW_Port::COM2);
gps._com_port[state.instance].set_default(default_com_port);
bool gsof_configured = true;
static_assert(sizeof(gsofmsgreq) != 0, "gsofmsgreq is not empty");
while (gsofmsgreq_index < (sizeof(gsofmsgreq))) {
const auto com_port = gps._com_port[state.instance].get();
if (!validate_com_port(com_port)) {
// The user parameter for COM port is not a valid GSOF port
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "GSOF instance %d has invalid COM port setting of %d", state.instance, com_port);
return false;
}
const HW_Baud baud = HW_Baud::BAUD115K;
// Start by disabling output during config
if (!disableOutput(static_cast<HW_Port>(com_port))) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "GSOF instance %d cannot disable output on port %d", state.instance, com_port);
return false;
}
is_baud_configured = requestBaud(static_cast<HW_Port>(com_port), baud);
if (!is_baud_configured) {
// Don't try further configuration if the baud request fails.
return false;
}
}
while (gsofmsgreq_index < sizeof(gsofmsgreq)) {
const auto com_port = gps._com_port[state.instance].get();
if (!validate_com_port(com_port)) {
// The user parameter for COM port is not a valid GSOF port.
return false;
}
const auto success = requestGSOF(gsofmsgreq[gsofmsgreq_index], static_cast<HW_Port>(com_port), Output_Rate::FREQ_10_HZ);
if (!success) {
// Try again to do the rest of the configuration next time.
return false;
} else {
if (gsofmsgreq_index == sizeof(gsofmsgreq) - 1) {
gsof_configured = true;
}
}
gsof_configured &= requestGSOF(gsofmsgreq[gsofmsgreq_index], static_cast<HW_Port>(com_port), Output_Rate::FREQ_10_HZ);
gsofmsgreq_index++;
}
if (!gsof_configured) {

4
libraries/AP_GPS/AP_GPS_GSOF.h
View File

@ -108,7 +108,7 @@ private:
ENDTX
};
State state = State::STARTTX;
State state;
uint8_t status;
uint8_t packettype;
@ -142,6 +142,6 @@ private:
uint8_t gsofmsgreq_index;
const uint8_t gsofmsgreq[5] = {1,2,8,9,12};
bool is_baud_configured {false};
bool gsof_configured {false};
bool is_configured {false};
};
#endif