system_state.hpp

/****************************************************************************
 *
 * Copyright 2023 Auterion AG. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this
 * list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its contributors
 * may be used to endorse or promote products derived from this software without
 * specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
 
#pragma once
 
#include <Eigen/Eigen>
#include <memory>
#include <optional>
 
namespace auterion {
enum class SubscriptionType {
    LOCAL_POSITION,
    GLOBAL_POSITION,
    ATTITUDE,
    ANGULAR_RATES,
    ODOMETRY,
    ARMED,
    SYSTEM_ID,
    HOME_POSITION,
    BATTERY,
    VTOL_STATE,
    MANUAL_INPUT,
    LAND_DETECTED,
    WIND,
    AIRSPEED,
    NAVIGATION_STATE,
    RC_INPUT
};
 
struct LocalPosition {
    Eigen::Vector3f position_enu_m;     
    Eigen::Vector3f velocity_enu_m;     
    Eigen::Vector3f acceleration_enu_m; 
    double heading;         
    double unaided_heading; 
    float dist_bottom;      
    bool horizontal_position_valid; 
    bool vertical_position_valid;   
    bool horizontal_velocity_valid; 
    bool vertical_velocity_valid;   
    bool dist_bottom_valid;         
};
 
struct GlobalPosition {
    double latitude_deg;    
    double longitude_deg;   
    double altitude_amsl_m; 
    inline Eigen::Vector3d toEigenVector() const {
        return Eigen::Vector3d{latitude_deg, longitude_deg, altitude_amsl_m};
    }
};
 
struct Odometry {
    Eigen::Quaternionf attitude;      
    Eigen::Vector3f angular_velocity; 
    Eigen::Vector3f position;         
    Eigen::Vector3f velocity;         
};
 
struct AngularRates {
    Eigen::Vector3f angular_velocity_flu_rad_s; 
    Eigen::Vector3f angular_acceleration_flu_rad_s; 
};
 
struct HomePosition {
    Eigen::Vector3f local_position_enu_m; 
    GlobalPosition global_position;       
    double yaw;                           
    bool altitude_valid;        
    bool global_position_valid; 
    bool local_position_valid;  
    bool was_set_manually; 
};
 
struct BatteryStatus {
    float voltage_v; 
    float current_a; 
    float discharged_mah; 
    float remaining;      
    int cell_count;                 
    Eigen::VectorXf cell_voltage_v; 
};
 
enum VtolState {
    UNDEFINED,
    TRANSITION_TO_FIXED_WING,
    TRANSITION_TO_MULTICOPTER,
    MULTICOPTER,
    FIXED_WING
};
 
struct ManualInput {
    float roll;     
    float pitch;    
    float yaw;      
    float throttle; 
    Eigen::Matrix<float, 6, 1> aux; 
    bool sticks_moving; 
    bool valid; 
};
 
struct NavigationState {
    uint8_t nav_state;
    bool external_mode; 
};
 
struct RcInput {
    bool failsafe; 
    bool lost;     
};
 
struct LandDetected {
    bool landed;     
    bool in_descend; 
};
 
struct Wind {
    float windspeed_north;               
    float windspeed_east;                
    std::optional<float> variance_north; 
    std::optional<float> variance_east;  
};
 
struct Airspeed {
    std::optional<float>
        indicated_airspeed_m_s; 
    std::optional<float> calibrated_airspeed_m_s; 
    std::optional<float> true_airspeed_m_s;       
};
 
class SDK;
 
template <SubscriptionType S, typename DataType>
class SubscriptionImpl;
 
class SystemStateImpl;
 
template <SubscriptionType S, typename DataType>
class Subscription {
   public:
    Subscription(const std::shared_ptr<SubscriptionImpl<S, DataType>>& impl);
 
    void onUpdate(std::function<void(DataType)> callback);
 
    void subscribe(std::function<void(DataType)> callback);
 
    DataType last() const;
 
    bool isLastValid() const;
 
   private:
    std::shared_ptr<SubscriptionImpl<S, DataType>> _impl;
};
 
class SystemState {
   public:
    SystemState(SDK& sdk);
    ~SystemState() = default;
 
    Subscription<SubscriptionType::LOCAL_POSITION, LocalPosition>& localPosition() const;
    Subscription<SubscriptionType::GLOBAL_POSITION, GlobalPosition>& globalPosition() const;
    Subscription<SubscriptionType::ATTITUDE, Eigen::Quaternionf>& attitude() const;
    Subscription<SubscriptionType::ODOMETRY, Odometry>& odometry() const;
    Subscription<SubscriptionType::ANGULAR_RATES, AngularRates>& angularRates() const;
    Subscription<SubscriptionType::ARMED, bool>& armed() const;
    Subscription<SubscriptionType::SYSTEM_ID, uint8_t>& systemId() const;
    Subscription<SubscriptionType::HOME_POSITION, HomePosition>& homePosition() const;
    Subscription<SubscriptionType::BATTERY, BatteryStatus>& battery() const;
    Subscription<SubscriptionType::VTOL_STATE, VtolState>& vtolState() const;
    Subscription<SubscriptionType::MANUAL_INPUT, ManualInput>& manualInput() const;
    Subscription<SubscriptionType::LAND_DETECTED, LandDetected>& landDetected() const;
    Subscription<SubscriptionType::WIND, Wind>& wind() const;
    Subscription<SubscriptionType::AIRSPEED, Airspeed>& airspeed() const;
    Subscription<SubscriptionType::NAVIGATION_STATE, NavigationState>& navigationState() const;
    Subscription<SubscriptionType::RC_INPUT, RcInput>& rcInput() const;
 
    SystemState& subscribeLocalPosition(std::function<void(LocalPosition)> callback = nullptr) {
        localPosition().subscribe(callback);
        return *this;
    }
 
    SystemState& subscribeGlobalPosition(std::function<void(GlobalPosition)> callback = nullptr) {
        globalPosition().subscribe(callback);
        return *this;
    }
 
    SystemState& subscribeAttitude(std::function<void(Eigen::Quaternionf)> callback = nullptr) {
        attitude().subscribe(callback);
        return *this;
    }
 
    SystemState& subscribeOdometry(std::function<void(Odometry)> callback = nullptr) {
        odometry().subscribe(callback);
        return *this;
    }
 
    SystemState& subscribeAngularRates(std::function<void(AngularRates)> callback = nullptr) {
        angularRates().subscribe(callback);
        return *this;
    }
 
    SystemState& subscribeArmed(std::function<void(bool)> callback = nullptr) {
        armed().subscribe(callback);
        return *this;
    }
 
    SystemState& subscribeSystemId(std::function<void(uint8_t)> callback = nullptr) {
        systemId().subscribe(callback);
        return *this;
    }
 
    SystemState& subscribeHomePosition(std::function<void(HomePosition)> callback = nullptr) {
        homePosition().subscribe(callback);
        return *this;
    }
 
    SystemState& subscribeBattery(std::function<void(BatteryStatus)> callback = nullptr) {
        battery().subscribe(callback);
        return *this;
    }
 
    SystemState& subscribeVtolState(std::function<void(VtolState)> callback = nullptr) {
        vtolState().subscribe(callback);
        return *this;
    }
 
    SystemState& subscribeManualInput(std::function<void(ManualInput)> callback = nullptr) {
        manualInput().subscribe(callback);
        return *this;
    }
 
    SystemState& subscribeLandDetected(std::function<void(LandDetected)> callback = nullptr) {
        landDetected().subscribe(callback);
        return *this;
    }
 
    SystemState& subscribeWind(std::function<void(Wind)> callback = nullptr) {
        wind().subscribe(callback);
        return *this;
    }
 
    SystemState& subscribeAirspeed(std::function<void(Airspeed)> callback = nullptr) {
        airspeed().subscribe(callback);
        return *this;
    }
 
    SystemState& subscribeNavigationState(std::function<void(NavigationState)> callback = nullptr) {
        navigationState().subscribe(callback);
        return *this;
    }
 
    SystemState& subscribeRcInput(std::function<void(RcInput)> callback = nullptr) {
        rcInput().subscribe(callback);
        return *this;
    }
 
   private:
    std::shared_ptr<SystemStateImpl> _impl;
};
 
}  // namespace auterion