<commit_before>//-- inludes -----
#include "AppStage_ControllerSettings.h"
#include "AppStage_AccelerometerCalibration.h"
#include "AppStage_GyroscopeCalibration.h"
#include "AppStage_MagnetometerCalibration.h"
#include "AppStage_MainMenu.h"
#include "AppStage_PairController.h"
#include "AppStage_TestRumble.h"
#include "App.h"
#include "Camera.h"
#include "MathUtility.h"
#include "Renderer.h"
#include "UIConstants.h"
#include "PSMoveProtocolInterface.h"
#include "PSMoveProtocol.pb.h"

#include "SDL_keycode.h"

#include <glm/gtc/matrix_transform.hpp>
#include <imgui.h>
#include <sstream>

#ifdef _MSC_VER
#pragma warning (disable: 4996) // 'This function or variable may be unsafe': snprintf
#define snprintf _snprintf
#endif

//-- statics ----
const char *AppStage_ControllerSettings::APP_STAGE_NAME= "ControllerSettings";

//-- constants -----

//-- public methods -----
AppStage_ControllerSettings::AppStage_ControllerSettings(App *app)
    : AppStage(app)
    , m_menuState(AppStage_ControllerSettings::inactive)
    , m_selectedControllerIndex(-1)
{ }

void AppStage_ControllerSettings::enter()
{
    m_app->setCameraType(_cameraFixed);
    m_selectedControllerIndex= -1;

    request_controller_list();
}

void AppStage_ControllerSettings::exit()
{
    m_menuState= AppStage_ControllerSettings::inactive;
}

void AppStage_ControllerSettings::update()
{
}

void AppStage_ControllerSettings::render()
{
    glm::mat4 scale2RotateX90=
        glm::rotate(
            glm::scale(glm::mat4(1.f), glm::vec3(2.f, 2.f, 2.f)),
            90.f, glm::vec3(1.f, 0.f, 0.f));

    switch (m_menuState)
    {
    case eControllerMenuState::idle:
        {
            if (m_selectedControllerIndex >= 0)
            {
                const ControllerInfo &controllerInfo= m_bluetoothControllerInfos[m_selectedControllerIndex];

                switch(controllerInfo.ControllerType)
                {
                    case ClientControllerView::eControllerType::PSMove:
                    case ClientControllerView::eControllerType::PSDualShock4:
                        {
                            const ControllerInfo &controllerInfo = m_bluetoothControllerInfos[m_selectedControllerIndex];

                            // Display the tracking color being used for the controller
                            glm::vec3 bulb_color = glm::vec3(1.f, 1.f, 1.f);

                            switch (controllerInfo.TrackingColorType)
                            {
                            case PSMoveTrackingColorType::Magenta:
                                bulb_color = glm::vec3(1.f, 0.f, 1.f);
                                break;
                            case PSMoveTrackingColorType::Cyan:
                                bulb_color = glm::vec3(0.f, 1.f, 1.f);
                                break;
                            case PSMoveTrackingColorType::Yellow:
                                bulb_color = glm::vec3(1.f, 1.f, 0.f);
                                break;
                            case PSMoveTrackingColorType::Red:
                                bulb_color = glm::vec3(1.f, 0.f, 0.f);
                                break;
                            case PSMoveTrackingColorType::Green:
                                bulb_color = glm::vec3(0.f, 1.f, 0.f);
                                break;
                            case PSMoveTrackingColorType::Blue:
                                bulb_color = glm::vec3(0.f, 0.f, 1.f);
                                break;
                            default:
                                break;
                            }

                            if (controllerInfo.ControllerType == ClientControllerView::PSMove)
                            {
                                drawPSMoveModel(scale2RotateX90, bulb_color);
                            }
                            else
                            {
                                drawPSDualShock4Model(scale2RotateX90, bulb_color);
                            }
                        } break;
                    case ClientControllerView::eControllerType::PSNavi:
                        {
                            drawPSNaviModel(scale2RotateX90);
                        } break;
                    default:
                        assert(0 && "Unreachable");
                }
            }
        } break;

    case eControllerMenuState::pendingControllerListRequest:
    case eControllerMenuState::failedControllerListRequest:
        {
        } break;

    default:
        assert(0 && "unreachable");
    }
}

void AppStage_ControllerSettings::renderUI()
{
    const char *k_window_title= "Controller Settings";
    const ImGuiWindowFlags window_flags =
        ImGuiWindowFlags_ShowBorders |
        ImGuiWindowFlags_NoResize |
        ImGuiWindowFlags_NoMove |
        ImGuiWindowFlags_NoScrollbar |
        ImGuiWindowFlags_NoCollapse;

    switch (m_menuState)
    {
    case eControllerMenuState::idle:
        {
            ImGui::SetNextWindowPosCenter();
            ImGui::SetNextWindowSize(ImVec2(350, 350));
            ImGui::Begin(k_window_title, nullptr, window_flags);

            if (m_hostSerial.length() > 1 && m_hostSerial != "00:00:00:00:00:00")
            {
                ImGui::Text("Host Serial: %s", m_hostSerial.c_str());
            }
            else
            {
                ImGui::Text("No bluetooth adapter detected!");
            }

            ImGui::Separator();

            if (m_bluetoothControllerInfos.size() > 0)
            {
                const ControllerInfo &controllerInfo= m_bluetoothControllerInfos[m_selectedControllerIndex];

                if (m_selectedControllerIndex > 0)
                {
                    if (ImGui::Button("<##ControllerIndex"))
                    {
                        --m_selectedControllerIndex;
                    }
                    ImGui::SameLine();
                }
                ImGui::Text("Controller: %d", m_selectedControllerIndex);
                if (m_selectedControllerIndex + 1 < static_cast<int>(m_bluetoothControllerInfos.size()))
                {
                    ImGui::SameLine();
                    if (ImGui::Button(">##ControllerIndex"))
                    {
                        ++m_selectedControllerIndex;
                    }
                }

                ImGui::BulletText("Controller ID: %d", controllerInfo.ControllerID);

                // Display the tracking color being used for the controller
                {
                    const char *color_string = "UNKNOWN";

                    switch (controllerInfo.TrackingColorType)
                    {
                    case PSMoveTrackingColorType::Magenta:
                        color_string = "Magenta";
                        break;
                    case PSMoveTrackingColorType::Cyan:
                        color_string = "Cyan";
                        break;
                    case PSMoveTrackingColorType::Yellow:
                        color_string = "Yellow";
                        break;
                    case PSMoveTrackingColorType::Red:
                        color_string = "Red";
                        break;
                    case PSMoveTrackingColorType::Green:
                        color_string = "Green";
                        break;
                    case PSMoveTrackingColorType::Blue:
                        color_string = "Blue";
                        break;
                    default:
                        break;
                    }

                    ImGui::BulletText("Tracking Color: %s", color_string);
                }

                switch(controllerInfo.ControllerType)
                {
                    case ClientControllerView::eControllerType::PSMove:
                        {
                            ImGui::BulletText("Controller Type: PSMove");
                        } break;
                    case ClientControllerView::eControllerType::PSNavi:
                        {
                            ImGui::BulletText("Controller Type: PSNavi");
                        } break;
                    case ClientControllerView::eControllerType::PSDualShock4:
                        {
                            ImGui::BulletText("Controller Type: PSDualShock4");
                        } break;
                    default:
                        assert(0 && "Unreachable");
                }

                ImGui::BulletText("Device Serial: %s", controllerInfo.DeviceSerial.c_str());
                ImGui::BulletText("Assigned Host Serial: %s", controllerInfo.AssignedHostSerial.c_str());
                ImGui::BulletText("Device Path:");
                ImGui::SameLine();
                ImGui::TextWrapped("%s", controllerInfo.DevicePath.c_str());

                if (controllerInfo.ControllerType == ClientControllerView::eControllerType::PSMove)
                {
                    if (ImGui::Button("Calibrate Magnetometer"))
                    {
                        m_app->getAppStage<AppStage_MagnetometerCalibration>()->setBypassCalibrationFlag(false);
                        m_app->setAppStage(AppStage_MagnetometerCalibration::APP_STAGE_NAME);
                    }

                    if (ImGui::Button("Test Orientation"))
                    {
                        m_app->getAppStage<AppStage_MagnetometerCalibration>()->setBypassCalibrationFlag(true);
                        m_app->setAppStage(AppStage_MagnetometerCalibration::APP_STAGE_NAME);
                    }
                }

                if (controllerInfo.ControllerType == ClientControllerView::eControllerType::PSDualShock4)
                {
                    if (ImGui::Button("Calibrate Accelerometer"))
                    {
                        m_app->getAppStage<AppStage_AccelerometerCalibration>()->setBypassCalibrationFlag(false);
                        m_app->setAppStage(AppStage_AccelerometerCalibration::APP_STAGE_NAME);
                    }

                    if (ImGui::Button("Test Accelerometer"))
                    {
                        m_app->getAppStage<AppStage_AccelerometerCalibration>()->setBypassCalibrationFlag(true);
                        m_app->setAppStage(AppStage_AccelerometerCalibration::APP_STAGE_NAME);
                    }

                    if (ImGui::Button("Calibrate Gyroscope"))
                    {
                        m_app->getAppStage<AppStage_GyroscopeCalibration>()->setBypassCalibrationFlag(false);
                        m_app->setAppStage(AppStage_GyroscopeCalibration::APP_STAGE_NAME);
                    }

                    if (ImGui::Button("Test Orientation"))
                    {
                        m_app->getAppStage<AppStage_GyroscopeCalibration>()->setBypassCalibrationFlag(true);
                        m_app->setAppStage(AppStage_GyroscopeCalibration::APP_STAGE_NAME);
                    }
                }

                if (controllerInfo.ControllerType == ClientControllerView::eControllerType::PSMove ||
                    controllerInfo.ControllerType == ClientControllerView::eControllerType::PSDualShock4)
                {
                    if (ImGui::Button("Test Rumble"))
                    {
                        m_app->setAppStage(AppStage_TestRumble::APP_STAGE_NAME);
                    }
                }
            }
            else
            {
                ImGui::Text("No Bluetooth connected controllers");
            }

            ImGui::Separator();

            // If there are any controllers waiting to be paired,
            // just present the first one as an option
            if (m_usbControllerInfos.size() > 0)
            {
                // Only consider the first controller connected via usb
                ControllerInfo &controllerInfo = m_usbControllerInfos[0];

                // We can only unpair controllers connected via usb
                if (controllerInfo.PairedToHost)
                {
                    if (ImGui::Button("Unpair USB Controller"))
                    {
                        m_app->getAppStage<AppStage_PairController>()->request_controller_unpair(controllerInfo.ControllerID);
                        m_app->setAppStage(AppStage_PairController::APP_STAGE_NAME);
                    }
                }
                else
                {
                    if (ImGui::Button("Pair USB Controller"))
                    {
                        m_app->getAppStage<AppStage_PairController>()->request_controller_pair(controllerInfo.ControllerID);
                        m_app->setAppStage(AppStage_PairController::APP_STAGE_NAME);
                    }
                }
            }
            else
            {
                ImGui::Text("No USB connected controllers");
            }

            ImGui::Separator();

            if (ImGui::Button("Return to Main Menu"))
            {
                m_app->setAppStage(AppStage_MainMenu::APP_STAGE_NAME);
            }

            ImGui::End();
        } break;
    case eControllerMenuState::pendingControllerListRequest:
        {
            ImGui::SetNextWindowPosCenter();
            ImGui::SetNextWindowSize(ImVec2(300, 150));
            ImGui::Begin(k_window_title, nullptr, window_flags);

            ImGui::Text("Waiting for controller list response...");

            ImGui::End();
        } break;
    case eControllerMenuState::failedControllerListRequest:
        {
            ImGui::SetNextWindowPosCenter();
            ImGui::SetNextWindowSize(ImVec2(300, 150));
            ImGui::Begin(k_window_title, nullptr, window_flags);

            ImGui::Text("Failed to get controller list!");

            if (ImGui::Button("Retry"))
            {
                request_controller_list();
            }

            if (ImGui::Button("Return to Main Menu"))
            {
                m_app->setAppStage(AppStage_MainMenu::APP_STAGE_NAME);
            }

            ImGui::End();
        } break;

    default:
        assert(0 && "unreachable");
    }
}

