<commit_before>// Copyright 2015 Toggl Desktop developers.

#include "../src/netconf.h"

#include <string>
#include <sstream>

#include "./https_client.h"

#include "Poco/Environment.h"
#include "Poco/Logger.h"
#include "Poco/Net/HTTPCredentials.h"
#include "Poco/Net/HTTPSClientSession.h"
#include "Poco/URI.h"
#include "Poco/UnicodeConverter.h"

#ifdef __MACH__
#include <CoreFoundation/CoreFoundation.h>  // NOLINT
#include <CoreServices/CoreServices.h>  // NOLINT
#endif

#ifdef _WIN32
#include <winhttp.h>
#pragma comment(lib, "winhttp")
#endif

#ifdef __linux__
#include <sys/types.h>
#include <sys/wait.h>
#include <spawn.h>
#endif

namespace toggl {

error Netconf::autodetectProxy(
    const std::string encoded_url,
    std::vector<std::string> *proxy_strings) {

    poco_assert(proxy_strings);

    proxy_strings->clear();

    if (encoded_url.empty()) {
        return noError;
    }

#ifdef _WIN32
    WINHTTP_CURRENT_USER_IE_PROXY_CONFIG ie_config = { 0 };
    if (!WinHttpGetIEProxyConfigForCurrentUser(&ie_config)) {
        std::stringstream ss;
        ss << "WinHttpGetIEProxyConfigForCurrentUser error: "
           << GetLastError();
        return ss.str();
    }
    if (ie_config.lpszProxy) {
        std::wstring proxy_url_wide(ie_config.lpszProxy);
        std::string s("");
        Poco::UnicodeConverter::toUTF8(proxy_url_wide, s);
        proxy_strings->push_back(s);
    }
#endif

    // Inspired by VLC source code
    // https://github.com/videolan/vlc/blob/master/src/darwin/netconf.c
#ifdef __MACH__
    CFDictionaryRef dicRef = CFNetworkCopySystemProxySettings();
    if (NULL != dicRef) {
        const CFStringRef proxyCFstr = (const CFStringRef)CFDictionaryGetValue(
            dicRef, (const void*)kCFNetworkProxiesHTTPProxy);
        const CFNumberRef portCFnum = (const CFNumberRef)CFDictionaryGetValue(
            dicRef, (const void*)kCFNetworkProxiesHTTPPort);
        if (NULL != proxyCFstr && NULL != portCFnum) {
            int port = 0;
            if (!CFNumberGetValue(portCFnum, kCFNumberIntType, &port)) {
                CFRelease(dicRef);
                return noError;
            }

            const std::size_t kBufsize(4096);
            char host_buffer[kBufsize];
            memset(host_buffer, 0, sizeof(host_buffer));
            if (CFStringGetCString(proxyCFstr, host_buffer, sizeof(host_buffer)
                                   - 1, kCFStringEncodingUTF8)) {
                char buffer[kBufsize];
                snprintf(buffer, kBufsize, "%s:%d", host_buffer, port);
                proxy_strings->push_back(std::string(buffer));
            }
        }

        CFRelease(dicRef);
    }
#endif

    // Inspired by VLC source code
    // https://github.com/videolan/vlc/blob/master/src/posix/netconf.c
#ifdef __linux__

    posix_spawn_file_actions_t actions;
    posix_spawn_file_actions_init(&actions);
    posix_spawn_file_actions_addopen(&actions, STDIN_FILENO, "/dev/null",
                                     O_RDONLY, 0644);
    int fd[2];
    posix_spawn_file_actions_adddup2(&actions, fd[1], STDOUT_FILENO);

    posix_spawnattr_t attr;
    posix_spawnattr_init(&attr);
    {
        sigset_t set;
        sigemptyset(&set);

        posix_spawnattr_setsigmask(&attr, &set);
        sigaddset(&set, SIGPIPE);
        posix_spawnattr_setsigdefault(&attr, &set);
        posix_spawnattr_setflags(&attr, POSIX_SPAWN_SETSIGDEF
                                 | POSIX_SPAWN_SETSIGMASK);
    }

    pid_t pid;
    char *argv[3] = { const_cast<char *>("proxy"),
                      const_cast<char *>(encoded_url.c_str()), NULL
                    };
    if (posix_spawnp(&pid, "proxy", &actions, &attr, argv, environ)) {
        pid = -1;
    }

    posix_spawnattr_destroy(&attr);
    posix_spawn_file_actions_destroy(&actions);
    close(fd[1]);

    if (-1 == pid) {
        close(fd[0]);
        return error("Failed to run proxy command");
    }

    char buf[1024];
    size_t len = 0;
    do {
        ssize_t val = read(fd[0], buf + len, sizeof (buf) - len);
        if (val <= 0) {
            break;
        }
        len += val;
    } while (len < sizeof (buf));

    close(fd[0]);

    while (true) {
        int status = {0};
        if (-1 != waitpid(pid, &status, 0)) {
            break;
        }
    }

    if (len >= 9 && !strncasecmp(buf, "direct://", 9)) {
        return noError;
    }

    char *end = <char *>(memchr(buf, '\n', len));
    if (end != NULL) {
        *end = '\0';
        proxy_strings->push_back(std::string(buf));
    }
#endif

    return noError;
}

error Netconf::ConfigureProxy(
    const std::string encoded_url,
    Poco::Net::HTTPSClientSession *session) {

    Poco::Logger &logger = Poco::Logger::get("ConfigureProxy");

    std::string proxy_url("");
    if (HTTPSClient::Config.AutodetectProxy) {
        if (Poco::Environment::has("HTTPS_PROXY")) {
            proxy_url = Poco::Environment::get("HTTPS_PROXY");
        }
        if (Poco::Environment::has("https_proxy")) {
            proxy_url = Poco::Environment::get("https_proxy");
        }
        if (Poco::Environment::has("HTTP_PROXY")) {
            proxy_url = Poco::Environment::get("HTTP_PROXY");
        }
        if (Poco::Environment::has("http_proxy")) {
            proxy_url = Poco::Environment::get("http_proxy");
        }
        if (proxy_url.empty()) {
            std::vector<std::string> proxy_strings;
            error err = autodetectProxy(encoded_url, &proxy_strings);
            if (err != noError) {
                return err;
            }
            if (!proxy_strings.empty()) {
                proxy_url = proxy_strings[0];
            }
        }

        if (!proxy_url.empty()) {
            if (proxy_url.find("://") == std::string::npos) {
                proxy_url = "http://" + proxy_url;
            }
            Poco::URI proxy_uri(proxy_url);

            std::stringstream ss;
            ss << "Using proxy URI=" + proxy_uri.toString()
               << " host=" << proxy_uri.getHost()
               << " port=" << proxy_uri.getPort();
            logger.debug(ss.str());

            session->setProxy(
                proxy_uri.getHost(),
                proxy_uri.getPort());

            if (!proxy_uri.getUserInfo().empty()) {
                Poco::Net::HTTPCredentials credentials;
                credentials.fromUserInfo(proxy_uri.getUserInfo());
                session->setProxyCredentials(
                    credentials.getUsername(),
                    credentials.getPassword());

                logger.debug("Proxy credentials detected username="
                             + credentials.getUsername());
            }
        }
    }

    // Try to use user-configured proxy
    if (proxy_url.empty() && HTTPSClient::Config.UseProxy &&
            HTTPSClient::Config.ProxySettings.IsConfigured()) {
        session->setProxy(
            HTTPSClient::Config.ProxySettings.Host(),
            static_cast<Poco::UInt16>(
                HTTPSClient::Config.ProxySettings.Port()));

        std::stringstream ss;
        ss << "Proxy configured "
           << " host=" << HTTPSClient::Config.ProxySettings.Host()
           << " port=" << HTTPSClient::Config.ProxySettings.Port();
        logger.debug(ss.str());

        if (HTTPSClient::Config.ProxySettings.HasCredentials()) {
            session->setProxyCredentials(
                HTTPSClient::Config.ProxySettings.Username(),
                HTTPSClient::Config.ProxySettings.Password());

            logger.debug("Proxy credentials configured username="
                         + HTTPSClient::Config.ProxySettings.Username());
        }
    }

    return noError;
}

}   // namespace toggl
<commit_msg>fix build (linux)<commit_after>// Copyright 2015 Toggl Desktop developers.

#include "../src/netconf.h"

#include <string>
#include <sstream>

#include "./https_client.h"

#include "Poco/Environment.h"
#include "Poco/Logger.h"
#include "Poco/Net/HTTPCredentials.h"
#include "Poco/Net/HTTPSClientSession.h"
#include "Poco/URI.h"
#include "Poco/UnicodeConverter.h"

#ifdef __MACH__
#include <CoreFoundation/CoreFoundation.h>  // NOLINT
#include <CoreServices/CoreServices.h>  // NOLINT
#endif

#ifdef _WIN32
#include <winhttp.h>
#pragma comment(lib, "winhttp")
#endif

#ifdef __linux__
#include <sys/types.h>
#include <sys/wait.h>
#include <spawn.h>
#endif

