Skip to content
This repository
Browse code

Modules reorganization and logic governor

  • Loading branch information...
commit 343766844465e0dcc2a74cb69ad4117e1ddb6937 1 parent 965a459
Jonatan Olofsson authored February 01, 2012
1  CMakeLists.txt
@@ -20,6 +20,7 @@ set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin)
20 20
 
21 21
 add_subdirectory(lib)
22 22
 
  23
+include_directories(${PROJECT_SOURCE_DIR})
23 24
 include_directories(${PROJECT_SOURCE_DIR}/include)
24 25
 include_directories(${PROJECT_SOURCE_DIR}/lib)
25 26
 include_directories(${YAML_CPP_SOURCE_DIR}/include)
18  configuration.yaml
@@ -6,10 +6,10 @@ modules:
6 6
 
7 7
  -  name: sender
8 8
     file: comm_sender.so
9  
-
10  
- -  name: receiver
11  
-    file: comm_receiver.so
12  
-    configuration: test
  9
+#~
  10
+ #~ -  name: receiver
  11
+    #~ file: comm_receiver.so
  12
+    #~ configuration: test
13 13
 
14 14
  -  name: observer
15 15
     file: observer.so
@@ -27,5 +27,11 @@ modules:
27 27
         #~ use_cmcu:   No
28 28
         #~ cmcu_port:  /dev/serial/by-id/
29 29
 
30  
-
31  
-
  30
+ -  name: logic
  31
+    file: logic.so
  32
+    configuration:
  33
+        frequency: 50
  34
+        initial_mode: hover
  35
+        modes:
  36
+            directory: flightmodes/
  37
+            extension: .so
6  lib/slicot/CMakeLists.txt
@@ -21,4 +21,8 @@ add_library(slicot_control
21 21
     SB02OW.f
22 22
     SB02OY.f
23 23
 )
24  
-target_link_libraries(slicot_control ${LAPACK_LIBRARIES})
  24
+
  25
+option(LINK_LAPACK On)
  26
+if(LINK_LAPACK)
  27
+    target_link_libraries(slicot_control ${LAPACK_LIBRARIES})
  28
+endif(LINK_LAPACK)
9  modules/controller/CMakeLists.txt
... ...
@@ -0,0 +1,9 @@
  1
+set(module controller)
  2
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -lgfortran")
  3
+
  4
+add_library(${module} SHARED ${module}.cpp)
  5
+set_target_properties(${module} PROPERTIES PREFIX "")
  6
+target_link_libraries(${module}
  7
+    ${Boost_LIBRARIES}
  8
+    slicot_control
  9
+)
77  modules/controller/controller.cpp
... ...
@@ -0,0 +1,77 @@
  1
+/**
  2
+ * Copyright 2011, 2012 Jonatan Olofsson
  3
+ *
  4
+ * This file is part of C++ Robot Automation Platform (CRAP).
  5
+ *
  6
+ * CRAP is free software: you can redistribute it and/or modify
  7
+ * it under the terms of the GNU General Public License as published by
  8
+ * the Free Software Foundation, either version 3 of the License, or
  9
+ * (at your option) any later version.
  10
+ *
  11
+ * CRAP is distributed in the hope that it will be useful,
  12
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14
+ * GNU General Public License for more details.
  15
+ *
  16
+ * You should have received a copy of the GNU General Public License
  17
+ * along with CRAP.  If not, see <http://www.gnu.org/licenses/>.
  18
+ */
  19
+
  20
+#include "crap/module.hpp"
  21
+#include "modules/controller/model.hpp"
  22
+#include "modules/observer/model.hpp"
  23
+#include "math/control/LQ.hpp"
  24
+
  25
+namespace CRAP {
  26
+    namespace controller {
  27
+        control_vector u;
  28
+        reference_vector r;
  29
+        YAML::Node config;
  30
+
  31
+        typedef control::LQ<model::number_of_states, model::number_of_controls> controller_type;
  32
+        controller_type reg;
  33
+
  34
+
  35
+        void calculate_control_signal(const observer::model::state_vector& state) {
  36
+            u = reg.control_signal(state, r);
  37
+            //~ cpplot::figure("Control signal") << u(0);
  38
+        }
  39
+
  40
+        void init_controller(YAML::Node& c) {
  41
+            r.setZero(); u.setZero();
  42
+            config = c;
  43
+            control::linear_model<model::number_of_states, model::number_of_controls> lmodel;
  44
+            lmodel.A << 0, 1,
  45
+                        0, 0;
  46
+            lmodel.B << 0,
  47
+                        1;
  48
+            lmodel.M << 1,
  49
+                        0;
  50
+            lmodel.Q.setIdentity();
  51
+            lmodel.R.setIdentity();
  52
+
  53
+            reg.set_model(lmodel);
  54
+        }
  55
+    }
  56
+}
  57