bool AppStage_ControllerSettings::onClientAPIEvent(
    ClientPSMoveAPI::eEventType event,
    ClientPSMoveAPI::t_event_data_handle opaque_event_handle)
{
    bool bHandled= false;

    switch(event)
    {
    case ClientPSMoveAPI::controllerListUpdated:
        {
            bHandled= true;
            request_controller_list();
        } break;
    }

    return bHandled;
}

void AppStage_ControllerSettings::request_controller_list()
{
    if (m_menuState != AppStage_ControllerSettings::pendingControllerListRequest)
    {
        m_menuState= AppStage_ControllerSettings::pendingControllerListRequest;
        m_selectedControllerIndex= -1;
        m_bluetoothControllerInfos.clear();
        m_usbControllerInfos.clear();

        // Tell the psmove service that we we want a list of controllers connected to this machine
        RequestPtr request(new PSMoveProtocol::Request());
        request->set_type(PSMoveProtocol::Request_RequestType_GET_CONTROLLER_LIST);

        // Do get controllers connected bia USB in this menu since we need the info for pairing/unpairing
        request->mutable_request_get_controller_list()->set_include_usb_controllers(true);

        ClientPSMoveAPI::register_callback(
            ClientPSMoveAPI::send_opaque_request(&request),
            AppStage_ControllerSettings::handle_controller_list_response, this);
    }
}

void AppStage_ControllerSettings::handle_controller_list_response(
    const ClientPSMoveAPI::ResponseMessage *response_message,
    void *userdata)
{
    AppStage_ControllerSettings *thisPtr= static_cast<AppStage_ControllerSettings *>(userdata);

    const ClientPSMoveAPI::eClientPSMoveResultCode ResultCode = response_message->result_code;
    const ClientPSMoveAPI::t_response_handle response_handle = response_message->opaque_response_handle;

    switch(ResultCode)
    {
        case ClientPSMoveAPI::_clientPSMoveResultCode_ok:
        {
            const PSMoveProtocol::Response *response= GET_PSMOVEPROTOCOL_RESPONSE(response_handle);

            thisPtr->m_hostSerial = response->result_controller_list().host_serial();

            for (int controller_index= 0; controller_index < response->result_controller_list().controllers_size(); ++controller_index)
            {
                const auto &ControllerResponse= response->result_controller_list().controllers(controller_index);

                AppStage_ControllerSettings::ControllerInfo ControllerInfo;

                ControllerInfo.ControllerID= ControllerResponse.controller_id();

                switch(ControllerResponse.controller_type())
                {
                case PSMoveProtocol::PSMOVE:
                    ControllerInfo.ControllerType = ClientControllerView::eControllerType::PSMove;
                    break;
                case PSMoveProtocol::PSNAVI:
                    ControllerInfo.ControllerType = ClientControllerView::eControllerType::PSNavi;
                    break;
                case PSMoveProtocol::PSDUALSHOCK4:
                    ControllerInfo.ControllerType = ClientControllerView::eControllerType::PSDualShock4;
                    break;
                default:
                    assert(0 && "unreachable");
                }

                ControllerInfo.TrackingColorType =
                    static_cast<PSMoveTrackingColorType>(ControllerResponse.tracking_color_type());
                ControllerInfo.DevicePath= ControllerResponse.device_path();
                ControllerInfo.DeviceSerial= ControllerResponse.device_serial();
                ControllerInfo.AssignedHostSerial= ControllerResponse.assigned_host_serial();
                ControllerInfo.PairedToHost=
                    ControllerResponse.assigned_host_serial().length() > 0 &&
                    ControllerResponse.assigned_host_serial() == thisPtr->m_hostSerial;

                // Add the controller to the appropriate connection list
                switch (ControllerResponse.connection_type())
                {
                case PSMoveProtocol::Response_ResultControllerList_ControllerInfo_ConnectionType_BLUETOOTH:
                    thisPtr->m_bluetoothControllerInfos.push_back(ControllerInfo);
                    break;
                case PSMoveProtocol::Response_ResultControllerList_ControllerInfo_ConnectionType_USB:
                    thisPtr->m_usbControllerInfos.push_back(ControllerInfo);
                    break;
                default:
                    assert(0 && "unreachable");
                }
            }

            thisPtr->m_selectedControllerIndex= (thisPtr->m_bluetoothControllerInfos.size() > 0) ? 0 : -1;
            thisPtr->m_menuState= AppStage_ControllerSettings::idle;
        } break;

        case ClientPSMoveAPI::_clientPSMoveResultCode_error:
        case ClientPSMoveAPI::_clientPSMoveResultCode_canceled:
        {
            thisPtr->m_menuState= AppStage_ControllerSettings::failedControllerListRequest;
        } break;
    }
}<commit_msg>Made controller settings panel larger to accommodate the new gyroscope calibration buttons.<commit_after>//-- inludes -----
#include "AppStage_ControllerSettings.h"
#include "AppStage_AccelerometerCalibration.h"
#include "AppStage_GyroscopeCalibration.h"
#include "AppStage_MagnetometerCalibration.h"
#include "AppStage_MainMenu.h"
#include "AppStage_PairController.h"
#include "AppStage_TestRumble.h"
#include "App.h"
#include "Camera.h"
#include "MathUtility.h"
#include "Renderer.h"
#include "UIConstants.h"
#include "PSMoveProtocolInterface.h"
#include "PSMoveProtocol.pb.h"

#include "SDL_keycode.h"

#include <glm/gtc/matrix_transform.hpp>
#include <imgui.h>
#include <sstream>

#ifdef _MSC_VER
#pragma warning (disable: 4996) // 'This function or variable may be unsafe': snprintf
#define snprintf _snprintf
#endif

//-- statics ----
const char *AppStage_ControllerSettings::APP_STAGE_NAME= "ControllerSettings";

//-- constants -----

//-- public methods -----
AppStage_ControllerSettings::AppStage_ControllerSettings(App *app)
    : AppStage(app)
    , m_menuState(AppStage_ControllerSettings::inactive)
    , m_selectedControllerIndex(-1)
{ }

