diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index a061e32740605..d47c8617debb5 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -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 diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index b8855d5c7cc57..52a0c42e5a2bd 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -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); @@ -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; } diff --git a/libraries/AP_Common/Location.cpp b/libraries/AP_Common/Location.cpp index e96130943804d..398656c650be1 100644 --- a/libraries/AP_Common/Location.cpp +++ b/libraries/AP_Common/Location.cpp @@ -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(); @@ -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) { @@ -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; @@ -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; @@ -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; @@ -182,6 +197,9 @@ 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; @@ -189,6 +207,7 @@ bool Location::get_alt_cm(AltFrame desired_frame, int32_t &ret_alt_cm) const 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; @@ -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 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/HerePro/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/HerePro/hwdef.dat index fb4c2e82451cd..49c88fe3f9537 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/HerePro/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/HerePro/hwdef.dat @@ -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 diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 22868311852d7..afb498e0c9ea5 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -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()) { @@ -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 { @@ -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(); @@ -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: