<commit_before>// Copyright (c) 2006-2008 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "base/platform_thread.h"

#include <dlfcn.h>
#include <errno.h>
#include <sched.h>

#if defined(OS_MACOSX)
#include <mach/mach.h>
#include <sys/resource.h>
#include <algorithm>
#endif

#if defined(OS_LINUX)
#include <sys/prctl.h>
#include <sys/syscall.h>
#include <unistd.h>
#endif

#include "base/logging.h"
#include "base/safe_strerror_posix.h"

#if defined(OS_MACOSX)
namespace base {
void InitThreading();
}  // namespace base
#endif

static void* ThreadFunc(void* closure) {
  PlatformThread::Delegate* delegate =
      static_cast<PlatformThread::Delegate*>(closure);
  delegate->ThreadMain();
  return NULL;
}

// static
PlatformThreadId PlatformThread::CurrentId() {
  // Pthreads doesn't have the concept of a thread ID, so we have to reach down
  // into the kernel.
#if defined(OS_MACOSX)
  return mach_thread_self();
#elif defined(OS_LINUX)
  return syscall(__NR_gettid);
#elif defined(OS_FREEBSD)
  // TODO(BSD): find a better thread ID
  return reinterpret_cast<int64>(pthread_self());
#endif
}

// static
void PlatformThread::YieldCurrentThread() {
  sched_yield();
}

// static
void PlatformThread::Sleep(int duration_ms) {
  struct timespec sleep_time, remaining;

  // Contains the portion of duration_ms >= 1 sec.
  sleep_time.tv_sec = duration_ms / 1000;
  duration_ms -= sleep_time.tv_sec * 1000;

  // Contains the portion of duration_ms < 1 sec.
  sleep_time.tv_nsec = duration_ms * 1000 * 1000;  // nanoseconds.

  while (nanosleep(&sleep_time, &remaining) == -1 && errno == EINTR)
    sleep_time = remaining;
}

// Linux SetName is currently disabled, as we need to distinguish between
// helper threads (where it's ok to make this call) and the main thread
// (where making this call renames our process, causing tools like killall
// to stop working).
#if 0 && defined(OS_LINUX)
// static
void PlatformThread::SetName(const char* name) {
  // http://0pointer.de/blog/projects/name-your-threads.html

  // glibc recently added support for pthread_setname_np, but it's not
  // commonly available yet.  So test for it at runtime.
  int (*dynamic_pthread_setname_np)(pthread_t, const char*);
  *reinterpret_cast<void**>(&dynamic_pthread_setname_np) =
      dlsym(RTLD_DEFAULT, "pthread_setname_np");

  if (dynamic_pthread_setname_np) {
    // This limit comes from glibc, which gets it from the kernel
    // (TASK_COMM_LEN).
    const int kMaxNameLength = 15;
    std::string shortened_name = std::string(name).substr(0, kMaxNameLength);
    int err = dynamic_pthread_setname_np(pthread_self(),
                                         shortened_name.c_str());
    if (err < 0)
      LOG(ERROR) << "pthread_setname_np: " << safe_strerror(err);
  } else {
    // Implementing this function without glibc is simple enough.  (We
    // don't do the name length clipping as above because it will be
    // truncated by the callee (see TASK_COMM_LEN above).)
    int err = prctl(PR_SET_NAME, name);
    if (err < 0)
      PLOG(ERROR) << "prctl(PR_SET_NAME)";
  }
}
#elif defined(OS_MACOSX)
// Mac is implemented in platform_thread_mac.mm.
#else
// static
void PlatformThread::SetName(const char* name) {
  // Leave it unimplemented.

  // (This should be relatively simple to implement for the BSDs; I
  // just don't have one handy to test the code on.)
}
#endif  // defined(OS_LINUX)

namespace {

bool CreateThread(size_t stack_size, bool joinable,
                  PlatformThread::Delegate* delegate,
                  PlatformThreadHandle* thread_handle) {
#if defined(OS_MACOSX)
  base::InitThreading();
#endif  // OS_MACOSX

  bool success = false;
  pthread_attr_t attributes;
  pthread_attr_init(&attributes);

  // Pthreads are joinable by default, so only specify the detached attribute if
  // the thread should be non-joinable.
  if (!joinable) {
    pthread_attr_setdetachstate(&attributes, PTHREAD_CREATE_DETACHED);
  }

#if defined(OS_MACOSX)
  // The Mac OS X default for a pthread stack size is 512kB.
  // Libc-594.1.4/pthreads/pthread.c's pthread_attr_init uses
  // DEFAULT_STACK_SIZE for this purpose.
  //
  // 512kB isn't quite generous enough for some deeply recursive threads that
  // otherwise request the default stack size by specifying 0. Here, adopt
  // glibc's behavior as on Linux, which is to use the current stack size
  // limit (ulimit -s) as the default stack size. See
  // glibc-2.11.1/nptl/nptl-init.c's __pthread_initialize_minimal_internal. To
  // avoid setting the limit below the Mac OS X default or the minimum usable
  // stack size, these values are also considered. If any of these values
  // can't be determined, or if stack size is unlimited (ulimit -s unlimited),
  // stack_size is left at 0 to get the system default.
  //
  // Mac OS X normally only applies ulimit -s to the main thread stack. On
  // contemporary OS X and Linux systems alike, this value is generally 8MB
  // or in that neighborhood.
  if (stack_size == 0) {
    size_t default_stack_size;
    struct rlimit stack_rlimit;
    if (pthread_attr_getstacksize(&attributes, &default_stack_size) == 0 &&
        getrlimit(RLIMIT_STACK, &stack_rlimit) == 0 &&
        stack_rlimit.rlim_cur != RLIM_INFINITY) {
      stack_size = std::max(std::max(default_stack_size,
                                     static_cast<size_t>(PTHREAD_STACK_MIN)),
                            static_cast<size_t>(stack_rlimit.rlim_cur));
    }
  }
#endif  // OS_MACOSX

  if (stack_size > 0)
    pthread_attr_setstacksize(&attributes, stack_size);

  success = !pthread_create(thread_handle, &attributes, ThreadFunc, delegate);

  pthread_attr_destroy(&attributes);
  return success;
}

}  // anonymous namespace

// static
bool PlatformThread::Create(size_t stack_size, Delegate* delegate,
                            PlatformThreadHandle* thread_handle) {
  return CreateThread(stack_size, true /* joinable thread */,
                      delegate, thread_handle);
}

// static
bool PlatformThread::CreateNonJoinable(size_t stack_size, Delegate* delegate) {
  PlatformThreadHandle unused;

  bool result = CreateThread(stack_size, false /* non-joinable thread */,
                             delegate, &unused);
  return result;
}

// static
void PlatformThread::Join(PlatformThreadHandle thread_handle) {
  pthread_join(thread_handle, NULL);
}
<commit_msg>NaCl bringup of base.<commit_after>// Copyright (c) 2006-2008 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "base/platform_thread.h"

#include <errno.h>
#include <sched.h>

#if defined(OS_MACOSX)
#include <mach/mach.h>
#include <sys/resource.h>
#include <algorithm>
#endif

#if defined(OS_LINUX)
#include <dlfcn.h>
#include <sys/prctl.h>
#include <sys/syscall.h>
#include <unistd.h>
#endif

#if defined(OS_NACL)
#include <sys/nacl_syscalls.h>
#endif

#include "base/logging.h"
#include "base/safe_strerror_posix.h"

#if defined(OS_MACOSX)
namespace base {
void InitThreading();
}  // namespace base
#endif

static void* ThreadFunc(void* closure) {
  PlatformThread::Delegate* delegate =
      static_cast<PlatformThread::Delegate*>(closure);
  delegate->ThreadMain();
  return NULL;
}

// static
PlatformThreadId PlatformThread::CurrentId() {
  // Pthreads doesn't have the concept of a thread ID, so we have to reach down
  // into the kernel.
#if defined(OS_MACOSX)
  return mach_thread_self();
#elif defined(OS_LINUX)
  return syscall(__NR_gettid);
#elif defined(OS_FREEBSD)
  // TODO(BSD): find a better thread ID
  return reinterpret_cast<int64>(pthread_self());
#elif defined(OS_NACL)
  return pthread_self();
#endif
}

// static
void PlatformThread::YieldCurrentThread() {
  sched_yield();
}

// static
void PlatformThread::Sleep(int duration_ms) {
  struct timespec sleep_time, remaining;

  // Contains the portion of duration_ms >= 1 sec.
  sleep_time.tv_sec = duration_ms / 1000;
  duration_ms -= sleep_time.tv_sec * 1000;

  // Contains the portion of duration_ms < 1 sec.
  sleep_time.tv_nsec = duration_ms * 1000 * 1000;  // nanoseconds.

  while (nanosleep(&sleep_time, &remaining) == -1 && errno == EINTR)
    sleep_time = remaining;
}

// Linux SetName is currently disabled, as we need to distinguish between
// helper threads (where it's ok to make this call) and the main thread
// (where making this call renames our process, causing tools like killall
// to stop working).
#if 0 && defined(OS_LINUX)
// static
void PlatformThread::SetName(const char* name) {
  // http://0pointer.de/blog/projects/name-your-threads.html

  // glibc recently added support for pthread_setname_np, but it's not
  // commonly available yet.  So test for it at runtime.
  int (*dynamic_pthread_setname_np)(pthread_t, const char*);
  *reinterpret_cast<void**>(&dynamic_pthread_setname_np) =
      dlsym(RTLD_DEFAULT, "pthread_setname_np");

  if (dynamic_pthread_setname_np) {
    // This limit comes from glibc, which gets it from the kernel
    // (TASK_COMM_LEN).
    const int kMaxNameLength = 15;
    std::string shortened_name = std::string(name).substr(0, kMaxNameLength);
    int err = dynamic_pthread_setname_np(pthread_self(),
                                         shortened_name.c_str());
    if (err < 0)
      LOG(ERROR) << "pthread_setname_np: " << safe_strerror(err);
  } else {
    // Implementing this function without glibc is simple enough.  (We
    // don't do the name length clipping as above because it will be
    // truncated by the callee (see TASK_COMM_LEN above).)
    int err = prctl(PR_SET_NAME, name);
    if (err < 0)
      PLOG(ERROR) << "prctl(PR_SET_NAME)";
  }
}
#elif defined(OS_MACOSX)
// Mac is implemented in platform_thread_mac.mm.
#else
// static
void PlatformThread::SetName(const char* name) {
  // Leave it unimplemented.

  // (This should be relatively simple to implement for the BSDs; I
  // just don't have one handy to test the code on.)
}
#endif  // defined(OS_LINUX)

namespace {

bool CreateThread(size_t stack_size, bool joinable,
                  PlatformThread::Delegate* delegate,
                  PlatformThreadHandle* thread_handle) {
#if defined(OS_MACOSX)
  base::InitThreading();
#endif  // OS_MACOSX

  bool success = false;
  pthread_attr_t attributes;
  pthread_attr_init(&attributes);

  // Pthreads are joinable by default, so only specify the detached attribute if
  // the thread should be non-joinable.
  if (!joinable) {
    pthread_attr_setdetachstate(&attributes, PTHREAD_CREATE_DETACHED);
  }

#if defined(OS_MACOSX)
  // The Mac OS X default for a pthread stack size is 512kB.
  // Libc-594.1.4/pthreads/pthread.c's pthread_attr_init uses
  // DEFAULT_STACK_SIZE for this purpose.
  //
  // 512kB isn't quite generous enough for some deeply recursive threads that
  // otherwise request the default stack size by specifying 0. Here, adopt
  // glibc's behavior as on Linux, which is to use the current stack size
  // limit (ulimit -s) as the default stack size. See
  // glibc-2.11.1/nptl/nptl-init.c's __pthread_initialize_minimal_internal. To
  // avoid setting the limit below the Mac OS X default or the minimum usable
  // stack size, these values are also considered. If any of these values
  // can't be determined, or if stack size is unlimited (ulimit -s unlimited),
  // stack_size is left at 0 to get the system default.
  //
  // Mac OS X normally only applies ulimit -s to the main thread stack. On
  // contemporary OS X and Linux systems alike, this value is generally 8MB
  // or in that neighborhood.
  if (stack_size == 0) {
    size_t default_stack_size;
    struct rlimit stack_rlimit;
    if (pthread_attr_getstacksize(&attributes, &default_stack_size) == 0 &&
        getrlimit(RLIMIT_STACK, &stack_rlimit) == 0 &&
        stack_rlimit.rlim_cur != RLIM_INFINITY) {
      stack_size = std::max(std::max(default_stack_size,
                                     static_cast<size_t>(PTHREAD_STACK_MIN)),
                            static_cast<size_t>(stack_rlimit.rlim_cur));
    }
  }
#endif  // OS_MACOSX

  if (stack_size > 0)
    pthread_attr_setstacksize(&attributes, stack_size);

  success = !pthread_create(thread_handle, &attributes, ThreadFunc, delegate);

  pthread_attr_destroy(&attributes);
  return success;
}

}  // anonymous namespace

// static
bool PlatformThread::Create(size_t stack_size, Delegate* delegate,
                            PlatformThreadHandle* thread_handle) {
  return CreateThread(stack_size, true /* joinable thread */,
                      delegate, thread_handle);
}

// static
bool PlatformThread::CreateNonJoinable(size_t stack_size, Delegate* delegate) {
  PlatformThreadHandle unused;

  bool result = CreateThread(stack_size, false /* non-joinable thread */,
                             delegate, &unused);
  return result;
}

// static
void PlatformThread::Join(PlatformThreadHandle thread_handle) {
  pthread_join(thread_handle, NULL);
}
<|endoftext|>