void AppStage_ControllerSettings::enter()
{
    m_app->setCameraType(_cameraFixed);
    m_selectedControllerIndex= -1;

    request_controller_list();
}

void AppStage_ControllerSettings::exit()
{
    m_menuState= AppStage_ControllerSettings::inactive;
}

void AppStage_ControllerSettings::update()
{
}

void AppStage_ControllerSettings::render()
{
    glm::mat4 scale2RotateX90=
        glm::rotate(
            glm::scale(glm::mat4(1.f), glm::vec3(2.f, 2.f, 2.f)),
            90.f, glm::vec3(1.f, 0.f, 0.f));

    switch (m_menuState)
    {
    case eControllerMenuState::idle:
        {
            if (m_selectedControllerIndex >= 0)
            {
                const ControllerInfo &controllerInfo= m_bluetoothControllerInfos[m_selectedControllerIndex];

                switch(controllerInfo.ControllerType)
                {
                    case ClientControllerView::eControllerType::PSMove:
                    case ClientControllerView::eControllerType::PSDualShock4:
                        {
                            const ControllerInfo &controllerInfo = m_bluetoothControllerInfos[m_selectedControllerIndex];

                            // Display the tracking color being used for the controller
                            glm::vec3 bulb_color = glm::vec3(1.f, 1.f, 1.f);

                            switch (controllerInfo.TrackingColorType)
                            {
                            case PSMoveTrackingColorType::Magenta:
                                bulb_color = glm::vec3(1.f, 0.f, 1.f);
                                break;
                            case PSMoveTrackingColorType::Cyan:
                                bulb_color = glm::vec3(0.f, 1.f, 1.f);
                                break;
                            case PSMoveTrackingColorType::Yellow:
                                bulb_color = glm::vec3(1.f, 1.f, 0.f);
                                break;
                            case PSMoveTrackingColorType::Red:
                                bulb_color = glm::vec3(1.f, 0.f, 0.f);
                                break;
                            case PSMoveTrackingColorType::Green:
                                bulb_color = glm::vec3(0.f, 1.f, 0.f);
                                break;
                            case PSMoveTrackingColorType::Blue:
                                bulb_color = glm::vec3(0.f, 0.f, 1.f);
                                break;
                            default:
                                break;
                            }

                            if (controllerInfo.ControllerType == ClientControllerView::PSMove)
                            {
                                drawPSMoveModel(scale2RotateX90, bulb_color);
                            }
                            else
                            {
                                drawPSDualShock4Model(scale2RotateX90, bulb_color);
                            }
                        } break;
                    case ClientControllerView::eControllerType::PSNavi:
                        {
                            drawPSNaviModel(scale2RotateX90);
                        } break;
                    default:
                        assert(0 && "Unreachable");
                }
            }
        } break;

    case eControllerMenuState::pendingControllerListRequest:
    case eControllerMenuState::failedControllerListRequest:
        {
        } break;

    default:
        assert(0 && "unreachable");
    }
}

void AppStage_ControllerSettings::renderUI()
{
    const char *k_window_title= "Controller Settings";
    const ImGuiWindowFlags window_flags =
        ImGuiWindowFlags_ShowBorders |
        ImGuiWindowFlags_NoResize |
        ImGuiWindowFlags_NoMove |
        ImGuiWindowFlags_NoScrollbar |
        ImGuiWindowFlags_NoCollapse;

    switch (m_menuState)
    {
    case eControllerMenuState::idle:
        {
            ImGui::SetNextWindowPosCenter();
            ImGui::SetNextWindowSize(ImVec2(350, 400));
            ImGui::Begin(k_window_title, nullptr, window_flags);

            if (m_hostSerial.length() > 1 && m_hostSerial != "00:00:00:00:00:00")
            {
                ImGui::Text("Host Serial: %s", m_hostSerial.c_str());
            }
            else
            {
                ImGui::Text("No bluetooth adapter detected!");
            }

            ImGui::Separator();

            if (m_bluetoothControllerInfos.size() > 0)
            {
                const ControllerInfo &controllerInfo= m_bluetoothControllerInfos[m_selectedControllerIndex];

                if (m_selectedControllerIndex > 0)
                {
                    if (ImGui::Button("<##ControllerIndex"))
                    {
                        --m_selectedControllerIndex;
                    }
                    ImGui::SameLine();
                }
                ImGui::Text("Controller: %d", m_selectedControllerIndex);
                if (m_selectedControllerIndex + 1 < static_cast<int>(m_bluetoothControllerInfos.size()))
                {
                    ImGui::SameLine();
                    if (ImGui::Button(">##ControllerIndex"))
                    {
                        ++m_selectedControllerIndex;
                    }
                }

                ImGui::BulletText("Controller ID: %d", controllerInfo.ControllerID);

                // Display the tracking color being used for the controller
                {
                    const char *color_string = "UNKNOWN";

                    switch (controllerInfo.TrackingColorType)
                    {
                    case PSMoveTrackingColorType::Magenta:
                        color_string = "Magenta";
                        break;
                    case PSMoveTrackingColorType::Cyan:
                        color_string = "Cyan";
                        break;
                    case PSMoveTrackingColorType::Yellow:
                        color_string = "Yellow";
                        break;
                    case PSMoveTrackingColorType::Red:
                        color_string = "Red";
                        break;
                    case PSMoveTrackingColorType::Green:
                        color_string = "Green";
                        break;
                    case PSMoveTrackingColorType::Blue:
                        color_string = "Blue";
                        break;
                    default:
                        break;
                    }

                    ImGui::BulletText("Tracking Color: %s", color_string);
                }

                switch(controllerInfo.ControllerType)
                {
                    case ClientControllerView::eControllerType::PSMove:
                        {
                            ImGui::BulletText("Controller Type: PSMove");
                        } break;
                    case ClientControllerView::eControllerType::PSNavi:
                        {
                            ImGui::BulletText("Controller Type: PSNavi");
                        } break;
                    case ClientControllerView::eControllerType::PSDualShock4:
                        {
                            ImGui::BulletText("Controller Type: PSDualShock4");
                        } break;
                    default:
                        assert(0 && "Unreachable");
                }

                ImGui::BulletText("Device Serial: %s", controllerInfo.DeviceSerial.c_str());
                ImGui::BulletText("Assigned Host Serial: %s", controllerInfo.AssignedHostSerial.c_str());
                ImGui::BulletText("Device Path:");
                ImGui::SameLine();
                ImGui::TextWrapped("%s", controllerInfo.DevicePath.c_str());

                if (controllerInfo.ControllerType == ClientControllerView::eControllerType::PSMove)
                {
                    if (ImGui::Button("Calibrate Magnetometer"))
                    {
                        m_app->getAppStage<AppStage_MagnetometerCalibration>()->setBypassCalibrationFlag(false);
                        m_app->setAppStage(AppStage_MagnetometerCalibration::APP_STAGE_NAME);
                    }

                    if (ImGui::Button("Test Orientation"))
                    {
                        m_app->getAppStage<AppStage_MagnetometerCalibration>()->setBypassCalibrationFlag(true);
                        m_app->setAppStage(AppStage_MagnetometerCalibration::APP_STAGE_NAME);
                    }
                }

                if (controllerInfo.ControllerType == ClientControllerView::eControllerType::PSDualShock4)
                {
                    if (ImGui::Button("Calibrate Accelerometer"))
                    {
                        m_app->getAppStage<AppStage_AccelerometerCalibration>()->setBypassCalibrationFlag(false);
                        m_app->setAppStage(AppStage_AccelerometerCalibration::APP_STAGE_NAME);
                    }

                    if (ImGui::Button("Test Accelerometer"))
                    {
                        m_app->getAppStage<AppStage_AccelerometerCalibration>()->setBypassCalibrationFlag(true);
                        m_app->setAppStage(AppStage_AccelerometerCalibration::APP_STAGE_NAME);
                    }

                    if (ImGui::Button("Calibrate Gyroscope"))
                    {
                        m_app->getAppStage<AppStage_GyroscopeCalibration>()->setBypassCalibrationFlag(false);
                        m_app->setAppStage(AppStage_GyroscopeCalibration::APP_STAGE_NAME);
                    }

                    if (ImGui::Button("Test Orientation"))
                    {
                        m_app->getAppStage<AppStage_GyroscopeCalibration>()->setBypassCalibrationFlag(true);
                        m_app->setAppStage(AppStage_GyroscopeCalibration::APP_STAGE_NAME);
                    }
                }

                if (controllerInfo.ControllerType == ClientControllerView::eControllerType::PSMove ||
                    controllerInfo.ControllerType == ClientControllerView::eControllerType::PSDualShock4)
                {
                    if (ImGui::Button("Test Rumble"))
                    {
                        m_app->setAppStage(AppStage_TestRumble::APP_STAGE_NAME);
                    }
                }
            }
            else
            {
                ImGui::Text("No Bluetooth connected controllers");
            }

            ImGui::Separator();

            // If there are any controllers waiting to be paired,
            // just present the first one as an option
            if (m_usbControllerInfos.size() > 0)
            {
                // Only consider the first controller connected via usb
                ControllerInfo &controllerInfo = m_usbControllerInfos[0];

                // We can only unpair controllers connected via usb
                if (controllerInfo.PairedToHost)
                {
                    if (ImGui::Button("Unpair USB Controller"))
                    {
                        m_app->getAppStage<AppStage_PairController>()->request_controller_unpair(controllerInfo.ControllerID);
                        m_app->setAppStage(AppStage_PairController::APP_STAGE_NAME);
                    }
                }
                else
                {
                    if (ImGui::Button("Pair USB Controller"))
                    {
                        m_app->getAppStage<AppStage_PairController>()->request_controller_pair(controllerInfo.ControllerID);
                        m_app->setAppStage(AppStage_PairController::APP_STAGE_NAME);
                    }
                }
            }
            else
            {
                ImGui::Text("No USB connected controllers");
            }

            ImGui::Separator();

            if (ImGui::Button("Return to Main Menu"))
            {
                m_app->setAppStage(AppStage_MainMenu::APP_STAGE_NAME);
            }

            ImGui::End();
        } break;
    case eControllerMenuState::pendingControllerListRequest:
        {
            ImGui::SetNextWindowPosCenter();
            ImGui::SetNextWindowSize(ImVec2(300, 150));
            ImGui::Begin(k_window_title, nullptr, window_flags);

            ImGui::Text("Waiting for controller list response...");

            ImGui::End();
        } break;
    case eControllerMenuState::failedControllerListRequest:
        {
            ImGui::SetNextWindowPosCenter();
            ImGui::SetNextWindowSize(ImVec2(300, 150));
            ImGui::Begin(k_window_title, nullptr, window_flags);

            ImGui::Text("Failed to get controller list!");

            if (ImGui::Button("Retry"))
            {
                request_controller_list();
            }

            if (ImGui::Button("Return to Main Menu"))
            {
                m_app->setAppStage(AppStage_MainMenu::APP_STAGE_NAME);
            }

            ImGui::End();
        } break;

    default:
        assert(0 && "unreachable");
    }
}