+
  58
+// Module interface
  59
+extern "C" {
  60
+    using namespace CRAP::controller;
  61
+
  62
+    control_vector control_signal() {
  63
+        return u;
  64
+    }
  65
+}
  66
+
  67
+// CRAP interface
  68
+extern "C" {
  69
+    using namespace CRAP;
  70
+    void configure(YAML::Node& c) {
  71
+        controller::init_controller(c);
  72
+    }
  73
+
  74
+    void run() {
  75
+        comm::listen("/state_estimate", controller::calculate_control_signal);
  76
+    }
  77
+}
39  modules/controller/model.hpp
... ...
@@ -0,0 +1,39 @@
  1
+/**
  2
+ * Copyright 2011, 2012 Jonatan Olofsson
  3
+ *
  4
+ * This file is part of C++ Robot Automation Platform (CRAP).
  5
+ *
  6
+ * CRAP is free software: you can redistribute it and/or modify
  7
+ * it under the terms of the GNU General Public License as published by
  8
+ * the Free Software Foundation, either version 3 of the License, or
  9
+ * (at your option) any later version.
  10
+ *
  11
+ * CRAP is distributed in the hope that it will be useful,
  12
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14
+ * GNU General Public License for more details.
  15
+ *
  16
+ * You should have received a copy of the GNU General Public License
  17
+ * along with CRAP.  If not, see <http://www.gnu.org/licenses/>.
  18
+ */
  19
+
  20
+#ifndef CRAP_CONTROLLER_MODEL_HPP_
  21
+#define CRAP_CONTROLLER_MODEL_HPP_
  22
+#include <Eigen/Core>
  23
+
  24
+namespace CRAP {
  25
+    namespace controller {
  26
+        using namespace Eigen;
  27
+        namespace model {
  28
+            const int number_of_states              = 2;
  29
+            const int number_of_controls            = 1;
  30
+        }
  31
+
  32
+        typedef Matrix<double, model::number_of_controls, 1> control_vector;
  33
+        typedef Matrix<double, model::number_of_controls, 1> reference_vector;
  34
+        typedef Matrix<double, model::number_of_states, 1> state_vector;
  35
+        typedef control_vector(*control_signal_fn)();
  36
+    }
  37
+}
  38
+
  39
+#endif
62  modules/logic/governor.cpp
... ...
@@ -0,0 +1,62 @@
  1
+#include <map>
  2
+#include <boost/thread/mutex.hpp>
  3
+#include <assert.h>
  4
+#include "modules/logic/governor.hpp"
  5
+#include <dlfcn.h>
  6
+#include "crap/state_engine.hpp"
  7
+
  8
+namespace CRAP {
  9
+    namespace logic {
  10
+        namespace governor {
  11
+            typedef std::map<std::string, void*> modemap_t;
  12
+            modemap_t modes;
  13
+            modemap_t mode_handles;
  14
+            state_engine_t state_machine;
  15
+
  16
+            std::string current_mode;
  17
+            YAML::Node config;
  18
+
  19
+
  20
+            void configure(YAML::Node& c) {
  21
+                config = c;
  22
+            }
  23
+
  24
+            bool load_mode(const std::string mode) {
  25
+                if(modes.find(mode) != modes.end()) return true;
  26
+                void* m;
  27
+                m = dlopen((config["modes"]["directory"].as<std::string>("") + mode + config["modes"]["extension"].as<std::string>(".so")).c_str(), RTLD_LAZY);
  28
+                if(m) {
  29
+                    void* f;
  30
+                    f = dlsym(m, mode.c_str());
  31
+                    if(f) {
  32
+                        modes[mode] = f;
  33
+                        mode_handles[mode] = m;
  34
+                        std::cout << "Loaded flightmode: " << mode << std::endl;
  35
+                        return true;
  36
+                    }
  37
+                    else {
  38
+                        std::cout << "Function not found in loaded file: " << mode << std::endl;
  39
+                        dlclose(m);
  40
+                    }
  41
+                }
  42
+                std::cout << "Flightmode loading failed: " << mode << std::endl;
  43
+                return false;
  44
+            }
  45
+
  46
+
  47
+            void switch_mode(const std::string& mode) {
  48
+                if(mode == current_mode) return;
  49
+                if(load_mode(mode)) {
  50
+                    state_machine.next(modes[mode]);
  51
+                    current_mode = mode;
  52
+                }
  53
+            }
  54
+
  55
+            void tick() {
  56
+                //if(battery level < battery threshold) switch_mode("landing");
  57
+
  58
+                state_machine();
  59
+            }
  60
+        }
  61
+    }
  62
+}
36  modules/logic/governor.hpp
... ...
@@ -0,0 +1,36 @@
  1
