Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Allow Periph to compile with !AP_GPS_ENABLED #25119

Merged
merged 5 commits into from
Oct 4, 2023
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion Tools/AP_Periph/AP_Periph.h
Original file line number Diff line number Diff line change
Expand Up @@ -475,10 +475,10 @@ class AP_Periph_FW {

#if AP_SIM_ENABLED
SITL::SIM sitl;
#endif
#if AP_AHRS_ENABLED
AP_AHRS ahrs;
#endif
#endif
};

#ifndef CAN_APP_NODE_NAME
Expand Down
8 changes: 5 additions & 3 deletions libraries/AP_AHRS/AP_AHRS.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -831,7 +831,7 @@ bool AP_AHRS::airspeed_sensor_enabled(void) const
// if we have an estimate
bool AP_AHRS::_airspeed_estimate(float &airspeed_ret) const
{
#if AP_AIRSPEED_ENABLED
#if AP_AIRSPEED_ENABLED && AP_GPS_ENABLED
if (airspeed_sensor_enabled()) {
uint8_t idx = get_active_airspeed_index();
airspeed_ret = AP::airspeed()->get_airspeed(idx);
Expand Down Expand Up @@ -1743,13 +1743,15 @@ void AP_AHRS::get_relative_position_D_home(float &posD) const
float originD;
if (!get_relative_position_D_origin(originD) ||
!_get_origin(originLLH)) {
#if AP_GPS_ENABLED
const auto &gps = AP::gps();
if (_gps_use == GPSUse::EnableWithHeight &&
gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
posD = (get_home().alt - gps.location().alt) * 0.01;
} else {
posD = -AP::baro().get_altitude();
return;
}
#endif
posD = -AP::baro().get_altitude();
return;
}

Expand Down
20 changes: 20 additions & 0 deletions libraries/AP_Common/Location.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@ Location::Location(int32_t latitude, int32_t longitude, int32_t alt_in_cm, AltFr
set_alt_cm(alt_in_cm, frame);
}

#if AP_AHRS_ENABLED
Location::Location(const Vector3f &ekf_offset_neu, AltFrame frame)
{
zero();
Expand All @@ -48,6 +49,7 @@ Location::Location(const Vector3f &ekf_offset_neu, AltFrame frame)
offset(ekf_offset_neu.x * 0.01, ekf_offset_neu.y * 0.01);
}
}
#endif // AP_AHRS_ENABLED

void Location::set_alt_cm(int32_t alt_cm, AltFrame frame)
{
Expand Down Expand Up @@ -141,12 +143,17 @@ bool Location::get_alt_cm(AltFrame desired_frame, int32_t &ret_alt_cm) const
alt_abs = alt;
break;
case AltFrame::ABOVE_HOME:
#if AP_AHRS_ENABLED
if (!AP::ahrs().home_is_set()) {
return false;
}
alt_abs = alt + AP::ahrs().get_home().alt;
#else
return false;
#endif // AP_AHRS_ENABLED
break;
case AltFrame::ABOVE_ORIGIN:
#if AP_AHRS_ENABLED
{
// fail if we cannot get ekf origin
Location ekf_origin;
Expand All @@ -156,6 +163,9 @@ bool Location::get_alt_cm(AltFrame desired_frame, int32_t &ret_alt_cm) const
alt_abs = alt + ekf_origin.alt;
}
break;
#else
return false;
#endif // AP_AHRS_ENABLED
case AltFrame::ABOVE_TERRAIN:
alt_abs = alt + alt_terr_cm;
break;
Expand All @@ -167,12 +177,17 @@ bool Location::get_alt_cm(AltFrame desired_frame, int32_t &ret_alt_cm) const
ret_alt_cm = alt_abs;
return true;
case AltFrame::ABOVE_HOME:
#if AP_AHRS_ENABLED
if (!AP::ahrs().home_is_set()) {
return false;
}
ret_alt_cm = alt_abs - AP::ahrs().get_home().alt;
#else
return false;
#endif // AP_AHRS_ENABLED
return true;
case AltFrame::ABOVE_ORIGIN:
#if AP_AHRS_ENABLED
{
// fail if we cannot get ekf origin
Location ekf_origin;
Expand All @@ -182,13 +197,17 @@ bool Location::get_alt_cm(AltFrame desired_frame, int32_t &ret_alt_cm) const
ret_alt_cm = alt_abs - ekf_origin.alt;
return true;
}
#else
return false;
#endif // AP_AHRS_ENABLED
case AltFrame::ABOVE_TERRAIN:
ret_alt_cm = alt_abs - alt_terr_cm;
return true;
}
return false; // LCOV_EXCL_LINE - not reachable
}

#if AP_AHRS_ENABLED
bool Location::get_vector_xy_from_origin_NE(Vector2f &vec_ne) const
{
Location ekf_origin;
Expand Down Expand Up @@ -216,6 +235,7 @@ bool Location::get_vector_from_origin_NEU(Vector3f &vec_neu) const

return true;
}
#endif // AP_AHRS_ENABLED

// return horizontal distance in meters between two locations
ftype Location::get_distance(const Location &loc2) const
Expand Down
1 change: 1 addition & 0 deletions libraries/AP_HAL_ChibiOS/hwdef/HerePro/hwdef.dat
Original file line number Diff line number Diff line change
Expand Up @@ -86,6 +86,7 @@ define GPS_MAX_INSTANCES 1
define HAL_COMPASS_MAX_SENSORS 1

# GPS+MAG
define HAL_PERIPH_ENABLE_AHRS
define HAL_PERIPH_ENABLE_GPS
define HAL_PERIPH_ENABLE_MAG
define AP_INERTIALSENSOR_ENABLED 1
Expand Down
4 changes: 4 additions & 0 deletions libraries/GCS_MAVLink/GCS_Common.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -2756,6 +2756,7 @@ void GCS_MAVLINK::send_named_float(const char *name, float value) const
mavlink_msg_named_value_float_send(chan, AP_HAL::millis(), float_name, value);
}

#if AP_AHRS_ENABLED
void GCS_MAVLINK::send_home_position() const
{
if (!AP::ahrs().home_is_set()) {
Expand Down Expand Up @@ -2801,6 +2802,7 @@ void GCS_MAVLINK::send_gps_global_origin() const
ekf_origin.alt * 10,
AP_HAL::micros64());
}
#endif // AP_AHRS_ENABLED

MAV_STATE GCS_MAVLINK::system_status() const
{
Expand Down Expand Up @@ -5652,6 +5654,7 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
send_global_position_int();
break;

#if AP_AHRS_ENABLED
case MSG_HOME:
CHECK_PAYLOAD_SIZE(HOME_POSITION);
send_home_position();
Expand All @@ -5661,6 +5664,7 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
CHECK_PAYLOAD_SIZE(GPS_GLOBAL_ORIGIN);
send_gps_global_origin();
break;
#endif // AP_AHRS_ENABLED

#if AP_RPM_ENABLED
case MSG_RPM:
Expand Down