bool AppStage_ControllerSettings::onClientAPIEvent(
    ClientPSMoveAPI::eEventType event,
    ClientPSMoveAPI::t_event_data_handle opaque_event_handle)
{
    bool bHandled= false;

    switch(event)
    {
    case ClientPSMoveAPI::controllerListUpdated:
        {
            bHandled= true;
            request_controller_list();
        } break;
    }

    return bHandled;
}

void AppStage_ControllerSettings::request_controller_list()
{
    if (m_menuState != AppStage_ControllerSettings::pendingControllerListRequest)
    {
        m_menuState= AppStage_ControllerSettings::pendingControllerListRequest;
        m_selectedControllerIndex= -1;
        m_bluetoothControllerInfos.clear();
        m_usbControllerInfos.clear();

        // Tell the psmove service that we we want a list of controllers connected to this machine
        RequestPtr request(new PSMoveProtocol::Request());
        request->set_type(PSMoveProtocol::Request_RequestType_GET_CONTROLLER_LIST);

        // Do get controllers connected bia USB in this menu since we need the info for pairing/unpairing
        request->mutable_request_get_controller_list()->set_include_usb_controllers(true);

        ClientPSMoveAPI::register_callback(
            ClientPSMoveAPI::send_opaque_request(&request),
            AppStage_ControllerSettings::handle_controller_list_response, this);
    }
}

void AppStage_ControllerSettings::handle_controller_list_response(
    const ClientPSMoveAPI::ResponseMessage *response_message,
    void *userdata)
{
    AppStage_ControllerSettings *thisPtr= static_cast<AppStage_ControllerSettings *>(userdata);

    const ClientPSMoveAPI::eClientPSMoveResultCode ResultCode = response_message->result_code;
    const ClientPSMoveAPI::t_response_handle response_handle = response_message->opaque_response_handle;

    switch(ResultCode)
    {
        case ClientPSMoveAPI::_clientPSMoveResultCode_ok:
        {
            const PSMoveProtocol::Response *response= GET_PSMOVEPROTOCOL_RESPONSE(response_handle);

            thisPtr->m_hostSerial = response->result_controller_list().host_serial();

            for (int controller_index= 0; controller_index < response->result_controller_list().controllers_size(); ++controller_index)
            {
                const auto &ControllerResponse= response->result_controller_list().controllers(controller_index);

                AppStage_ControllerSettings::ControllerInfo ControllerInfo;

                ControllerInfo.ControllerID= ControllerResponse.controller_id();

                switch(ControllerResponse.controller_type())
                {
                case PSMoveProtocol::PSMOVE:
                    ControllerInfo.ControllerType = ClientControllerView::eControllerType::PSMove;
                    break;
                case PSMoveProtocol::PSNAVI:
                    ControllerInfo.ControllerType = ClientControllerView::eControllerType::PSNavi;
                    break;
                case PSMoveProtocol::PSDUALSHOCK4:
                    ControllerInfo.ControllerType = ClientControllerView::eControllerType::PSDualShock4;
                    break;
                default:
                    assert(0 && "unreachable");
                }

                ControllerInfo.TrackingColorType =
                    static_cast<PSMoveTrackingColorType>(ControllerResponse.tracking_color_type());
                ControllerInfo.DevicePath= ControllerResponse.device_path();
                ControllerInfo.DeviceSerial= ControllerResponse.device_serial();
                ControllerInfo.AssignedHostSerial= ControllerResponse.assigned_host_serial();
                ControllerInfo.PairedToHost=
                    ControllerResponse.assigned_host_serial().length() > 0 &&
                    ControllerResponse.assigned_host_serial() == thisPtr->m_hostSerial;

                // Add the controller to the appropriate connection list
                switch (ControllerResponse.connection_type())
                {
                case PSMoveProtocol::Response_ResultControllerList_ControllerInfo_ConnectionType_BLUETOOTH:
                    thisPtr->m_bluetoothControllerInfos.push_back(ControllerInfo);
                    break;
                case PSMoveProtocol::Response_ResultControllerList_ControllerInfo_ConnectionType_USB:
                    thisPtr->m_usbControllerInfos.push_back(ControllerInfo);
                    break;
                default:
                    assert(0 && "unreachable");
                }
            }

            thisPtr->m_selectedControllerIndex= (thisPtr->m_bluetoothControllerInfos.size() > 0) ? 0 : -1;
            thisPtr->m_menuState= AppStage_ControllerSettings::idle;
        } break;

        case ClientPSMoveAPI::_clientPSMoveResultCode_error:
        case ClientPSMoveAPI::_clientPSMoveResultCode_canceled:
        {
            thisPtr->m_menuState= AppStage_ControllerSettings::failedControllerListRequest;
        } break;
    }
}<|endoftext|>


<commit_before>/*
 * Copyright (C) 2015 ScyllaDB
 */

/*
 * This file is part of Scylla.
 *
 * Scylla is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Scylla is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Scylla.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <boost/test/unit_test.hpp>
#include "utils/fb_utilities.hh"
#include "locator/network_topology_strategy.hh"
#include "tests/test-utils.hh"
#include "core/sstring.hh"
#include "log.hh"
#include <vector>
#include <string>
#include <map>
#include <iostream>
#include <sstream>
#include <boost/range/algorithm/adjacent_find.hpp>

static logging::logger nlogger("NetworkTopologyStrategyLogger");

using namespace locator;

struct ring_point {
    double point;
    inet_address host;
};

void print_natural_endpoints(double point, const std::vector<inet_address> v) {
    nlogger.debug("Natural endpoints for a token {}:", point);
    std::string str;
    std::ostringstream strm(str);

    for (auto& addr : v) {
        strm<<addr<<" ";
    }

    nlogger.debug("{}", strm.str());
}

#ifndef SEASTAR_DEBUG
static void verify_sorted(const dht::token_range_vector& trv) {
    auto not_strictly_before = [] (const dht::token_range a, const dht::token_range b) {
        return !b.start()
                || !a.end()
                || a.end()->value() > b.start()->value()
                || (a.end()->value() == b.start()->value() && a.end()->is_inclusive() && b.start()->is_inclusive());
    };
    BOOST_CHECK(boost::adjacent_find(trv, not_strictly_before) == trv.end());
}
#endif

static void check_ranges_are_sorted(abstract_replication_strategy* ars, gms::inet_address ep) {
    // Too slow in debug mode
#ifndef SEASTAR_DEBUG
    verify_sorted(ars->get_ranges(ep));
    verify_sorted(ars->get_primary_ranges(ep));
    verify_sorted(ars->get_primary_ranges_within_dc(ep));
#endif
}

void strategy_sanity_check(
    abstract_replication_strategy* ars_ptr,
    const std::map<sstring, sstring>& options) {

    network_topology_strategy* nts_ptr =
        dynamic_cast<network_topology_strategy*>(ars_ptr);

    //
    // Check that both total and per-DC RFs in options match the corresponding
    // value in the strategy object.
    //
    size_t total_rf = 0;
    for (auto& val : options) {
        size_t rf = std::stol(val.second);
        BOOST_CHECK(nts_ptr->get_replication_factor(val.first) == rf);

        total_rf += rf;
    }

    BOOST_CHECK(ars_ptr->get_replication_factor() == total_rf);
}

void endpoints_check(
    abstract_replication_strategy* ars_ptr,
    std::vector<inet_address>& endpoints) {

    // Check the total RF
    BOOST_CHECK(endpoints.size() == ars_ptr->get_replication_factor());

    // Check the uniqueness
    std::unordered_set<inet_address> ep_set(endpoints.begin(), endpoints.end());
    BOOST_CHECK(endpoints.size() == ep_set.size());

    // Check the per-DC RF
    auto& snitch = i_endpoint_snitch::get_local_snitch_ptr();
    std::unordered_map<sstring, size_t> dc_rf;
    for (auto ep : endpoints) {
        sstring dc = snitch->get_datacenter(ep);

        auto rf = dc_rf.find(dc);
        if (rf == dc_rf.end()) {
            dc_rf[dc] = 1;
        } else {
            rf->second++;
        }
    }

    network_topology_strategy* nts_ptr =
        dynamic_cast<network_topology_strategy*>(ars_ptr);
    for (auto& rf : dc_rf) {
        BOOST_CHECK(rf.second == nts_ptr->get_replication_factor(rf.first));
    }
}

auto d2t = [](double d) {
    unsigned long l = net::hton(static_cast<unsigned long>(d*(std::numeric_limits<unsigned long>::max())));
    std::array<char, 8> a;
    memcpy(a.data(), &l, 8);
    return a;
};

/**
 * Check the get_natural_endpoints() output for tokens between every two
 * adjacent ring points.
 * @param ring_points ring description
 * @param options strategy options
 * @param ars_ptr strategy object
 */