+/**
  2
+ * Copyright 2011, 2012 Jonatan Olofsson
  3
+ *
  4
+ * This file is part of C++ Robot Automation Platform (CRAP).
  5
+ *
  6
+ * CRAP is free software: you can redistribute it and/or modify
  7
+ * it under the terms of the GNU General Public License as published by
  8
+ * the Free Software Foundation, either version 3 of the License, or
  9
+ * (at your option) any later version.
  10
+ *
  11
+ * CRAP is distributed in the hope that it will be useful,
  12
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14
+ * GNU General Public License for more details.
  15
+ *
  16
+ * You should have received a copy of the GNU General Public License
  17
+ * along with CRAP.  If not, see <http://www.gnu.org/licenses/>.
  18
+ */
  19
+
  20
+#ifndef CRAP_LOGIC_GOVERNOR_HPP_
  21
+#define CRAP_LOGIC_GOVERNOR_HPP_
  22
+
  23
+#include <string>
  24
+#include "yaml-cpp/yaml.h"
  25
+
  26
+namespace CRAP {
  27
+    namespace logic {
  28
+        namespace governor {
  29
+            void configure(YAML::Node& c);
  30
+            void switch_mode(const std::string& mode);
  31
+            void tick();
  32
+        }
  33
+    }
  34
+}
  35
+
  36
+#endif
56  modules/logic/logic.cpp
... ...
@@ -0,0 +1,56 @@
  1
+/**
  2
+ * Copyright 2011, 2012 Jonatan Olofsson
  3
+ *
  4
+ * This file is part of C++ Robot Automation Platform (CRAP).
  5
+ *
  6
+ * CRAP is free software: you can redistribute it and/or modify
  7
+ * it under the terms of the GNU General Public License as published by
  8
+ * the Free Software Foundation, either version 3 of the License, or
  9
+ * (at your option) any later version.
  10
+ *
  11
+ * CRAP is distributed in the hope that it will be useful,
  12
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14
+ * GNU General Public License for more details.
  15
+ *
  16
+ * You should have received a copy of the GNU General Public License
  17
+ * along with CRAP.  If not, see <http://www.gnu.org/licenses/>.
  18
+ */
  19
+
  20
+
  21
+#include "crap/module.hpp"
  22
+#include "crap/state_engine.hpp"
  23
+#include "modules/logic/governor.hpp"
  24
+#include <iostream>
  25
+
  26
+namespace CRAP {
  27
+    namespace logic {
  28
+        CRAP::time::frequency_t frequency;
  29
+
  30
+        bool on = true;
  31
+
  32
+        void state_engine() {
  33
+            while(on && time::ticktock(frequency)) {
  34
+                governor::tick();
  35
+            }
  36
+        }
  37
+    }
  38
+}
  39
+
  40
+extern "C" {
  41
+    using namespace CRAP;
  42
+    void configure(YAML::Node& c) {
  43
+        logic::governor::configure(c);
  44
+        logic::frequency = time::frequency_t(c["frequency"].as<double>(50));
  45
+        logic::governor::switch_mode(c["initial_mode"].as<std::string>());
  46
+    }
  47
+
  48
+    void run() {
  49
+        comm::listen("/logic/mode", logic::governor::switch_mode);
  50
+        logic::state_engine();
  51
+    }
  52
+
  53
+    void shutdown() {
  54
+        logic::on = false;
  55
+    }
  56
+}
10  modules/logic/modes/CMakeLists.txt
... ...
@@ -0,0 +1,10 @@
  1
+set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/flightmodes)
  2