namespace toggl {

error Netconf::autodetectProxy(
    const std::string encoded_url,
    std::vector<std::string> *proxy_strings) {

    poco_assert(proxy_strings);

    proxy_strings->clear();

    if (encoded_url.empty()) {
        return noError;
    }

#ifdef _WIN32
    WINHTTP_CURRENT_USER_IE_PROXY_CONFIG ie_config = { 0 };
    if (!WinHttpGetIEProxyConfigForCurrentUser(&ie_config)) {
        std::stringstream ss;
        ss << "WinHttpGetIEProxyConfigForCurrentUser error: "
           << GetLastError();
        return ss.str();
    }
    if (ie_config.lpszProxy) {
        std::wstring proxy_url_wide(ie_config.lpszProxy);
        std::string s("");
        Poco::UnicodeConverter::toUTF8(proxy_url_wide, s);
        proxy_strings->push_back(s);
    }
#endif

    // Inspired by VLC source code
    // https://github.com/videolan/vlc/blob/master/src/darwin/netconf.c
#ifdef __MACH__
    CFDictionaryRef dicRef = CFNetworkCopySystemProxySettings();
    if (NULL != dicRef) {
        const CFStringRef proxyCFstr = (const CFStringRef)CFDictionaryGetValue(
            dicRef, (const void*)kCFNetworkProxiesHTTPProxy);
        const CFNumberRef portCFnum = (const CFNumberRef)CFDictionaryGetValue(
            dicRef, (const void*)kCFNetworkProxiesHTTPPort);
        if (NULL != proxyCFstr && NULL != portCFnum) {
            int port = 0;
            if (!CFNumberGetValue(portCFnum, kCFNumberIntType, &port)) {
                CFRelease(dicRef);
                return noError;
            }

            const std::size_t kBufsize(4096);
            char host_buffer[kBufsize];
            memset(host_buffer, 0, sizeof(host_buffer));
            if (CFStringGetCString(proxyCFstr, host_buffer, sizeof(host_buffer)
                                   - 1, kCFStringEncodingUTF8)) {
                char buffer[kBufsize];
                snprintf(buffer, kBufsize, "%s:%d", host_buffer, port);
                proxy_strings->push_back(std::string(buffer));
            }
        }

        CFRelease(dicRef);
    }
#endif

    // Inspired by VLC source code
    // https://github.com/videolan/vlc/blob/master/src/posix/netconf.c
#ifdef __linux__

    posix_spawn_file_actions_t actions;
    posix_spawn_file_actions_init(&actions);
    posix_spawn_file_actions_addopen(&actions, STDIN_FILENO, "/dev/null",
                                     O_RDONLY, 0644);
    int fd[2];
    posix_spawn_file_actions_adddup2(&actions, fd[1], STDOUT_FILENO);

    posix_spawnattr_t attr;
    posix_spawnattr_init(&attr);
    {
        sigset_t set;
        sigemptyset(&set);

        posix_spawnattr_setsigmask(&attr, &set);
        sigaddset(&set, SIGPIPE);
        posix_spawnattr_setsigdefault(&attr, &set);
        posix_spawnattr_setflags(&attr, POSIX_SPAWN_SETSIGDEF
                                 | POSIX_SPAWN_SETSIGMASK);
    }

    pid_t pid;
    char *argv[3] = { const_cast<char *>("proxy"),
                      const_cast<char *>(encoded_url.c_str()), NULL
                    };
    if (posix_spawnp(&pid, "proxy", &actions, &attr, argv, environ)) {
        pid = -1;
    }

    posix_spawnattr_destroy(&attr);
    posix_spawn_file_actions_destroy(&actions);
    close(fd[1]);

    if (-1 == pid) {
        close(fd[0]);
        return error("Failed to run proxy command");
    }

    char buf[1024];
    size_t len = 0;
    do {
        ssize_t val = read(fd[0], buf + len, sizeof (buf) - len);
        if (val <= 0) {
            break;
        }
        len += val;
    } while (len < sizeof (buf));

    close(fd[0]);

    while (true) {
        int status = {0};
        if (-1 != waitpid(pid, &status, 0)) {
            break;
        }
    }

    if (len >= 9 && !strncasecmp(buf, "direct://", 9)) {
        return noError;
    }

    char *end = static_cast<char *>(memchr(buf, '\n', len));
    if (end != NULL) {
        *end = '\0';
        proxy_strings->push_back(std::string(buf));
    }
#endif

    return noError;
}

error Netconf::ConfigureProxy(
    const std::string encoded_url,
    Poco::Net::HTTPSClientSession *session) {

    Poco::Logger &logger = Poco::Logger::get("ConfigureProxy");

    std::string proxy_url("");
    if (HTTPSClient::Config.AutodetectProxy) {
        if (Poco::Environment::has("HTTPS_PROXY")) {
            proxy_url = Poco::Environment::get("HTTPS_PROXY");
        }
        if (Poco::Environment::has("https_proxy")) {
            proxy_url = Poco::Environment::get("https_proxy");
        }
        if (Poco::Environment::has("HTTP_PROXY")) {
            proxy_url = Poco::Environment::get("HTTP_PROXY");
        }
        if (Poco::Environment::has("http_proxy")) {
            proxy_url = Poco::Environment::get("http_proxy");
        }
        if (proxy_url.empty()) {
            std::vector<std::string> proxy_strings;
            error err = autodetectProxy(encoded_url, &proxy_strings);
            if (err != noError) {
                return err;
            }
            if (!proxy_strings.empty()) {
                proxy_url = proxy_strings[0];
            }
        }

        if (!proxy_url.empty()) {
            if (proxy_url.find("://") == std::string::npos) {
                proxy_url = "http://" + proxy_url;
            }
            Poco::URI proxy_uri(proxy_url);

            std::stringstream ss;
            ss << "Using proxy URI=" + proxy_uri.toString()
               << " host=" << proxy_uri.getHost()
               << " port=" << proxy_uri.getPort();
            logger.debug(ss.str());

            session->setProxy(
                proxy_uri.getHost(),
                proxy_uri.getPort());

            if (!proxy_uri.getUserInfo().empty()) {
                Poco::Net::HTTPCredentials credentials;
                credentials.fromUserInfo(proxy_uri.getUserInfo());
                session->setProxyCredentials(
                    credentials.getUsername(),
                    credentials.getPassword());

                logger.debug("Proxy credentials detected username="
                             + credentials.getUsername());
            }
        }
    }

    // Try to use user-configured proxy
    if (proxy_url.empty() && HTTPSClient::Config.UseProxy &&
            HTTPSClient::Config.ProxySettings.IsConfigured()) {
        session->setProxy(
            HTTPSClient::Config.ProxySettings.Host(),
            static_cast<Poco::UInt16>(
                HTTPSClient::Config.ProxySettings.Port()));

        std::stringstream ss;
        ss << "Proxy configured "
           << " host=" << HTTPSClient::Config.ProxySettings.Host()
           << " port=" << HTTPSClient::Config.ProxySettings.Port();
        logger.debug(ss.str());

        if (HTTPSClient::Config.ProxySettings.HasCredentials()) {
            session->setProxyCredentials(
                HTTPSClient::Config.ProxySettings.Username(),
                HTTPSClient::Config.ProxySettings.Password());

            logger.debug("Proxy credentials configured username="
                         + HTTPSClient::Config.ProxySettings.Username());
        }
    }

    return noError;
}

}   // namespace toggl
<|endoftext|>


<commit_before>#include <cstdio>
#include <cmath>
#include <cstring>
#include <iostream>
#include <algorithm>
#include <set>
#include <vector>
#include <sstream>
#include <typeinfo>

using namespace std;

class ${ClassName} {
    public:
    ${Method.ReturnType} ${Method.Name}(${Method.Params}) {
        return ${Method.ReturnType;ZeroValue};
    }
};

${CutBegin}
${<TestCode}
${CutEnd}
<commit_msg>added ctime header for clock() and CLOCKS_PER_SEC<commit_after>#include <cstdio>
#include <cmath>
#include <cstring>
#include <ctime>
#include <iostream>
#include <algorithm>
#include <set>
#include <vector>
#include <sstream>
#include <typeinfo>

using namespace std;

class ${ClassName} {
    public:
    ${Method.ReturnType} ${Method.Name}(${Method.Params}) {
        return ${Method.ReturnType;ZeroValue};
    }
};

${CutBegin}
${<TestCode}
${CutEnd}
<|endoftext|>


<commit_before>/*
 * Copyright © 2012 Intel Corporation
 *
 * Permission is hereby granted, free of charge, to any person obtaining a
 * copy of this software and associated documentation files (the "Software"),
 * to deal in the Software without restriction, including without limitation
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
 * and/or sell copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice (including the next
 * paragraph) shall be included in all copies or substantial portions of the
 * Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
 * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 * IN THE SOFTWARE.
 */

#include "brw_fs.h"
#include "brw_cfg.h"

/** @file brw_fs_cse.cpp
 *
 * Support for local common subexpression elimination.
 *
 * See Muchnick's Advanced Compiler Design and Implementation, section
 * 13.1 (p378).
 */

using namespace brw;

namespace {
struct aeb_entry : public exec_node {
   /** The instruction that generates the expression value. */
   fs_inst *generator;

   /** The temporary where the value is stored. */
   fs_reg tmp;
};
}

static bool
is_expression(const fs_visitor *v, const fs_inst *const inst)
{
   switch (inst->opcode) {
   case BRW_OPCODE_MOV:
   case BRW_OPCODE_SEL:
   case BRW_OPCODE_NOT:
   case BRW_OPCODE_AND:
   case BRW_OPCODE_OR:
   case BRW_OPCODE_XOR:
   case BRW_OPCODE_SHR:
   case BRW_OPCODE_SHL:
   case BRW_OPCODE_ASR:
   case BRW_OPCODE_CMP:
   case BRW_OPCODE_CMPN:
   case BRW_OPCODE_ADD:
   case BRW_OPCODE_MUL:
   case SHADER_OPCODE_MULH:
   case BRW_OPCODE_FRC:
   case BRW_OPCODE_RNDU:
   case BRW_OPCODE_RNDD:
   case BRW_OPCODE_RNDE:
   case BRW_OPCODE_RNDZ:
   case BRW_OPCODE_LINE:
   case BRW_OPCODE_PLN:
   case BRW_OPCODE_MAD:
   case BRW_OPCODE_LRP:
   case FS_OPCODE_UNIFORM_PULL_CONSTANT_LOAD:
   case FS_OPCODE_VARYING_PULL_CONSTANT_LOAD_GEN7:
   case FS_OPCODE_VARYING_PULL_CONSTANT_LOAD:
   case FS_OPCODE_CINTERP:
   case FS_OPCODE_LINTERP:
   case SHADER_OPCODE_FIND_LIVE_CHANNEL:
   case SHADER_OPCODE_BROADCAST:
   case SHADER_OPCODE_MOV_INDIRECT:
      return true;
   case SHADER_OPCODE_RCP:
   case SHADER_OPCODE_RSQ:
   case SHADER_OPCODE_SQRT:
   case SHADER_OPCODE_EXP2:
   case SHADER_OPCODE_LOG2:
   case SHADER_OPCODE_POW:
   case SHADER_OPCODE_INT_QUOTIENT:
   case SHADER_OPCODE_INT_REMAINDER:
   case SHADER_OPCODE_SIN:
   case SHADER_OPCODE_COS:
      return inst->mlen < 2;
   case SHADER_OPCODE_LOAD_PAYLOAD:
      return !inst->is_copy_payload(v->alloc);
   default:
      return inst->is_send_from_grf() && !inst->has_side_effects() &&
         !inst->is_volatile();
   }
}