void full_ring_check(const std::vector<ring_point>& ring_points,
                     const std::map<sstring, sstring>& options,
                     abstract_replication_strategy* ars_ptr) {
    strategy_sanity_check(ars_ptr, options);

    for (auto& rp : ring_points) {
        double cur_point1 = rp.point - 0.5;
        token t1{dht::token::kind::key,
             {(int8_t*)d2t(cur_point1 / ring_points.size()).data(), 8}};
        uint64_t cache_hit_count = ars_ptr->get_cache_hits_count();
        auto endpoints1 = ars_ptr->get_natural_endpoints(t1);

        endpoints_check(ars_ptr, endpoints1);
        // validate that the result hasn't been taken from the cache
        BOOST_CHECK(cache_hit_count == ars_ptr->get_cache_hits_count());

        print_natural_endpoints(cur_point1, endpoints1);

        //
        // Check a different endpoint in the same range as t1 and validate that
        // the endpoints has been taken from the cache and that the output is
        // identical to the one not taken from the cache.
        //
        cache_hit_count = ars_ptr->get_cache_hits_count();
        double cur_point2 = rp.point - 0.2;
        token t2{dht::token::kind::key,
             {(int8_t*)d2t(cur_point2 / ring_points.size()).data(), 8}};
        auto endpoints2 = ars_ptr->get_natural_endpoints(t2);

        endpoints_check(ars_ptr, endpoints2);
        check_ranges_are_sorted(ars_ptr, rp.host);
        BOOST_CHECK(cache_hit_count + 1 == ars_ptr->get_cache_hits_count());
        BOOST_CHECK(endpoints1 == endpoints2);
    }
}

future<> simple_test() {
    utils::fb_utilities::set_broadcast_address(gms::inet_address("localhost"));
    utils::fb_utilities::set_broadcast_rpc_address(gms::inet_address("localhost"));

    // Create the RackInferringSnitch
    return i_endpoint_snitch::create_snitch("RackInferringSnitch").then(
        [] {

        lw_shared_ptr<token_metadata> tm = make_lw_shared<token_metadata>();
        std::vector<ring_point> ring_points = {
            { 1.0,  inet_address("192.100.10.1") },
            { 2.0,  inet_address("192.101.10.1") },
            { 3.0,  inet_address("192.102.10.1") },
            { 4.0,  inet_address("192.100.20.1") },
            { 5.0,  inet_address("192.101.20.1") },
            { 6.0,  inet_address("192.102.20.1") },
            { 7.0,  inet_address("192.100.30.1") },
            { 8.0,  inet_address("192.101.30.1") },
            { 9.0,  inet_address("192.102.30.1") },
            { 10.0, inet_address("192.102.40.1") },
            { 11.0, inet_address("192.102.40.2") }
        };
        // Initialize the token_metadata
        for (unsigned i = 0; i < ring_points.size(); i++) {
            tm->update_normal_token(
                {dht::token::kind::key,
                 {(int8_t*)d2t(ring_points[i].point / ring_points.size()).data(), 8}
                },
                ring_points[i].host);
        }

        /////////////////////////////////////
        // Create the replication strategy
        std::map<sstring, sstring> options323 = {
            {"100", "3"},
            {"101", "2"},
            {"102", "3"}
        };

        auto ars_uptr = abstract_replication_strategy::create_replication_strategy(
            "test keyspace", "NetworkTopologyStrategy", *tm, options323);

        auto ars_ptr = ars_uptr.get();

        full_ring_check(ring_points, options323, ars_ptr);

        ///////////////
        // Create the replication strategy
        std::map<sstring, sstring> options320 = {
            {"100", "3"},
            {"101", "2"},
            {"102", "0"}
        };

        ars_uptr = abstract_replication_strategy::create_replication_strategy(
            "test keyspace", "NetworkTopologyStrategy", *tm, options320);

        ars_ptr = ars_uptr.get();

        full_ring_check(ring_points, options320, ars_ptr);

        //
        // Check cache invalidation: invalidate the cache and run a full ring
        // check once again. If cache is not properly invalidated one of the
        // points will be taken from the cache when it shouldn't and the
        // corresponding check will fail.
        //
        tm->invalidate_cached_rings();
        full_ring_check(ring_points, options320, ars_ptr);

        return i_endpoint_snitch::stop_snitch();
    });
}

future<> heavy_origin_test() {
    utils::fb_utilities::set_broadcast_address(gms::inet_address("localhost"));
    utils::fb_utilities::set_broadcast_rpc_address(gms::inet_address("localhost"));

    // Create the RackInferringSnitch
    return i_endpoint_snitch::create_snitch("RackInferringSnitch").then(
        [] {
        std::vector<int> dc_racks = {2, 4, 8};
        std::vector<int> dc_endpoints = {128, 256, 512};
        std::vector<int> dc_replication = {2, 6, 6};

        lw_shared_ptr<token_metadata> tm = make_lw_shared<token_metadata>();
        std::map<sstring, sstring> config_options;
        std::unordered_map<inet_address, std::unordered_set<token>> tokens;
        std::vector<ring_point> ring_points;

        size_t total_eps = 0;
        for (size_t dc = 0; dc < dc_racks.size(); ++dc) {
            for (int rack = 0; rack < dc_racks[dc]; ++rack) {
                total_eps += dc_endpoints[dc]/dc_racks[dc];
            }
        }

        int total_rf = 0;
        double token_point = 1.0;
        for (size_t dc = 0; dc < dc_racks.size(); ++dc) {
            total_rf += dc_replication[dc];
            config_options.emplace(to_sstring(dc),
                                   to_sstring(dc_replication[dc]));
            for (int rack = 0; rack < dc_racks[dc]; ++rack) {
                for (int ep = 1; ep <= dc_endpoints[dc]/dc_racks[dc]; ++ep) {

                    // 10.dc.rack.ep
                    int32_t ip = 0x0a000000 + ((int8_t)dc << 16) +
                                 ((int8_t)rack << 8) + (int8_t)ep;
                    inet_address address(ip);
                    ring_point rp = {token_point, address};

                    ring_points.emplace_back(rp);
                    tokens[address].emplace(token{dht::token::kind::key,
                            {(int8_t*)d2t(token_point / total_eps).data(), 8}});

                    nlogger.debug("adding node {} at {}", address, token_point);

                    token_point++;
                }
            }
        }

        tm->update_normal_tokens(tokens);

        auto ars_uptr = abstract_replication_strategy::create_replication_strategy(
            "test keyspace", "NetworkTopologyStrategy", *tm, config_options);

        auto ars_ptr = ars_uptr.get();

        full_ring_check(ring_points, config_options, ars_ptr);

        return i_endpoint_snitch::stop_snitch();
    });
}

SEASTAR_TEST_CASE(NetworkTopologyStrategy_simple) {
    return simple_test();
}

SEASTAR_TEST_CASE(NetworkTopologyStrategy_heavy) {
    return heavy_origin_test();
}
<commit_msg>tests: network_topology_strategy_test: remove quadratic complexity<commit_after>/*
 * Copyright (C) 2015 ScyllaDB
 */