+
  3
+set(MODES
  4
+    hover
  5
+)
  6
+
  7
+
  8
+foreach(mode ${MODES})
  9
+    add_subdirectory(${mode})
  10
+endforeach(mode)
5  modules/logic/modes/hover/CMakeLists.txt
... ...
@@ -0,0 +1,5 @@
  1
+set(mode hover)
  2
+add_library(${mode} SHARED ${mode}.cpp)
  3
+set_target_properties(${mode} PROPERTIES PREFIX "")
  4
+target_link_libraries(${mode}
  5
+    ${Boost_LIBRARIES})
48  modules/logic/modes/hover/hover.cpp
... ...
@@ -0,0 +1,48 @@
  1
+/**
  2
+ * Copyright 2011, 2012 Jonatan Olofsson
  3
+ *
  4
+ * This file is part of C++ Robot Automation Platform (CRAP).
  5
+ *
  6
+ * CRAP is free software: you can redistribute it and/or modify
  7
+ * it under the terms of the GNU General Public License as published by
  8
+ * the Free Software Foundation, either version 3 of the License, or
  9
+ * (at your option) any later version.
  10
+ *
  11
+ * CRAP is distributed in the hope that it will be useful,
  12
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14
+ * GNU General Public License for more details.
  15
+ *
  16
+ * You should have received a copy of the GNU General Public License
  17
+ * along with CRAP.  If not, see <http://www.gnu.org/licenses/>.
  18
+ */
  19
+
  20
+#include "hover.hpp"
  21
+#include <iostream>
  22
+#include <Eigen/Core>
  23
+#include "modules/observer/model.hpp"
  24
+#include "crap/communication.hpp"
  25
+
  26
+extern "C" {
  27
+    using namespace CRAP;
  28
+    using namespace Eigen;
  29
+    using namespace observer::model;
  30
+
  31
+    Vector3d target_state;
  32
+
  33
+    typedef state_vector(*state_fn)();
  34
+    state_fn get_state = comm::bind<state_fn>("observer", "get_state");
  35
+
  36
+    void* hover() {
  37
+        std::cout << "Hovering state 1" << std::endl;
  38
+        state_vector current_state = get_state();
  39
+        target_state.x() = current_state(state::position);
  40
+        return (void*)&hover2;
  41
+    }
  42
+
  43
+    void* hover2() {
  44
+        std::cout << "Hovering state 2" << std::endl;
  45
+
  46
+        return (void*)&hover2;
  47
+    }
  48
+}
27  modules/logic/modes/hover/hover.hpp
... ...
@@ -0,0 +1,27 @@
  1
+/**
  2
+ * Copyright 2011, 2012 Jonatan Olofsson
  3
+ *
  4
+ * This file is part of C++ Robot Automation Platform (CRAP).
  5
+ *
  6
+ * CRAP is free software: you can redistribute it and/or modify
  7
+ * it under the terms of the GNU General Public License as published by
  8
+ * the Free Software Foundation, either version 3 of the License, or
  9
+ * (at your option) any later version.
  10
+ *
  11
+ * CRAP is distributed in the hope that it will be useful,
  12
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14
+ * GNU General Public License for more details.
  15
+ *
  16
+ * You should have received a copy of the GNU General Public License
  17
+ * along with CRAP.  If not, see <http://www.gnu.org/licenses/>.
  18
+ */
  19
+
  20
+#ifndef CRAP_FLIGHTMODE_HOVER
  21
+#define CRAP_FLIGHTMODE_HOVER
  22
+
  23
+extern "C" {
  24
+    void* hover();
  25
+    void* hover2();
  26
+}
  27
+#endif
5  modules/observer/CMakeLists.txt
... ...
@@ -0,0 +1,5 @@
  1
+set(module observer)
  2
+add_library(${module} SHARED ${module}.cpp)
  3
+set_target_properties(${module} PROPERTIES PREFIX "")
  4
+target_link_libraries(${module}
  5
+    ${Boost_LIBRARIES})
54  modules/observer/model.hpp
... ...
@@ -0,0 +1,54 @@
  1