static bool
operands_match(const fs_inst *a, const fs_inst *b, bool *negate)
{
   fs_reg *xs = a->src;
   fs_reg *ys = b->src;

   if (a->opcode == BRW_OPCODE_MAD) {
      return xs[0].equals(ys[0]) &&
             ((xs[1].equals(ys[1]) && xs[2].equals(ys[2])) ||
              (xs[2].equals(ys[1]) && xs[1].equals(ys[2])));
   } else if (a->opcode == BRW_OPCODE_MUL && a->dst.type == BRW_REGISTER_TYPE_F) {
      bool xs0_negate = xs[0].negate;
      bool xs1_negate = xs[1].file == IMM ? xs[1].f < 0.0f
                                          : xs[1].negate;
      bool ys0_negate = ys[0].negate;
      bool ys1_negate = ys[1].file == IMM ? ys[1].f < 0.0f
                                          : ys[1].negate;
      float xs1_imm = xs[1].f;
      float ys1_imm = ys[1].f;

      xs[0].negate = false;
      xs[1].negate = false;
      ys[0].negate = false;
      ys[1].negate = false;
      xs[1].f = fabsf(xs[1].f);
      ys[1].f = fabsf(ys[1].f);

      bool ret = (xs[0].equals(ys[0]) && xs[1].equals(ys[1])) ||
                 (xs[1].equals(ys[0]) && xs[0].equals(ys[1]));

      xs[0].negate = xs0_negate;
      xs[1].negate = xs[1].file == IMM ? false : xs1_negate;
      ys[0].negate = ys0_negate;
      ys[1].negate = ys[1].file == IMM ? false : ys1_negate;
      xs[1].f = xs1_imm;
      ys[1].f = ys1_imm;

      *negate = (xs0_negate != xs1_negate) != (ys0_negate != ys1_negate);
      return ret;
   } else if (!a->is_commutative()) {
      bool match = true;
      for (int i = 0; i < a->sources; i++) {
         if (!xs[i].equals(ys[i])) {
            match = false;
            break;
         }
      }
      return match;
   } else {
      return (xs[0].equals(ys[0]) && xs[1].equals(ys[1])) ||
             (xs[1].equals(ys[0]) && xs[0].equals(ys[1]));
   }
}

static bool
instructions_match(fs_inst *a, fs_inst *b, bool *negate)
{
   return a->opcode == b->opcode &&
          a->force_writemask_all == b->force_writemask_all &&
          a->exec_size == b->exec_size &&
          a->force_sechalf == b->force_sechalf &&
          a->saturate == b->saturate &&
          a->predicate == b->predicate &&
          a->predicate_inverse == b->predicate_inverse &&
          a->conditional_mod == b->conditional_mod &&
          a->flag_subreg == b->flag_subreg &&
          a->dst.type == b->dst.type &&
          a->offset == b->offset &&
          a->mlen == b->mlen &&
          a->regs_written == b->regs_written &&
          a->base_mrf == b->base_mrf &&
          a->eot == b->eot &&
          a->header_size == b->header_size &&
          a->shadow_compare == b->shadow_compare &&
          a->pi_noperspective == b->pi_noperspective &&
          a->sources == b->sources &&
          operands_match(a, b, negate);
}

static void
create_copy_instr(const fs_builder &bld, fs_inst *inst, fs_reg src, bool negate)
{
   int written = inst->regs_written;
   int dst_width = inst->exec_size / 8;
   fs_inst *copy;

   if (written > dst_width) {
      fs_reg *payload;
      int sources, header_size;
      if (inst->opcode == SHADER_OPCODE_LOAD_PAYLOAD) {
         sources = inst->sources;
         header_size = inst->header_size;
      } else {
         assert(written % dst_width == 0);
         sources = written / dst_width;
         header_size = 0;
      }

      assert(src.file == VGRF);
      payload = ralloc_array(bld.shader->mem_ctx, fs_reg, sources);
      for (int i = 0; i < header_size; i++) {
         payload[i] = src;
         src.reg_offset++;
      }
      for (int i = header_size; i < sources; i++) {
         payload[i] = src;
         src = offset(src, bld, 1);
      }
      copy = bld.LOAD_PAYLOAD(inst->dst, payload, sources, header_size);
   } else {
      copy = bld.MOV(inst->dst, src);
      copy->src[0].negate = negate;
   }
   assert(copy->regs_written == written);
}

bool
fs_visitor::opt_cse_local(bblock_t *block)
{
   bool progress = false;
   exec_list aeb;

   void *cse_ctx = ralloc_context(NULL);

   int ip = block->start_ip;
   foreach_inst_in_block(fs_inst, inst, block) {
      /* Skip some cases. */
      if (is_expression(this, inst) && !inst->is_partial_write() &&
          ((inst->dst.file != ARF && inst->dst.file != FIXED_GRF) ||
           inst->dst.is_null()))
      {
         bool found = false;
         bool negate = false;

         foreach_in_list_use_after(aeb_entry, entry, &aeb) {
            /* Match current instruction's expression against those in AEB. */
            if (!(entry->generator->dst.is_null() && !inst->dst.is_null()) &&
                instructions_match(inst, entry->generator, &negate)) {
               found = true;
               progress = true;
               break;
            }
         }

         if (!found) {
            if (inst->opcode != BRW_OPCODE_MOV ||
                (inst->opcode == BRW_OPCODE_MOV &&
                 inst->src[0].file == IMM &&
                 inst->src[0].type == BRW_REGISTER_TYPE_VF)) {
               /* Our first sighting of this expression.  Create an entry. */
               aeb_entry *entry = ralloc(cse_ctx, aeb_entry);
               entry->tmp = reg_undef;
               entry->generator = inst;
               aeb.push_tail(entry);
            }
         } else {
            /* This is at least our second sighting of this expression.
             * If we don't have a temporary already, make one.
             */
            bool no_existing_temp = entry->tmp.file == BAD_FILE;
            if (no_existing_temp && !entry->generator->dst.is_null()) {
               const fs_builder ibld = fs_builder(this, block, entry->generator)
                                       .at(block, entry->generator->next);
               int written = entry->generator->regs_written;

               entry->tmp = fs_reg(VGRF, alloc.allocate(written),
                                   entry->generator->dst.type);

               create_copy_instr(ibld, entry->generator, entry->tmp, false);

               entry->generator->dst = entry->tmp;
            }

            /* dest <- temp */
            if (!inst->dst.is_null()) {
               assert(inst->regs_written == entry->generator->regs_written);
               assert(inst->dst.type == entry->tmp.type);
               const fs_builder ibld(this, block, inst);

               create_copy_instr(ibld, inst, entry->tmp, negate);
            }

            /* Set our iterator so that next time through the loop inst->next
             * will get the instruction in the basic block after the one we've
             * removed.
             */
            fs_inst *prev = (fs_inst *)inst->prev;

            inst->remove(block);
            inst = prev;
         }
      }

      foreach_in_list_safe(aeb_entry, entry, &aeb) {
         /* Kill all AEB entries that write a different value to or read from
          * the flag register if we just wrote it.
          */
         if (inst->writes_flag()) {
            bool negate; /* dummy */
            if (entry->generator->reads_flag() ||
                (entry->generator->writes_flag() &&
                 !instructions_match(inst, entry->generator, &negate))) {
               entry->remove();
               ralloc_free(entry);
               continue;
            }
         }

         for (int i = 0; i < entry->generator->sources; i++) {
            fs_reg *src_reg = &entry->generator->src[i];

            /* Kill all AEB entries that use the destination we just
             * overwrote.
             */
            if (inst->overwrites_reg(entry->generator->src[i])) {
               entry->remove();
               ralloc_free(entry);
               break;
            }

            /* Kill any AEB entries using registers that don't get reused any
             * more -- a sure sign they'll fail operands_match().
             */
            if (src_reg->file == VGRF && virtual_grf_end[src_reg->nr] < ip) {
               entry->remove();
               ralloc_free(entry);
               break;
            }
         }
      }

      ip++;
   }

   ralloc_free(cse_ctx);

   return progress;
}

bool
fs_visitor::opt_cse()
{
   bool progress = false;

   calculate_live_intervals();

   foreach_block (block, cfg) {
      progress = opt_cse_local(block) || progress;
   }

   if (progress)
      invalidate_live_intervals();

   return progress;
}
<commit_msg>i965/fs: respect force_sechalf/force_writemask_all in CSE<commit_after>/*
 * Copyright © 2012 Intel Corporation
 *
 * Permission is hereby granted, free of charge, to any person obtaining a
 * copy of this software and associated documentation files (the "Software"),
 * to deal in the Software without restriction, including without limitation
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
 * and/or sell copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice (including the next
 * paragraph) shall be included in all copies or substantial portions of the
 * Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
 * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 * IN THE SOFTWARE.
 */

#include "brw_fs.h"
#include "brw_cfg.h"

/** @file brw_fs_cse.cpp
 *
 * Support for local common subexpression elimination.
 *
 * See Muchnick's Advanced Compiler Design and Implementation, section
 * 13.1 (p378).
 */

using namespace brw;

namespace {
struct aeb_entry : public exec_node {
   /** The instruction that generates the expression value. */
   fs_inst *generator;

   /** The temporary where the value is stored. */
   fs_reg tmp;
};
}

static bool
is_expression(const fs_visitor *v, const fs_inst *const inst)
{
   switch (inst->opcode) {
   case BRW_OPCODE_MOV:
   case BRW_OPCODE_SEL:
   case BRW_OPCODE_NOT:
   case BRW_OPCODE_AND:
   case BRW_OPCODE_OR:
   case BRW_OPCODE_XOR:
   case BRW_OPCODE_SHR:
   case BRW_OPCODE_SHL:
   case BRW_OPCODE_ASR:
   case BRW_OPCODE_CMP:
   case BRW_OPCODE_CMPN:
   case BRW_OPCODE_ADD:
   case BRW_OPCODE_MUL:
   case SHADER_OPCODE_MULH:
   case BRW_OPCODE_FRC:
   case BRW_OPCODE_RNDU:
   case BRW_OPCODE_RNDD:
   case BRW_OPCODE_RNDE:
   case BRW_OPCODE_RNDZ:
   case BRW_OPCODE_LINE:
   case BRW_OPCODE_PLN:
   case BRW_OPCODE_MAD:
   case BRW_OPCODE_LRP:
   case FS_OPCODE_UNIFORM_PULL_CONSTANT_LOAD:
   case FS_OPCODE_VARYING_PULL_CONSTANT_LOAD_GEN7:
   case FS_OPCODE_VARYING_PULL_CONSTANT_LOAD:
   case FS_OPCODE_CINTERP:
   case FS_OPCODE_LINTERP:
   case SHADER_OPCODE_FIND_LIVE_CHANNEL:
   case SHADER_OPCODE_BROADCAST:
   case SHADER_OPCODE_MOV_INDIRECT:
      return true;
   case SHADER_OPCODE_RCP:
   case SHADER_OPCODE_RSQ:
   case SHADER_OPCODE_SQRT:
   case SHADER_OPCODE_EXP2:
   case SHADER_OPCODE_LOG2:
   case SHADER_OPCODE_POW:
   case SHADER_OPCODE_INT_QUOTIENT:
   case SHADER_OPCODE_INT_REMAINDER:
   case SHADER_OPCODE_SIN:
   case SHADER_OPCODE_COS:
      return inst->mlen < 2;
   case SHADER_OPCODE_LOAD_PAYLOAD:
      return !inst->is_copy_payload(v->alloc);
   default:
      return inst->is_send_from_grf() && !inst->has_side_effects() &&
         !inst->is_volatile();
   }
}