/*
 * This file is part of Scylla.
 *
 * Scylla is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Scylla is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Scylla.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <boost/test/unit_test.hpp>
#include "utils/fb_utilities.hh"
#include "locator/network_topology_strategy.hh"
#include "tests/test-utils.hh"
#include "core/sstring.hh"
#include "log.hh"
#include <vector>
#include <string>
#include <map>
#include <iostream>
#include <sstream>
#include <boost/range/algorithm/adjacent_find.hpp>
#include <boost/algorithm/cxx11/iota.hpp>

static logging::logger nlogger("NetworkTopologyStrategyLogger");

using namespace locator;

struct ring_point {
    double point;
    inet_address host;
};

void print_natural_endpoints(double point, const std::vector<inet_address> v) {
    nlogger.debug("Natural endpoints for a token {}:", point);
    std::string str;
    std::ostringstream strm(str);

    for (auto& addr : v) {
        strm<<addr<<" ";
    }

    nlogger.debug("{}", strm.str());
}

#ifndef SEASTAR_DEBUG
static void verify_sorted(const dht::token_range_vector& trv) {
    auto not_strictly_before = [] (const dht::token_range a, const dht::token_range b) {
        return !b.start()
                || !a.end()
                || a.end()->value() > b.start()->value()
                || (a.end()->value() == b.start()->value() && a.end()->is_inclusive() && b.start()->is_inclusive());
    };
    BOOST_CHECK(boost::adjacent_find(trv, not_strictly_before) == trv.end());
}
#endif

static void check_ranges_are_sorted(abstract_replication_strategy* ars, gms::inet_address ep) {
    // Too slow in debug mode
#ifndef SEASTAR_DEBUG
    verify_sorted(ars->get_ranges(ep));
    verify_sorted(ars->get_primary_ranges(ep));
    verify_sorted(ars->get_primary_ranges_within_dc(ep));
#endif
}

void strategy_sanity_check(
    abstract_replication_strategy* ars_ptr,
    const std::map<sstring, sstring>& options) {

    network_topology_strategy* nts_ptr =
        dynamic_cast<network_topology_strategy*>(ars_ptr);

    //
    // Check that both total and per-DC RFs in options match the corresponding
    // value in the strategy object.
    //
    size_t total_rf = 0;
    for (auto& val : options) {
        size_t rf = std::stol(val.second);
        BOOST_CHECK(nts_ptr->get_replication_factor(val.first) == rf);

        total_rf += rf;
    }

    BOOST_CHECK(ars_ptr->get_replication_factor() == total_rf);
}

void endpoints_check(
    abstract_replication_strategy* ars_ptr,
    std::vector<inet_address>& endpoints) {

    // Check the total RF
    BOOST_CHECK(endpoints.size() == ars_ptr->get_replication_factor());

    // Check the uniqueness
    std::unordered_set<inet_address> ep_set(endpoints.begin(), endpoints.end());
    BOOST_CHECK(endpoints.size() == ep_set.size());

    // Check the per-DC RF
    auto& snitch = i_endpoint_snitch::get_local_snitch_ptr();
    std::unordered_map<sstring, size_t> dc_rf;
    for (auto ep : endpoints) {
        sstring dc = snitch->get_datacenter(ep);

        auto rf = dc_rf.find(dc);
        if (rf == dc_rf.end()) {
            dc_rf[dc] = 1;
        } else {
            rf->second++;
        }
    }

    network_topology_strategy* nts_ptr =
        dynamic_cast<network_topology_strategy*>(ars_ptr);
    for (auto& rf : dc_rf) {
        BOOST_CHECK(rf.second == nts_ptr->get_replication_factor(rf.first));
    }
}

auto d2t = [](double d) {
    unsigned long l = net::hton(static_cast<unsigned long>(d*(std::numeric_limits<unsigned long>::max())));
    std::array<char, 8> a;
    memcpy(a.data(), &l, 8);
    return a;
};

/**
 * Check the get_natural_endpoints() output for tokens between every two
 * adjacent ring points.
 * @param ring_points ring description
 * @param options strategy options
 * @param ars_ptr strategy object
 */
void full_ring_check(const std::vector<ring_point>& ring_points,
                     const std::map<sstring, sstring>& options,
                     abstract_replication_strategy* ars_ptr) {
    strategy_sanity_check(ars_ptr, options);

    for (auto& rp : ring_points) {
        double cur_point1 = rp.point - 0.5;
        token t1{dht::token::kind::key,
             {(int8_t*)d2t(cur_point1 / ring_points.size()).data(), 8}};
        uint64_t cache_hit_count = ars_ptr->get_cache_hits_count();
        auto endpoints1 = ars_ptr->get_natural_endpoints(t1);

        endpoints_check(ars_ptr, endpoints1);
        // validate that the result hasn't been taken from the cache
        BOOST_CHECK(cache_hit_count == ars_ptr->get_cache_hits_count());

        print_natural_endpoints(cur_point1, endpoints1);

        //
        // Check a different endpoint in the same range as t1 and validate that
        // the endpoints has been taken from the cache and that the output is
        // identical to the one not taken from the cache.
        //
        cache_hit_count = ars_ptr->get_cache_hits_count();
        double cur_point2 = rp.point - 0.2;
        token t2{dht::token::kind::key,
             {(int8_t*)d2t(cur_point2 / ring_points.size()).data(), 8}};
        auto endpoints2 = ars_ptr->get_natural_endpoints(t2);

        endpoints_check(ars_ptr, endpoints2);
        check_ranges_are_sorted(ars_ptr, rp.host);
        BOOST_CHECK(cache_hit_count + 1 == ars_ptr->get_cache_hits_count());
        BOOST_CHECK(endpoints1 == endpoints2);
    }
}

future<> simple_test() {
    utils::fb_utilities::set_broadcast_address(gms::inet_address("localhost"));
    utils::fb_utilities::set_broadcast_rpc_address(gms::inet_address("localhost"));

    // Create the RackInferringSnitch
    return i_endpoint_snitch::create_snitch("RackInferringSnitch").then(
        [] {

        lw_shared_ptr<token_metadata> tm = make_lw_shared<token_metadata>();
        std::vector<ring_point> ring_points = {
            { 1.0,  inet_address("192.100.10.1") },
            { 2.0,  inet_address("192.101.10.1") },
            { 3.0,  inet_address("192.102.10.1") },
            { 4.0,  inet_address("192.100.20.1") },
            { 5.0,  inet_address("192.101.20.1") },
            { 6.0,  inet_address("192.102.20.1") },
            { 7.0,  inet_address("192.100.30.1") },
            { 8.0,  inet_address("192.101.30.1") },
            { 9.0,  inet_address("192.102.30.1") },
            { 10.0, inet_address("192.102.40.1") },
            { 11.0, inet_address("192.102.40.2") }
        };
        // Initialize the token_metadata
        for (unsigned i = 0; i < ring_points.size(); i++) {
            tm->update_normal_token(
                {dht::token::kind::key,
                 {(int8_t*)d2t(ring_points[i].point / ring_points.size()).data(), 8}
                },
                ring_points[i].host);
        }

        /////////////////////////////////////
        // Create the replication strategy
        std::map<sstring, sstring> options323 = {
            {"100", "3"},
            {"101", "2"},
            {"102", "3"}
        };

        auto ars_uptr = abstract_replication_strategy::create_replication_strategy(
            "test keyspace", "NetworkTopologyStrategy", *tm, options323);

        auto ars_ptr = ars_uptr.get();

        full_ring_check(ring_points, options323, ars_ptr);

        ///////////////
        // Create the replication strategy
        std::map<sstring, sstring> options320 = {
            {"100", "3"},
            {"101", "2"},
            {"102", "0"}
        };

        ars_uptr = abstract_replication_strategy::create_replication_strategy(
            "test keyspace", "NetworkTopologyStrategy", *tm, options320);

        ars_ptr = ars_uptr.get();

        full_ring_check(ring_points, options320, ars_ptr);

        //
        // Check cache invalidation: invalidate the cache and run a full ring
        // check once again. If cache is not properly invalidated one of the
        // points will be taken from the cache when it shouldn't and the
        // corresponding check will fail.
        //
        tm->invalidate_cached_rings();
        full_ring_check(ring_points, options320, ars_ptr);

        return i_endpoint_snitch::stop_snitch();
    });
}

future<> heavy_origin_test() {
    utils::fb_utilities::set_broadcast_address(gms::inet_address("localhost"));
    utils::fb_utilities::set_broadcast_rpc_address(gms::inet_address("localhost"));

    // Create the RackInferringSnitch
    return i_endpoint_snitch::create_snitch("RackInferringSnitch").then(
        [] {
        std::vector<int> dc_racks = {2, 4, 8};
        std::vector<int> dc_endpoints = {128, 256, 512};
        std::vector<int> dc_replication = {2, 6, 6};

        lw_shared_ptr<token_metadata> tm = make_lw_shared<token_metadata>();
        std::map<sstring, sstring> config_options;
        std::unordered_map<inet_address, std::unordered_set<token>> tokens;
        std::vector<ring_point> ring_points;

        size_t total_eps = 0;
        for (size_t dc = 0; dc < dc_racks.size(); ++dc) {
            for (int rack = 0; rack < dc_racks[dc]; ++rack) {
                total_eps += dc_endpoints[dc]/dc_racks[dc];
            }
        }

        int total_rf = 0;
        std::default_random_engine random_engine{};
        std::vector<double> token_points(total_eps, 0.0);
        boost::algorithm::iota(token_points, 1.0);
        std::shuffle(token_points.begin(), token_points.end(), random_engine);
        auto token_point_iterator = token_points.begin();
        for (size_t dc = 0; dc < dc_racks.size(); ++dc) {
            total_rf += dc_replication[dc];
            config_options.emplace(to_sstring(dc),
                                   to_sstring(dc_replication[dc]));
            for (int rack = 0; rack < dc_racks[dc]; ++rack) {
                for (int ep = 1; ep <= dc_endpoints[dc]/dc_racks[dc]; ++ep) {
                    double token_point = *token_point_iterator++;
                    // 10.dc.rack.ep
                    int32_t ip = 0x0a000000 + ((int8_t)dc << 16) +
                                 ((int8_t)rack << 8) + (int8_t)ep;
                    inet_address address(ip);
                    ring_point rp = {token_point, address};

                    ring_points.emplace_back(rp);
                    tokens[address].emplace(token{dht::token::kind::key,
                            {(int8_t*)d2t(token_point / total_eps).data(), 8}});

                    nlogger.debug("adding node {} at {}", address, token_point);

                    token_point++;
                }
            }
        }

        tm->update_normal_tokens(tokens);

        auto ars_uptr = abstract_replication_strategy::create_replication_strategy(
            "test keyspace", "NetworkTopologyStrategy", *tm, config_options);

        auto ars_ptr = ars_uptr.get();

        full_ring_check(ring_points, config_options, ars_ptr);

        return i_endpoint_snitch::stop_snitch();
    });
}