<commit_before><commit_msg>Fix ProcessUtilTest.FDRemapping hang on x86_64 by counting open fds in child process nondestructively BUG=25270 TEST=base_unittests  --gtest_filter=ProcessUtilTest.FDRemapping  on x86_64 linux<commit_after><|endoftext|>


<commit_before>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: sbjsmeth.hxx,v $
 *
 *  $Revision: 1.5 $
 *
 *  last change: $Author: hr $ $Date: 2007-06-27 14:22:32 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    This library is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *    Lesser General Public License for more details.
 *
 *    You should have received a copy of the GNU Lesser General Public
 *    License along with this library; if not, write to the Free Software
 *    Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/

#ifndef _SB_SBJSMETH_HXX
#define _SB_SBJSMETH_HXX

#ifndef _SB_SBMETH_HXX
#include <basic/sbmeth.hxx>
#endif

// Basic-Modul fuer JavaScript-Sourcen.
// Alle Basic-spezifischen Methoden muessen virtuell ueberladen und deaktiviert
// werden. Die Unterscheidung von normalen Modulen erfolgt uebr RTTI.

class SbJScriptMethod : public SbMethod
{
public:
    SbJScriptMethod( const String&, SbxDataType, SbModule* );
    virtual ~SbJScriptMethod();

    SBX_DECL_PERSIST_NODATA(SBXCR_SBX,SBXID_JSCRIPTMETH,2);
    TYPEINFO();
};

#ifndef __SB_SBJSCRIPTMETHODREF_HXX
#define __SB_SBJSCRIPTMETHODREF_HXX
SV_DECL_IMPL_REF(SbJScriptMethod)
#endif

#endif
<commit_msg>INTEGRATION: CWS changefileheader (1.5.88); FILE MERGED 2008/04/01 10:48:42 thb 1.5.88.2: #i85898# Stripping all external header guards 2008/03/28 16:07:28 rt 1.5.88.1: #i87441# Change license header to LPGL v3.<commit_after>/*************************************************************************
 *
 * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
 *
 * Copyright 2008 by Sun Microsystems, Inc.
 *
 * OpenOffice.org - a multi-platform office productivity suite
 *
 * $RCSfile: sbjsmeth.hxx,v $
 * $Revision: 1.6 $
 *
 * This file is part of OpenOffice.org.
 *
 * OpenOffice.org is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License version 3
 * only, as published by the Free Software Foundation.
 *
 * OpenOffice.org is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License version 3 for more details
 * (a copy is included in the LICENSE file that accompanied this code).
 *
 * You should have received a copy of the GNU Lesser General Public License
 * version 3 along with OpenOffice.org.  If not, see
 * <http://www.openoffice.org/license.html>
 * for a copy of the LGPLv3 License.
 *
 ************************************************************************/

#ifndef _SB_SBJSMETH_HXX
#define _SB_SBJSMETH_HXX

#include <basic/sbmeth.hxx>

// Basic-Modul fuer JavaScript-Sourcen.
// Alle Basic-spezifischen Methoden muessen virtuell ueberladen und deaktiviert
// werden. Die Unterscheidung von normalen Modulen erfolgt uebr RTTI.

class SbJScriptMethod : public SbMethod
{
public:
    SbJScriptMethod( const String&, SbxDataType, SbModule* );
    virtual ~SbJScriptMethod();

    SBX_DECL_PERSIST_NODATA(SBXCR_SBX,SBXID_JSCRIPTMETH,2);
    TYPEINFO();
};

#ifndef __SB_SBJSCRIPTMETHODREF_HXX
#define __SB_SBJSCRIPTMETHODREF_HXX
SV_DECL_IMPL_REF(SbJScriptMethod)
#endif

#endif
<|endoftext|>


<commit_before>// RUN: %clang_cc1 -std=c++11 -fsyntax-only -fdiagnostics-show-option -verify %s

// C++98 [basic.lookup.classref]p1:
//   In a class member access expression (5.2.5), if the . or -> token is
//   immediately followed by an identifier followed by a <, the identifier must
//   be looked up to determine whether the < is the beginning of a template
//   argument list (14.2) or a less-than operator. The identifier is first
//   looked up in the class of the object expression. If the identifier is not
//   found, it is then looked up in the context of the entire postfix-expression
//   and shall name a class or function template. If the lookup in the class of
//   the object expression finds a template, the name is also looked up in the
//   context of the entire postfix-expression and
//    -- if the name is not found, the name found in the class of the object
//       expression is used, otherwise
//    -- if the name is found in the context of the entire postfix-expression
//       and does not name a class template, the name found in the class of the
//       object expression is used, otherwise
//    -- if the name found is a class template, it must refer to the same
//       entity as the one found in the class of the object expression,
//       otherwise the program is ill-formed.

// From PR 7247
template<typename T>
struct set{};
struct Value {
  template<typename T>
  void set(T value) {}

  void resolves_to_same() {
    Value v;
    v.set<double>(3.2);
  }
};
void resolves_to_different() {
  {
    Value v;
    // The fact that the next line is a warning rather than an error is an
    // extension.
    v.set<double>(3.2);
  }
  {
    int set;  // Non-template.
    Value v;
    v.set<double>(3.2);
  }
}

namespace rdar9915664 {
  struct A {
    template<typename T> void a();
  };

  struct B : A { };

  struct C : A { };

  struct D : B, C {
    A &getA() { return static_cast<B&>(*this); }

    void test_a() {
      getA().a<int>();
    }
  };
}

namespace PR11856 {
  template<typename T> T end(T);

  template <typename T>
  void Foo() {
    T it1;
    if (it1->end < it1->end) {
    }
  }

  template<typename T> T *end(T*);

  class X { };
  template <typename T>
  void Foo2() {
    T it1;
    if (it1->end < it1->end) {
    }

    X *x;
    if (x->end < 7) {  // expected-error{{no member named 'end' in 'PR11856::X'}}
    }
  }
}
<commit_msg>We don't need a lengthy quote from the wrong standard.<commit_after>// RUN: %clang_cc1 -std=c++11 -fsyntax-only -fdiagnostics-show-option -verify %s

template<typename T>
struct set{};
struct Value {
  template<typename T>
  void set(T value) {}

  void resolves_to_same() {
    Value v;
    v.set<double>(3.2);
  }
};
void resolves_to_different() {
  {
    Value v;
    // The fact that the next line is a warning rather than an error is an
    // extension.
    v.set<double>(3.2);
  }
  {
    int set;  // Non-template.
    Value v;
    v.set<double>(3.2);
  }
}

namespace rdar9915664 {
  struct A {
    template<typename T> void a();
  };

  struct B : A { };

  struct C : A { };

  struct D : B, C {
    A &getA() { return static_cast<B&>(*this); }

    void test_a() {
      getA().a<int>();
    }
  };
}

namespace PR11856 {
  template<typename T> T end(T);

  template <typename T>
  void Foo() {
    T it1;
    if (it1->end < it1->end) {
    }
  }

  template<typename T> T *end(T*);

  class X { };
  template <typename T>
  void Foo2() {
    T it1;
    if (it1->end < it1->end) {
    }

    X *x;
    if (x->end < 7) {  // expected-error{{no member named 'end' in 'PR11856::X'}}
    }
  }
}
<|endoftext|>


<commit_before><commit_msg>IMPALA-1735: ExpandRmReservation only check parent pools with limit<commit_after><|endoftext|>


<commit_before>#include <ctime>
#include <iostream>
#include <cstdlib>
#include <iterator>

#include <dariadb.h>
#include <utils/fs.h>
#include <storage/page_manager.h>
#include <ctime>
#include <limits>
#include <cmath>
#include <chrono>
#include <thread>
#include <atomic>
#include <cassert>

class BenchCallback :public dariadb::storage::Cursor::Callback {
public:
	void call(dariadb::storage::Chunk_Ptr &) {
		count++;
	}
	size_t count;
};

const std::string storagePath = "benchStorage/";
const size_t chunks_count = 1024;
const size_t chunks_size = 1024;

int main(int argc, char *argv[]) {
	(void)argc;
	(void)argv;
	{
		dariadb::Time t = 0;

		dariadb::Meas first;
		first.id = 1;
		first.time = t;

		dariadb::storage::Chunk_Ptr ch = std::make_shared<dariadb::storage::Chunk>(chunks_size, first);
		for (int i = 0;; i++, t++) {
			first.flag = dariadb::Flag(i);
			first.time = t;
			first.value = dariadb::Value(i);
			if (!ch->append(first)) {
				break;
			}
		}

		if (dariadb::utils::fs::path_exists(storagePath)) {
			dariadb::utils::fs::rm(storagePath);
		}
        dariadb::storage::PageManager::start(dariadb::storage::PageManager::Params(storagePath, dariadb::storage::MODE::SINGLE, chunks_count, chunks_size));

		auto start = clock();

		for (size_t i = 0; i < chunks_count; ++i) {
            dariadb::storage::PageManager::instance()->append(ch);
			ch->maxTime += dariadb::Time(chunks_size);
			ch->minTime += dariadb::Time(chunks_size);
		}

		auto elapsed = ((float)clock() - start) / CLOCKS_PER_SEC;
		std::cout << "insert : " << elapsed << std::endl;

		BenchCallback*clbk = new BenchCallback;
		start = clock();

		dariadb::Time to_t = chunks_size;
		for (size_t i = 1; i < chunks_count; i++) {
			auto cursor = dariadb::storage::PageManager::instance()->chunksByIterval(dariadb::IdArray{}, 0, to_t, 0);
			cursor->readAll(clbk);
			cursor = nullptr;
			to_t *= 2;
		}

		elapsed = ((float)clock() - start) / CLOCKS_PER_SEC;
		std::cout << "readAll : " << elapsed << std::endl;

		delete clbk;
		dariadb::storage::PageManager::stop();
		if (dariadb::utils::fs::path_exists(storagePath)) {
			dariadb::utils::fs::rm(storagePath);
		}
		ch = nullptr;
	}
}
<commit_msg>page_benchmark: read benchmarks.<commit_after>#include <ctime>
#include <iostream>
#include <cstdlib>
#include <iterator>

#include <dariadb.h>
#include <utils/fs.h>
#include <storage/page_manager.h>
#include <ctime>
#include <limits>
#include <cmath>
#include <chrono>
#include <thread>
#include <atomic>
#include <cassert>
#include <random>

class BenchCallback :public dariadb::storage::Cursor::Callback {
public:
    void call(dariadb::storage::Chunk_Ptr &) {
        count++;
    }
    size_t count;
};

const std::string storagePath = "benchStorage/";
const size_t chunks_count = 1024;
const size_t chunks_size = 1024;

int main(int argc, char *argv[]) {
    (void)argc;
    (void)argv;
    {
        dariadb::Time t = 0;

        dariadb::Meas first;
        first.id = 1;
        first.time = t;

        dariadb::storage::Chunk_Ptr ch = std::make_shared<dariadb::storage::Chunk>(chunks_size, first);
        for (int i = 0;; i++, t++) {
            first.flag = dariadb::Flag(i);
            first.time = t;
            first.value = dariadb::Value(i);
            if (!ch->append(first)) {
                break;
            }
        }

        if (dariadb::utils::fs::path_exists(storagePath)) {
            dariadb::utils::fs::rm(storagePath);
        }
        dariadb::storage::PageManager::start(dariadb::storage::PageManager::Params(storagePath, dariadb::storage::MODE::SINGLE, chunks_count, chunks_size));

        auto start = clock();

        for (size_t i = 0; i < chunks_count; ++i) {
            dariadb::storage::PageManager::instance()->append(ch);
            ch->maxTime += dariadb::Time(chunks_size);
            ch->minTime += dariadb::Time(chunks_size);
        }

        auto elapsed = ((float)clock() - start) / CLOCKS_PER_SEC;
        std::cout << "insert : " << elapsed << std::endl;
        dariadb::storage::PageManager::instance()->flush();
        std::random_device r;
        std::default_random_engine e1(r());
        std::uniform_int_distribution<dariadb::Id> uniform_dist(dariadb::storage::PageManager::instance()->minTime(), dariadb::storage::PageManager::instance()->maxTime());

        BenchCallback*clbk = new BenchCallback;

        start = clock();

        for (size_t i = 0; i < size_t(100); i++) {
            auto time_point1 = uniform_dist(e1);
            auto time_point2 = uniform_dist(e1);
            auto from=std::min(time_point1,time_point2);
            auto to=std::max(time_point1,time_point2);
            auto cursor = dariadb::storage::PageManager::instance()->chunksByIterval(dariadb::IdArray{}, from, to, 0);
            cursor->readAll(clbk);
            cursor = nullptr;
        }

        elapsed = ((float)clock() - start) / CLOCKS_PER_SEC;
        std::cout << "interval: " << elapsed << std::endl;

        start = clock();

        for (size_t i = 0; i < size_t(100); i++) {
            auto time_point = uniform_dist(e1);
            dariadb::storage::PageManager::instance()->chunksBeforeTimePoint(dariadb::IdArray{}, 0,time_point);
        }

        elapsed = ((float)clock() - start) / CLOCKS_PER_SEC;
        std::cout << "timePoint: " << elapsed << std::endl;

        delete clbk;

        dariadb::storage::PageManager::stop();
        if (dariadb::utils::fs::path_exists(storagePath)) {
            dariadb::utils::fs::rm(storagePath);
        }
        ch = nullptr;
    }
}
<|endoftext|>