static bool
operands_match(const fs_inst *a, const fs_inst *b, bool *negate)
{
   fs_reg *xs = a->src;
   fs_reg *ys = b->src;

   if (a->opcode == BRW_OPCODE_MAD) {
      return xs[0].equals(ys[0]) &&
             ((xs[1].equals(ys[1]) && xs[2].equals(ys[2])) ||
              (xs[2].equals(ys[1]) && xs[1].equals(ys[2])));
   } else if (a->opcode == BRW_OPCODE_MUL && a->dst.type == BRW_REGISTER_TYPE_F) {
      bool xs0_negate = xs[0].negate;
      bool xs1_negate = xs[1].file == IMM ? xs[1].f < 0.0f
                                          : xs[1].negate;
      bool ys0_negate = ys[0].negate;
      bool ys1_negate = ys[1].file == IMM ? ys[1].f < 0.0f
                                          : ys[1].negate;
      float xs1_imm = xs[1].f;
      float ys1_imm = ys[1].f;

      xs[0].negate = false;
      xs[1].negate = false;
      ys[0].negate = false;
      ys[1].negate = false;
      xs[1].f = fabsf(xs[1].f);
      ys[1].f = fabsf(ys[1].f);

      bool ret = (xs[0].equals(ys[0]) && xs[1].equals(ys[1])) ||
                 (xs[1].equals(ys[0]) && xs[0].equals(ys[1]));

      xs[0].negate = xs0_negate;
      xs[1].negate = xs[1].file == IMM ? false : xs1_negate;
      ys[0].negate = ys0_negate;
      ys[1].negate = ys[1].file == IMM ? false : ys1_negate;
      xs[1].f = xs1_imm;
      ys[1].f = ys1_imm;

      *negate = (xs0_negate != xs1_negate) != (ys0_negate != ys1_negate);
      return ret;
   } else if (!a->is_commutative()) {
      bool match = true;
      for (int i = 0; i < a->sources; i++) {
         if (!xs[i].equals(ys[i])) {
            match = false;
            break;
         }
      }
      return match;
   } else {
      return (xs[0].equals(ys[0]) && xs[1].equals(ys[1])) ||
             (xs[1].equals(ys[0]) && xs[0].equals(ys[1]));
   }
}

static bool
instructions_match(fs_inst *a, fs_inst *b, bool *negate)
{
   return a->opcode == b->opcode &&
          a->force_writemask_all == b->force_writemask_all &&
          a->exec_size == b->exec_size &&
          a->force_sechalf == b->force_sechalf &&
          a->saturate == b->saturate &&
          a->predicate == b->predicate &&
          a->predicate_inverse == b->predicate_inverse &&
          a->conditional_mod == b->conditional_mod &&
          a->flag_subreg == b->flag_subreg &&
          a->dst.type == b->dst.type &&
          a->offset == b->offset &&
          a->mlen == b->mlen &&
          a->regs_written == b->regs_written &&
          a->base_mrf == b->base_mrf &&
          a->eot == b->eot &&
          a->header_size == b->header_size &&
          a->shadow_compare == b->shadow_compare &&
          a->pi_noperspective == b->pi_noperspective &&
          a->sources == b->sources &&
          operands_match(a, b, negate);
}

static void
create_copy_instr(const fs_builder &bld, fs_inst *inst, fs_reg src, bool negate)
{
   int written = inst->regs_written;
   int dst_width = inst->exec_size / 8;
   fs_inst *copy;

   if (written > dst_width) {
      fs_reg *payload;
      int sources, header_size;
      if (inst->opcode == SHADER_OPCODE_LOAD_PAYLOAD) {
         sources = inst->sources;
         header_size = inst->header_size;
      } else {
         assert(written % dst_width == 0);
         sources = written / dst_width;
         header_size = 0;
      }

      assert(src.file == VGRF);
      payload = ralloc_array(bld.shader->mem_ctx, fs_reg, sources);
      for (int i = 0; i < header_size; i++) {
         payload[i] = src;
         src.reg_offset++;
      }
      for (int i = header_size; i < sources; i++) {
         payload[i] = src;
         src = offset(src, bld, 1);
      }
      copy = bld.LOAD_PAYLOAD(inst->dst, payload, sources, header_size);
   } else {
      copy = bld.MOV(inst->dst, src);
      copy->force_sechalf = inst->force_sechalf;
      copy->force_writemask_all = inst->force_writemask_all;
      copy->src[0].negate = negate;
   }
   assert(copy->regs_written == written);
}

bool
fs_visitor::opt_cse_local(bblock_t *block)
{
   bool progress = false;
   exec_list aeb;

   void *cse_ctx = ralloc_context(NULL);

   int ip = block->start_ip;
   foreach_inst_in_block(fs_inst, inst, block) {
      /* Skip some cases. */
      if (is_expression(this, inst) && !inst->is_partial_write() &&
          ((inst->dst.file != ARF && inst->dst.file != FIXED_GRF) ||
           inst->dst.is_null()))
      {
         bool found = false;
         bool negate = false;

         foreach_in_list_use_after(aeb_entry, entry, &aeb) {
            /* Match current instruction's expression against those in AEB. */
            if (!(entry->generator->dst.is_null() && !inst->dst.is_null()) &&
                instructions_match(inst, entry->generator, &negate)) {
               found = true;
               progress = true;
               break;
            }
         }

         if (!found) {
            if (inst->opcode != BRW_OPCODE_MOV ||
                (inst->opcode == BRW_OPCODE_MOV &&
                 inst->src[0].file == IMM &&
                 inst->src[0].type == BRW_REGISTER_TYPE_VF)) {
               /* Our first sighting of this expression.  Create an entry. */
               aeb_entry *entry = ralloc(cse_ctx, aeb_entry);
               entry->tmp = reg_undef;
               entry->generator = inst;
               aeb.push_tail(entry);
            }
         } else {
            /* This is at least our second sighting of this expression.
             * If we don't have a temporary already, make one.
             */
            bool no_existing_temp = entry->tmp.file == BAD_FILE;
            if (no_existing_temp && !entry->generator->dst.is_null()) {
               const fs_builder ibld = fs_builder(this, block, entry->generator)
                                       .at(block, entry->generator->next);
               int written = entry->generator->regs_written;

               entry->tmp = fs_reg(VGRF, alloc.allocate(written),
                                   entry->generator->dst.type);

               create_copy_instr(ibld, entry->generator, entry->tmp, false);

               entry->generator->dst = entry->tmp;
            }

            /* dest <- temp */
            if (!inst->dst.is_null()) {
               assert(inst->regs_written == entry->generator->regs_written);
               assert(inst->dst.type == entry->tmp.type);
               const fs_builder ibld(this, block, inst);

               create_copy_instr(ibld, inst, entry->tmp, negate);
            }

            /* Set our iterator so that next time through the loop inst->next
             * will get the instruction in the basic block after the one we've
             * removed.
             */
            fs_inst *prev = (fs_inst *)inst->prev;

            inst->remove(block);
            inst = prev;
         }
      }

      foreach_in_list_safe(aeb_entry, entry, &aeb) {
         /* Kill all AEB entries that write a different value to or read from
          * the flag register if we just wrote it.
          */
         if (inst->writes_flag()) {
            bool negate; /* dummy */
            if (entry->generator->reads_flag() ||
                (entry->generator->writes_flag() &&
                 !instructions_match(inst, entry->generator, &negate))) {
               entry->remove();
               ralloc_free(entry);
               continue;
            }
         }

         for (int i = 0; i < entry->generator->sources; i++) {
            fs_reg *src_reg = &entry->generator->src[i];

            /* Kill all AEB entries that use the destination we just
             * overwrote.
             */
            if (inst->overwrites_reg(entry->generator->src[i])) {
               entry->remove();
               ralloc_free(entry);
               break;
            }

            /* Kill any AEB entries using registers that don't get reused any
             * more -- a sure sign they'll fail operands_match().
             */
            if (src_reg->file == VGRF && virtual_grf_end[src_reg->nr] < ip) {
               entry->remove();
               ralloc_free(entry);
               break;
            }
         }
      }

      ip++;
   }

   ralloc_free(cse_ctx);

   return progress;
}

bool
fs_visitor::opt_cse()
{
   bool progress = false;

   calculate_live_intervals();

   foreach_block (block, cfg) {
      progress = opt_cse_local(block) || progress;
   }

   if (progress)
      invalidate_live_intervals();

   return progress;
}
<|endoftext|>


<commit_before>#ifndef TOML_PARSER
#define TOML_PARSER
#include "definitions.hpp"
#include "exceptions.hpp"
#include "line_operation.hpp"
#include "toml_values.hpp"
#include "parse_to_value.hpp"