SEASTAR_TEST_CASE(NetworkTopologyStrategy_simple) {
    return simple_test();
}

SEASTAR_TEST_CASE(NetworkTopologyStrategy_heavy) {
    return heavy_origin_test();
}
<|endoftext|>


<commit_before>/****************************************************************************
 *
 *   Copyright (c) 2013-2016 PX4 Development Team. 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 PX4 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 OWNER 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.
 *
 ****************************************************************************/

/**
 * @file MulticopterLandDetector.cpp
 * Land detection algorithm for multicopters
 *
 * @author Johan Jansen <jnsn.johan@gmail.com>
 * @author Morten Lysgaard <morten@lysgaard.no>
 */

#include "MulticopterLandDetector.h"

#include <cmath>
#include <drivers/drv_hrt.h>
#include <mathlib/mathlib.h>

namespace landdetection
{

MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
	_paramHandle(),
	_params(),
	_vehicleLocalPositionSub(-1),
	_actuatorsSub(-1),
	_armingSub(-1),
	_parameterSub(-1),
	_attitudeSub(-1),
	_manualSub(-1),
	_ctrl_state_sub(-1),
	_vehicle_control_mode_sub(-1),
	_vehicleLocalPosition{},
	_actuators{},
	_arming{},
	_vehicleAttitude{},
	_manual{},
	_ctrl_state{},
	_ctrl_mode{},
	_landTimer(0),
	_freefallTimer(0),
	_min_trust_start(0)
{
	_paramHandle.maxRotation = param_find("LNDMC_ROT_MAX");
	_paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX");
	_paramHandle.maxClimbRate = param_find("LNDMC_Z_VEL_MAX");
	_paramHandle.maxThrottle = param_find("MPC_THR_MIN");
	_paramHandle.minManThrottle = param_find("MPC_MANTHR_MIN");
	_paramHandle.acc_threshold_m_s2 = param_find("LNDMC_FFALL_THR");
	_paramHandle.ff_trigger_time = param_find("LNDMC_FFALL_TTRI");
}

void MulticopterLandDetector::initialize()
{
	// subscribe to position, attitude, arming and velocity changes
	_vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position));
	_attitudeSub = orb_subscribe(ORB_ID(vehicle_attitude));
	_actuatorsSub = orb_subscribe(ORB_ID(actuator_controls_0));
	_armingSub = orb_subscribe(ORB_ID(actuator_armed));
	_parameterSub = orb_subscribe(ORB_ID(parameter_update));
	_manualSub = orb_subscribe(ORB_ID(manual_control_setpoint));
	_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
	_vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));

	// download parameters
	updateParameterCache(true);
}

void MulticopterLandDetector::updateSubscriptions()
{
	orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition);
	orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude);
	orb_update(ORB_ID(actuator_controls_0), _actuatorsSub, &_actuators);
	orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
	orb_update(ORB_ID(manual_control_setpoint), _manualSub, &_manual);
	orb_update(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
	orb_update(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_ctrl_mode);
}

LandDetectionResult MulticopterLandDetector::update()
{
	// first poll for new data from our subscriptions
	updateSubscriptions();

	updateParameterCache(false);

	if (get_freefall_state()) {
		_state = LANDDETECTION_RES_FREEFALL;

	} else if (get_landed_state()) {
		_state = LANDDETECTION_RES_LANDED;

	} else {
		_state = LANDDETECTION_RES_FLYING;
	}

	return _state;
}

bool MulticopterLandDetector::get_freefall_state()
{
	if (_params.acc_threshold_m_s2 < 0.1f
	    || _params.acc_threshold_m_s2 > 10.0f) {	//if parameter is set to zero or invalid, disable free-fall detection.
		return false;
	}

	const uint64_t now = hrt_absolute_time();

	float acc_norm = _ctrl_state.x_acc * _ctrl_state.x_acc
			 + _ctrl_state.y_acc * _ctrl_state.y_acc
			 + _ctrl_state.z_acc * _ctrl_state.z_acc;
	acc_norm = sqrtf(acc_norm);	//norm of specific force. Should be close to 9.8 m/s^2 when landed.

	bool freefall = (acc_norm < _params.acc_threshold_m_s2);	//true if we are currently falling

	if (!freefall || _freefallTimer == 0) {	//reset timer if uav not falling
		_freefallTimer = now;
		return false;
	}

	return (now - _freefallTimer) / 1000000.0f > _params.ff_trigger_time;
}

bool MulticopterLandDetector::get_landed_state()
{
	// Time base for this function
	const uint64_t now = hrt_absolute_time();

	float sys_min_throttle = (_params.maxThrottle + 0.01f);

	// Determine the system min throttle based on flight mode
	if (!_ctrl_mode.flag_control_altitude_enabled) {
		sys_min_throttle = (_params.minManThrottle + 0.01f);
	}

	// Check if thrust output is less than the minimum auto throttle param.
	bool minimalThrust = (_actuators.control[3] <= sys_min_throttle);

	if (minimalThrust && _min_trust_start == 0) {
		_min_trust_start = now;

	} else if (!minimalThrust) {
		_min_trust_start = 0;
	}

	// only trigger flight conditions if we are armed
	if (!_arming.armed) {
		_arming_time = 0;
		return true;

	} else if (_arming_time == 0) {
		_arming_time = now;
	}

	// If in manual flight mode never report landed if the user has more than idle throttle
	// Check if user commands throttle and if so, report not landed based on
	// the user intent to take off (even if the system might physically still have
	// ground contact at this point).
	if (_manual.timestamp > 0 && _manual.z > 0.15f && _ctrl_mode.flag_control_manual_enabled) {
		return false;
	}

	// Return status based on armed state and throttle if no position lock is available.
	if (_vehicleLocalPosition.timestamp == 0 ||
	    hrt_elapsed_time(&_vehicleLocalPosition.timestamp) > 500000 ||
	    !_vehicleLocalPosition.xy_valid ||
	    !_vehicleLocalPosition.z_valid) {

		// The system has minimum trust set (manual or in failsafe)
		// if this persists for 8 seconds AND the drone is not
		// falling consider it to be landed. This should even sustain
		// quite acrobatic flight.
		if ((_min_trust_start > 0) &&
		    (hrt_elapsed_time(&_min_trust_start) > 8 * 1000 * 1000)) {
			return !get_freefall_state();

		} else {
			return false;
		}
	}

	float armThresholdFactor = 1.0f;

	// Widen acceptance thresholds for landed state right after arming
	// so that motor spool-up and other effects do not trigger false negatives.
	if (hrt_elapsed_time(&_arming_time) < LAND_DETECTOR_ARM_PHASE_TIME) {
		armThresholdFactor = 2.5f;
	}

	// Check if we are moving vertically - this might see a spike after arming due to
	// throttle-up vibration. If accelerating fast the throttle thresholds will still give
	// an accurate in-air indication.
	bool verticalMovement = fabsf(_vehicleLocalPosition.vz) > _params.maxClimbRate * armThresholdFactor;

	// Check if we are moving horizontally.
	bool horizontalMovement = sqrtf(_vehicleLocalPosition.vx * _vehicleLocalPosition.vx
					+ _vehicleLocalPosition.vy * _vehicleLocalPosition.vy) > _params.maxVelocity;

	// Next look if all rotation angles are not moving.
	float maxRotationScaled = _params.maxRotation_rad_s * armThresholdFactor;

	bool rotating = (fabsf(_vehicleAttitude.rollspeed)  > maxRotationScaled) ||
			(fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) ||
			(fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled);

	if (verticalMovement || rotating || !minimalThrust || horizontalMovement) {
		// Sensed movement or thottle high, so reset the land detector.
		_landTimer = now;
		return false;
	}

	return (now - _landTimer > LAND_DETECTOR_TRIGGER_TIME);
}