<commit_before>/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

 Header:       FGTrimAxis.cpp
 Author:       Tony Peden
 Date started: 7/3/00

 --------- Copyright (C) 1999  Anthony K. Peden (apeden@earthlink.net) ---------

 This program is free software; you can redistribute it and/or modify it under
 the terms of the GNU General Public License as published by the Free Software
 Foundation; either version 2 of the License, or (at your option) any later
 version.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
 details.

 You should have received a copy of the GNU General Public License along with
 this program; if not, write to the Free Software Foundation, Inc., 59 Temple
 Place - Suite 330, Boston, MA  02111-1307, USA.

 Further information about the GNU General Public License can also be found on
 the world wide web at http://www.gnu.org.

 HISTORY
--------------------------------------------------------------------------------
7/3/00   TP   Created

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

#include <string>
#include <stdlib.h>

#include "FGFDMExec.h"
#include "FGAtmosphere.h"
#include "FGInitialCondition.h"
#include "FGTrimAxis.h"
#include "FGAircraft.h"

static const char *IdSrc = "$Header: /cvsroot/jsbsim/JSBSim/Attic/FGTrimAxis.cpp,v 1.10 2000/11/01 11:38:09 jsb Exp $";
static const char *IdHdr = ID_TRIMAXIS;

/*****************************************************************************/

FGTrimAxis::FGTrimAxis(FGFDMExec* fdex, FGInitialCondition* ic, Accel acc,
                       Control ctrl, float ff) {

  fdmex=fdex;
  fgic=ic;
  accel=acc;
  control=ctrl;
  tolerance=ff;
  solver_eps=tolerance;
  max_iterations=10;
  control_value=0;
  its_to_stable_value=0;
  total_iterations=0;
  total_stability_iterations=0;
  accel_convert=1.0;
  control_convert=1.0;
  accel_value=0;
  switch(control) {
  case tThrottle:
    control_min=0;
    control_max=1;
    control_value=0.5;
    break;
  case tBeta:
    control_min=-30*DEGTORAD;
    control_max=30*DEGTORAD;
    control_convert=RADTODEG;
    break;
  case tAlpha:
    control_min=fdmex->GetAircraft()->GetAlphaCLMin();
    control_max=fdmex->GetAircraft()->GetAlphaCLMax();
    if(control_max <= control_min) {
      control_max=20*DEGTORAD;
      control_min=-5*DEGTORAD;
    }
    control_value= (control_min+control_max)/2;
    control_convert=RADTODEG;
    solver_eps=tolerance/100;
    break;
  case tPitchTrim:
  case tElevator:
  case tRollTrim:
  case tAileron:
  case tYawTrim:
  case tRudder:
    control_min=-1;
    control_max=1;
    accel_convert=RADTODEG;
    solver_eps=tolerance/100;
    break;
  case tAltAGL:
    control_min=0;
    control_max=30;
    control_value=fdmex->GetPosition()->GetDistanceAGL();
    solver_eps=tolerance/100;
    break;
  case tTheta:
    control_min=fdmex->GetRotation()->Gettht() - 5*DEGTORAD;
    control_max=fdmex->GetRotation()->Gettht() + 5*DEGTORAD;
    accel_convert=RADTODEG;
    break;
  case tPhi:
    control_min=fdmex->GetRotation()->Getphi() - 5*DEGTORAD;
    control_max=fdmex->GetRotation()->Getphi() + 5*DEGTORAD;
    accel_convert=RADTODEG;
    break;
  case tGamma:
    solver_eps=tolerance/100;
    control_min=-80*DEGTORAD;
    control_max=80*DEGTORAD;
    control_convert=RADTODEG;
    break;
  }

}

/*****************************************************************************/

FGTrimAxis::~FGTrimAxis() {}

/*****************************************************************************/

void FGTrimAxis::getAccel(void) {
  switch(accel) {
  case tUdot: accel_value=fdmex -> GetTranslation()->GetUVWdot()(1); break;
  case tVdot: accel_value=fdmex -> GetTranslation()->GetUVWdot()(2); break;
  case tWdot: accel_value=fdmex -> GetTranslation()->GetUVWdot()(3); break;
  case tQdot: accel_value=fdmex -> GetRotation()->GetPQRdot()(2);break;
  case tPdot: accel_value=fdmex -> GetRotation()->GetPQRdot()(1); break;
  case tRdot: accel_value=fdmex -> GetRotation()->GetPQRdot()(3); break;
  }
}

/*****************************************************************************/

//Accels are not settable

void FGTrimAxis::getControl(void) {
  switch(control) {
  case tThrottle:  control_value=fdmex->GetFCS()->GetThrottleCmd(0); break;
  case tBeta:      control_value=fdmex->GetTranslation()->Getalpha(); break;
  case tAlpha:     control_value=fdmex->GetTranslation()->Getbeta();  break;
  case tPitchTrim: control_value=fdmex->GetFCS() -> GetPitchTrimCmd(); break;
  case tElevator:  control_value=fdmex->GetFCS() -> GetDeCmd(); break;
  case tRollTrim:
  case tAileron:   control_value=fdmex->GetFCS() -> GetDaCmd(); break;
  case tYawTrim:
  case tRudder:    control_value=fdmex->GetFCS() -> GetDrCmd(); break;
  case tAltAGL:    control_value=fdmex->GetPosition()->GetDistanceAGL();break;
  case tTheta:     control_value=fdmex->GetRotation()->Gettht(); break;
  case tPhi:       control_value=fdmex->GetRotation()->Getphi(); break;
  case tGamma:     control_value=fdmex->GetPosition()->GetGamma();break;
  }
}

/*****************************************************************************/

void FGTrimAxis::setControl(void) {
  switch(control) {
  case tThrottle:  setThrottlesPct(); break;
  case tBeta:      fgic->SetBetaRadIC(control_value); break;
  case tAlpha:     fgic->SetAlphaRadIC(control_value);  break;
  case tPitchTrim: fdmex->GetFCS() -> SetPitchTrimCmd(control_value); break;
  case tElevator:  fdmex-> GetFCS() -> SetDeCmd(control_value); break;
  case tRollTrim:
  case tAileron:   fdmex-> GetFCS() -> SetDaCmd(control_value); break;
  case tYawTrim:
  case tRudder:    fdmex-> GetFCS() -> SetDrCmd(control_value); break;
  case tAltAGL:    fgic->SetAltitudeAGLFtIC(control_value); break;
  case tTheta:     fgic->SetPitchAngleRadIC(control_value); break;
  case tPhi:       fgic->SetRollAngleRadIC(control_value); break;
  case tGamma:     fgic->SetFlightPathAngleRadIC(control_value); break;
  }
}

/*****************************************************************************/

// the aircraft center of rotation is no longer the cg once the gear
// contact the ground so the altitude needs to be changed when pitch
// and roll angle are adjusted.  Instead of attempting to calculate the
// new center of rotation, pick a gear unit as a reference and use its
// location vector to calculate the new height change. i.e. new altitude =
// earth z component of that vector (which is in body axes )
void FGTrimAxis::SetThetaOnGround(float ff) {
  int center,i,ref;

  // favor an off-center unit so that the same one can be used for both
  // pitch and roll.  An on-center unit is used (for pitch)if that's all
  // that's in contact with the ground.
  i=0; ref=-1; center=-1;
  while( (ref < 0) && (i < fdmex->GetAircraft()->GetNumGearUnits()) ) {
    if(fdmex->GetAircraft()->GetGearUnit(i)->GetWOW()) {
      if(fabs(fdmex->GetAircraft()->GetGearUnit(i)->GetBodyLocation()(2)) > 0.01)
        ref=i;
      else
        center=i;
    }
    i++;
  }
  if((ref < 0) && (center >= 0)) {
    ref=center;
  }
  cout << "SetThetaOnGround ref gear: " << ref << endl;
  if(ref >= 0) {
    float sp=fdmex->GetRotation()->GetSinphi();
    float cp=fdmex->GetRotation()->GetCosphi();
    float lx=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation()(1);
    float ly=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation()(2);
    float lz=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation()(3);
    float hagl = -1*lx*sin(ff) +
                    ly*sp*cos(ff) +
                    lz*cp*cos(ff);

    fgic->SetAltitudeAGLFtIC(hagl);
    cout << "SetThetaOnGround new alt: " << hagl << endl;
  }
  fgic->SetPitchAngleRadIC(ff);
  cout << "SetThetaOnGround new theta: " << ff << endl;
}

/*****************************************************************************/

bool FGTrimAxis::initTheta(void) {
  int i,N,iAft, iForward;
  float zAft,zForward,zDiff,theta;
  bool level;
  float saveAlt;

  saveAlt=fgic->GetAltitudeAGLFtIC();
  fgic->SetAltitudeAGLFtIC(100);

  N=fdmex->GetAircraft()->GetNumGearUnits();

  //find the first wheel unit forward of the cg
  //the list is short so a simple linear search is fine
  for( i=0; i<N; i++ ) {
    if(fdmex->GetAircraft()->GetGearUnit(i)->GetBodyLocation()(1) > 0 ) {
        iForward=i;
        break;
    }
  }
  //now find the first wheel unit aft of the cg
  for( i=0; i<N; i++ ) {
    if(fdmex->GetAircraft()->GetGearUnit(i)->GetBodyLocation()(1) < 0 ) {
        iAft=i;
        break;
    }
  }
  cout << "iForward: " << iForward << endl;
  cout << "iAft: " << iAft << endl;

  // now adjust theta till the wheels are the same distance from the ground
  zAft=fdmex->GetAircraft()->GetGearUnit(1)->GetLocalGear()(3);
  zForward=fdmex->GetAircraft()->GetGearUnit(0)->GetLocalGear()(3);
  zDiff = zForward - zAft;
  level=false;
  theta=fgic->GetPitchAngleDegIC();
  while(!level && (i < 100)) {
	theta+=2.0*zDiff;
	fgic->SetPitchAngleDegIC(theta);
	fdmex->RunIC(fgic);
	zAft=fdmex->GetAircraft()->GetGearUnit(1)->GetLocalGear()(3);
        zForward=fdmex->GetAircraft()->GetGearUnit(0)->GetLocalGear()(3);
        zDiff = zForward - zAft;
	//cout << endl << theta << "  " << zDiff << endl;
	//cout << "0: " << fdmex->GetAircraft()->GetGearUnit(0)->GetLocalGear() << endl;
	//cout << "1: " << fdmex->GetAircraft()->GetGearUnit(1)->GetLocalGear() << endl;

	if(fabs(zDiff ) < 0.1)
	    level=true;
	i++;
  }
  //cout << i << endl;
  cout << "Initial Theta: " << fdmex->GetRotation()->Gettht()*RADTODEG << endl;
  control_min=(theta+5)*DEGTORAD;
  control_max=(theta-5)*DEGTORAD;
  fgic->SetAltitudeAGLFtIC(saveAlt);
  if(i < 100)
    return true;
  else
    return false;
}

/*****************************************************************************/

void FGTrimAxis::SetPhiOnGround(float ff) {
  int i,ref;

  i=0; ref=-1;
  //must have an off-center unit here
  while( (ref < 0) && (i < fdmex->GetAircraft()->GetNumGearUnits()) ) {
    if( (fdmex->GetAircraft()->GetGearUnit(i)->GetWOW()) &&
      (fabs(fdmex->GetAircraft()->GetGearUnit(i)->GetBodyLocation()(2)) > 0.01))
        ref=i;
    i++;
  }
  if(ref >= 0) {
    float st=fdmex->GetRotation()->GetSintht();
    float ct=fdmex->GetRotation()->GetCostht();
    float lx=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation()(1);
    float ly=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation()(2);
    float lz=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation()(3);
    float hagl = -1*lx*st +
                    ly*sin(ff)*ct +
                    lz*cos(ff)*ct;

    fgic->SetAltitudeAGLFtIC(hagl);
  }
  fgic->SetRollAngleRadIC(ff);

}

/*****************************************************************************/

void FGTrimAxis::Run(void) {

  float last_accel_value;
  int i;
  setControl();
  //cout << "FGTrimAxis::Run: " << control_value << endl;
  i=0;
  bool stable=false;
  while(!stable) {
    i++;
    last_accel_value=accel_value;
    fdmex->RunIC(fgic);
    getAccel();
    if(i > 1) {
      if((fabs(last_accel_value - accel_value) < tolerance) || (i >= 100) )
        stable=true;
    }
  }

  its_to_stable_value=i;
  total_stability_iterations+=its_to_stable_value;
  total_iterations++;
}

/*****************************************************************************/

void FGTrimAxis::setThrottlesPct(void) {
  float tMin,tMax;
  for(unsigned i=0;i<fdmex->GetAircraft()->GetNumEngines();i++) {
      tMin=fdmex->GetAircraft()->GetEngine(i)->GetThrottleMin();
      tMax=fdmex->GetAircraft()->GetEngine(i)->GetThrottleMax();
      //cout << "setThrottlespct: " << i << ", " << control_min << ", " << control_max << ", " << control_value;
      fdmex -> GetFCS() -> SetThrottleCmd(i,tMin+control_value*(tMax-tMin));
  }
}

/*****************************************************************************/

void FGTrimAxis::AxisReport(void) {

  char out[80];
  sprintf(out,"  %20s: %6.2f %5s: %9.2e Tolerance: %3.0e\n",
           GetControlName().c_str(), GetControl()*control_convert,
           GetAccelName().c_str(), GetAccel(), GetTolerance());
  cout << out;

}

/*****************************************************************************/

float FGTrimAxis::GetAvgStability( void ) {
  if(total_iterations > 0) {
    return float(total_stability_iterations)/float(total_iterations);
  }
  return 0;
}