namespace toml
{

enum class LineKind
{
    TABLE_TITLE,          // (ex) [tablename]
    ARRAY_OF_TABLE_TITLE, // (ex) [[tablename]]
    KEY_VALUE,            // (ex) key = value
    UNKNOWN,
};

template<typename charT, typename traits, typename alloc>
inline std::basic_string<charT, traits, alloc>
remove_comment(const std::basic_string<charT, traits, alloc>& line)
{
    return std::basic_string<charT, traits, alloc>(line.cbegin(),
            std::find(line.cbegin(), line.cend(), '#'));
}

template<typename charT, typename traits>
std::basic_string<charT, traits>
read_line(std::basic_istream<charT, traits>& is)
{
    std::basic_string<charT, traits> line;
    std::getline(is, line);
    return remove_indent(remove_extraneous_whitespace(remove_comment(line)));
}

template<typename charT, typename traits, typename alloc>
std::basic_istream<charT, traits>&
putback_line(std::basic_istream<charT, traits>& is,
             const std::basic_string<charT, traits, alloc>& line)
{
    for(auto iter = line.crbegin(); iter != line.crend(); ++iter)
        is.putback(*iter);
    return is;
}

template<typename charT, typename traits, typename alloc>
LineKind
determine_line_kind(const std::basic_string<charT, traits, alloc>& line)
{
    if(line.front() == '[' && line.back() == ']')
    {
        if(line.size() >= 4)
            if(line.at(1) == '[' && line.at(line.size() - 2) == ']')
                return LineKind::ARRAY_OF_TABLE_TITLE;
        return LineKind::TABLE_TITLE;
    }
    else if(std::count(line.begin(), line.end(), '=') >= 1)
    {
        if(line.front() == '=')
            throw syntax_error("line starts with =");
        return LineKind::KEY_VALUE;
    }
    else
        return LineKind::UNKNOWN;// a part of multi-line value?
}

template<typename charT, typename traits, typename alloc>
std::basic_string<charT, traits, alloc>
extract_table_title(const std::basic_string<charT, traits, alloc>& line)
{
    if(line.front() == '[' && line.back() == ']')
    {
        if(line.size() >= 4)
            if(line.at(1) == '[' && line.at(line.size() - 2) == ']')
                return line.substr(2, line.size()-4);
        return line.substr(1, line.size()-2);
    }
    else
        throw internal_error("not table title line");
}

template<typename charT, typename traits, typename alloc>
bool
is_multi_line_value(const std::basic_string<charT, traits, alloc>& line)
{
    // multi-line string/string-literal, multi-line array.
    // multi-line inline table(!) is ignored
         if(count(line, std::basic_string<charT, traits, alloc>("\"\"\"")) == 1)
        return true;
    else if(count(line, std::basic_string<charT, traits, alloc>("\'\'\'")) == 1)
        return true;
    else if(line.front() == '[')
        return !is_closed(line, '[', ']');
    else
        return false;
}

template<typename charT, typename traits, typename alloc>
std::pair<std::string, std::shared_ptr<value_base>>
parse_key_value(const std::basic_string<charT, traits, alloc>& line,
                std::basic_istream<charT, traits>& is)
{
    auto equal = std::find(line.begin(), line.end(), '=');

    std::basic_string<charT, traits, alloc> key(line.cbegin(), equal);
    key = remove_extraneous_whitespace(key);

    std::basic_string<charT, traits, alloc> value(equal+1, line.cend());
    value = remove_indent(value);

    while(is_multi_line_value(value))
    {
        if(is.eof()) throw end_of_file("eof");
        const auto additional = read_line(is);
        if(additional.empty()) continue;
        value += additional;
    }

    return std::make_pair(key, parse_value(value));
}

template<typename charT, typename traits>
std::shared_ptr<value_base>
parse_table(std::basic_istream<charT, traits>& is)
{
    std::shared_ptr<value_base> retval(new table_type);
    std::shared_ptr<table_type> table =
        std::dynamic_pointer_cast<table_type>(retval);
    while(!is.eof())
    {
        const std::basic_string<charT, traits> line = read_line(is);
        if(line.empty()) continue;
        LineKind kind = determine_line_kind(line);
        switch(kind)
        {
            case LineKind::TABLE_TITLE:
            {
                putback_line(is, line);
                return retval;
            }
            case LineKind::ARRAY_OF_TABLE_TITLE:
            {
                putback_line(is, line);
                return retval;
            }
            case LineKind::KEY_VALUE:
            {
                try
                {
                    const auto kv = parse_key_value(line, is);
                    table->value[kv.first] = kv.second;
                }
                catch(end_of_file& eof){throw syntax_error("sudden eof");}
                break;
            }
            case LineKind::UNKNOWN:
                throw syntax_error("unknown line: " + line); // XXX!
            default:
                throw internal_error("invalid line kind");
        }
    }
    return retval;
}

template<typename charT, typename traits>
Data parse(std::basic_istream<charT, traits>& is)
{
    Data data;
    while(!is.eof())
    {
        const std::basic_string<charT, traits> line = read_line(is);
        if(line.empty()) continue;
        LineKind kind = determine_line_kind(line);
        switch(kind)
        {
            case LineKind::TABLE_TITLE:
            {
                data[extract_table_title(line)] = parse_table(is);
                break;
            }
            case LineKind::ARRAY_OF_TABLE_TITLE:
            {

                break;
            }
            case LineKind::KEY_VALUE:
            {
                try
                {
                    const auto kv = parse_key_value(line, is);
                    data[kv.first] = kv.second;
                }
                catch(end_of_file& eof)
                {
                    throw syntax_error("sudden eof");
                }
                break;
            }
            case LineKind::UNKNOWN:
                throw syntax_error("unknown line: " + line);
            default:
                throw internal_error("invalid line kind");
        }
    }
    return data;
}

}//toml

#endif /* TOML_PARSER */
<commit_msg>fix putback_line using seekg<commit_after>#ifndef TOML_PARSER
#define TOML_PARSER
#include "definitions.hpp"
#include "exceptions.hpp"
#include "line_operation.hpp"
#include "toml_values.hpp"
#include "parse_value.hpp"

namespace toml
{

enum class LineKind
{
    TABLE_TITLE,          // (ex) [tablename]
    ARRAY_OF_TABLE_TITLE, // (ex) [[tablename]]
    KEY_VALUE,            // (ex) key = value
    UNKNOWN,
};

template<typename charT, typename traits, typename alloc>
inline std::basic_string<charT, traits, alloc>
remove_comment(const std::basic_string<charT, traits, alloc>& line)
{
    return std::basic_string<charT, traits, alloc>(line.cbegin(),
            std::find(line.cbegin(), line.cend(), '#'));
}

template<typename charT, typename traits>
std::basic_string<charT, traits>
read_line(std::basic_istream<charT, traits>& is)
{
    std::basic_string<charT, traits> line;
    std::getline(is, line);
    return remove_indent(remove_extraneous_whitespace(remove_comment(line)));
}

template<typename charT, typename traits, typename alloc>
LineKind
determine_line_kind(const std::basic_string<charT, traits, alloc>& line)
{
    if(line.front() == '[' && line.back() == ']')
    {
        if(line.size() >= 4)
            if(line.at(1) == '[' && line.at(line.size() - 2) == ']')
                return LineKind::ARRAY_OF_TABLE_TITLE;
        return LineKind::TABLE_TITLE;
    }
    else if(std::count(line.begin(), line.end(), '=') >= 1)
    {
        if(line.front() == '=')
            throw syntax_error("line starts with =");
        return LineKind::KEY_VALUE;
    }
    else
        return LineKind::UNKNOWN;// a part of multi-line value?
}

template<typename charT, typename traits, typename alloc>
std::basic_string<charT, traits, alloc>
extract_table_title(const std::basic_string<charT, traits, alloc>& line)
{
    if(line.front() == '[' && line.back() == ']')
    {
        if(line.size() >= 4)
            if(line.at(1) == '[' && line.at(line.size() - 2) == ']')
                return line.substr(2, line.size()-4);
        return line.substr(1, line.size()-2);
    }
    else
        throw internal_error("not table title line");
}

template<typename charT, typename traits, typename alloc>
bool
is_multi_line_value(const std::basic_string<charT, traits, alloc>& line)
{
    // multi-line string/string-literal, multi-line array.
    // multi-line inline table(!) is ignored
         if(count(line, std::basic_string<charT, traits, alloc>("\"\"\"")) == 1)
        return true;
    else if(count(line, std::basic_string<charT, traits, alloc>("\'\'\'")) == 1)
        return true;
    else if(line.front() == '[')
        return !is_closed(line, '[', ']');
    else
        return false;
}

template<typename charT, typename traits, typename alloc>
std::pair<std::string, std::shared_ptr<value_base>>
parse_key_value(const std::basic_string<charT, traits, alloc>& line,
                std::basic_istream<charT, traits>& is)
{
    auto equal = std::find(line.begin(), line.end(), '=');

    std::basic_string<charT, traits, alloc> key(line.cbegin(), equal);
    key = remove_extraneous_whitespace(key);

    std::basic_string<charT, traits, alloc> value(equal+1, line.cend());
    value = remove_indent(value);

    while(is_multi_line_value(value))
    {
        if(is.eof()) throw end_of_file("eof");
        const auto additional = read_line(is);
        if(additional.empty()) continue;
        value += additional;
    }

    return std::make_pair(key, parse_value(value));
}

template<typename charT, typename traits>
std::shared_ptr<value_base>
parse_table(std::basic_istream<charT, traits>& is)
{
    std::shared_ptr<value_base> retval(new table_type);
    std::shared_ptr<table_type> table =
        std::dynamic_pointer_cast<table_type>(retval);
    while(!is.eof())
    {
        const auto current_pos = is.tellg();
        const std::basic_string<charT, traits> line = read_line(is);
        if(line.empty()) continue;
        LineKind kind = determine_line_kind(line);
        switch(kind)
        {
            case LineKind::TABLE_TITLE:
            {
                is.seekg(current_pos);
                return retval;
            }
            case LineKind::ARRAY_OF_TABLE_TITLE:
            {
                is.seekg(current_pos);
                return retval;
            }
            case LineKind::KEY_VALUE:
            {
                try
                {
                    const auto kv = parse_key_value(line, is);
                    table->value[kv.first] = kv.second;
                }
                catch(end_of_file& eof){throw syntax_error("sudden eof");}
                break;
            }
            case LineKind::UNKNOWN:
                throw syntax_error("unknown line: " + line); // XXX!
            default:
                throw internal_error("invalid line kind");
        }
    }
    return retval;
}

template<typename charT, typename traits>
Data parse(std::basic_istream<charT, traits>& is)
{
    Data data;
    while(!is.eof())
    {
        const std::basic_string<charT, traits> line = read_line(is);
        if(line.empty()) continue;
        LineKind kind = determine_line_kind(line);
        switch(kind)
        {
            case LineKind::TABLE_TITLE:
            {
                data[extract_table_title(line)] = parse_table(is);
                break;
            }
            case LineKind::ARRAY_OF_TABLE_TITLE:
            {
                break;
            }
            case LineKind::KEY_VALUE:
            {
                try
                {
                    const auto kv = parse_key_value(line, is);
                    data[kv.first] = kv.second;
                }
                catch(end_of_file& eof)
                {
                    throw syntax_error("sudden eof");
                }
                break;
            }
            case LineKind::UNKNOWN:
                throw syntax_error("unknown line: " + line);
            default:
                throw internal_error("invalid line kind");
        }
    }
    return data;
}

}//toml

#endif /* TOML_PARSER */
<|endoftext|>


<commit_before>/****************************************************************************
*
*   Copyright (c) 2012-2015 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 PreflightCheck.cpp
*
* Preflight check for main system components
*
* @author Lorenz Meier <lorenz@px4.io>
* @author Johan Jansen <jnsn.johan@gmail.com>
*/

#include <nuttx/config.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>

#include <systemlib/err.h>
#include <systemlib/param/param.h>
#include <systemlib/rc_check.h>

#include <drivers/drv_hrt.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_gyro.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_baro.h>
#include <drivers/drv_airspeed.h>

#include <uORB/topics/airspeed.h>

#include <mavlink/mavlink_log.h>

#include "PreflightCheck.h"