+/**
  2
+ * Copyright 2011, 2012 Jonatan Olofsson
  3
+ *
  4
+ * This file is part of C++ Robot Automation Platform (CRAP).
  5
+ *
  6
+ * CRAP is free software: you can redistribute it and/or modify
  7
+ * it under the terms of the GNU General Public License as published by
  8
+ * the Free Software Foundation, either version 3 of the License, or
  9
+ * (at your option) any later version.
  10
+ *
  11
+ * CRAP is distributed in the hope that it will be useful,
  12
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14
+ * GNU General Public License for more details.
  15
+ *
  16
+ * You should have received a copy of the GNU General Public License
  17
+ * along with CRAP.  If not, see <http://www.gnu.org/licenses/>.
  18
+ */
  19
+
  20
+#ifndef CRAP_OBSERVER_MODEL_HPP_
  21
+#define CRAP_OBSERVER_MODEL_HPP_
  22
+
  23
+#include <Eigen/Core>
  24
+#include "math/filtering/filtering.hpp"
  25
+
  26
+namespace CRAP {
  27
+    namespace observer {
  28
+        using namespace Eigen;
  29
+        using namespace filtering;
  30
+        namespace model {
  31
+            namespace state {
  32
+                const int position = 0;
  33
+                const int velocity = 1;
  34
+            }
  35
+
  36
+            namespace control {
  37
+                const int velocity = 0;
  38
+            }
  39
+
  40
+
  41
+            const double frequency                  = 30; ///< Hz
  42
+            const int number_of_states              = 2;
  43
+            const int number_of_controls            = 1;
  44
+
  45
+            typedef Matrix<double, number_of_states, 1> state_vector;
  46
+            typedef Matrix<double, number_of_controls, 1> control_vector;
  47
+            typedef Matrix<double, number_of_states, number_of_states> state_covariance_matrix;
  48
+        }
  49
+
  50
+        typedef KalmanFilter<model::number_of_states, model::number_of_controls> KF;
  51
+    }
  52
+}
  53
+
  54
+#endif
160  modules/observer/observer.cpp
... ...
@@ -0,0 +1,160 @@
  1
+/**
  2
+ * Copyright 2011, 2012 Jonatan Olofsson
  3
+ *
  4
+ * This file is part of C++ Robot Automation Platform (CRAP).
  5
+ *
  6
+ * CRAP is free software: you can redistribute it and/or modify
  7
+ * it under the terms of the GNU General Public License as published by
  8
+ * the Free Software Foundation, either version 3 of the License, or
  9
+ * (at your option) any later version.
  10
+ *
  11
+ * CRAP is distributed in the hope that it will be useful,
  12
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14
+ * GNU General Public License for more details.
  15
+ *
  16
+ * You should have received a copy of the GNU General Public License
  17
+ * along with CRAP.  If not, see <http://www.gnu.org/licenses/>.
  18
+ */
  19
+
  20
+#include "crap/module.hpp"
  21
+#include "math/math.hpp"
  22
+#include "boost/date_time/posix_time/posix_time.hpp"
  23
+#include <string>
  24
+#include <utility>
  25
+
  26
+#include <Eigen/Core>
  27
+#include "modules/observer/model.hpp"
  28
+#include "modules/controller/model.hpp"
  29
+#include "math/filtering/ukf.hpp"
  30
+
  31