<commit_msg>AKP Cleanup<commit_after>/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

 Header:       FGTrimAxis.cpp
 Author:       Tony Peden
 Date started: 7/3/00

 --------- Copyright (C) 1999  Anthony K. Peden (apeden@earthlink.net) ---------

 This program is free software; you can redistribute it and/or modify it under
 the terms of the GNU General Public License as published by the Free Software
 Foundation; either version 2 of the License, or (at your option) any later
 version.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
 details.

 You should have received a copy of the GNU General Public License along with
 this program; if not, write to the Free Software Foundation, Inc., 59 Temple
 Place - Suite 330, Boston, MA  02111-1307, USA.

 Further information about the GNU General Public License can also be found on
 the world wide web at http://www.gnu.org.

 HISTORY
--------------------------------------------------------------------------------
7/3/00   TP   Created

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

#include <string>
#include <stdlib.h>

#include "FGFDMExec.h"
#include "FGAtmosphere.h"
#include "FGInitialCondition.h"
#include "FGTrimAxis.h"
#include "FGAircraft.h"

static const char *IdSrc = "$Header: /cvsroot/jsbsim/JSBSim/Attic/FGTrimAxis.cpp,v 1.11 2000/11/01 12:14:48 jsb Exp $";
static const char *IdHdr = ID_TRIMAXIS;

/*****************************************************************************/

FGTrimAxis::FGTrimAxis(FGFDMExec* fdex, FGInitialCondition* ic, Accel acc,
                       Control ctrl, float ff) {

  fdmex=fdex;
  fgic=ic;
  accel=acc;
  control=ctrl;
  tolerance=ff;
  solver_eps=tolerance;
  max_iterations=10;
  control_value=0;
  its_to_stable_value=0;
  total_iterations=0;
  total_stability_iterations=0;
  accel_convert=1.0;
  control_convert=1.0;
  accel_value=0;
  switch(control) {
  case tThrottle:
    control_min=0;
    control_max=1;
    control_value=0.5;
    break;
  case tBeta:
    control_min=-30*DEGTORAD;
    control_max=30*DEGTORAD;
    control_convert=RADTODEG;
    break;
  case tAlpha:
    control_min=fdmex->GetAircraft()->GetAlphaCLMin();
    control_max=fdmex->GetAircraft()->GetAlphaCLMax();
    if(control_max <= control_min) {
      control_max=20*DEGTORAD;
      control_min=-5*DEGTORAD;
    }
    control_value= (control_min+control_max)/2;
    control_convert=RADTODEG;
    solver_eps=tolerance/100;
    break;
  case tPitchTrim:
  case tElevator:
  case tRollTrim:
  case tAileron:
  case tYawTrim:
  case tRudder:
    control_min=-1;
    control_max=1;
    accel_convert=RADTODEG;
    solver_eps=tolerance/100;
    break;
  case tAltAGL:
    control_min=0;
    control_max=30;
    control_value=fdmex->GetPosition()->GetDistanceAGL();
    solver_eps=tolerance/100;
    break;
  case tTheta:
    control_min=fdmex->GetRotation()->Gettht() - 5*DEGTORAD;
    control_max=fdmex->GetRotation()->Gettht() + 5*DEGTORAD;
    accel_convert=RADTODEG;
    break;
  case tPhi:
    control_min=fdmex->GetRotation()->Getphi() - 5*DEGTORAD;
    control_max=fdmex->GetRotation()->Getphi() + 5*DEGTORAD;
    accel_convert=RADTODEG;
    break;
  case tGamma:
    solver_eps=tolerance/100;
    control_min=-80*DEGTORAD;
    control_max=80*DEGTORAD;
    control_convert=RADTODEG;
    break;
  }

}

/*****************************************************************************/

FGTrimAxis::~FGTrimAxis() {}

/*****************************************************************************/

void FGTrimAxis::getAccel(void) {
  switch(accel) {
  case tUdot: accel_value=fdmex -> GetTranslation()->GetUVWdot()(1); break;
  case tVdot: accel_value=fdmex -> GetTranslation()->GetUVWdot()(2); break;
  case tWdot: accel_value=fdmex -> GetTranslation()->GetUVWdot()(3); break;
  case tQdot: accel_value=fdmex -> GetRotation()->GetPQRdot()(2);break;
  case tPdot: accel_value=fdmex -> GetRotation()->GetPQRdot()(1); break;
  case tRdot: accel_value=fdmex -> GetRotation()->GetPQRdot()(3); break;
  }
}

/*****************************************************************************/

//Accels are not settable

void FGTrimAxis::getControl(void) {
  switch(control) {
  case tThrottle:  control_value=fdmex->GetFCS()->GetThrottleCmd(0); break;
  case tBeta:      control_value=fdmex->GetTranslation()->Getalpha(); break;
  case tAlpha:     control_value=fdmex->GetTranslation()->Getbeta();  break;
  case tPitchTrim: control_value=fdmex->GetFCS() -> GetPitchTrimCmd(); break;
  case tElevator:  control_value=fdmex->GetFCS() -> GetDeCmd(); break;
  case tRollTrim:
  case tAileron:   control_value=fdmex->GetFCS() -> GetDaCmd(); break;
  case tYawTrim:
  case tRudder:    control_value=fdmex->GetFCS() -> GetDrCmd(); break;
  case tAltAGL:    control_value=fdmex->GetPosition()->GetDistanceAGL();break;
  case tTheta:     control_value=fdmex->GetRotation()->Gettht(); break;
  case tPhi:       control_value=fdmex->GetRotation()->Getphi(); break;
  case tGamma:     control_value=fdmex->GetPosition()->GetGamma();break;
  }
}

/*****************************************************************************/

void FGTrimAxis::setControl(void) {
  switch(control) {
  case tThrottle:  setThrottlesPct(); break;
  case tBeta:      fgic->SetBetaRadIC(control_value); break;
  case tAlpha:     fgic->SetAlphaRadIC(control_value);  break;
  case tPitchTrim: fdmex->GetFCS() -> SetPitchTrimCmd(control_value); break;
  case tElevator:  fdmex-> GetFCS() -> SetDeCmd(control_value); break;
  case tRollTrim:
  case tAileron:   fdmex-> GetFCS() -> SetDaCmd(control_value); break;
  case tYawTrim:
  case tRudder:    fdmex-> GetFCS() -> SetDrCmd(control_value); break;
  case tAltAGL:    fgic->SetAltitudeAGLFtIC(control_value); break;
  case tTheta:     fgic->SetPitchAngleRadIC(control_value); break;
  case tPhi:       fgic->SetRollAngleRadIC(control_value); break;
  case tGamma:     fgic->SetFlightPathAngleRadIC(control_value); break;
  }
}

/*****************************************************************************/

// the aircraft center of rotation is no longer the cg once the gear
// contact the ground so the altitude needs to be changed when pitch
// and roll angle are adjusted.  Instead of attempting to calculate the
// new center of rotation, pick a gear unit as a reference and use its
// location vector to calculate the new height change. i.e. new altitude =
// earth z component of that vector (which is in body axes )
void FGTrimAxis::SetThetaOnGround(float ff) {
  int center,i,ref;

  // favor an off-center unit so that the same one can be used for both
  // pitch and roll.  An on-center unit is used (for pitch)if that's all
  // that's in contact with the ground.
  i=0; ref=-1; center=-1;
  while( (ref < 0) && (i < fdmex->GetAircraft()->GetNumGearUnits()) ) {
    if(fdmex->GetAircraft()->GetGearUnit(i)->GetWOW()) {
      if(fabs(fdmex->GetAircraft()->GetGearUnit(i)->GetBodyLocation()(2)) > 0.01)
        ref=i;
      else
        center=i;
    }
    i++;
  }
  if((ref < 0) && (center >= 0)) {
    ref=center;
  }
  cout << "SetThetaOnGround ref gear: " << ref << endl;
  if(ref >= 0) {
    float sp=fdmex->GetRotation()->GetSinphi();
    float cp=fdmex->GetRotation()->GetCosphi();
    float lx=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation()(1);
    float ly=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation()(2);
    float lz=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation()(3);
    float hagl = -1*lx*sin(ff) +
                    ly*sp*cos(ff) +
                    lz*cp*cos(ff);

    fgic->SetAltitudeAGLFtIC(hagl);
    cout << "SetThetaOnGround new alt: " << hagl << endl;
  }
  fgic->SetPitchAngleRadIC(ff);
  cout << "SetThetaOnGround new theta: " << ff << endl;
}

/*****************************************************************************/

bool FGTrimAxis::initTheta(void) {
  int i,N,iAft, iForward;
  float zAft,zForward,zDiff,theta;
  bool level;
  float saveAlt;

  saveAlt=fgic->GetAltitudeAGLFtIC();
  fgic->SetAltitudeAGLFtIC(100);

  N=fdmex->GetAircraft()->GetNumGearUnits();

  //find the first wheel unit forward of the cg
  //the list is short so a simple linear search is fine
  for( i=0; i<N; i++ ) {
    if(fdmex->GetAircraft()->GetGearUnit(i)->GetBodyLocation()(1) > 0 ) {
        iForward=i;
        break;
    }
  }
  //now find the first wheel unit aft of the cg
  for( i=0; i<N; i++ ) {
    if(fdmex->GetAircraft()->GetGearUnit(i)->GetBodyLocation()(1) < 0 ) {
        iAft=i;
        break;
    }
  }

  // now adjust theta till the wheels are the same distance from the ground
  zAft=fdmex->GetAircraft()->GetGearUnit(1)->GetLocalGear()(3);
  zForward=fdmex->GetAircraft()->GetGearUnit(0)->GetLocalGear()(3);
  zDiff = zForward - zAft;
  level=false;
  theta=fgic->GetPitchAngleDegIC();
  while(!level && (i < 100)) {
	theta+=2.0*zDiff;
	fgic->SetPitchAngleDegIC(theta);
	fdmex->RunIC(fgic);
	zAft=fdmex->GetAircraft()->GetGearUnit(1)->GetLocalGear()(3);
        zForward=fdmex->GetAircraft()->GetGearUnit(0)->GetLocalGear()(3);
        zDiff = zForward - zAft;
	//cout << endl << theta << "  " << zDiff << endl;
	//cout << "0: " << fdmex->GetAircraft()->GetGearUnit(0)->GetLocalGear() << endl;
	//cout << "1: " << fdmex->GetAircraft()->GetGearUnit(1)->GetLocalGear() << endl;

	if(fabs(zDiff ) < 0.1)
	    level=true;
	i++;
  }
  //cout << i << endl;
  cout << "    Initial Theta: " << fdmex->GetRotation()->Gettht()*RADTODEG << endl;
  control_min=(theta+5)*DEGTORAD;
  control_max=(theta-5)*DEGTORAD;
  fgic->SetAltitudeAGLFtIC(saveAlt);
  if(i < 100)
    return true;
  else
    return false;
}

/*****************************************************************************/

void FGTrimAxis::SetPhiOnGround(float ff) {
  int i,ref;

  i=0; ref=-1;
  //must have an off-center unit here
  while( (ref < 0) && (i < fdmex->GetAircraft()->GetNumGearUnits()) ) {
    if( (fdmex->GetAircraft()->GetGearUnit(i)->GetWOW()) &&
      (fabs(fdmex->GetAircraft()->GetGearUnit(i)->GetBodyLocation()(2)) > 0.01))
        ref=i;
    i++;
  }
  if(ref >= 0) {
    float st=fdmex->GetRotation()->GetSintht();
    float ct=fdmex->GetRotation()->GetCostht();
    float lx=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation()(1);
    float ly=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation()(2);
    float lz=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation()(3);
    float hagl = -1*lx*st +
                    ly*sin(ff)*ct +
                    lz*cos(ff)*ct;

    fgic->SetAltitudeAGLFtIC(hagl);
  }
  fgic->SetRollAngleRadIC(ff);

}

/*****************************************************************************/

void FGTrimAxis::Run(void) {

  float last_accel_value;
  int i;
  setControl();
  //cout << "FGTrimAxis::Run: " << control_value << endl;
  i=0;
  bool stable=false;
  while(!stable) {
    i++;
    last_accel_value=accel_value;
    fdmex->RunIC(fgic);
    getAccel();
    if(i > 1) {
      if((fabs(last_accel_value - accel_value) < tolerance) || (i >= 100) )
        stable=true;
    }
  }

  its_to_stable_value=i;
  total_stability_iterations+=its_to_stable_value;
  total_iterations++;
}

/*****************************************************************************/

void FGTrimAxis::setThrottlesPct(void) {
  float tMin,tMax;
  for(unsigned i=0;i<fdmex->GetAircraft()->GetNumEngines();i++) {
      tMin=fdmex->GetAircraft()->GetEngine(i)->GetThrottleMin();
      tMax=fdmex->GetAircraft()->GetEngine(i)->GetThrottleMax();
      //cout << "setThrottlespct: " << i << ", " << control_min << ", " << control_max << ", " << control_value;
      fdmex -> GetFCS() -> SetThrottleCmd(i,tMin+control_value*(tMax-tMin));
  }
}

/*****************************************************************************/

void FGTrimAxis::AxisReport(void) {

  char out[80];
  sprintf(out,"  %20s: %6.2f %5s: %9.2e Tolerance: %3.0e\n",
           GetControlName().c_str(), GetControl()*control_convert,
           GetAccelName().c_str(), GetAccel(), GetTolerance());
  cout << out;

}

/*****************************************************************************/

float FGTrimAxis::GetAvgStability( void ) {
  if(total_iterations > 0) {
    return float(total_stability_iterations)/float(total_iterations);
  }
  return 0;
}