namespace Commander
{
static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional)
{
	bool success = true;

	char s[30];
	sprintf(s, "%s%u", MAG_BASE_DEVICE_PATH, instance);
	int fd = open(s, 0);

	if (fd < 0) {
		if (!optional) {
			mavlink_and_console_log_critical(mavlink_fd,
							 "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance);
		}

		return false;
	}

	int calibration_devid;
	int ret;
	int devid = ioctl(fd, DEVIOCGDEVICEID, 0);
	sprintf(s, "CAL_MAG%u_ID", instance);
	param_get(param_find(s), &(calibration_devid));

	if (devid != calibration_devid) {
		mavlink_and_console_log_critical(mavlink_fd,
						 "PREFLIGHT FAIL: MAG #%u UNCALIBRATED (NO ID)", instance);
		success = false;
		goto out;
	}

	ret = ioctl(fd, MAGIOCSELFTEST, 0);

	if (ret != OK) {
		mavlink_and_console_log_critical(mavlink_fd,
						 "PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance);
		success = false;
		goto out;
	}

out:
	close(fd);
	return success;
}

static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic)
{
	bool success = true;

	char s[30];
	sprintf(s, "%s%u", ACCEL_BASE_DEVICE_PATH, instance);
	int fd = open(s, O_RDONLY);

	if (fd < 0) {
		if (!optional) {
			mavlink_and_console_log_critical(mavlink_fd,
							 "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance);
		}

		return false;
	}

	int calibration_devid;
	int ret;
	int devid = ioctl(fd, DEVIOCGDEVICEID, 0);
	sprintf(s, "CAL_ACC%u_ID", instance);
	param_get(param_find(s), &(calibration_devid));

	if (devid != calibration_devid) {
		mavlink_and_console_log_critical(mavlink_fd,
						 "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED (NO ID)", instance);
		success = false;
		goto out;
	}

	ret = ioctl(fd, ACCELIOCSELFTEST, 0);

	if (ret != OK) {
		mavlink_and_console_log_critical(mavlink_fd,
						 "PREFLIGHT FAIL: ACCEL #%u SELFTEST FAILED", instance);
		success = false;
		goto out;
	}

	if (dynamic) {
		/* check measurement result range */
		struct accel_report acc;
		ret = read(fd, &acc, sizeof(acc));

		if (ret == sizeof(acc)) {
			/* evaluate values */
			float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);

			if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
				mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still");
				/* this is frickin' fatal */
				success = false;
				goto out;
			}
		} else {
			mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ");
			/* this is frickin' fatal */
			success = false;
			goto out;
		}
	}

out:
	close(fd);
	return success;
}

static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional)
{
	bool success = true;

	char s[30];
	sprintf(s, "%s%u", GYRO_BASE_DEVICE_PATH, instance);
	int fd = open(s, 0);

	if (fd < 0) {
		if (!optional) {
			mavlink_and_console_log_critical(mavlink_fd,
							 "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance);
		}

		return false;
	}

	int calibration_devid;
	int ret;
	int devid = ioctl(fd, DEVIOCGDEVICEID, 0);
	sprintf(s, "CAL_GYRO%u_ID", instance);
	param_get(param_find(s), &(calibration_devid));

	if (devid != calibration_devid) {
		mavlink_and_console_log_critical(mavlink_fd,
						 "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED (NO ID)", instance);
		success = false;
		goto out;
	}

	ret = ioctl(fd, GYROIOCSELFTEST, 0);

	if (ret != OK) {
		mavlink_and_console_log_critical(mavlink_fd,
						 "PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance);
		success = false;
		goto out;
	}

out:
	close(fd);
	return success;
}

static bool baroCheck(int mavlink_fd, unsigned instance, bool optional)
{
	bool success = true;

	char s[30];
	sprintf(s, "%s%u", BARO_BASE_DEVICE_PATH, instance);
	int fd = open(s, 0);

	if (fd < 0) {
		if (!optional) {
			mavlink_and_console_log_critical(mavlink_fd,
							 "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance);
		}

		return false;
	}

	close(fd);
	return success;
}

static bool airspeedCheck(int mavlink_fd, bool optional)
{
	bool success = true;
	int ret;
	int fd = orb_subscribe(ORB_ID(airspeed));

	struct airspeed_s airspeed;

	if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||
	    (hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) {
		mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
		success = false;
		goto out;
	}

	if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) {
		mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
		// XXX do not make this fatal yet
	}

out:
	close(fd);
	return success;
}

bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro,
		    bool checkBaro, bool checkAirspeed, bool checkRC, bool checkDynamic)
{
	bool failed = false;

	/* ---- MAG ---- */
	if (checkMag) {
		/* check all sensors, but fail only for mandatory ones */
		for (unsigned i = 0; i < max_optional_mag_count; i++) {
			bool required = (i < max_mandatory_mag_count);

			if (!magnometerCheck(mavlink_fd, i, !required) && required) {
				failed = true;
			}
		}
	}

	/* ---- ACCEL ---- */
	if (checkAcc) {
		/* check all sensors, but fail only for mandatory ones */
		for (unsigned i = 0; i < max_optional_accel_count; i++) {
			bool required = (i < max_mandatory_accel_count);

			if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic) && required) {
				failed = true;
			}
		}
	}

	/* ---- GYRO ---- */
	if (checkGyro) {
		/* check all sensors, but fail only for mandatory ones */
		for (unsigned i = 0; i < max_optional_gyro_count; i++) {
			bool required = (i < max_mandatory_gyro_count);

			if (!gyroCheck(mavlink_fd, i, !required) && required) {
				failed = true;
			}
		}
	}

	/* ---- BARO ---- */
	if (checkBaro) {
		/* check all sensors, but fail only for mandatory ones */
		for (unsigned i = 0; i < max_optional_baro_count; i++) {
			bool required = (i < max_mandatory_baro_count);

			if (!baroCheck(mavlink_fd, i, !required) && required) {
				failed = true;
			}
		}
	}

	/* ---- AIRSPEED ---- */
	if (checkAirspeed) {
		if (!airspeedCheck(mavlink_fd, true)) {
			failed = true;
		}
	}

	/* ---- RC CALIBRATION ---- */
	if (checkRC) {
		if (rc_calibration_check(mavlink_fd) != OK) {
			failed = true;
		}
	}

	/* Report status */
	return !failed;
}

}
<commit_msg>Fixed parenthesis bug<commit_after>/****************************************************************************
*
*   Copyright (c) 2012-2015 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 PreflightCheck.cpp
*
* Preflight check for main system components
*
* @author Lorenz Meier <lorenz@px4.io>
* @author Johan Jansen <jnsn.johan@gmail.com>
*/

#include <px4_config.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>

#include <systemlib/err.h>
#include <systemlib/param/param.h>
#include <systemlib/rc_check.h>

#include <drivers/drv_hrt.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_gyro.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_baro.h>
#include <drivers/drv_airspeed.h>

#include <uORB/topics/airspeed.h>

#include <mavlink/mavlink_log.h>

#include "PreflightCheck.h"

namespace Commander
{
static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional)
{
	bool success = true;

	char s[30];
	sprintf(s, "%s%u", MAG_BASE_DEVICE_PATH, instance);
	int fd = open(s, 0);

	if (fd < 0) {
		if (!optional) {
			mavlink_and_console_log_critical(mavlink_fd,
							 "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance);
		}

		return false;
	}

	int calibration_devid;
	int ret;
	int devid = ioctl(fd, DEVIOCGDEVICEID, 0);
	sprintf(s, "CAL_MAG%u_ID", instance);
	param_get(param_find(s), &(calibration_devid));

	if (devid != calibration_devid) {
		mavlink_and_console_log_critical(mavlink_fd,
						 "PREFLIGHT FAIL: MAG #%u UNCALIBRATED (NO ID)", instance);
		success = false;
		goto out;
	}

	ret = ioctl(fd, MAGIOCSELFTEST, 0);

	if (ret != OK) {
		mavlink_and_console_log_critical(mavlink_fd,
						 "PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance);
		success = false;
		goto out;
	}

out:
	close(fd);
	return success;
}

static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic)
{
	bool success = true;

	char s[30];
	sprintf(s, "%s%u", ACCEL_BASE_DEVICE_PATH, instance);
	int fd = open(s, O_RDONLY);

	if (fd < 0) {
		if (!optional) {
			mavlink_and_console_log_critical(mavlink_fd,
							 "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance);
		}

		return false;
	}

	int calibration_devid;
	int ret;
	int devid = ioctl(fd, DEVIOCGDEVICEID, 0);
	sprintf(s, "CAL_ACC%u_ID", instance);
	param_get(param_find(s), &(calibration_devid));

	if (devid != calibration_devid) {
		mavlink_and_console_log_critical(mavlink_fd,
						 "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED (NO ID)", instance);
		success = false;
		goto out;
	}

	ret = ioctl(fd, ACCELIOCSELFTEST, 0);

	if (ret != OK) {
		mavlink_and_console_log_critical(mavlink_fd,
						 "PREFLIGHT FAIL: ACCEL #%u SELFTEST FAILED", instance);
		success = false;
		goto out;
	}

	if (dynamic) {
		/* check measurement result range */
		struct accel_report acc;
		ret = read(fd, &acc, sizeof(acc));

		if (ret == sizeof(acc)) {
			/* evaluate values */
			float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);

			if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
				mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still");
				/* this is frickin' fatal */
				success = false;
				goto out;
			}
		} else {
			mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ");
			/* this is frickin' fatal */
			success = false;
			goto out;
		}
	}

out:
	close(fd);
	return success;
}

static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional)
{
	bool success = true;

	char s[30];
	sprintf(s, "%s%u", GYRO_BASE_DEVICE_PATH, instance);
	int fd = open(s, 0);

	if (fd < 0) {
		if (!optional) {
			mavlink_and_console_log_critical(mavlink_fd,
							 "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance);
		}

		return false;
	}

	int calibration_devid;
	int ret;
	int devid = ioctl(fd, DEVIOCGDEVICEID, 0);
	sprintf(s, "CAL_GYRO%u_ID", instance);
	param_get(param_find(s), &(calibration_devid));

	if (devid != calibration_devid) {
		mavlink_and_console_log_critical(mavlink_fd,
						 "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED (NO ID)", instance);
		success = false;
		goto out;
	}

	ret = ioctl(fd, GYROIOCSELFTEST, 0);

	if (ret != OK) {
		mavlink_and_console_log_critical(mavlink_fd,
						 "PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance);
		success = false;
		goto out;
	}

out:
	close(fd);
	return success;
}

static bool baroCheck(int mavlink_fd, unsigned instance, bool optional)
{
	bool success = true;

	char s[30];
	sprintf(s, "%s%u", BARO_BASE_DEVICE_PATH, instance);
	int fd = open(s, 0);

	if (fd < 0) {
		if (!optional) {
			mavlink_and_console_log_critical(mavlink_fd,
							 "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance);
		}

		return false;
	}

	close(fd);
	return success;
}

static bool airspeedCheck(int mavlink_fd, bool optional)
{
	bool success = true;
	int ret;
	int fd = orb_subscribe(ORB_ID(airspeed));

	struct airspeed_s airspeed;

	if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||
	    (hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) {
		mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
		success = false;
		goto out;
	}

	if (fabsf(airspeed.indicated_airspeed_m_s) > 6.0f) {
		mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
		// XXX do not make this fatal yet
	}

out:
	close(fd);
	return success;
}

bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro,
		    bool checkBaro, bool checkAirspeed, bool checkRC, bool checkDynamic)
{
	bool failed = false;

	/* ---- MAG ---- */
	if (checkMag) {
		/* check all sensors, but fail only for mandatory ones */
		for (unsigned i = 0; i < max_optional_mag_count; i++) {
			bool required = (i < max_mandatory_mag_count);

			if (!magnometerCheck(mavlink_fd, i, !required) && required) {
				failed = true;
			}
		}
	}

	/* ---- ACCEL ---- */
	if (checkAcc) {
		/* check all sensors, but fail only for mandatory ones */
		for (unsigned i = 0; i < max_optional_accel_count; i++) {
			bool required = (i < max_mandatory_accel_count);

			if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic) && required) {
				failed = true;
			}
		}
	}

	/* ---- GYRO ---- */
	if (checkGyro) {
		/* check all sensors, but fail only for mandatory ones */
		for (unsigned i = 0; i < max_optional_gyro_count; i++) {
			bool required = (i < max_mandatory_gyro_count);

			if (!gyroCheck(mavlink_fd, i, !required) && required) {
				failed = true;
			}
		}
	}

	/* ---- BARO ---- */
	if (checkBaro) {
		/* check all sensors, but fail only for mandatory ones */
		for (unsigned i = 0; i < max_optional_baro_count; i++) {
			bool required = (i < max_mandatory_baro_count);

			if (!baroCheck(mavlink_fd, i, !required) && required) {
				failed = true;
			}
		}
	}

	/* ---- AIRSPEED ---- */
	if (checkAirspeed) {
		if (!airspeedCheck(mavlink_fd, true)) {
			failed = true;
		}
	}

	/* ---- RC CALIBRATION ---- */
	if (checkRC) {
		if (rc_calibration_check(mavlink_fd) != OK) {
			failed = true;
		}
	}

	/* Report status */
	return !failed;
}

}
<|endoftext|>