+namespace CRAP {
  32
+    namespace observer {
  33
+        using namespace filtering;
  34
+        using namespace model;
  35
+
  36
+        controller::control_signal_fn u;
  37
+        KF filter; ///< Filter instance
  38
+
  39
+        void publish() {
  40
+            ::CRAP::comm::send<state_vector>("/state_estimate", filter.x);
  41
+            ::CRAP::comm::send<state_covariance_matrix>("/state_estimate/covariance", filter.P);
  42
+            //~ std::cout << "Position: " << filter.x(0) << std::endl;
  43
+            #ifdef CRAP_PLOT
  44
+                using namespace cpplot;
  45
+                figure("Observer")->subplot(3,1,1)->title("Position")->gco<Line>()->set_capacity(30*5) << std::make_pair(CRAP::starting_time.elapsed(), filter.x(0));
  46
+                figure("Observer")->subplot(3,1,2)->title("Velocity")->gco<Line>()->set_capacity(30*5) << std::make_pair(CRAP::starting_time.elapsed(), filter.x(1));
  47
+                figure("Observer")->subplot(3,1,3)->title("Control signal")->gco<Line>()->set_capacity(30*5) << std::make_pair(CRAP::starting_time.elapsed(), u()(0));
  48
+            #endif
  49
+        }
  50
+
  51
+        namespace observe {
  52
+            using namespace Eigen;
  53
+
  54
+            typedef Matrix<double, 1,1> taco_t; taco_t v;
  55
+            const taco_t& h_tachometer(const state_vector& x) {
  56
+                v(0) = x(state::velocity);
  57
+                return v;
  58
+            }
  59
+            void tachometer(const observation<1>& vel) {
  60
+                ukf::observe<model::number_of_states,model::number_of_controls,1>(filter, h_tachometer, vel);
  61
+                publish();
  62
+            }
  63
+        }
  64
+
  65
+        namespace predict {
  66
+            state_vector xdot; ///< Internal storage for state calculation
  67
+
  68
+            /*!
  69
+             * \brief   Calculate the derivative of the states
  70
+             *
  71
+             * This function contains the expressions for the mathematical
  72
+             * derivatives of each state.
  73
+             * \param   x   Current state
  74
+             * \param   u   Current control signal
  75
+             * \return  The derivative of the states at the point defined by x and u
  76
+             */
  77
+            state_vector& fdot(const state_vector& x, const control_vector& u) {
  78
+                xdot(state::position) = x(state::velocity);
  79
+                xdot(state::velocity) = (-x(state::velocity) + u(control::velocity)); // /config.tau_v;
  80
+                return xdot;
  81
+            }
  82
+
  83
+            /*!
  84
+             * \brief   Predict the filter's next time step
  85
+             *
  86
+             * Using the model, current state and the current control signal,
  87
+             * the state at the next time-step is predicted using a RK4 method
  88
+             * for integrating the state, given the derivatives of the states.
  89
+             * \see     fdot
  90
+             * \param   x   Current state
  91
+             * \param   u   Current control signal
  92
+             * \return  Prediction of state in the next time step
  93
+             */
  94
+            const state_vector& f(const state_vector& x, const control_vector& u) {
  95
+                xdot = math::rk4<model::number_of_states, model::number_of_controls>(fdot, x, u, 1.0/observer::model::frequency);
  96
+                return xdot;
  97
+            }
  98
+
  99
+            state_covariance_matrix Q_; ///< Internal storage for state covariance
  100
+
  101
+            /*!
  102
+             * \brief   Get the state prediction covariance
  103
+             *
  104
+             * Calculate and get the estimated covariance of the covariance of
  105
+             * the state prediction.
  106
+             * \param   x   Current state
  107
+             * \param   u   Current control signal
  108
+             * \return  Prediction covariance
  109
+             */
  110
+            const state_covariance_matrix& Q(const state_vector&, const control_vector&)
  111
+            {
  112
+                return Q_;
  113
+            }
  114
+
  115
+            typedef prediction_model_tmpl<model::number_of_states, model::number_of_controls> prediction_model_t;
  116
+            prediction_model_t prediction_model(f,Q);
  117
+
  118
+            void time_update() {
  119
+                ukf::predict<model::number_of_states, model::number_of_controls>(filter, prediction_model, u());
  120
+                publish();
  121
+            }
  122
+        }
  123
+    }
  124
+}
  125
+
  126
+extern "C" {
  127
+    using namespace CRAP::observer;
  128
+    state_vector get_state() {
  129
+        return filter.x;
  130
+    }
  131
+}
  132
+
  133
+extern "C" {
  134
+    using namespace CRAP;
  135
+    using namespace CRAP::observer;
  136
+    void configure(YAML::Node& c) {
  137
+        std::cout << "Reconfiguring observer" << std::endl;
  138
+        filter.x << 1,
  139
+                    1;
  140
+        //~ filter.x << c["initial_position"].as<double>(),
  141
+                    //~ c["initial_velocity"].as<double>();
  142
+        filter.P.setIdentity();
  143
+    }
  144
+
  145
+    bool running = true;
  146
+    time::frequency_t frequency(model::frequency);
  147
+
  148
+    void run() {
  149
+        comm::listen("tachometer", observe::tachometer);
  150
+        observer::u = comm::bind<controller::control_signal_fn>("controller", "control_signal");
  151
+
  152
+        while(running && time::ticktock(::frequency)) {
  153
+            observer::predict::time_update();
  154
+        }
  155
+    }
  156
+
  157
+    void stop() {
  158
+        running = false;
  159
+    }
  160
+}

0 notes on commit 3437668

Please sign in to comment.
Something went wrong with that request. Please try again.