Skip to content

Comparing changes

Choose two branches to see what’s changed or to start a new pull request. If you need to, you can also .

Open a pull request

Create a new pull request by comparing changes across two branches. If you need to, you can also .
...
  • 7 commits
  • 22 files changed
  • 0 commit comments
  • 1 contributor
View
1 CMakeLists.txt
@@ -20,6 +20,7 @@ set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin)
add_subdirectory(lib)
+include_directories(${PROJECT_SOURCE_DIR})
include_directories(${PROJECT_SOURCE_DIR}/include)
include_directories(${PROJECT_SOURCE_DIR}/lib)
include_directories(${YAML_CPP_SOURCE_DIR}/include)
View
18 configuration.yaml
@@ -6,10 +6,10 @@ modules:
- name: sender
file: comm_sender.so
-
- - name: receiver
- file: comm_receiver.so
- configuration: test
+#~
+ #~ - name: receiver
+ #~ file: comm_receiver.so
+ #~ configuration: test
- name: observer
file: observer.so
@@ -27,5 +27,11 @@ modules:
#~ use_cmcu: No
#~ cmcu_port: /dev/serial/by-id/
-
-
+ - name: logic
+ file: logic.so
+ configuration:
+ frequency: 50
+ initial_mode: hover
+ modes:
+ directory: flightmodes/
+ extension: .so
View
2 include/crap/config.hpp
@@ -20,6 +20,6 @@
#ifndef CRAP_CONFIG_HPP
#define CRAP_CONFIG_HPP
-#define CRAP_PLOT
+/* #undef CRAP_PLOT */
#endif // CRAP_CONFIG_HPP
View
55 include/crap/state_engine.hpp
@@ -0,0 +1,55 @@
+/**
+ * Copyright 2011, 2012 Jonatan Olofsson
+ *
+ * This file is part of C++ Robot Automation Platform (CRAP).
+ *
+ * CRAP 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 3 of the License, or
+ * (at your option) any later version.
+ *
+ * CRAP 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 CRAP. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef CRAP_STATE_ENGINE_HPP_
+#define CRAP_STATE_ENGINE_HPP_
+
+#include <boost/thread/mutex.hpp>
+
+namespace CRAP {
+ namespace logic {
+ class state_engine_t {
+ private:
+ void* next_state;
+ boost::mutex nxtlock;
+ public:
+ typedef void*(*next_t)();
+
+ state_engine_t() {}
+ state_engine_t(void* n)
+ : next_state(n)
+ {}
+
+ void operator()() {
+ boost::mutex::scoped_lock l(nxtlock);
+ if(next_state == NULL) return;
+ next_state = ((next_t)next_state)();
+ }
+ void next(next_t n) {
+ boost::mutex::scoped_lock l(nxtlock);
+ next_state = (void*)n;
+ }
+ void next(void* n) {
+ boost::mutex::scoped_lock l(nxtlock);
+ next_state = n;
+ }
+ };
+ }
+}
+#endif
View
1 include/crap/time.hpp
@@ -31,6 +31,7 @@ namespace CRAP {
bool first;
frequency_t(double f) : dt(1.0/f), first(true) {}
+ frequency_t() : first(true) {}
};
View
6 lib/slicot/CMakeLists.txt
@@ -21,4 +21,8 @@ add_library(slicot_control
SB02OW.f
SB02OY.f
)
-target_link_libraries(slicot_control ${LAPACK_LIBRARIES})
+
+option(LINK_LAPACK On)
+if(LINK_LAPACK)
+ target_link_libraries(slicot_control ${LAPACK_LIBRARIES})
+endif(LINK_LAPACK)
View
20 modules/CMakeLists.txt
@@ -6,7 +6,6 @@ link_directories ( ${Boost_LIBRARY_DIRS} )
include_directories ( ${Boost_INCLUDE_DIRS} )
include_directories ( ${Eigen3_INCLUDE_DIR} )
include_directories(${YAML_CPP_SOURCE_DIR}/include)
-include_directories(${MATPLOTPP_SOURCE_DIR})
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
@@ -15,19 +14,24 @@ set(MODULES
#~ testmodule
comm_sender
comm_receiver
- observer
#~ sensor_reader
)
+
+# Include directories
+set(SUBDIRS
+ logic
+ controller
+ observer
+)
+
+
foreach(module ${MODULES})
add_library(${module} SHARED ${module}.cpp)
set_target_properties(${module} PROPERTIES PREFIX "")
target_link_libraries(${module}
${Boost_LIBRARIES})
endforeach(module)
-# Controller with slicot library
-add_library(controller SHARED controller.cpp)
-set_target_properties(controller PROPERTIES PREFIX "")
-target_link_libraries(controller
- slicot_control
- ${Boost_LIBRARIES})
+foreach(dir ${SUBDIRS})
+add_subdirectory(${dir})
+endforeach(dir)
View
9 modules/controller/CMakeLists.txt
@@ -0,0 +1,9 @@
+set(module controller)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -lgfortran")
+
+add_library(${module} SHARED ${module}.cpp)
+set_target_properties(${module} PROPERTIES PREFIX "")
+target_link_libraries(${module}
+ ${Boost_LIBRARIES}
+ slicot_control
+)
View
4 modules/controller.cpp → modules/controller/controller.cpp
@@ -18,8 +18,8 @@
*/
#include "crap/module.hpp"
-#include "crap/modules/controller/model.hpp"
-#include "crap/modules/observer/model.hpp"
+#include "modules/controller/model.hpp"
+#include "modules/observer/model.hpp"
#include "math/control/LQ.hpp"
namespace CRAP {
View
0 include/crap/modules/controller/model.hpp → modules/controller/model.hpp
File renamed without changes.
View
4 modules/logic/CMakeLists.txt
@@ -0,0 +1,4 @@
+set(module logic)
+add_library(${module} SHARED ${module}.cpp governor.cpp)
+set_target_properties(${module} PROPERTIES PREFIX "")
+add_subdirectory(modes)
View
62 modules/logic/governor.cpp
@@ -0,0 +1,62 @@
+#include <map>
+#include <boost/thread/mutex.hpp>
+#include <assert.h>
+#include "modules/logic/governor.hpp"
+#include <dlfcn.h>
+#include "crap/state_engine.hpp"
+
+namespace CRAP {
+ namespace logic {
+ namespace governor {
+ typedef std::map<std::string, void*> modemap_t;
+ modemap_t modes;
+ modemap_t mode_handles;
+ state_engine_t state_machine;
+
+ std::string current_mode;
+ YAML::Node config;
+
+
+ void configure(YAML::Node& c) {
+ config = c;
+ }
+
+ bool load_mode(const std::string mode) {
+ if(modes.find(mode) != modes.end()) return true;
+ void* m;
+ m = dlopen((config["modes"]["directory"].as<std::string>("") + mode + config["modes"]["extension"].as<std::string>(".so")).c_str(), RTLD_LAZY);
+ if(m) {
+ void* f;
+ f = dlsym(m, mode.c_str());
+ if(f) {
+ modes[mode] = f;
+ mode_handles[mode] = m;
+ std::cout << "Loaded flightmode: " << mode << std::endl;
+ return true;
+ }
+ else {
+ std::cout << "Function not found in loaded file: " << mode << std::endl;
+ dlclose(m);
+ }
+ }
+ std::cout << "Flightmode loading failed: " << mode << std::endl;
+ return false;
+ }
+
+
+ void switch_mode(const std::string& mode) {
+ if(mode == current_mode) return;
+ if(load_mode(mode)) {
+ state_machine.next(modes[mode]);
+ current_mode = mode;
+ }
+ }
+
+ void tick() {
+ //if(battery level < battery threshold) switch_mode("landing");
+
+ state_machine();
+ }
+ }
+ }
+}
View
36 modules/logic/governor.hpp
@@ -0,0 +1,36 @@
+/**
+ * Copyright 2011, 2012 Jonatan Olofsson
+ *
+ * This file is part of C++ Robot Automation Platform (CRAP).
+ *
+ * CRAP 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 3 of the License, or
+ * (at your option) any later version.
+ *
+ * CRAP 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 CRAP. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef CRAP_LOGIC_GOVERNOR_HPP_
+#define CRAP_LOGIC_GOVERNOR_HPP_
+
+#include <string>
+#include "yaml-cpp/yaml.h"
+
+namespace CRAP {
+ namespace logic {
+ namespace governor {
+ void configure(YAML::Node& c);
+ void switch_mode(const std::string& mode);
+ void tick();
+ }
+ }
+}
+
+#endif
View
56 modules/logic/logic.cpp
@@ -0,0 +1,56 @@
+/**
+ * Copyright 2011, 2012 Jonatan Olofsson
+ *
+ * This file is part of C++ Robot Automation Platform (CRAP).
+ *
+ * CRAP 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 3 of the License, or
+ * (at your option) any later version.
+ *
+ * CRAP 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 CRAP. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+
+#include "crap/module.hpp"
+#include "crap/state_engine.hpp"
+#include "modules/logic/governor.hpp"
+#include <iostream>
+
+namespace CRAP {
+ namespace logic {
+ CRAP::time::frequency_t frequency;
+
+ bool on = true;
+
+ void state_engine() {
+ while(on && time::ticktock(frequency)) {
+ governor::tick();
+ }
+ }
+ }
+}
+
+extern "C" {
+ using namespace CRAP;
+ void configure(YAML::Node& c) {
+ logic::governor::configure(c);
+ logic::frequency = time::frequency_t(c["frequency"].as<double>(50));
+ logic::governor::switch_mode(c["initial_mode"].as<std::string>());
+ }
+
+ void run() {
+ comm::listen("/logic/mode", logic::governor::switch_mode);
+ logic::state_engine();
+ }
+
+ void shutdown() {
+ logic::on = false;
+ }
+}
View
10 modules/logic/modes/CMakeLists.txt
@@ -0,0 +1,10 @@
+set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/flightmodes)
+
+set(MODES
+ hover
+)
+
+
+foreach(mode ${MODES})
+ add_subdirectory(${mode})
+endforeach(mode)
View
5 modules/logic/modes/hover/CMakeLists.txt
@@ -0,0 +1,5 @@
+set(mode hover)
+add_library(${mode} SHARED ${mode}.cpp)
+set_target_properties(${mode} PROPERTIES PREFIX "")
+target_link_libraries(${mode}
+ ${Boost_LIBRARIES})
View
48 modules/logic/modes/hover/hover.cpp
@@ -0,0 +1,48 @@
+/**
+ * Copyright 2011, 2012 Jonatan Olofsson
+ *
+ * This file is part of C++ Robot Automation Platform (CRAP).
+ *
+ * CRAP 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 3 of the License, or
+ * (at your option) any later version.
+ *
+ * CRAP 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 CRAP. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "hover.hpp"
+#include <iostream>
+#include <Eigen/Core>
+#include "modules/observer/model.hpp"
+#include "crap/communication.hpp"
+
+extern "C" {
+ using namespace CRAP;
+ using namespace Eigen;
+ using namespace observer::model;
+
+ Vector3d target_state;
+
+ typedef state_vector(*state_fn)();
+ state_fn get_state = comm::bind<state_fn>("observer", "get_state");
+
+ void* hover() {
+ std::cout << "Hovering state 1" << std::endl;
+ state_vector current_state = get_state();
+ target_state.x() = current_state(state::position);
+ return (void*)&hover2;
+ }
+
+ void* hover2() {
+ std::cout << "Hovering state 2" << std::endl;
+
+ return (void*)&hover2;
+ }
+}
View
27 modules/logic/modes/hover/hover.hpp
@@ -0,0 +1,27 @@
+/**
+ * Copyright 2011, 2012 Jonatan Olofsson
+ *
+ * This file is part of C++ Robot Automation Platform (CRAP).
+ *
+ * CRAP 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 3 of the License, or
+ * (at your option) any later version.
+ *
+ * CRAP 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 CRAP. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef CRAP_FLIGHTMODE_HOVER
+#define CRAP_FLIGHTMODE_HOVER
+
+extern "C" {
+ void* hover();
+ void* hover2();
+}
+#endif
View
5 modules/observer/CMakeLists.txt
@@ -0,0 +1,5 @@
+set(module observer)
+add_library(${module} SHARED ${module}.cpp)
+set_target_properties(${module} PROPERTIES PREFIX "")
+target_link_libraries(${module}
+ ${Boost_LIBRARIES})
View
0 include/crap/modules/observer/model.hpp → modules/observer/model.hpp
File renamed without changes.
View
11 modules/observer.cpp → modules/observer/observer.cpp
@@ -24,8 +24,8 @@
#include <utility>
#include <Eigen/Core>
-#include "crap/modules/observer/model.hpp"
-#include "crap/modules/controller/model.hpp"
+#include "modules/observer/model.hpp"
+#include "modules/controller/model.hpp"
#include "math/filtering/ukf.hpp"
namespace CRAP {
@@ -124,6 +124,13 @@ namespace CRAP {
}
extern "C" {
+ using namespace CRAP::observer;
+ state_vector get_state() {
+ return filter.x;
+ }
+}
+
+extern "C" {
using namespace CRAP;
using namespace CRAP::observer;
void configure(YAML::Node& c) {
View
15 src/core.cpp
@@ -42,6 +42,7 @@ namespace CRAP {
typedef std::map<std::string, void*> module_map;
typedef std::map<std::string, boost::shared_ptr<boost::thread> > thread_map;
typedef void(*configuration_function)(YAML::Node&);
+ typedef void(*close_function)();
typedef std::map<std::string, configuration_function> configuration_map;
/**
@@ -152,12 +153,18 @@ namespace CRAP {
* Join a thread and remove it from the list of running threads
* \param key Name of the module whos thread to close down.
*/
- void close_thread(const std::string key) {
- threads[key]->interrupt();
- if(!threads[key]->timed_join(boost::posix_time::milliseconds(5000))) {
+ void close_thread(const std::string name) {
+ void* cfn = dlsym(modules[name], "shutdown");
+ if(cfn == NULL) {
+ threads[name]->interrupt();
+ std::cerr << "No function 'close' was found, interrupting" << std::endl;
+ } else {
+ ((close_function)cfn)();
+ }
+ if(!threads[name]->timed_join(boost::posix_time::milliseconds(5000))) {
std::cerr << "Failed to join thread within five seconds. Dropping thread" << std::endl;
}
- threads.erase(key);
+ threads.erase(name);
}
/**

No commit comments for this range

Something went wrong with that request. Please try again.