void MulticopterLandDetector::updateParameterCache(const bool force)
{
	bool updated;
	parameter_update_s paramUpdate;

	orb_check(_parameterSub, &updated);

	if (updated) {
		orb_copy(ORB_ID(parameter_update), _parameterSub, &paramUpdate);
	}

	if (updated || force) {
		param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate);
		param_get(_paramHandle.maxVelocity, &_params.maxVelocity);
		param_get(_paramHandle.maxRotation, &_params.maxRotation_rad_s);
		_params.maxRotation_rad_s = math::radians(_params.maxRotation_rad_s);
		param_get(_paramHandle.maxThrottle, &_params.maxThrottle);
		param_get(_paramHandle.minManThrottle, &_params.minManThrottle);
		param_get(_paramHandle.acc_threshold_m_s2, &_params.acc_threshold_m_s2);
		param_get(_paramHandle.ff_trigger_time, &_params.ff_trigger_time);
	}
}

}
<commit_msg>land_detector: don't estimate freefall on 0 data<commit_after>/****************************************************************************
 *
 *   Copyright (c) 2013-2016 PX4 Development Team. 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 PX4 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 OWNER 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.
 *
 ****************************************************************************/

/**
 * @file MulticopterLandDetector.cpp
 * Land detection algorithm for multicopters
 *
 * @author Johan Jansen <jnsn.johan@gmail.com>
 * @author Morten Lysgaard <morten@lysgaard.no>
 */

#include "MulticopterLandDetector.h"

#include <cmath>
#include <drivers/drv_hrt.h>
#include <mathlib/mathlib.h>

namespace landdetection
{

MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
	_paramHandle(),
	_params(),
	_vehicleLocalPositionSub(-1),
	_actuatorsSub(-1),
	_armingSub(-1),
	_parameterSub(-1),
	_attitudeSub(-1),
	_manualSub(-1),
	_ctrl_state_sub(-1),
	_vehicle_control_mode_sub(-1),
	_vehicleLocalPosition{},
	_actuators{},
	_arming{},
	_vehicleAttitude{},
	_manual{},
	_ctrl_state{},
	_ctrl_mode{},
	_landTimer(0),
	_freefallTimer(0),
	_min_trust_start(0)
{
	_paramHandle.maxRotation = param_find("LNDMC_ROT_MAX");
	_paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX");
	_paramHandle.maxClimbRate = param_find("LNDMC_Z_VEL_MAX");
	_paramHandle.maxThrottle = param_find("MPC_THR_MIN");
	_paramHandle.minManThrottle = param_find("MPC_MANTHR_MIN");
	_paramHandle.acc_threshold_m_s2 = param_find("LNDMC_FFALL_THR");
	_paramHandle.ff_trigger_time = param_find("LNDMC_FFALL_TTRI");
}

void MulticopterLandDetector::initialize()
{
	// subscribe to position, attitude, arming and velocity changes
	_vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position));
	_attitudeSub = orb_subscribe(ORB_ID(vehicle_attitude));
	_actuatorsSub = orb_subscribe(ORB_ID(actuator_controls_0));
	_armingSub = orb_subscribe(ORB_ID(actuator_armed));
	_parameterSub = orb_subscribe(ORB_ID(parameter_update));
	_manualSub = orb_subscribe(ORB_ID(manual_control_setpoint));
	_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
	_vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));

	// download parameters
	updateParameterCache(true);
}

void MulticopterLandDetector::updateSubscriptions()
{
	orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition);
	orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude);
	orb_update(ORB_ID(actuator_controls_0), _actuatorsSub, &_actuators);
	orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
	orb_update(ORB_ID(manual_control_setpoint), _manualSub, &_manual);
	orb_update(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
	orb_update(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_ctrl_mode);
}

LandDetectionResult MulticopterLandDetector::update()
{
	// first poll for new data from our subscriptions
	updateSubscriptions();

	updateParameterCache(false);

	if (get_freefall_state()) {
		_state = LANDDETECTION_RES_FREEFALL;

	} else if (get_landed_state()) {
		_state = LANDDETECTION_RES_LANDED;

	} else {
		_state = LANDDETECTION_RES_FLYING;
	}

	return _state;
}

bool MulticopterLandDetector::get_freefall_state()
{
	if (_params.acc_threshold_m_s2 < 0.1f
	    || _params.acc_threshold_m_s2 > 10.0f) {	//if parameter is set to zero or invalid, disable free-fall detection.
		return false;
	}

	const uint64_t now = hrt_absolute_time();

	if (_ctrl_state.timestamp == 0) {
		// _ctrl_state is not valid yet, we have to assume we're not falling.
		return false;
	}

	float acc_norm = _ctrl_state.x_acc * _ctrl_state.x_acc
			 + _ctrl_state.y_acc * _ctrl_state.y_acc
			 + _ctrl_state.z_acc * _ctrl_state.z_acc;
	acc_norm = sqrtf(acc_norm);	//norm of specific force. Should be close to 9.8 m/s^2 when landed.

	bool freefall = (acc_norm < _params.acc_threshold_m_s2);	//true if we are currently falling

	if (!freefall || _freefallTimer == 0) {	//reset timer if uav not falling
		_freefallTimer = now;
		return false;
	}

	return (now - _freefallTimer) / 1000000.0f > _params.ff_trigger_time;
}

bool MulticopterLandDetector::get_landed_state()
{
	// Time base for this function
	const uint64_t now = hrt_absolute_time();

	float sys_min_throttle = (_params.maxThrottle + 0.01f);

	// Determine the system min throttle based on flight mode
	if (!_ctrl_mode.flag_control_altitude_enabled) {
		sys_min_throttle = (_params.minManThrottle + 0.01f);
	}

	// Check if thrust output is less than the minimum auto throttle param.
	bool minimalThrust = (_actuators.control[3] <= sys_min_throttle);

	if (minimalThrust && _min_trust_start == 0) {
		_min_trust_start = now;

	} else if (!minimalThrust) {
		_min_trust_start = 0;
	}

	// only trigger flight conditions if we are armed
	if (!_arming.armed) {
		_arming_time = 0;
		return true;

	} else if (_arming_time == 0) {
		_arming_time = now;
	}

	// If in manual flight mode never report landed if the user has more than idle throttle
	// Check if user commands throttle and if so, report not landed based on
	// the user intent to take off (even if the system might physically still have
	// ground contact at this point).
	if (_manual.timestamp > 0 && _manual.z > 0.15f && _ctrl_mode.flag_control_manual_enabled) {
		return false;
	}

	// Return status based on armed state and throttle if no position lock is available.
	if (_vehicleLocalPosition.timestamp == 0 ||
	    hrt_elapsed_time(&_vehicleLocalPosition.timestamp) > 500000 ||
	    !_vehicleLocalPosition.xy_valid ||
	    !_vehicleLocalPosition.z_valid) {

		// The system has minimum trust set (manual or in failsafe)
		// if this persists for 8 seconds AND the drone is not
		// falling consider it to be landed. This should even sustain
		// quite acrobatic flight.
		if ((_min_trust_start > 0) &&
		    (hrt_elapsed_time(&_min_trust_start) > 8 * 1000 * 1000)) {
			return !get_freefall_state();

		} else {
			return false;
		}
	}

	float armThresholdFactor = 1.0f;

	// Widen acceptance thresholds for landed state right after arming
	// so that motor spool-up and other effects do not trigger false negatives.
	if (hrt_elapsed_time(&_arming_time) < LAND_DETECTOR_ARM_PHASE_TIME) {
		armThresholdFactor = 2.5f;
	}

	// Check if we are moving vertically - this might see a spike after arming due to
	// throttle-up vibration. If accelerating fast the throttle thresholds will still give
	// an accurate in-air indication.
	bool verticalMovement = fabsf(_vehicleLocalPosition.vz) > _params.maxClimbRate * armThresholdFactor;

	// Check if we are moving horizontally.
	bool horizontalMovement = sqrtf(_vehicleLocalPosition.vx * _vehicleLocalPosition.vx
					+ _vehicleLocalPosition.vy * _vehicleLocalPosition.vy) > _params.maxVelocity;

	// Next look if all rotation angles are not moving.
	float maxRotationScaled = _params.maxRotation_rad_s * armThresholdFactor;

	bool rotating = (fabsf(_vehicleAttitude.rollspeed)  > maxRotationScaled) ||
			(fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) ||
			(fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled);

	if (verticalMovement || rotating || !minimalThrust || horizontalMovement) {
		// Sensed movement or thottle high, so reset the land detector.
		_landTimer = now;
		return false;
	}

	return (now - _landTimer > LAND_DETECTOR_TRIGGER_TIME);
}

void MulticopterLandDetector::updateParameterCache(const bool force)
{
	bool updated;
	parameter_update_s paramUpdate;

	orb_check(_parameterSub, &updated);

	if (updated) {
		orb_copy(ORB_ID(parameter_update), _parameterSub, &paramUpdate);
	}

	if (updated || force) {
		param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate);
		param_get(_paramHandle.maxVelocity, &_params.maxVelocity);
		param_get(_paramHandle.maxRotation, &_params.maxRotation_rad_s);
		_params.maxRotation_rad_s = math::radians(_params.maxRotation_rad_s);
		param_get(_paramHandle.maxThrottle, &_params.maxThrottle);
		param_get(_paramHandle.minManThrottle, &_params.minManThrottle);
		param_get(_paramHandle.acc_threshold_m_s2, &_params.acc_threshold_m_s2);
		param_get(_paramHandle.ff_trigger_time, &_params.ff_trigger_time);
	}
}

}
<|endoftext|>