<|endoftext|>


<commit_before>#include "types.h"
#include "mmu.h"
#include "kernel.hh"
#include "amd64.h"
#include "cpu.hh"
#include "traps.h"
#include "queue.h"
#include "spinlock.h"
#include "condvar.h"
#include "proc.hh"
#include "kmtrace.hh"
#include "bits.hh"
#include "kalloc.hh"
#include "apic.hh"
#include "kstream.hh"

extern "C" void __uaccess_end(void);

struct intdesc idt[256] __attribute__((aligned(16)));

// boot.S
extern u64 trapentry[];

u64
sysentry_c(u64 a0, u64 a1, u64 a2, u64 a3, u64 a4, u64 a5, u64 num)
{
  sti();

  if(myproc()->killed) {
    mtstart(trap, myproc());
    exit();
  }

  trapframe *tf = (trapframe*) (myproc()->kstack + KSTACKSIZE - sizeof(*tf));
  myproc()->tf = tf;
  u64 r = syscall(a0, a1, a2, a3, a4, a5, num);

  if(myproc()->killed) {
    mtstart(trap, myproc());
    exit();
  }

  return r;
}

int
do_pagefault(struct trapframe *tf)
{
  uptr addr = rcr2();
  if (myproc()->uaccess_) {
    if (addr >= USERTOP)
      panic("do_pagefault: %lx", addr);

    sti();
    if(pagefault(myproc()->vmap, addr, tf->err) >= 0){
#if MTRACE
      mtstop(myproc());
      if (myproc()->mtrace_stacks.curr >= 0)
        mtresume(myproc());
#endif
      return 0;
    }
    cprintf("pagefault: failed in kernel\n");
    tf->rax = -1;
    tf->rip = (u64)__uaccess_end;
    return 0;
  } else if (tf->err & FEC_U) {
      sti();
      if(pagefault(myproc()->vmap, addr, tf->err) >= 0){
#if MTRACE
        mtstop(myproc());
        if (myproc()->mtrace_stacks.curr >= 0)
          mtresume(myproc());
#endif
        return 0;
      }
      uerr.println("pagefault: failed in user");
      cli();
  }
  return -1;
}

void
trap(struct trapframe *tf)
{
  if (tf->trapno == T_NMI) {
    // The only locks that we can acquire during NMI are ones
    // we acquire only during NMI.
    if (sampintr(tf))
      return;
    panic("NMI");
  }

#if MTRACE
  if (myproc()->mtrace_stacks.curr >= 0)
    mtpause(myproc());
  mtstart(trap, myproc());
  // XXX mt_ascope ascope("trap:%d", tf->trapno);
#endif

  switch(tf->trapno){
  case T_IRQ0 + IRQ_TIMER:
    if (mycpu()->timer_printpc) {
      cprintf("cpu%d: proc %s rip %lx rsp %lx cs %x\n",
              mycpu()->id,
              myproc() ? myproc()->name : "(none)",
              tf->rip, tf->rsp, tf->cs);
      if (mycpu()->timer_printpc == 2 && tf->rbp > KBASE) {
        uptr pc[10];
        getcallerpcs((void *) tf->rbp, pc, NELEM(pc));
        for (int i = 0; i < 10 && pc[i]; i++)
          cprintf("cpu%d:   %lx\n", mycpu()->id, pc[i]);
      }
      mycpu()->timer_printpc = 0;
    }
    if (mycpu()->id == 0)
      timerintr();
    lapiceoi();
    break;
  case T_IRQ0 + IRQ_IDE:
    ideintr();
    lapiceoi();
    piceoi();
    break;
  case T_IRQ0 + IRQ_IDE+1:
    // Bochs generates spurious IDE1 interrupts.
    break;
  case T_IRQ0 + IRQ_KBD:
    kbdintr();
    lapiceoi();
    piceoi();
    break;
  case T_IRQ0 + IRQ_COM2:
  case T_IRQ0 + IRQ_COM1:
    uartintr();
    lapiceoi();
    piceoi();
    break;
  case T_IRQ0 + 7:
  case T_IRQ0 + IRQ_SPURIOUS:
    cprintf("cpu%d: spurious interrupt at %x:%lx\n",
            mycpu()->id, tf->cs, tf->rip);
    lapiceoi();
    break;
  case T_IRQ0 + IRQ_ERROR:
    cprintf("cpu%d: lapic error?\n", mycpu()->id);
    lapiceoi();
    break;
  case T_TLBFLUSH: {
    lapiceoi();
    u64 nreq = tlbflush_req.load();
    lcr3(rcr3());
    mycpu()->tlbflush_done = nreq;
    break;
  }
  case T_SAMPCONF:
    lapiceoi();
    sampconf();
    break;
  default:
    if (tf->trapno == T_IRQ0+e1000irq) {
      e1000intr();
      lapiceoi();
      piceoi();
      break;
    }

    if (tf->trapno == T_PGFLT && do_pagefault(tf) == 0)
      return;

    if (myproc() == 0 || (tf->cs&3) == 0)
      kerneltrap(tf);

    // In user space, assume process misbehaved.
    uerr.println("pid ", myproc()->pid, ' ', myproc()->name,
                 ": trap ", (u64)tf->trapno, " err ", (u32)tf->err,
                 " on cpu ", myid(), " rip ", shex(tf->rip),
                 " rsp ", shex(tf->rsp), " addr ", shex(rcr2()),
                 "--kill proc");
    myproc()->killed = 1;
  }

  // Force process exit if it has been killed and is in user space.
  // (If it is still executing in the kernel, let it keep running
  // until it gets to the regular system call return.)
  if(myproc() && myproc()->killed && (tf->cs&3) == 0x3)
    exit();

  // Force process to give up CPU on clock tick.
  // If interrupts were on while locks held, would need to check nlock.
  if(myproc() && myproc()->get_state() == RUNNING && tf->trapno == T_IRQ0+IRQ_TIMER)
    yield();

  // Check if the process has been killed since we yielded
  if(myproc() && myproc()->killed && (tf->cs&3) == 0x3)
    exit();

#if MTRACE
  mtstop(myproc());
  if (myproc()->mtrace_stacks.curr >= 0)
    mtresume(myproc());
#endif
}

void
inittrap(void)
{
  u64 entry;
  u8 bits;
  int i;

  bits = INT_P | SEG_INTR64;  // present, interrupt gate
  for(i=0; i<256; i++) {
    entry = trapentry[i];
    idt[i] = INTDESC(KCSEG, entry, bits);
  }
}

void
initnmi(void)
{
  void *nmistackbase = ksalloc(slab_stack);
  mycpu()->ts.ist[1] = (u64) nmistackbase + KSTACKSIZE;

  if (mycpu()->id == 0)
    idt[T_NMI].ist = 1;
}

void
initseg(void)
{
  volatile struct desctr dtr;
  struct cpu *c;

  dtr.limit = sizeof(idt) - 1;
  dtr.base = (u64)idt;
  lidt((void *)&dtr.limit);

  // TLS might not be ready
  c = &cpus[myid()];
  // Load per-CPU GDT
  memmove(c->gdt, bootgdt, sizeof(bootgdt));
  dtr.limit = sizeof(c->gdt) - 1;
  dtr.base = (u64)c->gdt;
  lgdt((void *)&dtr.limit);

  // When executing a syscall instruction the CPU sets the SS selector
  // to (star >> 32) + 8 and the CS selector to (star >> 32).
  // When executing a sysret instruction the CPU sets the SS selector
  // to (star >> 48) + 8 and the CS selector to (star >> 48) + 16.
  u64 star = ((((u64)UCSEG|0x3) - 16)<<48)|((u64)KCSEG<<32);
  writemsr(MSR_STAR, star);
  writemsr(MSR_LSTAR, (u64)&sysentry);
  writemsr(MSR_SFMASK, FL_TF | FL_IF);
}

// Pushcli/popcli are like cli/sti except that they are matched:
// it takes two popcli to undo two pushcli.  Also, if interrupts
// are off, then pushcli, popcli leaves them off.
void
pushcli(void)
{
  u64 rflags;

  rflags = readrflags();
  cli();
  if(mycpu()->ncli++ == 0)
    mycpu()->intena = rflags & FL_IF;
}

void
popcli(void)
{
  if(readrflags()&FL_IF)
    panic("popcli - interruptible");
  if(--mycpu()->ncli < 0)
    panic("popcli");
  if(mycpu()->ncli == 0 && mycpu()->intena)
    sti();
}

// Record the current call stack in pcs[] by following the %rbp chain.
void
getcallerpcs(void *v, uptr pcs[], int n)
{
  uptr *rbp;
  int i;

  rbp = (uptr*)v;
  for(i = 0; i < n; i++){
    if(rbp == 0 || rbp < (uptr*)KBASE || rbp == (uptr*)(~0UL) ||
       (rbp >= (uptr*)KBASEEND && rbp < (uptr*)KCODE))
      break;
    pcs[i] = rbp[1];     // saved %rip
    rbp = (uptr*)rbp[0]; // saved %rbp
  }
  for(; i < n; i++)
    pcs[i] = 0;
}
<commit_msg>Subtract one from PCs recorded by getcallerpcs<commit_after>#include "types.h"
#include "mmu.h"
#include "kernel.hh"
#include "amd64.h"
#include "cpu.hh"
#include "traps.h"
#include "queue.h"
#include "spinlock.h"
#include "condvar.h"
#include "proc.hh"
#include "kmtrace.hh"
#include "bits.hh"
#include "kalloc.hh"
#include "apic.hh"
#include "kstream.hh"

extern "C" void __uaccess_end(void);

struct intdesc idt[256] __attribute__((aligned(16)));

// boot.S
extern u64 trapentry[];

u64
sysentry_c(u64 a0, u64 a1, u64 a2, u64 a3, u64 a4, u64 a5, u64 num)
{
  sti();

  if(myproc()->killed) {
    mtstart(trap, myproc());
    exit();
  }

  trapframe *tf = (trapframe*) (myproc()->kstack + KSTACKSIZE - sizeof(*tf));
  myproc()->tf = tf;
  u64 r = syscall(a0, a1, a2, a3, a4, a5, num);

  if(myproc()->killed) {
    mtstart(trap, myproc());
    exit();
  }

  return r;
}

int
do_pagefault(struct trapframe *tf)
{
  uptr addr = rcr2();
  if (myproc()->uaccess_) {
    if (addr >= USERTOP)
      panic("do_pagefault: %lx", addr);

    sti();
    if(pagefault(myproc()->vmap, addr, tf->err) >= 0){
#if MTRACE
      mtstop(myproc());
      if (myproc()->mtrace_stacks.curr >= 0)
        mtresume(myproc());
#endif
      return 0;
    }
    cprintf("pagefault: failed in kernel\n");
    tf->rax = -1;
    tf->rip = (u64)__uaccess_end;
    return 0;
  } else if (tf->err & FEC_U) {
      sti();
      if(pagefault(myproc()->vmap, addr, tf->err) >= 0){
#if MTRACE
        mtstop(myproc());
        if (myproc()->mtrace_stacks.curr >= 0)
          mtresume(myproc());
#endif
        return 0;
      }
      uerr.println("pagefault: failed in user");
      cli();
  }
  return -1;
}

void
trap(struct trapframe *tf)
{
  if (tf->trapno == T_NMI) {
    // The only locks that we can acquire during NMI are ones
    // we acquire only during NMI.
    if (sampintr(tf))
      return;
    panic("NMI");
  }

#if MTRACE
  if (myproc()->mtrace_stacks.curr >= 0)
    mtpause(myproc());
  mtstart(trap, myproc());
  // XXX mt_ascope ascope("trap:%d", tf->trapno);
#endif

  switch(tf->trapno){
  case T_IRQ0 + IRQ_TIMER:
    if (mycpu()->timer_printpc) {
      cprintf("cpu%d: proc %s rip %lx rsp %lx cs %x\n",
              mycpu()->id,
              myproc() ? myproc()->name : "(none)",
              tf->rip, tf->rsp, tf->cs);
      if (mycpu()->timer_printpc == 2 && tf->rbp > KBASE) {
        uptr pc[10];
        getcallerpcs((void *) tf->rbp, pc, NELEM(pc));
        for (int i = 0; i < 10 && pc[i]; i++)
          cprintf("cpu%d:   %lx\n", mycpu()->id, pc[i]);
      }
      mycpu()->timer_printpc = 0;
    }
    if (mycpu()->id == 0)
      timerintr();
    lapiceoi();
    break;
  case T_IRQ0 + IRQ_IDE:
    ideintr();
    lapiceoi();
    piceoi();
    break;
  case T_IRQ0 + IRQ_IDE+1:
    // Bochs generates spurious IDE1 interrupts.
    break;
  case T_IRQ0 + IRQ_KBD:
    kbdintr();
    lapiceoi();
    piceoi();
    break;
  case T_IRQ0 + IRQ_COM2:
  case T_IRQ0 + IRQ_COM1:
    uartintr();
    lapiceoi();
    piceoi();
    break;
  case T_IRQ0 + 7:
  case T_IRQ0 + IRQ_SPURIOUS:
    cprintf("cpu%d: spurious interrupt at %x:%lx\n",
            mycpu()->id, tf->cs, tf->rip);
    lapiceoi();
    break;
  case T_IRQ0 + IRQ_ERROR:
    cprintf("cpu%d: lapic error?\n", mycpu()->id);
    lapiceoi();
    break;
  case T_TLBFLUSH: {
    lapiceoi();
    u64 nreq = tlbflush_req.load();
    lcr3(rcr3());
    mycpu()->tlbflush_done = nreq;
    break;
  }
  case T_SAMPCONF:
    lapiceoi();
    sampconf();
    break;
  default:
    if (tf->trapno == T_IRQ0+e1000irq) {
      e1000intr();
      lapiceoi();
      piceoi();
      break;
    }

    if (tf->trapno == T_PGFLT && do_pagefault(tf) == 0)
      return;

    if (myproc() == 0 || (tf->cs&3) == 0)
      kerneltrap(tf);

    // In user space, assume process misbehaved.
    uerr.println("pid ", myproc()->pid, ' ', myproc()->name,
                 ": trap ", (u64)tf->trapno, " err ", (u32)tf->err,
                 " on cpu ", myid(), " rip ", shex(tf->rip),
                 " rsp ", shex(tf->rsp), " addr ", shex(rcr2()),
                 "--kill proc");
    myproc()->killed = 1;
  }

  // Force process exit if it has been killed and is in user space.
  // (If it is still executing in the kernel, let it keep running
  // until it gets to the regular system call return.)
  if(myproc() && myproc()->killed && (tf->cs&3) == 0x3)
    exit();

  // Force process to give up CPU on clock tick.
  // If interrupts were on while locks held, would need to check nlock.
  if(myproc() && myproc()->get_state() == RUNNING && tf->trapno == T_IRQ0+IRQ_TIMER)
    yield();

  // Check if the process has been killed since we yielded
  if(myproc() && myproc()->killed && (tf->cs&3) == 0x3)
    exit();

#if MTRACE
  mtstop(myproc());
  if (myproc()->mtrace_stacks.curr >= 0)
    mtresume(myproc());
#endif
}