<commit_before>/****************************************************************************
*
*   Copyright (c) 2012-2015 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 PreflightCheck.cpp
*
* Preflight check for main system components
*
* @author Lorenz Meier <lorenz@px4.io>
* @author Johan Jansen <jnsn.johan@gmail.com>
*/

#include <nuttx/config.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>

#include <systemlib/err.h>
#include <systemlib/param/param.h>
#include <systemlib/rc_check.h>

#include <drivers/drv_hrt.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_gyro.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_baro.h>
#include <drivers/drv_airspeed.h>

#include <uORB/topics/airspeed.h>
#include <uORB/topics/vehicle_gps_position.h>

#include <mavlink/mavlink_log.h>

#include "PreflightCheck.h"

namespace Commander
{
static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional)
{
	bool success = true;

	char s[30];
	sprintf(s, "%s%u", MAG_BASE_DEVICE_PATH, instance);
	int fd = open(s, 0);

	if (fd < 0) {
		if (!optional) {
			mavlink_and_console_log_critical(mavlink_fd,
							 "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance);
		}

		return false;
	}

	int calibration_devid;
	int ret;
	int devid = ioctl(fd, DEVIOCGDEVICEID, 0);
	sprintf(s, "CAL_MAG%u_ID", instance);
	param_get(param_find(s), &(calibration_devid));

	if (devid != calibration_devid) {
		mavlink_and_console_log_critical(mavlink_fd,
						 "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance);
		success = false;
		goto out;
	}

	ret = ioctl(fd, MAGIOCSELFTEST, 0);

	if (ret != OK) {
		mavlink_and_console_log_critical(mavlink_fd,
						 "PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance);
		success = false;
		goto out;
	}

out:
	close(fd);
	return success;
}

static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic)
{
	bool success = true;

	char s[30];
	sprintf(s, "%s%u", ACCEL_BASE_DEVICE_PATH, instance);
	int fd = open(s, O_RDONLY);

	if (fd < 0) {
		if (!optional) {
			mavlink_and_console_log_critical(mavlink_fd,
							 "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance);
		}

		return false;
	}

	int calibration_devid;
	int ret;
	int devid = ioctl(fd, DEVIOCGDEVICEID, 0);
	sprintf(s, "CAL_ACC%u_ID", instance);
	param_get(param_find(s), &(calibration_devid));

	if (devid != calibration_devid) {
		mavlink_and_console_log_critical(mavlink_fd,
						 "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance);
		success = false;
		goto out;
	}

	ret = ioctl(fd, ACCELIOCSELFTEST, 0);

	if (ret != OK) {
		mavlink_and_console_log_critical(mavlink_fd,
						 "PREFLIGHT FAIL: ACCEL #%u SELFTEST FAILED", instance);
		success = false;
		goto out;
	}

	if (dynamic) {
		/* check measurement result range */
		struct accel_report acc;
		ret = read(fd, &acc, sizeof(acc));

		if (ret == sizeof(acc)) {
			/* evaluate values */
			float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);

			if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
				mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming");
				/* this is frickin' fatal */
				success = false;
				goto out;
			}
		} else {
			mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ");
			/* this is frickin' fatal */
			success = false;
			goto out;
		}
	}

out:
	close(fd);
	return success;
}

static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional)
{
	bool success = true;

	char s[30];
	sprintf(s, "%s%u", GYRO_BASE_DEVICE_PATH, instance);
	int fd = open(s, 0);

	if (fd < 0) {
		if (!optional) {
			mavlink_and_console_log_critical(mavlink_fd,
							 "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance);
		}

		return false;
	}

	int calibration_devid;
	int ret;
	int devid = ioctl(fd, DEVIOCGDEVICEID, 0);
	sprintf(s, "CAL_GYRO%u_ID", instance);
	param_get(param_find(s), &(calibration_devid));

	if (devid != calibration_devid) {
		mavlink_and_console_log_critical(mavlink_fd,
						 "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance);
		success = false;
		goto out;
	}

	ret = ioctl(fd, GYROIOCSELFTEST, 0);

	if (ret != OK) {
		mavlink_and_console_log_critical(mavlink_fd,
						 "PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance);
		success = false;
		goto out;
	}

out:
	close(fd);
	return success;
}

static bool baroCheck(int mavlink_fd, unsigned instance, bool optional)
{
	bool success = true;

	char s[30];
	sprintf(s, "%s%u", BARO_BASE_DEVICE_PATH, instance);
	int fd = open(s, 0);

	if (fd < 0) {
		if (!optional) {
			mavlink_and_console_log_critical(mavlink_fd,
							 "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance);
		}

		return false;
	}

	close(fd);
	return success;
}

static bool airspeedCheck(int mavlink_fd, bool optional)
{
	bool success = true;
	int ret;
	int fd = orb_subscribe(ORB_ID(airspeed));

	struct airspeed_s airspeed;

	if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||
	    (hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) {
		mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
		success = false;
		goto out;
	}

	if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) {
		mavlink_and_console_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
		// XXX do not make this fatal yet
	}

out:
	close(fd);
	return success;
}

static bool gnssCheck(int mavlink_fd)
{
	bool success = true;
	int gpsSub = orb_subscribe(ORB_ID(vehicle_gps_position));
	struct vehicle_gps_position_s gps;

	if (!orb_copy(ORB_ID(vehicle_gps_position), gpsSub, &gps) ||
	    (hrt_elapsed_time(&gps.timestamp_position) > 500000)) {
		mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING");
		success = false;
	}

	close(gpsSub);
	return success;
}

bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro,
		    bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic)
{
	bool failed = false;

	/* ---- MAG ---- */
	if (checkMag) {
		/* check all sensors, but fail only for mandatory ones */
		for (unsigned i = 0; i < max_optional_mag_count; i++) {
			bool required = (i < max_mandatory_mag_count);

			if (!magnometerCheck(mavlink_fd, i, !required) && required) {
				failed = true;
			}
		}
	}

	/* ---- ACCEL ---- */
	if (checkAcc) {
		/* check all sensors, but fail only for mandatory ones */
		for (unsigned i = 0; i < max_optional_accel_count; i++) {
			bool required = (i < max_mandatory_accel_count);

			if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic) && required) {
				failed = true;
			}
		}
	}

	/* ---- GYRO ---- */
	if (checkGyro) {
		/* check all sensors, but fail only for mandatory ones */
		for (unsigned i = 0; i < max_optional_gyro_count; i++) {
			bool required = (i < max_mandatory_gyro_count);

			if (!gyroCheck(mavlink_fd, i, !required) && required) {
				failed = true;
			}
		}
	}

	/* ---- BARO ---- */
	if (checkBaro) {
		/* check all sensors, but fail only for mandatory ones */
		for (unsigned i = 0; i < max_optional_baro_count; i++) {
			bool required = (i < max_mandatory_baro_count);

			if (!baroCheck(mavlink_fd, i, !required) && required) {
				failed = true;
			}
		}
	}

	/* ---- AIRSPEED ---- */
	if (checkAirspeed) {
		if (!airspeedCheck(mavlink_fd, true)) {
			failed = true;
		}
	}

	/* ---- RC CALIBRATION ---- */
	if (checkRC) {
		if (rc_calibration_check(mavlink_fd) != OK) {
			failed = true;
		}
	}

	/* ---- Global Navigation Satellite System receiver ---- */
	if(checkGNSS) {
		if(!gnssCheck(mavlink_fd)) {
			failed = true;
		}
	}

	/* Report status */
	return !failed;
}

}
<commit_msg>Commander: Wait up to 1 second to allow GPS module to be detected<commit_after>/****************************************************************************
*
*   Copyright (c) 2012-2015 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 PreflightCheck.cpp
*
* Preflight check for main system components
*
* @author Lorenz Meier <lorenz@px4.io>
* @author Johan Jansen <jnsn.johan@gmail.com>
*/

#include <nuttx/config.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <poll.h>

#include <systemlib/err.h>
#include <systemlib/param/param.h>
#include <systemlib/rc_check.h>

#include <drivers/drv_hrt.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_gyro.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_baro.h>
#include <drivers/drv_airspeed.h>

#include <uORB/topics/airspeed.h>
#include <uORB/topics/vehicle_gps_position.h>

#include <mavlink/mavlink_log.h>

#include "PreflightCheck.h"