void
inittrap(void)
{
  u64 entry;
  u8 bits;
  int i;

  bits = INT_P | SEG_INTR64;  // present, interrupt gate
  for(i=0; i<256; i++) {
    entry = trapentry[i];
    idt[i] = INTDESC(KCSEG, entry, bits);
  }
}

void
initnmi(void)
{
  void *nmistackbase = ksalloc(slab_stack);
  mycpu()->ts.ist[1] = (u64) nmistackbase + KSTACKSIZE;

  if (mycpu()->id == 0)
    idt[T_NMI].ist = 1;
}

void
initseg(void)
{
  volatile struct desctr dtr;
  struct cpu *c;

  dtr.limit = sizeof(idt) - 1;
  dtr.base = (u64)idt;
  lidt((void *)&dtr.limit);

  // TLS might not be ready
  c = &cpus[myid()];
  // Load per-CPU GDT
  memmove(c->gdt, bootgdt, sizeof(bootgdt));
  dtr.limit = sizeof(c->gdt) - 1;
  dtr.base = (u64)c->gdt;
  lgdt((void *)&dtr.limit);

  // When executing a syscall instruction the CPU sets the SS selector
  // to (star >> 32) + 8 and the CS selector to (star >> 32).
  // When executing a sysret instruction the CPU sets the SS selector
  // to (star >> 48) + 8 and the CS selector to (star >> 48) + 16.
  u64 star = ((((u64)UCSEG|0x3) - 16)<<48)|((u64)KCSEG<<32);
  writemsr(MSR_STAR, star);
  writemsr(MSR_LSTAR, (u64)&sysentry);
  writemsr(MSR_SFMASK, FL_TF | FL_IF);
}

// Pushcli/popcli are like cli/sti except that they are matched:
// it takes two popcli to undo two pushcli.  Also, if interrupts
// are off, then pushcli, popcli leaves them off.
void
pushcli(void)
{
  u64 rflags;

  rflags = readrflags();
  cli();
  if(mycpu()->ncli++ == 0)
    mycpu()->intena = rflags & FL_IF;
}

void
popcli(void)
{
  if(readrflags()&FL_IF)
    panic("popcli - interruptible");
  if(--mycpu()->ncli < 0)
    panic("popcli");
  if(mycpu()->ncli == 0 && mycpu()->intena)
    sti();
}

// Record the current call stack in pcs[] by following the %rbp chain.
void
getcallerpcs(void *v, uptr pcs[], int n)
{
  uptr *rbp;
  int i;

  rbp = (uptr*)v;
  for(i = 0; i < n; i++){
    if(rbp == 0 || rbp < (uptr*)KBASE || rbp == (uptr*)(~0UL) ||
       (rbp >= (uptr*)KBASEEND && rbp < (uptr*)KCODE))
      break;
    pcs[i] = rbp[1] - 1; // saved %rip; - 1 so it points to the call
                         // instruction
    rbp = (uptr*)rbp[0]; // saved %rbp
  }
  for(; i < n; i++)
    pcs[i] = 0;
}
<|endoftext|>


<commit_before>// File:  ebdpConfigFile.cpp
// Date:  9/27/2017
// Auth:  K. Loux
// Desc:  Configuration file object.

// Local headers
#include "ebdpConfigFile.h"

void EBDPConfigFile::BuildConfigItems()
{
	AddConfigItem(_T("OBS_DATA_FILE"), config.dataFileName);
	AddConfigItem(_T("OUTPUT_FILE"), config.outputFileName);

	AddConfigItem(_T("COUNTRY"), config.countryFilter);
	AddConfigItem(_T("STATE"), config.stateFilter);
	AddConfigItem(_T("COUNTY"), config.countyFilter);
	AddConfigItem(_T("LOCATION"), config.locationFilter);

	AddConfigItem(_T("LIST_TYPE"), config.listType);

	AddConfigItem(_T("SCORE_RARITIES"), config.generateRarityScores);
	AddConfigItem(_T("SPECIES_COUNT_ONLY"), config.speciesCountOnly);
	AddConfigItem(_T("INCLUDE_PARTIAL_IDS"), config.includePartialIDs);

	AddConfigItem(_T("PHOTO_FILE"), config.photoFileName);
	AddConfigItem(_T("SHOW_PHOTO_NEEDS"), config.showOnlyPhotoNeeds);

	AddConfigItem(_T("YEAR"), config.yearFilter);
	AddConfigItem(_T("MONTH"), config.monthFilter);
	AddConfigItem(_T("WEEK"), config.weekFilter);
	AddConfigItem(_T("DAY"), config.dayFilter);

	AddConfigItem(_T("SORT_FIRST"), config.primarySort);
	AddConfigItem(_T("SORT_SECOND"), config.secondarySort);

	AddConfigItem(_T("SHOW_UNIQUE_OBS"), config.uniqueObservations);

	AddConfigItem(_T("CALENDAR"), config.generateTargetCalendar);
	AddConfigItem(_T("TARGET_AREA"), config.targetNeedArea);
	AddConfigItem(_T("TOP_COUNT"), config.topBirdCount);
	AddConfigItem(_T("FREQUENCY_FILES"), config.frequencyFilePath);
	AddConfigItem(_T("TARGET_INFO_FILE_NAME"), config.targetInfoFileName);
	AddConfigItem(_T("RECENT_PERIOD"), config.recentObservationPeriod);

	AddConfigItem(_T("GOOGLE_MAPS_KEY"), config.googleMapsAPIKey);
	AddConfigItem(_T("HOME_LOCATION"), config.homeLocation);
	AddConfigItem(_T("EBIRD_API_KEY"), config.eBirdApiKey);

	AddConfigItem(_T("FIND_MAX_NEEDS"), config.findMaxNeedsLocations);
	AddConfigItem(_T("KML_LIBRARY"), config.kmlLibraryPath);

	AddConfigItem(_T("OAUTH_CLIENT_ID"), config.oAuthClientId);
	AddConfigItem(_T("OAUTH_CLIENT_SECRET"), config.oAuthClientSecret);

	AddConfigItem(_T("DATASET"), config.eBirdDatasetPath);
}

void EBDPConfigFile::AssignDefaults()
{
	config.listType = EBDPConfig::ListType::Life;
	config.speciesCountOnly = false;
	config.includePartialIDs = false;

	config.yearFilter = 0;
	config.monthFilter = 0;
	config.weekFilter = 0;
	config.dayFilter = 0;

	config.primarySort = EBDPConfig::SortBy::None;
	config.secondarySort = EBDPConfig::SortBy::None;

	config.uniqueObservations = EBDPConfig::UniquenessType::None;

	config.targetNeedArea = EBDPConfig::TargetNeedArea::None;
	config.generateTargetCalendar = false;
	config.generateRarityScores = false;
	config.topBirdCount = 20;
	config.recentObservationPeriod = 15;

	config.showOnlyPhotoNeeds = false;
	config.findMaxNeedsLocations = false;
}

bool EBDPConfigFile::ConfigIsOK()
{
	bool configurationOK(true);

	if (!GeneralConfigIsOK())
		configurationOK = false;

	if (!FrequencyHarvestConfigIsOK())
		configurationOK = false;

	if (!TargetCalendarConfigIsOK())
		configurationOK = false;

	if (!FindMaxNeedsConfigIsOK())
		configurationOK = false;

	if (!RaritiesConfigIsOK())
		configurationOK = false;

	return configurationOK;
}

bool EBDPConfigFile::FrequencyHarvestConfigIsOK()
{
	bool configurationOK(true);

	if (!config.eBirdDatasetPath.empty() && config.frequencyFilePath.empty())
	{
		Cerr << GetKey(config.eBirdDatasetPath) << " requires " << GetKey(config.frequencyFilePath) << '\n';
		configurationOK = false;
	}

	return configurationOK;
}

bool EBDPConfigFile::TargetCalendarConfigIsOK()
{
    if (!config.generateTargetCalendar)
        return true;

	bool configurationOK(true);

	if (config.frequencyFilePath.empty())
	{
		Cerr << "Must specify " << GetKey(config.frequencyFilePath) << " when using " << GetKey(config.generateTargetCalendar) << '\n';
		configurationOK = false;
	}

	if (config.eBirdApiKey.empty())
	{
		Cerr << "Must specify " << GetKey(config.eBirdApiKey) << " when using " << GetKey(config.generateTargetCalendar) << '\n';
		configurationOK = false;
	}

	if (config.uniqueObservations != EBDPConfig::UniquenessType::None)
	{
		Cerr << "Cannot specify both " << GetKey(config.generateTargetCalendar) << " and " << GetKey(config.uniqueObservations) << '\n';
		configurationOK = false;
	}

	return configurationOK;
}

bool EBDPConfigFile::FindMaxNeedsConfigIsOK()
{
    if (!config.findMaxNeedsLocations)
        return true;

	bool configurationOK(true);

	if (config.oAuthClientId.empty() || config.oAuthClientSecret.empty())
	{
		Cerr << GetKey(config.findMaxNeedsLocations) << " requires "
			<< GetKey(config.oAuthClientId) << " and " << GetKey(config.oAuthClientSecret) << '\n';
		configurationOK = false;
	}

	if (config.kmlLibraryPath.empty())
	{
		Cerr << GetKey(config.findMaxNeedsLocations) << " requires " << GetKey(config.kmlLibraryPath) << '\n';
		configurationOK = false;
	}

	if (config.eBirdApiKey.empty())
	{
		Cerr << GetKey(config.findMaxNeedsLocations) << " requires " << GetKey(config.eBirdApiKey) << '\n';
		configurationOK = false;
	}

    if (config.frequencyFilePath.empty())
	{
		Cerr << "Must specify " << GetKey(config.frequencyFilePath) << " when using " << GetKey(config.findMaxNeedsLocations) << '\n';
		configurationOK = false;
	}

	return configurationOK;
}

bool EBDPConfigFile::GeneralConfigIsOK()
{
	bool configurationOK(true);

	if (config.dataFileName.empty())
	{
		Cerr << "Must specify '" << GetKey(config.dataFileName) << "'\n";
		configurationOK = false;
	}

	if (!config.countryFilter.empty() &&
		config.countryFilter.length() != 2)
	{
		Cerr << "Country (" << GetKey(config.countryFilter) << ") must be specified using 2-digit abbreviation\n";
		configurationOK = false;
	}

	if (!config.stateFilter.empty() &&
		(config.stateFilter.length() < 2 || config.stateFilter.length() > 3))
	{
		Cerr << "State/providence (" << GetKey(config.stateFilter) << ") must be specified using 2- or 3-digit abbreviation\n";
		configurationOK = false;
	}

	if (config.dayFilter > 31)
	{
		Cerr << "Day (" << GetKey(config.dayFilter) << ") must be in the range 0 - 31\n";
		configurationOK = false;
	}

	if (config.monthFilter > 12)
	{
		Cerr << "Month (" << GetKey(config.monthFilter) << ") must be in the range 0 - 12\n";
		configurationOK = false;
	}

	if (config.weekFilter > 53)
	{
		Cerr << "Week (" << GetKey(config.weekFilter) << ") must be in the range 0 - 53\n";
		configurationOK = false;
	}

	if (config.recentObservationPeriod < 1 || config.recentObservationPeriod > 30)
	{
		Cerr << GetKey(config.recentObservationPeriod) << " must be between 1 and 30\n";
		configurationOK = false;
	}

	/*if (!config.googleMapsAPIKey.empty() && config.homeLocation.empty())
	{
		Cerr << "Must specify " << GetKey(config.homeLocation) << " when using " << GetKey(config.googleMapsAPIKey) << '\n';
		configurationOK = false;
	}*/

	if (config.uniqueObservations != EBDPConfig::UniquenessType::None && !config.countryFilter.empty())
	{
		Cerr << "Cannot specify both " << GetKey(config.countryFilter) << " and " << GetKey(config.uniqueObservations) << '\n';
		configurationOK = false;
	}

	if (config.showOnlyPhotoNeeds && config.photoFileName.empty())
	{
		Cerr << "Must specify " << GetKey(config.photoFileName) << " when using " << GetKey(config.showOnlyPhotoNeeds) << '\n';
		configurationOK = false;
	}

	return configurationOK;
}