namespace Commander
{
static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional)
{
	bool success = true;

	char s[30];
	sprintf(s, "%s%u", MAG_BASE_DEVICE_PATH, instance);
	int fd = open(s, 0);

	if (fd < 0) {
		if (!optional) {
			mavlink_and_console_log_critical(mavlink_fd,
							 "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance);
		}

		return false;
	}

	int calibration_devid;
	int ret;
	int devid = ioctl(fd, DEVIOCGDEVICEID, 0);
	sprintf(s, "CAL_MAG%u_ID", instance);
	param_get(param_find(s), &(calibration_devid));

	if (devid != calibration_devid) {
		mavlink_and_console_log_critical(mavlink_fd,
						 "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance);
		success = false;
		goto out;
	}

	ret = ioctl(fd, MAGIOCSELFTEST, 0);

	if (ret != OK) {
		mavlink_and_console_log_critical(mavlink_fd,
						 "PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance);
		success = false;
		goto out;
	}

out:
	close(fd);
	return success;
}

static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic)
{
	bool success = true;

	char s[30];
	sprintf(s, "%s%u", ACCEL_BASE_DEVICE_PATH, instance);
	int fd = open(s, O_RDONLY);

	if (fd < 0) {
		if (!optional) {
			mavlink_and_console_log_critical(mavlink_fd,
							 "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance);
		}

		return false;
	}

	int calibration_devid;
	int ret;
	int devid = ioctl(fd, DEVIOCGDEVICEID, 0);
	sprintf(s, "CAL_ACC%u_ID", instance);
	param_get(param_find(s), &(calibration_devid));

	if (devid != calibration_devid) {
		mavlink_and_console_log_critical(mavlink_fd,
						 "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance);
		success = false;
		goto out;
	}

	ret = ioctl(fd, ACCELIOCSELFTEST, 0);

	if (ret != OK) {
		mavlink_and_console_log_critical(mavlink_fd,
						 "PREFLIGHT FAIL: ACCEL #%u SELFTEST FAILED", instance);
		success = false;
		goto out;
	}

	if (dynamic) {
		/* check measurement result range */
		struct accel_report acc;
		ret = read(fd, &acc, sizeof(acc));

		if (ret == sizeof(acc)) {
			/* evaluate values */
			float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);

			if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
				mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming");
				/* this is frickin' fatal */
				success = false;
				goto out;
			}
		} else {
			mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ");
			/* this is frickin' fatal */
			success = false;
			goto out;
		}
	}

out:
	close(fd);
	return success;
}

static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional)
{
	bool success = true;

	char s[30];
	sprintf(s, "%s%u", GYRO_BASE_DEVICE_PATH, instance);
	int fd = open(s, 0);

	if (fd < 0) {
		if (!optional) {
			mavlink_and_console_log_critical(mavlink_fd,
							 "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance);
		}

		return false;
	}

	int calibration_devid;
	int ret;
	int devid = ioctl(fd, DEVIOCGDEVICEID, 0);
	sprintf(s, "CAL_GYRO%u_ID", instance);
	param_get(param_find(s), &(calibration_devid));

	if (devid != calibration_devid) {
		mavlink_and_console_log_critical(mavlink_fd,
						 "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance);
		success = false;
		goto out;
	}

	ret = ioctl(fd, GYROIOCSELFTEST, 0);

	if (ret != OK) {
		mavlink_and_console_log_critical(mavlink_fd,
						 "PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance);
		success = false;
		goto out;
	}

out:
	close(fd);
	return success;
}

static bool baroCheck(int mavlink_fd, unsigned instance, bool optional)
{
	bool success = true;

	char s[30];
	sprintf(s, "%s%u", BARO_BASE_DEVICE_PATH, instance);
	int fd = open(s, 0);

	if (fd < 0) {
		if (!optional) {
			mavlink_and_console_log_critical(mavlink_fd,
							 "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance);
		}

		return false;
	}

	close(fd);
	return success;
}

static bool airspeedCheck(int mavlink_fd, bool optional)
{
	bool success = true;
	int ret;
	int fd = orb_subscribe(ORB_ID(airspeed));

	struct airspeed_s airspeed;

	if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||
	    (hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) {
		mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
		success = false;
		goto out;
	}

	if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) {
		mavlink_and_console_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
		// XXX do not make this fatal yet
	}

out:
	close(fd);
	return success;
}

static bool gnssCheck(int mavlink_fd)
{
	bool success = true;

	int gpsSub = orb_subscribe(ORB_ID(vehicle_gps_position));

	//Wait up to 1000ms to allow the driver to detect a GNSS receiver module
	struct pollfd fds[1];
	fds[0].fd = gpsSub;
	fds[0].events = POLLIN;
	if(poll(fds, 1, 1000) <= 0) {
		success = false;
	}
	else {
		struct vehicle_gps_position_s gps;
		if ( (OK != orb_copy(ORB_ID(vehicle_gps_position), gpsSub, &gps)) ||
		    (hrt_elapsed_time(&gps.timestamp_position) > 1000000)) {
			success = false;
		}
	}

	//Report failure to detect module
	if(!success) {
		mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING");
	}

	close(gpsSub);
	return success;
}

bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro,
		    bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic)
{
	bool failed = false;

	/* ---- MAG ---- */
	if (checkMag) {
		/* check all sensors, but fail only for mandatory ones */
		for (unsigned i = 0; i < max_optional_mag_count; i++) {
			bool required = (i < max_mandatory_mag_count);

			if (!magnometerCheck(mavlink_fd, i, !required) && required) {
				failed = true;
			}
		}
	}

	/* ---- ACCEL ---- */
	if (checkAcc) {
		/* check all sensors, but fail only for mandatory ones */
		for (unsigned i = 0; i < max_optional_accel_count; i++) {
			bool required = (i < max_mandatory_accel_count);

			if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic) && required) {
				failed = true;
			}
		}
	}

	/* ---- GYRO ---- */
	if (checkGyro) {
		/* check all sensors, but fail only for mandatory ones */
		for (unsigned i = 0; i < max_optional_gyro_count; i++) {
			bool required = (i < max_mandatory_gyro_count);

			if (!gyroCheck(mavlink_fd, i, !required) && required) {
				failed = true;
			}
		}
	}

	/* ---- BARO ---- */
	if (checkBaro) {
		/* check all sensors, but fail only for mandatory ones */
		for (unsigned i = 0; i < max_optional_baro_count; i++) {
			bool required = (i < max_mandatory_baro_count);

			if (!baroCheck(mavlink_fd, i, !required) && required) {
				failed = true;
			}
		}
	}

	/* ---- AIRSPEED ---- */
	if (checkAirspeed) {
		if (!airspeedCheck(mavlink_fd, true)) {
			failed = true;
		}
	}

	/* ---- RC CALIBRATION ---- */
	if (checkRC) {
		if (rc_calibration_check(mavlink_fd) != OK) {
			failed = true;
		}
	}

	/* ---- Global Navigation Satellite System receiver ---- */
	if(checkGNSS) {
		if(!gnssCheck(mavlink_fd)) {
			failed = true;
		}
	}

	/* Report status */
	return !failed;
}

}
<|endoftext|>


<commit_before>/****************************************************************************
 *
 *   Copyright (C) 2013 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 rc_calibration.cpp
 * Remote Control calibration routine
 */

#include <px4_posix.h>
#include <px4_time.h>

#include "rc_calibration.h"
#include "commander_helper.h"

#include <poll.h>
#include <unistd.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>

/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;

int do_trim_calibration(orb_advert_t *mavlink_log_pub)
{
	int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
	usleep(400000);
	struct manual_control_setpoint_s sp;
	bool changed;
	orb_check(sub_man, &changed);

	if (!changed) {
		mavlink_log_critical(mavlink_log_pub, "no inputs, aborting");
		return ERROR;
	}

	orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);

	/* load trim values which are active */
	float roll_trim_active;
	param_get(param_find("TRIM_ROLL"), &roll_trim_active);
	float pitch_trim_active;
	param_get(param_find("TRIM_PITCH"), &pitch_trim_active);
	float yaw_trim_active;
	param_get(param_find("TRIM_YAW"), &yaw_trim_active);

	/* set parameters: the new trim values are the combination of active trim values
	   and the values coming from the remote control of the user
	*/
	float p = sp.y + roll_trim_active;
	int p1r = param_set(param_find("TRIM_ROLL"), &p);
	/*
	 we explicitly swap sign here because the trim is added to the actuator controls
	 which are moving in an inverse sense to manual pitch inputs
	*/
	p = -sp.x + pitch_trim_active;
	int p2r = param_set(param_find("TRIM_PITCH"), &p);
	p = sp.r + yaw_trim_active;
	int p3r = param_set(param_find("TRIM_YAW"), &p);

	/* store to permanent storage */
	/* auto-save */
	int save_ret = param_save_default();

	if (save_ret != 0 || p1r != 0 || p2r != 0 || p3r != 0) {
		mavlink_log_critical(mavlink_log_pub, "TRIM: PARAM SET FAIL");
		px4_close(sub_man);
		return ERROR;
	}

	mavlink_log_info(mavlink_log_pub, "trim cal done");
	px4_close(sub_man);
	return OK;
}
<commit_msg>consider scale parameters in rc calibration code<commit_after>/****************************************************************************
 *
 *   Copyright (C) 2013 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 rc_calibration.cpp
 * Remote Control calibration routine
 */

#include <px4_posix.h>
#include <px4_time.h>

#include "rc_calibration.h"
#include "commander_helper.h"

#include <poll.h>
#include <unistd.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>

/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;

int do_trim_calibration(orb_advert_t *mavlink_log_pub)
{
	int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
	usleep(400000);
	struct manual_control_setpoint_s sp;
	bool changed;
	orb_check(sub_man, &changed);

	if (!changed) {
		mavlink_log_critical(mavlink_log_pub, "no inputs, aborting");
		return ERROR;
	}

	orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);

	/* load trim values which are active */
	float roll_trim_active;
	param_get(param_find("TRIM_ROLL"), &roll_trim_active);
	float pitch_trim_active;
	param_get(param_find("TRIM_PITCH"), &pitch_trim_active);
	float yaw_trim_active;
	param_get(param_find("TRIM_YAW"), &yaw_trim_active);

	/* get manual control scale values */
	float roll_scale;
	param_get(param_find("FW_MAN_R_SC"), &roll_scale);
	float pitch_scale;
	param_get(param_find("FW_MAN_P_SC"), &pitch_scale);
	float yaw_scale;
	param_get(param_find("FW_MAN_Y_SC"), &yaw_scale);

	/* set parameters: the new trim values are the combination of active trim values
	   and the values coming from the remote control of the user
	*/
	float p = sp.y * roll_scale + roll_trim_active;
	int p1r = param_set(param_find("TRIM_ROLL"), &p);
	/*
	 we explicitly swap sign here because the trim is added to the actuator controls
	 which are moving in an inverse sense to manual pitch inputs
	*/
	p = -sp.x * pitch_scale + pitch_trim_active;
	int p2r = param_set(param_find("TRIM_PITCH"), &p);
	p = sp.r * yaw_scale + yaw_trim_active;
	int p3r = param_set(param_find("TRIM_YAW"), &p);

	/* store to permanent storage */
	/* auto-save */
	int save_ret = param_save_default();

	if (save_ret != 0 || p1r != 0 || p2r != 0 || p3r != 0) {
		mavlink_log_critical(mavlink_log_pub, "TRIM: PARAM SET FAIL");
		px4_close(sub_man);
		return ERROR;
	}

	mavlink_log_info(mavlink_log_pub, "trim cal done");
	px4_close(sub_man);
	return OK;
}
<|endoftext|>