bool EBDPConfigFile::RaritiesConfigIsOK()
{
    if (!config.generateRarityScores)
        return true;

	bool configurationOK(true);

	if (config.frequencyFilePath.empty())
	{
		Cerr << "Must specify " << GetKey(config.frequencyFilePath) << " when using " << GetKey(config.generateRarityScores) << '\n';
		configurationOK = false;
	}

	if (config.eBirdApiKey.empty())
	{
		Cerr << "Must specify " << GetKey(config.eBirdApiKey) << " when using " << GetKey(config.generateRarityScores) << '\n';
		configurationOK = false;
	}

	if (config.generateTargetCalendar)
	{
		Cerr << "Cannot specify both " << GetKey(config.generateRarityScores) << " and " << GetKey(config.generateTargetCalendar) << '\n';
		configurationOK = false;
	}

	if (config.uniqueObservations != EBDPConfig::UniquenessType::None)
	{
		Cerr << "Cannot specify both " << GetKey(config.generateRarityScores) << " and " << GetKey(config.uniqueObservations) << '\n';
		configurationOK = false;
	}

	return configurationOK;
}
<commit_msg>Fixed indentation that was generating compiler warning<commit_after>// File:  ebdpConfigFile.cpp
// Date:  9/27/2017
// Auth:  K. Loux
// Desc:  Configuration file object.

// Local headers
#include "ebdpConfigFile.h"

void EBDPConfigFile::BuildConfigItems()
{
	AddConfigItem(_T("OBS_DATA_FILE"), config.dataFileName);
	AddConfigItem(_T("OUTPUT_FILE"), config.outputFileName);

	AddConfigItem(_T("COUNTRY"), config.countryFilter);
	AddConfigItem(_T("STATE"), config.stateFilter);
	AddConfigItem(_T("COUNTY"), config.countyFilter);
	AddConfigItem(_T("LOCATION"), config.locationFilter);

	AddConfigItem(_T("LIST_TYPE"), config.listType);

	AddConfigItem(_T("SCORE_RARITIES"), config.generateRarityScores);
	AddConfigItem(_T("SPECIES_COUNT_ONLY"), config.speciesCountOnly);
	AddConfigItem(_T("INCLUDE_PARTIAL_IDS"), config.includePartialIDs);

	AddConfigItem(_T("PHOTO_FILE"), config.photoFileName);
	AddConfigItem(_T("SHOW_PHOTO_NEEDS"), config.showOnlyPhotoNeeds);

	AddConfigItem(_T("YEAR"), config.yearFilter);
	AddConfigItem(_T("MONTH"), config.monthFilter);
	AddConfigItem(_T("WEEK"), config.weekFilter);
	AddConfigItem(_T("DAY"), config.dayFilter);

	AddConfigItem(_T("SORT_FIRST"), config.primarySort);
	AddConfigItem(_T("SORT_SECOND"), config.secondarySort);

	AddConfigItem(_T("SHOW_UNIQUE_OBS"), config.uniqueObservations);

	AddConfigItem(_T("CALENDAR"), config.generateTargetCalendar);
	AddConfigItem(_T("TARGET_AREA"), config.targetNeedArea);
	AddConfigItem(_T("TOP_COUNT"), config.topBirdCount);
	AddConfigItem(_T("FREQUENCY_FILES"), config.frequencyFilePath);
	AddConfigItem(_T("TARGET_INFO_FILE_NAME"), config.targetInfoFileName);
	AddConfigItem(_T("RECENT_PERIOD"), config.recentObservationPeriod);

	AddConfigItem(_T("GOOGLE_MAPS_KEY"), config.googleMapsAPIKey);
	AddConfigItem(_T("HOME_LOCATION"), config.homeLocation);
	AddConfigItem(_T("EBIRD_API_KEY"), config.eBirdApiKey);

	AddConfigItem(_T("FIND_MAX_NEEDS"), config.findMaxNeedsLocations);
	AddConfigItem(_T("KML_LIBRARY"), config.kmlLibraryPath);

	AddConfigItem(_T("OAUTH_CLIENT_ID"), config.oAuthClientId);
	AddConfigItem(_T("OAUTH_CLIENT_SECRET"), config.oAuthClientSecret);

	AddConfigItem(_T("DATASET"), config.eBirdDatasetPath);
}

void EBDPConfigFile::AssignDefaults()
{
	config.listType = EBDPConfig::ListType::Life;
	config.speciesCountOnly = false;
	config.includePartialIDs = false;

	config.yearFilter = 0;
	config.monthFilter = 0;
	config.weekFilter = 0;
	config.dayFilter = 0;

	config.primarySort = EBDPConfig::SortBy::None;
	config.secondarySort = EBDPConfig::SortBy::None;

	config.uniqueObservations = EBDPConfig::UniquenessType::None;

	config.targetNeedArea = EBDPConfig::TargetNeedArea::None;
	config.generateTargetCalendar = false;
	config.generateRarityScores = false;
	config.topBirdCount = 20;
	config.recentObservationPeriod = 15;

	config.showOnlyPhotoNeeds = false;
	config.findMaxNeedsLocations = false;
}

bool EBDPConfigFile::ConfigIsOK()
{
	bool configurationOK(true);

	if (!GeneralConfigIsOK())
		configurationOK = false;

	if (!FrequencyHarvestConfigIsOK())
		configurationOK = false;

	if (!TargetCalendarConfigIsOK())
		configurationOK = false;

	if (!FindMaxNeedsConfigIsOK())
		configurationOK = false;

	if (!RaritiesConfigIsOK())
		configurationOK = false;

	return configurationOK;
}

bool EBDPConfigFile::FrequencyHarvestConfigIsOK()
{
	bool configurationOK(true);

	if (!config.eBirdDatasetPath.empty() && config.frequencyFilePath.empty())
	{
		Cerr << GetKey(config.eBirdDatasetPath) << " requires " << GetKey(config.frequencyFilePath) << '\n';
		configurationOK = false;
	}

	return configurationOK;
}

bool EBDPConfigFile::TargetCalendarConfigIsOK()
{
	if (!config.generateTargetCalendar)
		return true;

	bool configurationOK(true);

	if (config.frequencyFilePath.empty())
	{
		Cerr << "Must specify " << GetKey(config.frequencyFilePath) << " when using " << GetKey(config.generateTargetCalendar) << '\n';
		configurationOK = false;
	}

	if (config.eBirdApiKey.empty())
	{
		Cerr << "Must specify " << GetKey(config.eBirdApiKey) << " when using " << GetKey(config.generateTargetCalendar) << '\n';
		configurationOK = false;
	}

	if (config.uniqueObservations != EBDPConfig::UniquenessType::None)
	{
		Cerr << "Cannot specify both " << GetKey(config.generateTargetCalendar) << " and " << GetKey(config.uniqueObservations) << '\n';
		configurationOK = false;
	}

	return configurationOK;
}

bool EBDPConfigFile::FindMaxNeedsConfigIsOK()
{
	if (!config.findMaxNeedsLocations)
		return true;

	bool configurationOK(true);

	if (config.oAuthClientId.empty() || config.oAuthClientSecret.empty())
	{
		Cerr << GetKey(config.findMaxNeedsLocations) << " requires "
			<< GetKey(config.oAuthClientId) << " and " << GetKey(config.oAuthClientSecret) << '\n';
		configurationOK = false;
	}

	if (config.kmlLibraryPath.empty())
	{
		Cerr << GetKey(config.findMaxNeedsLocations) << " requires " << GetKey(config.kmlLibraryPath) << '\n';
		configurationOK = false;
	}

	if (config.eBirdApiKey.empty())
	{
		Cerr << GetKey(config.findMaxNeedsLocations) << " requires " << GetKey(config.eBirdApiKey) << '\n';
		configurationOK = false;
	}

    if (config.frequencyFilePath.empty())
	{
		Cerr << "Must specify " << GetKey(config.frequencyFilePath) << " when using " << GetKey(config.findMaxNeedsLocations) << '\n';
		configurationOK = false;
	}

	return configurationOK;
}

bool EBDPConfigFile::GeneralConfigIsOK()
{
	bool configurationOK(true);

	if (config.dataFileName.empty())
	{
		Cerr << "Must specify '" << GetKey(config.dataFileName) << "'\n";
		configurationOK = false;
	}

	if (!config.countryFilter.empty() &&
		config.countryFilter.length() != 2)
	{
		Cerr << "Country (" << GetKey(config.countryFilter) << ") must be specified using 2-digit abbreviation\n";
		configurationOK = false;
	}

	if (!config.stateFilter.empty() &&
		(config.stateFilter.length() < 2 || config.stateFilter.length() > 3))
	{
		Cerr << "State/providence (" << GetKey(config.stateFilter) << ") must be specified using 2- or 3-digit abbreviation\n";
		configurationOK = false;
	}

	if (config.dayFilter > 31)
	{
		Cerr << "Day (" << GetKey(config.dayFilter) << ") must be in the range 0 - 31\n";
		configurationOK = false;
	}

	if (config.monthFilter > 12)
	{
		Cerr << "Month (" << GetKey(config.monthFilter) << ") must be in the range 0 - 12\n";
		configurationOK = false;
	}

	if (config.weekFilter > 53)
	{
		Cerr << "Week (" << GetKey(config.weekFilter) << ") must be in the range 0 - 53\n";
		configurationOK = false;
	}

	if (config.recentObservationPeriod < 1 || config.recentObservationPeriod > 30)
	{
		Cerr << GetKey(config.recentObservationPeriod) << " must be between 1 and 30\n";
		configurationOK = false;
	}

	/*if (!config.googleMapsAPIKey.empty() && config.homeLocation.empty())
	{
		Cerr << "Must specify " << GetKey(config.homeLocation) << " when using " << GetKey(config.googleMapsAPIKey) << '\n';
		configurationOK = false;
	}*/

	if (config.uniqueObservations != EBDPConfig::UniquenessType::None && !config.countryFilter.empty())
	{
		Cerr << "Cannot specify both " << GetKey(config.countryFilter) << " and " << GetKey(config.uniqueObservations) << '\n';
		configurationOK = false;
	}

	if (config.showOnlyPhotoNeeds && config.photoFileName.empty())
	{
		Cerr << "Must specify " << GetKey(config.photoFileName) << " when using " << GetKey(config.showOnlyPhotoNeeds) << '\n';
		configurationOK = false;
	}

	return configurationOK;
}

bool EBDPConfigFile::RaritiesConfigIsOK()
{
	if (!config.generateRarityScores)
		return true;

	bool configurationOK(true);

	if (config.frequencyFilePath.empty())
	{
		Cerr << "Must specify " << GetKey(config.frequencyFilePath) << " when using " << GetKey(config.generateRarityScores) << '\n';
		configurationOK = false;
	}

	if (config.eBirdApiKey.empty())
	{
		Cerr << "Must specify " << GetKey(config.eBirdApiKey) << " when using " << GetKey(config.generateRarityScores) << '\n';
		configurationOK = false;
	}

	if (config.generateTargetCalendar)
	{
		Cerr << "Cannot specify both " << GetKey(config.generateRarityScores) << " and " << GetKey(config.generateTargetCalendar) << '\n';
		configurationOK = false;
	}

	if (config.uniqueObservations != EBDPConfig::UniquenessType::None)
	{
		Cerr << "Cannot specify both " << GetKey(config.generateRarityScores) << " and " << GetKey(config.uniqueObservations) << '\n';
		configurationOK = false;
	}

	return configurationOK;
}
<|endoftext|>


<commit_before>/*
 * This is part of the FL library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2014 Jan Issac (jan.issac@gmail.com)
 * Copyright (c) 2014 Manuel Wuthrich (manuel.wuthrich@gmail.com)
 *
 * Max-Planck Institute for Intelligent Systems, AMD Lab
 * University of Southern California, CLMC Lab
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file multi_sensor_sigma_point_update_policy.hpp
 * \date August 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#ifndef FL__FILTER__GAUSSIAN__MULTI_SENSOR_SIGMA_POINT_UPDATE_POLICY_HPP
#define FL__FILTER__GAUSSIAN__MULTI_SENSOR_SIGMA_POINT_UPDATE_POLICY_HPP

#include <Eigen/Dense>

#include <fl/util/meta.hpp>
#include <fl/util/types.hpp>
#include <fl/util/traits.hpp>
#include <fl/util/descriptor.hpp>
#include <fl/model/observation/joint_observation_model_iid.hpp>
#include <fl/filter/gaussian/transform/point_set.hpp>
#include <fl/filter/gaussian/quadrature/sigma_point_quadrature.hpp>

namespace fl
{

// Forward declarations
template <typename...> class MultiSensorSigmaPointUpdatePolicy;

/**
 * \internal
 */
template <
    typename SigmaPointQuadrature,
    typename NonJoinObservationModel
>
class MultiSensorSigmaPointUpdatePolicy<
          SigmaPointQuadrature,
          NonJoinObservationModel>
{
    static_assert(
        std::is_base_of<
            internal::JointObservationModelIidType, NonJoinObservationModel
        >::value,
        "\n\n\n"
        "====================================================================\n"
        "= Static Assert: You are using the wrong observation model type    =\n"
        "====================================================================\n"
        "  Observation model type must be a JointObservationModel<...>.      \n"
        "  For single observation model, use the regular Gaussian filter     \n"
        "  or the regular SigmaPointUpdatePolicy if you are specifying       \n"
        "  the update policy explicitly fo the GaussianFilter.               \n"
        "====================================================================\n"
    );
};

template <
    typename SigmaPointQuadrature,
    typename MultipleOfLocalObsrvModel
>
class MultiSensorSigmaPointUpdatePolicy<
          SigmaPointQuadrature,
          JointObservationModel<MultipleOfLocalObsrvModel>>
    : public MultiSensorSigmaPointUpdatePolicy<
                SigmaPointQuadrature,
                NonAdditive<JointObservationModel<MultipleOfLocalObsrvModel>>>
{ };

template <
    typename SigmaPointQuadrature,
    typename MultipleOfLocalObsrvModel
>
class MultiSensorSigmaPointUpdatePolicy<
          SigmaPointQuadrature,
          NonAdditive<JointObservationModel<MultipleOfLocalObsrvModel>>>
    : public Descriptor
{
public:
    typedef JointObservationModel<MultipleOfLocalObsrvModel> JointModel;
    typedef typename MultipleOfLocalObsrvModel::Type LocalModel;

    typedef typename JointModel::State State;
    typedef typename JointModel::Obsrv Obsrv;
    typedef typename JointModel::Noise Noise;

    enum : signed int
    {
        NumberOfPoints = SigmaPointQuadrature::number_of_points(
                             JoinSizes<
                                 SizeOf<State>::Value,
                                 SizeOf<Noise>::Value
                             >::Size)
    };

    typedef PointSet<State, NumberOfPoints> StatePointSet;
    typedef PointSet<Noise, NumberOfPoints> NoisePointSet;
    typedef PointSet<Obsrv, NumberOfPoints> ObsrvPointSet;

    template <
        typename Belief
    >
    void operator()(const JointModel& obsrv_function,
                    const SigmaPointQuadrature& quadrature,
                    const Belief& prior_belief,
                    const Obsrv& obsrv,
                    Belief& posterior_belief)
    {
        // static_assert() is non-additive

        INIT_PROFILING
        noise_distr_.dimension(obsrv_function.noise_dimension());

        auto&& h = [&](const State& x, const Noise& w)
        {
           return obsrv_function.observation(x, w);
        };

        quadrature.propergate_gaussian(h, prior_belief, noise_distr_, X, Y, Z);
        MEASURE("Integrate");

        auto&& mu_y = Z.center();
        auto&& mu_x = X.center();
        auto&& Z_c = Z.points();
        auto&& X_c = X.points();
        auto&& W = X.covariance_weights_vector();
        auto cov_xx = (X_c * W.asDiagonal() * X_c.transpose()).eval();
        auto cov_xx_inv = (cov_xx.inverse()).eval();
        auto innovation = (obsrv - mu_y).eval();

        auto C = cov_xx_inv;
        auto D = mu_x;
        D.setZero();

        const int dim_Z_i = obsrv_function.local_obsrv_model().obsrv_dimension();
        const int obsrv_count = obsrv_function.count_local_models();

        MEASURE("Preparation");

//        for (int i = 0; i < obsrv_count; ++i)
//        {
//            auto Z_i = Z_c.middleRows(i * dim_Z_i, dim_Z_i).eval();
//            auto cov_xy_i = (X_c * W.asDiagonal() * Z_i.transpose()).eval();
//            auto cov_yy_i = (Z_i * W.asDiagonal() * Z_i.transpose()).eval();
//            auto innov_i  = innovation.middleRows(i * dim_Z_i, dim_Z_i).eval();

//            auto A_i = (cov_xy_i.transpose() * cov_xx_inv).eval();

//            auto cov_yy_i_inv_given_x =
//                (cov_yy_i - cov_xy_i.transpose() * cov_xx_inv * cov_xy_i)
//                    .inverse();

//            auto T = (A_i.transpose() * cov_yy_i_inv_given_x).eval();

//            C += T * A_i;
//            D += T * innov_i;
//        }

        auto Z_0 = Z_c.middleRows(0 * dim_Z_i, dim_Z_i).eval();
        auto cov_xy_0 = (X_c * W.asDiagonal() * Z_0.transpose()).eval();
        auto cov_yy_0 = (Z_0 * W.asDiagonal() * Z_0.transpose()).eval();
        auto innov_0  = innovation.middleRows(0 * dim_Z_i, dim_Z_i).eval();
        auto A_0 = (cov_xy_0.transpose() * cov_xx_inv).eval();
        auto cov_yy_i_inv_given_x =
            (cov_yy_0 - cov_xy_0.transpose() * cov_xx_inv * cov_xy_0)
                .inverse();
        auto T0 = (A_0.transpose() * cov_yy_i_inv_given_x).eval();
        MEASURE("Debugging temp preparation");

        for (int i = 0; i < obsrv_count; ++i) { auto Z_i = Z_c.middleRows(i * dim_Z_i, dim_Z_i).eval(); }
        MEASURE("auto Z_i = ...");

        for (int i = 0; i < obsrv_count; ++i) { auto cov_xy_i = (X_c * W.asDiagonal() * Z_0.transpose()).eval(); }
        MEASURE("auto cov_xy_i = ...");

        for (int i = 0; i < obsrv_count; ++i) { auto cov_yy_i = (Z_0 * W.asDiagonal() * Z_0.transpose()).eval(); }
        MEASURE("auto cov_yy_i = ...");

        for (int i = 0; i < obsrv_count; ++i){ auto innov_i  = innovation.middleRows(i * dim_Z_i, dim_Z_i).eval(); }
        MEASURE("auto innov_i = ...");

        for (int i = 0; i < obsrv_count; ++i){ auto A_i = (cov_xy_0.transpose() * cov_xx_inv).eval(); }
        MEASURE("auto A_i = ...");

        for (int i = 0; i < obsrv_count; ++i){ auto cov_yy_i_inv_given_x =
                    (cov_yy_0 - cov_xy_0.transpose() * cov_xx_inv * cov_xy_0)
                        .inverse(); }
        MEASURE("auto cov_yy_i_inv_given_x = ...");

        for (int i = 0; i < obsrv_count; ++i){ auto T = (A_0.transpose() * cov_yy_i_inv_given_x).eval(); }
        MEASURE("auto T = ...");

        for (int i = 0; i < obsrv_count; ++i){ C += T0 * A_0; }
        MEASURE("auto C = ...");

        for (int i = 0; i < obsrv_count; ++i){ D += T0 * innov_0; }
        MEASURE("auto D = ...");

        posterior_belief.covariance(C.inverse());
        MEASURE("posterior_belief.covariance(C.inverse())");
        posterior_belief.mean(mu_x + posterior_belief.covariance() * D);
        MEASURE("posterior_belief.mean(mu_x + posterior_belief.covariance() * D)");

        std::cout << "==================================================================" << std::endl;
    }

    virtual std::string name() const
    {
        return "MultiSensorSigmaPointUpdatePolicy<"
                + this->list_arguments(
                       "SigmaPointQuadrature",
                       "NonAdditive<ObservationFunction>")
                + ">";
    }

    virtual std::string description() const
    {
        return "Multi-Sensor Sigma Point based filter update policy "
               "for joint observation model of multiple local observation "
               "models with non-additive noise.";
    }

protected:
    StatePointSet X;
    NoisePointSet Y;
    ObsrvPointSet Z;
    Gaussian<Noise> noise_distr_;
};

}

#endif
<commit_msg>Updated MultiSensor update policy. Minor factoriztion and :lipstick:<commit_after>/*
 * This is part of the FL library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2014 Jan Issac (jan.issac@gmail.com)
 * Copyright (c) 2014 Manuel Wuthrich (manuel.wuthrich@gmail.com)
 *
 * Max-Planck Institute for Intelligent Systems, AMD Lab
 * University of Southern California, CLMC Lab
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file multi_sensor_sigma_point_update_policy.hpp
 * \date August 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#ifndef FL__FILTER__GAUSSIAN__MULTI_SENSOR_SIGMA_POINT_UPDATE_POLICY_HPP
#define FL__FILTER__GAUSSIAN__MULTI_SENSOR_SIGMA_POINT_UPDATE_POLICY_HPP

#include <Eigen/Dense>

#include <fl/util/meta.hpp>
#include <fl/util/types.hpp>
#include <fl/util/traits.hpp>
#include <fl/util/descriptor.hpp>
#include <fl/model/observation/joint_observation_model_iid.hpp>
#include <fl/filter/gaussian/transform/point_set.hpp>
#include <fl/filter/gaussian/quadrature/sigma_point_quadrature.hpp>

namespace fl
{

// Forward declarations
template <typename...> class MultiSensorSigmaPointUpdatePolicy;

/**
 * \internal
 */
template <
    typename SigmaPointQuadrature,
    typename NonJoinObservationModel
>
class MultiSensorSigmaPointUpdatePolicy<
          SigmaPointQuadrature,
          NonJoinObservationModel>
{
    static_assert(
        std::is_base_of<
            internal::JointObservationModelIidType, NonJoinObservationModel
        >::value,
        "\n\n\n"
        "====================================================================\n"
        "= Static Assert: You are using the wrong observation model type    =\n"
        "====================================================================\n"
        "  Observation model type must be a JointObservationModel<...>.      \n"
        "  For single observation model, use the regular Gaussian filter     \n"
        "  or the regular SigmaPointUpdatePolicy if you are specifying       \n"
        "  the update policy explicitly fo the GaussianFilter.               \n"
        "====================================================================\n"
    );
};

template <
    typename SigmaPointQuadrature,
    typename MultipleOfLocalObsrvModel
>
class MultiSensorSigmaPointUpdatePolicy<
          SigmaPointQuadrature,
          JointObservationModel<MultipleOfLocalObsrvModel>>
    : public MultiSensorSigmaPointUpdatePolicy<
                SigmaPointQuadrature,
                NonAdditive<JointObservationModel<MultipleOfLocalObsrvModel>>>
{ };

template <
    typename SigmaPointQuadrature,
    typename MultipleOfLocalObsrvModel
>
class MultiSensorSigmaPointUpdatePolicy<
          SigmaPointQuadrature,
          NonAdditive<JointObservationModel<MultipleOfLocalObsrvModel>>>
    : public Descriptor
{
public:
    typedef JointObservationModel<MultipleOfLocalObsrvModel> JointModel;
    typedef typename MultipleOfLocalObsrvModel::Type LocalModel;

    typedef typename JointModel::State State;
    typedef typename JointModel::Obsrv Obsrv;
    typedef typename JointModel::Noise Noise;

    typedef typename Traits<JointModel>::LocalObsrv LocalObsrv;
    typedef typename Traits<JointModel>::LocalNoise LocalObsrvNoise;

    enum : signed int
    {
        NumberOfPoints = SigmaPointQuadrature::number_of_points(
                             JoinSizes<
                                 SizeOf<State>::Value,
                                 SizeOf<LocalObsrvNoise>::Value
                             >::Size)
    };

    typedef PointSet<State, NumberOfPoints> StatePointSet;
    typedef PointSet<LocalObsrv, NumberOfPoints> LocalObsrvPointSet;
    typedef PointSet<LocalObsrvNoise, NumberOfPoints> LocalNoisePointSet;

    template <
        typename Belief
    >
    void operator()(JointModel& obsrv_function,
                    const SigmaPointQuadrature& quadrature,
                    const Belief& prior_belief,
                    const Obsrv& y,
                    Belief& posterior_belief)
    {
        quadrature.transform_to_points(prior_belief, noise_distr_, p_X, p_Q);

        auto& model = obsrv_function.local_obsrv_model();
        auto&& h = [&](const State& x, const LocalObsrvNoise& w)
        {
            return model.observation(x, w);
        };

        auto&& mu_x = p_X.center();
        auto&& X = p_X.points();
        auto c_xx = cov(X, X);
        auto c_xx_inv = c_xx.inverse().eval();

        auto C = c_xx_inv;
        auto D = State();
        D.setZero(mu_x.size());

        const int sensors = obsrv_function.count_local_models();
        for (int i = 0; i < sensors; ++i)
        {
            model.id(i);
            quadrature.propergate_points(h, p_X, p_Q, p_Y);

            auto mu_y = p_Y.center();
            auto Y = p_Y.points();
            auto c_yy = cov(Y, Y);
            auto c_xy = cov(X, Y);
            auto c_yx = c_xy.transpose();
            auto A_i = (c_yx * c_xx_inv).eval();
            auto c_yy_given_x_inv = (c_yy - c_yx * c_xx_inv * c_xy).inverse();
            auto T = (A_i.transpose() * c_yy_given_x_inv).eval();

            C += T * A_i;
            D += T * (y.middleRows(i * mu_y.size(), mu_y.size()) - mu_y);
        }

        posterior_belief.covariance(C.inverse());
        posterior_belief.mean(mu_x + posterior_belief.covariance() * D);
    }

    virtual std::string name() const
    {
        return "MultiSensorSigmaPointUpdatePolicy<"
                + this->list_arguments(
                       "SigmaPointQuadrature",
                       "NonAdditive<ObservationFunction>")
                + ">";
    }

    virtual std::string description() const
    {
        return "Multi-Sensor Sigma Point based filter update policy "
               "for joint observation model of multiple local observation "
               "models with non-additive noise.";
    }

private:
    template <typename A, typename B>
    auto cov(const A& a, const B& b) -> decltype((a * b.transpose()).eval())
    {
        auto&& W = p_X.covariance_weights_vector().asDiagonal();
        auto c = (a * W * b.transpose()).eval();
        return c;
    }

protected:
    StatePointSet p_X;
    LocalNoisePointSet p_Q;
    LocalObsrvPointSet p_Y;
    Gaussian<LocalObsrvNoise> noise_distr_;
};

}

#endif
<|endoftext|>