Index: trunk/demos/CMakeLists.txt
===================================================================
--- trunk/demos/CMakeLists.txt (revision 404)
+++ trunk/demos/CMakeLists.txt (revision 405)
@@ -5,8 +5,8 @@
add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/CircleFollower/)
add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/TwoWheelRobotCircleFollower)
-add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/Gps/)
+add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/DemoGps/)
add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/HelloWorld/)
add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/MixedReality/)
-add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/OpticalFlow/)
+add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/DemoOpticalFlow/)
add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/PidStandalone/)
add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/SimpleFleet/)
Index: trunk/demos/DemoGps/CMakeLists.txt
===================================================================
--- trunk/demos/DemoGps/CMakeLists.txt (revision 405)
+++ trunk/demos/DemoGps/CMakeLists.txt (revision 405)
@@ -0,0 +1,6 @@
+PROJECT(DemoGps)
+cmake_minimum_required(VERSION 2.8)
+include($ENV{FLAIR_ROOT}/flair-src/cmake-modules/GlobalCmakeFlair.cmake)
+
+add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/simulator)
+add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/uav)
Index: trunk/demos/DemoGps/simulator/CMakeLists.txt
===================================================================
--- trunk/demos/DemoGps/simulator/CMakeLists.txt (revision 405)
+++ trunk/demos/DemoGps/simulator/CMakeLists.txt (revision 405)
@@ -0,0 +1,16 @@
+PROJECT(DemoGps_simulator)
+cmake_minimum_required(VERSION 2.8)
+
+SET(FLAIR_USE_SIMULATOR TRUE)
+SET(FLAIR_USE_SIMULATOR_GL TRUE)
+
+include($ENV{FLAIR_ROOT}/flair-src/cmake-modules/GlobalCmakeFlair.cmake)
+
+SET(SRC_FILES
+ src/main.cpp
+)
+
+FLAIR_DEMO_HOST_CXX(${PROJECT_NAME} "${SRC_FILES}"
+ DEST_DIR DemoGps
+ COPY_RESOURCES
+)
Index: trunk/demos/DemoGps/simulator/resources/core2-64/simulator_x4.sh
===================================================================
--- trunk/demos/DemoGps/simulator/resources/core2-64/simulator_x4.sh (revision 405)
+++ trunk/demos/DemoGps/simulator/resources/core2-64/simulator_x4.sh (revision 405)
@@ -0,0 +1,11 @@
+#! /bin/bash
+
+if [ -f /proc/xenomai/version ];then
+ EXEC=./DemoGps_simulator_rt
+else
+ EXEC=./DemoGps_simulator_nrt
+fi
+
+. $FLAIR_ROOT/flair-src/scripts/distribution_specific_hack.sh
+
+$EXEC -n x4_0 -t x4 -p 9000 -a 127.0.0.1 -x simulator_x4.xml -o 10 -m $FLAIR_ROOT/flair-src/models -s $FLAIR_ROOT/flair-src/models/indoor_flight_arena.xml
Index: trunk/demos/DemoGps/simulator/resources/core2-64/simulator_x4.xml
===================================================================
--- trunk/demos/DemoGps/simulator/resources/core2-64/simulator_x4.xml (revision 405)
+++ trunk/demos/DemoGps/simulator/resources/core2-64/simulator_x4.xml (revision 405)
@@ -0,0 +1,102 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Index: trunk/demos/DemoGps/simulator/resources/core2-64/simulator_x8.sh
===================================================================
--- trunk/demos/DemoGps/simulator/resources/core2-64/simulator_x8.sh (revision 405)
+++ trunk/demos/DemoGps/simulator/resources/core2-64/simulator_x8.sh (revision 405)
@@ -0,0 +1,11 @@
+#! /bin/bash
+
+if [ -f /proc/xenomai/version ];then
+ EXEC=./DemoGps_simulator_rt
+else
+ EXEC=./DemoGps_simulator_nrt
+fi
+
+. $FLAIR_ROOT/flair-src/scripts/distribution_specific_hack.sh
+
+$EXEC -n x8_0 -t x8 -p 9000 -a 127.0.0.1 -x simulator_x8.xml -o 10 -m $FLAIR_ROOT/flair-src/models -s $FLAIR_ROOT/flair-src/models/indoor_flight_arena.xml
Index: trunk/demos/DemoGps/simulator/resources/core2-64/simulator_x8.xml
===================================================================
--- trunk/demos/DemoGps/simulator/resources/core2-64/simulator_x8.xml (revision 405)
+++ trunk/demos/DemoGps/simulator/resources/core2-64/simulator_x8.xml (revision 405)
@@ -0,0 +1,106 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Index: trunk/demos/DemoGps/simulator/src/main.cpp
===================================================================
--- trunk/demos/DemoGps/simulator/src/main.cpp (revision 405)
+++ trunk/demos/DemoGps/simulator/src/main.cpp (revision 405)
@@ -0,0 +1,129 @@
+// created: 2012/04/18
+// filename: main.cpp
+//
+// author: Guillaume Sanahuja
+// Copyright Heudiasyc UMR UTC/CNRS 6599
+//
+// version: $Id: $
+//
+// purpose: main simulateur
+//
+//
+/*********************************************************************/
+
+#include
+#include
+#include
+#include
+#include
+#include
+#ifdef GL
+#include
+#include
+#include
+#endif
+
+using namespace TCLAP;
+using namespace std;
+using namespace flair::simulator;
+using namespace flair::sensor;
+
+int port;
+int opti_time;
+string xml_file;
+string media_path;
+string scene_file;
+string type;
+string name;
+string address;
+
+void parseOptions(int argc, char** argv)
+{
+ try {
+ CmdLine cmd("Command description message", ' ', "0.1");
+
+ ValueArg nameArg("n", "name", "uav name, also used for vrpn", true, "x4", "string");
+ cmd.add(nameArg);
+
+ ValueArg xmlArg("x", "xml", "xml file", true, "./reglages.xml", "string");
+ cmd.add(xmlArg);
+
+ ValueArg portArg("p", "port", "ground station port", true, 9002, "int");
+ cmd.add(portArg);
+
+ ValueArg addressArg("a", "address", "ground station address", true, "127.0.0.1", "string");
+ cmd.add(addressArg);
+
+ ValueArg typeArg("t", "type", "uav type, x4 or x8", true, "x4", "string");
+ cmd.add(typeArg);
+
+ ValueArg optiArg("o", "opti", "optitrack time ms", false, 0, "int");
+ cmd.add(optiArg);
+
+#ifdef GL
+ ValueArg mediaArg("m", "media", "path to media files", true, "./", "string");
+ cmd.add(mediaArg);
+
+ ValueArg sceneArg("s", "scene", "path to scene file", true, "./voliere.xml", "string");
+ cmd.add(sceneArg);
+#endif
+
+ cmd.parse(argc, argv);
+
+ // Get the value parsed by each arg.
+ port = portArg.getValue();
+ xml_file = xmlArg.getValue();
+ opti_time = optiArg.getValue();
+ type = typeArg.getValue();
+ name = nameArg.getValue();
+ address = addressArg.getValue();
+#ifdef GL
+ media_path = mediaArg.getValue();
+ scene_file = sceneArg.getValue();
+#endif
+
+ } catch(ArgException& e) {
+ cerr << "error: " << e.error() << " for arg " << e.argId() << endl;
+ exit(EXIT_FAILURE);
+ }
+}
+
+int main(int argc, char* argv[])
+{
+ Simulator* simu;
+ Model* drone;
+ SimuImu* imu;
+ SimuGps* gps;
+#ifdef GL
+ SimuUsGL* us_gl;
+ Parser* gui;
+#endif
+ parseOptions(argc, argv);
+
+ simu = new Simulator("simulator", opti_time, 0);
+ simu->SetupConnection(address, port);
+ simu->SetupUserInterface(xml_file);
+
+#ifdef GL
+ gui = new Parser(1024, 768, 1024, 768, media_path, scene_file);
+#endif
+
+ if(type == "x4") {
+ drone = new X4(name, 0);
+ } else {
+ drone = new X8(name, 0);
+ }
+
+ imu = new SimuImu(drone, "imu", 0,0);
+
+#ifdef GL
+ us_gl = new SimuUsGL(drone, "us", 0,0);
+#endif
+ gps = new SimuGps(drone, "gps", 0,0);
+
+ simu->RunSimu();
+
+ delete simu;
+
+ return 0;
+}
Index: trunk/demos/DemoGps/uav/CMakeLists.txt
===================================================================
--- trunk/demos/DemoGps/uav/CMakeLists.txt (revision 405)
+++ trunk/demos/DemoGps/uav/CMakeLists.txt (revision 405)
@@ -0,0 +1,18 @@
+PROJECT(DemoGps)
+cmake_minimum_required(VERSION 2.8)
+
+SET(FLAIR_USE_FILTER TRUE)
+SET(FLAIR_USE_SENSOR_ACTUATOR TRUE)
+SET(FLAIR_USE_META TRUE)
+
+include($ENV{FLAIR_ROOT}/flair-src/cmake-modules/GlobalCmakeFlair.cmake)
+
+SET(SRC_FILES
+ src/main.cpp
+ src/DemoGps.cpp
+)
+
+FLAIR_DEMO(${PROJECT_NAME} "${SRC_FILES}"
+ DEST_DIR DemoGps
+ COPY_RESOURCES
+)
Index: trunk/demos/DemoGps/uav/resources/armv7a-neon/Gps_bebop.sh
===================================================================
--- trunk/demos/DemoGps/uav/resources/armv7a-neon/Gps_bebop.sh (revision 405)
+++ trunk/demos/DemoGps/uav/resources/armv7a-neon/Gps_bebop.sh (revision 405)
@@ -0,0 +1,1 @@
+./DemoGps_nrt -a 192.168.42.24 -p 9000 -l /tmp -x Gps_bebop.xml -n Drone_0 -t bebop
Index: trunk/demos/DemoGps/uav/resources/armv7a-neon/Gps_bebop.xml
===================================================================
--- trunk/demos/DemoGps/uav/resources/armv7a-neon/Gps_bebop.xml (revision 405)
+++ trunk/demos/DemoGps/uav/resources/armv7a-neon/Gps_bebop.xml (revision 405)
@@ -0,0 +1,268 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Index: trunk/demos/DemoGps/uav/resources/core2-64/setup_x4.xml
===================================================================
--- trunk/demos/DemoGps/uav/resources/core2-64/setup_x4.xml (revision 405)
+++ trunk/demos/DemoGps/uav/resources/core2-64/setup_x4.xml (revision 405)
@@ -0,0 +1,361 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Index: trunk/demos/DemoGps/uav/resources/core2-64/setup_x8.xml
===================================================================
--- trunk/demos/DemoGps/uav/resources/core2-64/setup_x8.xml (revision 405)
+++ trunk/demos/DemoGps/uav/resources/core2-64/setup_x8.xml (revision 405)
@@ -0,0 +1,342 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Index: trunk/demos/DemoGps/uav/resources/core2-64/x4.sh
===================================================================
--- trunk/demos/DemoGps/uav/resources/core2-64/x4.sh (revision 405)
+++ trunk/demos/DemoGps/uav/resources/core2-64/x4.sh (revision 405)
@@ -0,0 +1,11 @@
+#! /bin/bash
+
+if [ -f /proc/xenomai/version ];then
+ EXEC=./DemoGps_rt
+else
+ EXEC=./DemoGps_nrt
+fi
+
+. $FLAIR_ROOT/flair-src/scripts/distribution_specific_hack.sh
+
+$EXEC -n x4_0 -a 127.0.0.1 -p 9000 -l /tmp -x setup_x4.xml -t x4_simu
Index: trunk/demos/DemoGps/uav/resources/core2-64/x8.sh
===================================================================
--- trunk/demos/DemoGps/uav/resources/core2-64/x8.sh (revision 405)
+++ trunk/demos/DemoGps/uav/resources/core2-64/x8.sh (revision 405)
@@ -0,0 +1,11 @@
+#! /bin/bash
+
+if [ -f /proc/xenomai/version ];then
+ EXEC=./Gps_rt
+else
+ EXEC=./Gps_nrt
+fi
+
+. $FLAIR_ROOT/flair-src/scripts/distribution_specific_hack.sh
+
+$EXEC -n x8_0 -a 127.0.0.1 -p 9000 -l ./ -x setup_x8.xml -t x8_simu
Index: trunk/demos/DemoGps/uav/src/DemoGps.cpp
===================================================================
--- trunk/demos/DemoGps/uav/src/DemoGps.cpp (revision 405)
+++ trunk/demos/DemoGps/uav/src/DemoGps.cpp (revision 405)
@@ -0,0 +1,212 @@
+// created: 2016/07/01
+// filename: DemoGps.cpp
+//
+// author: Guillaume Sanahuja
+// Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+// version: $Id: $
+//
+// purpose: demo GPS
+//
+//
+/*********************************************************************/
+
+#include "DemoGps.h"
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+using namespace std;
+using namespace flair::core;
+using namespace flair::gui;
+using namespace flair::sensor;
+using namespace flair::filter;
+using namespace flair::meta;
+
+DemoGps::DemoGps(TargetController* controller)
+ : UavStateMachine(controller)
+ , behaviourMode(BehaviourMode_t::Default) {
+ Uav* uav = GetUav();
+ startCircle = new PushButton(GetButtonsLayout()->NewRow(), "start_circle");
+ stopCircle = new PushButton(GetButtonsLayout()->LastRowLastCol(), "stop_circle");
+
+ circle = new TrajectoryGenerator2DCircle(uav->GetGps()->GetLayout()->NewRow(), "circle");
+ // todo: add graphs in gps plot
+ /*
+ uav->GetVrpnObject()->xPlot()->AddCurve(circle->Matrix()->Element(0,0),DataPlot::Blue);
+ uav->GetVrpnObject()->yPlot()->AddCurve(circle->Matrix()->Element(0,1),DataPlot::Blue);
+ uav->GetVrpnObject()->VxPlot()->AddCurve(circle->Matrix()->Element(1,0),DataPlot::Blue);
+ uav->GetVrpnObject()->VyPlot()->AddCurve(circle->Matrix()->Element(1,1),DataPlot::Blue);
+ uav->GetVrpnObject()->XyPlot()->AddCurve(circle->Matrix()->Element(0,1),circle->Matrix()->Element(0,0),DataPlot::Blue,"circle");*/
+
+ uX = new Pid(setupLawTab->At(1, 0), "u_x");
+ uX->UseDefaultPlot(graphLawTab->NewRow());
+ uY = new Pid(setupLawTab->At(1, 1), "u_y");
+ uY->UseDefaultPlot(graphLawTab->LastRowLastCol());
+
+ customReferenceOrientation = new AhrsData(this, "reference");
+ uav->GetAhrs()->AddPlot(customReferenceOrientation, DataPlot::Yellow);
+ AddDataToControlLawLog(customReferenceOrientation);
+
+ customOrientation = new AhrsData(this, "orientation");
+}
+
+DemoGps::~DemoGps() {
+}
+
+AhrsData* DemoGps::GetReferenceOrientation(void) {
+ Vector2Df pos_err, vel_err; // in Uav coordinate system
+ float yaw_ref;
+ Euler refAngles;
+
+ PositionValues(pos_err, vel_err, yaw_ref);
+
+ refAngles.yaw = yaw_ref;
+
+ uX->SetValues(pos_err.x, vel_err.x);
+ uX->Update(GetTime());
+ refAngles.pitch = uX->Output();
+
+ uY->SetValues(pos_err.y, vel_err.y);
+ uY->Update(GetTime());
+ refAngles.roll = -uY->Output();
+
+ customReferenceOrientation->SetQuaternionAndAngularRates(refAngles.ToQuaternion(), Vector3Df(0, 0, 0));
+
+ return customReferenceOrientation;
+}
+
+void DemoGps::PositionValues(Vector2Df& pos_error, Vector2Df& vel_error, float& yaw_ref) {
+ Vector3Df uav_pos, uav_vel;
+ Vector2Df uav_2Dpos, uav_2Dvel;
+
+ // TODO GPS position and circle center
+ // GetUav()->GetVrpnObject()->GetPosition(uav_pos);
+ // GetUav()->GetVrpnObject()->GetSpeed(uav_vel);
+
+ uav_pos.To2Dxy(uav_2Dpos);
+ uav_vel.To2Dxy(uav_2Dvel);
+
+ if(behaviourMode == BehaviourMode_t::PositionHold) {
+ pos_error = uav_2Dpos - posHold;
+ vel_error = uav_2Dvel;
+ yaw_ref = yawHold;
+ } else { // Circle
+ Vector2Df circle_pos, circle_vel;
+ Vector2Df target_2Dpos;
+
+ circle->SetCenter(target_2Dpos);
+
+ // circle reference
+ circle->Update(GetTime());
+ circle->GetPosition(circle_pos);
+ circle->GetSpeed(circle_vel);
+
+ // error in optitrack frame
+ pos_error = uav_2Dpos - circle_pos;
+ vel_error = uav_2Dvel - circle_vel;
+ yaw_ref = atan2(target_2Dpos.y - uav_pos.y, target_2Dpos.x - uav_pos.x);
+ }
+
+ // error in uav frame
+ Quaternion currentQuaternion = GetCurrentQuaternion();
+ Euler currentAngles; // in vrpn frame
+ currentQuaternion.ToEuler(currentAngles);
+ pos_error.Rotate(-currentAngles.yaw);
+ vel_error.Rotate(-currentAngles.yaw);
+}
+
+void DemoGps::SignalEvent(Event_t event) {
+ UavStateMachine::SignalEvent(event);
+ switch(event) {
+ case Event_t::TakingOff:
+ behaviourMode = BehaviourMode_t::Default;
+ break;
+ case Event_t::EnteringControlLoop:
+ if((behaviourMode == BehaviourMode_t::Circle) && (!circle->IsRunning())) {
+ GpsPositionHold();
+ }
+ break;
+ case Event_t::EnteringFailSafeMode:
+ behaviourMode = BehaviourMode_t::Default;
+ break;
+ }
+}
+
+void DemoGps::ExtraSecurityCheck(void) {
+}
+
+void DemoGps::ExtraCheckPushButton(void) {
+ if(startCircle->Clicked() && (behaviourMode != BehaviourMode_t::Circle)) {
+ StartCircle();
+ }
+ if(stopCircle->Clicked() && (behaviourMode == BehaviourMode_t::Circle)) {
+ StopCircle();
+ }
+}
+
+void DemoGps::ExtraCheckJoystick(void) {
+ // R1 and Circle
+ if(GetTargetController()->IsButtonPressed(9) && GetTargetController()->IsButtonPressed(4) &&
+ (behaviourMode != BehaviourMode_t::Circle)) {
+ StartCircle();
+ }
+
+ // R1 and Cross
+ if(GetTargetController()->IsButtonPressed(9) && GetTargetController()->IsButtonPressed(5) &&
+ (behaviourMode == BehaviourMode_t::Circle)) {
+ StopCircle();
+ }
+}
+
+void DemoGps::StartCircle(void) {
+ if(SetOrientationMode(OrientationMode_t::Custom)) {
+ Thread::Info("DemoGps: start circle\n");
+ } else {
+ Thread::Warn("DemoGps: could not start circle\n");
+ return;
+ }
+ Vector3Df uav_pos;
+ Vector2Df uav_2Dpos, target_2Dpos;
+
+ circle->SetCenter(target_2Dpos);
+
+ // todo get uav and circle pos by gps
+ uav_pos.To2Dxy(uav_2Dpos);
+ circle->StartTraj(uav_2Dpos);
+
+ uX->Reset();
+ uY->Reset();
+ behaviourMode = BehaviourMode_t::Circle;
+}
+
+void DemoGps::StopCircle(void) {
+ circle->FinishTraj();
+ // GetJoystick()->Rumble(0x70);
+ Thread::Info("DemoGps: finishing circle\n");
+}
+
+void DemoGps::GpsPositionHold(void) {
+
+ // tood set yawHold and posHold
+
+ uX->Reset();
+ uY->Reset();
+ behaviourMode = BehaviourMode_t::PositionHold;
+ SetOrientationMode(OrientationMode_t::Custom);
+ Thread::Info("DemoGps: holding position\n");
+}
Index: trunk/demos/DemoGps/uav/src/DemoGps.h
===================================================================
--- trunk/demos/DemoGps/uav/src/DemoGps.h (revision 405)
+++ trunk/demos/DemoGps/uav/src/DemoGps.h (revision 405)
@@ -0,0 +1,65 @@
+// created: 2016/07/01
+// filename: DemoGps.h
+//
+// author: Guillaume Sanahuja
+// Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+// version: $Id: $
+//
+// purpose: demo GPS
+//
+//
+/*********************************************************************/
+
+#ifndef DEMOGPS_H
+#define DEMOGPS_H
+
+#include
+
+namespace flair {
+ namespace gui {
+ class PushButton;
+ }
+ namespace filter {
+ class TrajectoryGenerator2DCircle;
+ }
+ namespace sensor {
+ class TargetController;
+ }
+}
+
+class DemoGps : public flair::meta::UavStateMachine {
+ public:
+ DemoGps(flair::sensor::TargetController *controller);
+ ~DemoGps();
+
+ private:
+ enum class BehaviourMode_t {
+ Default,
+ PositionHold,
+ Circle
+ };
+
+ BehaviourMode_t behaviourMode;
+
+ void GpsPositionHold(void);
+ void StartCircle(void);
+ void StopCircle(void);
+ void ExtraSecurityCheck(void);
+ void ExtraCheckPushButton(void);
+ void ExtraCheckJoystick(void);
+ void PositionValues(flair::core::Vector2Df &pos_error,flair::core::Vector2Df &vel_error,float &yaw_ref);
+ flair::core::AhrsData *GetReferenceOrientation(void);
+ void SignalEvent(Event_t event);
+
+ flair::filter::Pid *uX, *uY;
+
+ flair::core::Vector2Df posHold;
+ float yawHold;
+
+ flair::gui::PushButton *startCircle,*stopCircle;
+ flair::filter::TrajectoryGenerator2DCircle *circle;
+ flair::core::AhrsData *customReferenceOrientation,*customOrientation;
+};
+
+#endif // DEMOGPS_H
Index: trunk/demos/DemoGps/uav/src/main.cpp
===================================================================
--- trunk/demos/DemoGps/uav/src/main.cpp (revision 405)
+++ trunk/demos/DemoGps/uav/src/main.cpp (revision 405)
@@ -0,0 +1,98 @@
+// created: 2011/05/01
+// filename: main.cpp
+//
+// author: Guillaume Sanahuja
+// Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+// version: $Id: $
+//
+// purpose: exemple de code uav
+//
+//
+/*********************************************************************/
+
+#include "DemoGps.h"
+#include
+#include
+#include
+#include
+#include
+
+using namespace TCLAP;
+using namespace std;
+using namespace flair::core;
+using namespace flair::meta;
+using namespace flair::sensor;
+
+string uav_type;
+string log_path;
+int port;
+int ds3port;
+string xml_file;
+string name;
+string address;
+
+void parseOptions(int argc, char** argv);
+
+
+int main(int argc, char* argv[]) {
+ parseOptions(argc,argv);
+
+ FrameworkManager *manager;
+ manager= new FrameworkManager(name);
+ manager->SetupConnection(address,port);
+ manager->SetupUserInterface(xml_file);
+ manager->SetupLogger(log_path);
+
+ Uav* drone=CreateUav(name,uav_type,"use_gps=true");
+ TargetEthController *controller=new TargetEthController("Dualshock3",ds3port);
+ DemoGps* demo=new DemoGps(controller);
+
+ demo->Start();
+ demo->Join();
+
+ delete manager;
+}
+
+void parseOptions(int argc, char** argv) {
+ try {
+
+ CmdLine cmd("Command description message", ' ', "0.1");
+
+ ValueArg nameArg("n","name","uav name, also used for vrpn",true,"x4","string");
+ cmd.add( nameArg );
+
+ ValueArg typeArg("t","type","uav type: ardrone2, hds_x4, hds_x8, hds_xufo, x4_simu, x8_simu or x4_simux (with x the number of the simulated uav)",true,"hds_x4","string");
+ cmd.add( typeArg );
+
+ ValueArg xmlArg("x","xml","fichier xml",true,"./reglages.xml","string");
+ cmd.add( xmlArg );
+
+ ValueArg logsArg("l","logs","repertoire des logs",true,"/media/ram","string");
+ cmd.add( logsArg );
+
+ ValueArg portArg("p","port","port pour station sol",true,9000,"int");
+ cmd.add( portArg );
+
+ ValueArg addressArg("a","address","addresse station sol",true,"127.0.0.1","string");
+ cmd.add( addressArg );
+
+ ValueArg ds3portArg("d","ds3_port","port pour ds3",false,20000,"int");
+ cmd.add( ds3portArg );
+
+ cmd.parse( argc, argv );
+
+ // Get the value parsed by each arg.
+ log_path = logsArg.getValue();
+ port=portArg.getValue();
+ ds3port=ds3portArg.getValue();
+ xml_file = xmlArg.getValue();
+ name=nameArg.getValue();
+ uav_type=typeArg.getValue();
+ address=addressArg.getValue();
+
+ } catch (ArgException &e) { // catch any exceptions
+ cerr << "error: " << e.error() << " for arg " << e.argId() << endl;
+ exit(EXIT_FAILURE);
+ }
+}
Index: trunk/demos/DemoOpticalFlow/CMakeLists.txt
===================================================================
--- trunk/demos/DemoOpticalFlow/CMakeLists.txt (revision 405)
+++ trunk/demos/DemoOpticalFlow/CMakeLists.txt (revision 405)
@@ -0,0 +1,16 @@
+PROJECT(DemoOpticalFlow)
+cmake_minimum_required(VERSION 2.8)
+include($ENV{FLAIR_ROOT}/flair-src/cmake-modules/GlobalCmakeFlair.cmake)
+
+if(NOT "${CMAKE_SYSTEM_PROCESSOR_DEFAULTTUNE}" MATCHES "armv5te")
+
+add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/simulator)
+add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/uav)
+
+else()
+ if(DEFINED CMAKE_SYSTEM_PROCESSOR_DEFAULTTUNE)
+ warn("${PROJECT_NAME} will not be built for ${CMAKE_SYSTEM_PROCESSOR_DEFAULTTUNE} architecture")
+ else()
+ warn("${PROJECT_NAME} will not be built for ${CMAKE_SYSTEM_PROCESSOR} architecture")
+ endif()
+endif()
Index: trunk/demos/DemoOpticalFlow/simulator/CMakeLists.txt
===================================================================
--- trunk/demos/DemoOpticalFlow/simulator/CMakeLists.txt (revision 405)
+++ trunk/demos/DemoOpticalFlow/simulator/CMakeLists.txt (revision 405)
@@ -0,0 +1,16 @@
+PROJECT(DemoOpticalFlow_simulator)
+cmake_minimum_required(VERSION 2.8)
+
+SET(FLAIR_USE_SIMULATOR TRUE)
+SET(FLAIR_USE_SIMULATOR_GL TRUE)
+
+include($ENV{FLAIR_ROOT}/flair-src/cmake-modules/GlobalCmakeFlair.cmake)
+
+SET(SRC_FILES
+ src/main.cpp
+)
+
+FLAIR_DEMO_HOST_CXX(${PROJECT_NAME} "${SRC_FILES}"
+ DEST_DIR DemoOpticalFlow
+ COPY_RESOURCES
+)
Index: trunk/demos/DemoOpticalFlow/simulator/resources/core2-64/simulator_x4.sh
===================================================================
--- trunk/demos/DemoOpticalFlow/simulator/resources/core2-64/simulator_x4.sh (revision 405)
+++ trunk/demos/DemoOpticalFlow/simulator/resources/core2-64/simulator_x4.sh (revision 405)
@@ -0,0 +1,11 @@
+#! /bin/bash
+
+if [ -f /proc/xenomai/version ];then
+ EXEC=./DemoOpticalFlow_simulator_rt
+else
+ EXEC=./DemoOpticalFlow_simulator_nrt
+fi
+
+. $FLAIR_ROOT/flair-src/scripts/distribution_specific_hack.sh
+
+$EXEC -n x4_0 -t x4 -a 127.0.0.1 -p 9000 -x simulator_x4.xml -o 10 -m $FLAIR_ROOT/flair-src/models -s $FLAIR_ROOT/flair-src/models/indoor_flight_arena.xml
Index: trunk/demos/DemoOpticalFlow/simulator/resources/core2-64/simulator_x4.xml
===================================================================
--- trunk/demos/DemoOpticalFlow/simulator/resources/core2-64/simulator_x4.xml (revision 405)
+++ trunk/demos/DemoOpticalFlow/simulator/resources/core2-64/simulator_x4.xml (revision 405)
@@ -0,0 +1,64 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Index: trunk/demos/DemoOpticalFlow/simulator/resources/core2-64/simulator_x8.sh
===================================================================
--- trunk/demos/DemoOpticalFlow/simulator/resources/core2-64/simulator_x8.sh (revision 405)
+++ trunk/demos/DemoOpticalFlow/simulator/resources/core2-64/simulator_x8.sh (revision 405)
@@ -0,0 +1,11 @@
+#! /bin/bash
+
+if [ -f /proc/xenomai/version ];then
+ EXEC=./DemoOpticalFlow_simulator_rt
+else
+ EXEC=./DemoOpticalFlow_simulator_nrt
+fi
+
+. $FLAIR_ROOT/flair-src/scripts/distribution_specific_hack.sh
+
+$EXEC -n x8_0 -t x8 -a 127.0.0.1 -p 9000 -x simulator_x8.xml -o 10 -m $FLAIR_ROOT/flair-src/models -s $FLAIR_ROOT/flair-src/models/indoor_flight_arena.xml
Index: trunk/demos/DemoOpticalFlow/simulator/resources/core2-64/simulator_x8.xml
===================================================================
--- trunk/demos/DemoOpticalFlow/simulator/resources/core2-64/simulator_x8.xml (revision 405)
+++ trunk/demos/DemoOpticalFlow/simulator/resources/core2-64/simulator_x8.xml (revision 405)
@@ -0,0 +1,68 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Index: trunk/demos/DemoOpticalFlow/simulator/src/main.cpp
===================================================================
--- trunk/demos/DemoOpticalFlow/simulator/src/main.cpp (revision 405)
+++ trunk/demos/DemoOpticalFlow/simulator/src/main.cpp (revision 405)
@@ -0,0 +1,129 @@
+// created: 2012/04/18
+// filename: main.cpp
+//
+// author: Guillaume Sanahuja
+// Copyright Heudiasyc UMR UTC/CNRS 6599
+//
+// version: $Id: $
+//
+// purpose: main simulateur
+//
+//
+/*********************************************************************/
+
+#include
+#include
+#include
+#include
+#include
+#ifdef GL
+#include
+#include
+#include
+#endif
+
+using namespace TCLAP;
+using namespace std;
+using namespace flair::simulator;
+using namespace flair::sensor;
+
+int port;
+int opti_time;
+string xml_file;
+string media_path;
+string scene_file;
+string type;
+string name;
+string address;
+
+void parseOptions(int argc, char** argv)
+{
+ try {
+ CmdLine cmd("Command description message", ' ', "0.1");
+
+ ValueArg nameArg("n", "name", "uav name, also used for vrpn", true, "x4", "string");
+ cmd.add(nameArg);
+
+ ValueArg xmlArg("x", "xml", "xml file", true, "./reglages.xml", "string");
+ cmd.add(xmlArg);
+
+ ValueArg portArg("p", "port", "ground station port", true, 9002, "int");
+ cmd.add(portArg);
+
+ ValueArg addressArg("a", "address", "ground station address", true, "127.0.0.1", "string");
+ cmd.add(addressArg);
+
+ ValueArg typeArg("t", "type", "uav type, x4 or x8", true, "x4", "string");
+ cmd.add(typeArg);
+
+ ValueArg optiArg("o", "opti", "optitrack time ms", false, 0, "int");
+ cmd.add(optiArg);
+
+#ifdef GL
+ ValueArg mediaArg("m", "media", "path to media files", true, "./", "string");
+ cmd.add(mediaArg);
+
+ ValueArg sceneArg("s", "scene", "path to scene file", true, "./voliere.xml", "string");
+ cmd.add(sceneArg);
+#endif
+
+ cmd.parse(argc, argv);
+
+ // Get the value parsed by each arg.
+ port = portArg.getValue();
+ xml_file = xmlArg.getValue();
+ opti_time = optiArg.getValue();
+ type = typeArg.getValue();
+ name = nameArg.getValue();
+ address = addressArg.getValue();
+#ifdef GL
+ media_path = mediaArg.getValue();
+ scene_file = sceneArg.getValue();
+#endif
+
+ } catch(ArgException& e) // catch any exceptions
+ {
+ cerr << "error: " << e.error() << " for arg " << e.argId() << endl;
+ exit(EXIT_FAILURE);
+ }
+}
+
+int main(int argc, char* argv[])
+{
+ Simulator* simu;
+ Model* drone;
+ SimuImu* imu;
+#ifdef GL
+ SimuUsGL* us_gl;
+
+ SimuCameraGL* cam_bas;
+ Parser* gui;
+#endif
+ parseOptions(argc, argv);
+
+ simu = new Simulator("simulator", opti_time, 90);
+ simu->SetupConnection(address, port);
+ simu->SetupUserInterface(xml_file);
+
+#ifdef GL
+ gui = new Parser(960, 480, 640, 480, media_path, scene_file);
+#endif
+
+ if(type == "x4") {
+ drone = new X4(name, 0);
+ } else {
+ drone = new X8(name, 0);
+ }
+ imu = new SimuImu(drone, "imu", 0,0);
+
+#ifdef GL
+ us_gl = new SimuUsGL(drone, "us", 0,0);
+ cam_bas = new SimuCameraGL(drone, "bottom camera", 320, 240, 640, 0, 0,0);
+#endif
+
+ simu->RunSimu();
+
+ delete simu;
+
+ return 0;
+}
Index: trunk/demos/DemoOpticalFlow/uav/CMakeLists.txt
===================================================================
--- trunk/demos/DemoOpticalFlow/uav/CMakeLists.txt (revision 405)
+++ trunk/demos/DemoOpticalFlow/uav/CMakeLists.txt (revision 405)
@@ -0,0 +1,28 @@
+PROJECT(DemoOpticalFlow)
+cmake_minimum_required(VERSION 2.8)
+include($ENV{FLAIR_ROOT}/flair-src/cmake-modules/GlobalCmakeFlair.cmake)
+
+if(NOT "${CMAKE_SYSTEM_PROCESSOR_DEFAULTTUNE}" MATCHES "armv5te")
+
+SET(FLAIR_USE_FILTER TRUE)
+SET(FLAIR_USE_SENSOR_ACTUATOR TRUE)
+SET(FLAIR_USE_VISION_FILTER TRUE)
+SET(FLAIR_USE_META TRUE)
+
+SET(SRC_FILES
+ src/main.cpp
+ src/DemoOpticalFlow.cpp
+)
+
+FLAIR_DEMO(${PROJECT_NAME} "${SRC_FILES}"
+ DEST_DIR DemoOpticalFlow
+ COPY_RESOURCES
+)
+
+else()
+ if(DEFINED CMAKE_SYSTEM_PROCESSOR_DEFAULTTUNE)
+ warn("${PROJECT_NAME} will not be built for ${CMAKE_SYSTEM_PROCESSOR_DEFAULTTUNE} architecture")
+ else()
+ warn("${PROJECT_NAME} will not be built for ${CMAKE_SYSTEM_PROCESSOR} architecture")
+ endif()
+endif()
Index: trunk/demos/DemoOpticalFlow/uav/resources/armv7a-neon/OpticalFlow_ardrone2.sh
===================================================================
--- trunk/demos/DemoOpticalFlow/uav/resources/armv7a-neon/OpticalFlow_ardrone2.sh (revision 405)
+++ trunk/demos/DemoOpticalFlow/uav/resources/armv7a-neon/OpticalFlow_ardrone2.sh (revision 405)
@@ -0,0 +1,1 @@
+./DemoOpticalFlow_nrt -a 192.168.147.217 -p 9000 -l /tmp -x OpticalFlow_ardrone2.xml -n Drone_0 -t ardrone2 -v ./dspcv_dsp.out
Index: trunk/demos/DemoOpticalFlow/uav/resources/armv7a-neon/OpticalFlow_ardrone2.xml
===================================================================
--- trunk/demos/DemoOpticalFlow/uav/resources/armv7a-neon/OpticalFlow_ardrone2.xml (revision 405)
+++ trunk/demos/DemoOpticalFlow/uav/resources/armv7a-neon/OpticalFlow_ardrone2.xml (revision 405)
@@ -0,0 +1,267 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Index: trunk/demos/DemoOpticalFlow/uav/resources/core2-64/setup_x4.xml
===================================================================
--- trunk/demos/DemoOpticalFlow/uav/resources/core2-64/setup_x4.xml (revision 405)
+++ trunk/demos/DemoOpticalFlow/uav/resources/core2-64/setup_x4.xml (revision 405)
@@ -0,0 +1,287 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Index: trunk/demos/DemoOpticalFlow/uav/resources/core2-64/setup_x8.xml
===================================================================
--- trunk/demos/DemoOpticalFlow/uav/resources/core2-64/setup_x8.xml (revision 405)
+++ trunk/demos/DemoOpticalFlow/uav/resources/core2-64/setup_x8.xml (revision 405)
@@ -0,0 +1,287 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Index: trunk/demos/DemoOpticalFlow/uav/resources/core2-64/x4.sh
===================================================================
--- trunk/demos/DemoOpticalFlow/uav/resources/core2-64/x4.sh (revision 405)
+++ trunk/demos/DemoOpticalFlow/uav/resources/core2-64/x4.sh (revision 405)
@@ -0,0 +1,11 @@
+#! /bin/bash
+
+if [ -f /proc/xenomai/version ];then
+ EXEC=./DemoOpticalFlow_rt
+else
+ EXEC=./DemoOpticalFlow_nrt
+fi
+
+. $FLAIR_ROOT/flair-src/scripts/distribution_specific_hack.sh
+
+$EXEC -n x4_0 -a 127.0.0.1 -p 9000 -l /tmp -x setup_x4.xml -t x4_simu -v dsp_stub
Index: trunk/demos/DemoOpticalFlow/uav/resources/core2-64/x8.sh
===================================================================
--- trunk/demos/DemoOpticalFlow/uav/resources/core2-64/x8.sh (revision 405)
+++ trunk/demos/DemoOpticalFlow/uav/resources/core2-64/x8.sh (revision 405)
@@ -0,0 +1,11 @@
+#! /bin/bash
+
+if [ -f /proc/xenomai/version ];then
+ EXEC=./DemoOpticalFlow_rt
+else
+ EXEC=./DemoOpticalFlow_nrt
+fi
+
+. $FLAIR_ROOT/flair-src/scripts/distribution_specific_hack.sh
+
+$EXEC -n x8_0 -a 127.0.0.1 -p 9000 -l /tmp -x setup_x8.xml -t x8_simu -v dsp_stub
Index: trunk/demos/DemoOpticalFlow/uav/src/DemoOpticalFlow.cpp
===================================================================
--- trunk/demos/DemoOpticalFlow/uav/src/DemoOpticalFlow.cpp (revision 405)
+++ trunk/demos/DemoOpticalFlow/uav/src/DemoOpticalFlow.cpp (revision 405)
@@ -0,0 +1,200 @@
+// created: 2011/05/01
+// filename: DemoOpticalFlow.cpp
+//
+// author: Guillaume Sanahuja
+// Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+// version: $Id: $
+//
+// purpose: demo optical flow
+//
+//
+/*********************************************************************/
+
+#include "DemoOpticalFlow.h"
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+
+using namespace std;
+using namespace flair::core;
+using namespace flair::gui;
+using namespace flair::filter;
+using namespace flair::meta;
+using namespace flair::sensor;
+
+DemoOpticalFlow::DemoOpticalFlow(TargetController *controller): UavStateMachine(controller) {
+ Uav* uav=GetUav();
+ if (uav->GetVerticalCamera() == NULL) {
+ Err("no vertical camera found\n");
+ exit(1);
+ }
+
+ startOpticalflow=new PushButton(GetButtonsLayout()->NewRow(),"start optical flow");
+
+ greyCameraImage=new CvtColor(uav->GetVerticalCamera(),"gray",CvtColor::Conversion_t::ToGray);
+
+ uav->GetVerticalCamera()->UseDefaultPlot(greyCameraImage->Output());
+
+ //optical flow stack
+ //opticalFlow= matrice de déplacements en pixels entre 2 images consécutives
+ opticalFlow=new OpticalFlow(greyCameraImage,uav->GetVerticalCamera()->GetLayout()->NewRow(),"flux optique");
+ opticalFlowCompensated=new OpticalFlowCompensated(opticalFlow,uav->GetAhrs(),uav->GetVerticalCamera()->GetLayout()->NewRow(),"flux optique compense");
+ opticalFlowSpeedRaw=new OpticalFlowSpeed(opticalFlowCompensated,uav->GetVerticalCamera()->GetLayout()->NewRow(),"vitesse du Flux Optique");
+ //opticalFlowSpeed=vitesse de déplacement en pixels par seconde (moyenne sur tous les points et division par le delta T)
+ Matrix* twoByOneOFS=new Matrix((const Thread*)this,2,1,floatType);
+ Matrix* twoByOneOFAR=new Matrix((const Thread*)this,2,1,floatType);
+ Matrix* twoByOneOFA=new Matrix((const Thread*)this,2,1,floatType);
+ opticalFlowSpeed=new LowPassFilter(opticalFlowSpeedRaw,uav->GetVerticalCamera()->GetLayout()->NewRow(),"Speed lowPass",twoByOneOFS);
+ opticalFlowAccelerationRaw=new EulerDerivative(opticalFlowSpeed,uav->GetVerticalCamera()->GetLayout()->NewRow(),"derivative",twoByOneOFAR);
+ opticalFlowAcceleration=new LowPassFilter(opticalFlowAccelerationRaw,uav->GetVerticalCamera()->GetLayout()->NewRow(),"Acceleration lowPass",twoByOneOFA);
+
+ getFrameworkManager()->AddDeviceToLog(opticalFlowSpeedRaw);
+
+ Tab* opticalFlowTab=new Tab(getFrameworkManager()->GetTabWidget(),"flux optique");
+ DataPlot1D* xVelocityPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x speed (px/s)",-250,250);
+ DataPlot1D* yVelocityPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y speed (px/s)",-250,250);
+
+ xVelocityPlot->AddCurve(opticalFlowSpeedRaw->Output()->Element(0,0));
+ xVelocityPlot->AddCurve(opticalFlowSpeed->GetMatrix()->Element(0,0),DataPlot::Blue);
+ yVelocityPlot->AddCurve(opticalFlowSpeedRaw->Output()->Element(1,0));
+ yVelocityPlot->AddCurve(opticalFlowSpeed->GetMatrix()->Element(1,0),DataPlot::Blue);
+
+ u_x=new Pid(setupLawTab->At(1,0),"u_x");
+ u_x->UseDefaultPlot(graphLawTab->NewRow());
+ u_y=new Pid(setupLawTab->At(1,1),"u_y");
+ u_y->UseDefaultPlot(graphLawTab->LastRowLastCol());
+
+ opticalFlowGroupBox=new GroupBox(GetTargetController()->GetTab()->NewRow(),"consignes fo");
+ maxXSpeed=new DoubleSpinBox(opticalFlowGroupBox->NewRow(),"debattement x"," m/s",-5,5,0.1,1);
+ maxYSpeed=new DoubleSpinBox(opticalFlowGroupBox->LastRowLastCol(),"debattement y"," m/s",-5,5,0.1,1);
+
+ Tab* opticalFlowRealTab=new Tab(getFrameworkManager()->GetTabWidget(),"real speed");
+ opticalFlowRealSpeed=new Matrix((const Thread*)this,2,1,floatType);
+ opticalFlowReference=new Matrix((const Thread*)this,2,1,floatType);
+ opticalFlowRealAcceleration=new Matrix((const Thread*)this,2,1,floatType);
+ DataPlot1D* xRealVelocityPlot=new DataPlot1D(opticalFlowRealTab->NewRow(),"x speed (m/s)",-2,2);
+ DataPlot1D* yRealVelocityPlot=new DataPlot1D(opticalFlowRealTab->LastRowLastCol(),"y speed (m/s)",-2,2);
+ DataPlot1D* xRealAccelerationPlot=new DataPlot1D(opticalFlowRealTab->NewRow(),"x acceleration (m/s2)",-2,2);
+ DataPlot1D* yRealAccelerationPlot=new DataPlot1D(opticalFlowRealTab->LastRowLastCol(),"y acceleration (m/s2)",-2,2);
+ xRealVelocityPlot->AddCurve(opticalFlowRealSpeed->Element(0));
+ xRealVelocityPlot->AddCurve(opticalFlowReference->Element(0),DataPlot::Blue,"consigne");
+ yRealVelocityPlot->AddCurve(opticalFlowRealSpeed->Element(1));
+ yRealVelocityPlot->AddCurve(opticalFlowReference->Element(1),DataPlot::Blue,"consigne");
+ xRealAccelerationPlot->AddCurve(opticalFlowRealAcceleration->Element(0));
+ yRealAccelerationPlot->AddCurve(opticalFlowRealAcceleration->Element(1));
+
+ customReferenceOrientation= new AhrsData(this,"reference");
+ uav->GetAhrs()->AddPlot(customReferenceOrientation,DataPlot::Yellow);
+ AddDataToControlLawLog(customReferenceOrientation);
+
+ flagCameraLost=false;
+}
+
+void DemoOpticalFlow::SignalEvent(Event_t event) {
+ UavStateMachine::SignalEvent(event);
+ switch(event) {
+ case Event_t::EnteringControlLoop:
+ opticalFlowReference->SetValue(0,0,GetTargetController()->GetAxisValue(1)*maxXSpeed->Value());//joy axis 0 maps to x displacement
+ opticalFlowReference->SetValue(1,0,GetTargetController()->GetAxisValue(0)*maxYSpeed->Value());//joy axis 1 maps to y displacement
+ float focal=271.76;
+ float z,dz;
+ AltitudeValues(z, dz);
+ float scale=z/focal;
+ opticalFlowRealSpeed->SetValue(0,0,opticalFlowSpeed->Output(0,0)*scale);
+ opticalFlowRealSpeed->SetValue(1,0,opticalFlowSpeed->Output(1,0)*scale);
+ opticalFlowRealAcceleration->SetValue(0,0,opticalFlowAcceleration->Output(0,0)*scale);
+ opticalFlowRealAcceleration->SetValue(1,0,opticalFlowAcceleration->Output(1,0)*scale);
+ break;
+ }
+}
+
+void DemoOpticalFlow::StartOpticalFlow(void) {
+ if (SetOrientationMode(OrientationMode_t::Custom)) {
+ Thread::Info("(Re)entering optical flow mode\n");
+ u_x->Reset();
+ u_y->Reset();
+ } else {
+ Thread::Warn("Could not enter optical flow mode\n");
+ }
+}
+
+void DemoOpticalFlow::ExtraCheckPushButton(void) {
+ if(startOpticalflow->Clicked()) {
+ StartOpticalFlow();
+ }
+}
+
+void DemoOpticalFlow::ExtraCheckJoystick(void) {
+ static bool wasOpticalFlowModeButtonPressed=false;
+ // controller button R1 enters optical flow mode
+ if(GetTargetController()->IsButtonPressed(9)) { // R1
+ if (!wasOpticalFlowModeButtonPressed) {
+ wasOpticalFlowModeButtonPressed=true;
+ StartOpticalFlow();
+ }
+ } else {
+ wasOpticalFlowModeButtonPressed=false;
+ }
+}
+
+const AhrsData *DemoOpticalFlow::GetReferenceOrientation(void) {
+ Euler refAngles=GetDefaultReferenceOrientation()->GetQuaternion().ToEuler();//to keep default yaw reference
+
+ // /!\ in this demo, the target value is a speed (in m/s). As a consequence the error is the difference between the current speed and the target speed
+ Vector2Df error, errorVariation; // in Uav coordinate system
+
+ error.x=opticalFlowRealSpeed->Value(0,0)-opticalFlowReference->Value(0,0);
+ error.y=opticalFlowRealSpeed->Value(1,0)-opticalFlowReference->Value(1,0);
+ errorVariation.x=opticalFlowRealAcceleration->Value(0,0);
+ errorVariation.y=opticalFlowRealAcceleration->Value(1,0);
+//Printf("Altitude=%f, Error=(%f,%f)\n",z,error.x,error.y);
+
+ u_x->SetValues(error.x, errorVariation.x);
+ u_x->Update(GetTime());
+ refAngles.pitch=u_x->Output();
+
+ u_y->SetValues(error.y, errorVariation.y);
+ u_y->Update(GetTime());
+ refAngles.roll=-u_y->Output();
+
+ customReferenceOrientation->SetQuaternionAndAngularRates(refAngles.ToQuaternion(),Vector3Df(0,0,0));
+
+ return customReferenceOrientation;
+}
+
+void DemoOpticalFlow::ExtraSecurityCheck(void) {
+ if(GetUav()->GetType()=="hds_x8") {
+ if(((V4LCamera*)GetUav()->GetVerticalCamera())->HasProblems() && !flagCameraLost) {
+ flagCameraLost = true;
+ Thread::Err("Camera lost\n");
+ SafeStop();
+ Land();
+ }
+ }
+}
+
+DemoOpticalFlow::~DemoOpticalFlow() {
+}
Index: trunk/demos/DemoOpticalFlow/uav/src/DemoOpticalFlow.h
===================================================================
--- trunk/demos/DemoOpticalFlow/uav/src/DemoOpticalFlow.h (revision 405)
+++ trunk/demos/DemoOpticalFlow/uav/src/DemoOpticalFlow.h (revision 405)
@@ -0,0 +1,75 @@
+// created: 2011/05/01
+// filename: DemoOpticalFlow.h
+//
+// author: Guillaume Sanahuja
+// Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+// version: $Id: $
+//
+// purpose: demo optical flow
+//
+//
+/*********************************************************************/
+//------------
+
+#ifndef DEMOOPTICALFLOW_H
+#define DEMOOPTICALFLOW_H
+
+#include
+
+namespace flair {
+ namespace core {
+ class Matrix;
+ }
+ namespace gui {
+ class GroupBox;
+ class DoubleSpinBox;
+ class PushButton;
+ }
+ namespace filter {
+ class OpticalFlow;
+ class OpticalFlowCompensated;
+ class OpticalFlowSpeed;
+ class LowPassFilter;
+ class EulerDerivative;
+ class CvtColor;
+ }
+ namespace sensor {
+ class TargetController;
+ }
+}
+
+class DemoOpticalFlow : public flair::meta::UavStateMachine {
+
+ public:
+ DemoOpticalFlow(flair::sensor::TargetController *controller);
+ ~DemoOpticalFlow();
+
+ protected:
+ void SignalEvent(Event_t event);
+ void ExtraCheckJoystick(void);
+ void ExtraCheckPushButton(void);
+ void ExtraSecurityCheck(void);
+ const flair::core::AhrsData *GetReferenceOrientation(void);
+
+ flair::gui::GroupBox* opticalFlowGroupBox;
+ flair::gui::DoubleSpinBox *maxXSpeed,*maxYSpeed;
+ flair::core::Matrix *opticalFlowReference;
+ flair::filter::Pid *u_x, *u_y;
+ flair::filter::CvtColor* greyCameraImage;
+ flair::core::AhrsData *customReferenceOrientation;
+ flair::core::Matrix *opticalFlowRealSpeed,*opticalFlowRealAcceleration;
+
+ private:
+ flair::filter::OpticalFlow *opticalFlow;
+ flair::filter::OpticalFlowCompensated *opticalFlowCompensated;
+ flair::filter::OpticalFlowSpeed *opticalFlowSpeedRaw;
+ flair::filter::EulerDerivative *opticalFlowAccelerationRaw;
+ flair::gui::PushButton *startOpticalflow,*stopOpticalflow;
+ void StartOpticalFlow(void);
+ flair::filter::LowPassFilter* opticalFlowSpeed;
+ flair::filter::LowPassFilter* opticalFlowAcceleration;
+ bool flagCameraLost;
+};
+
+#endif // DEMOOPTICALFLOW_H
Index: trunk/demos/DemoOpticalFlow/uav/src/main.cpp
===================================================================
--- trunk/demos/DemoOpticalFlow/uav/src/main.cpp (revision 405)
+++ trunk/demos/DemoOpticalFlow/uav/src/main.cpp (revision 405)
@@ -0,0 +1,112 @@
+// created: 2011/05/01
+// filename: main.cpp
+//
+// author: Guillaume Sanahuja
+// Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+// version: $Id: $
+//
+// purpose: exemple de code x4
+//
+//
+/*********************************************************************/
+
+#include "DemoOpticalFlow.h"
+#include
+#include
+#include
+#include
+#include
+#include
+
+using namespace TCLAP;
+using namespace std;
+using namespace flair::core;
+using namespace flair::meta;
+using namespace flair::sensor;
+
+string uav_type;
+string dsp_file;
+string log_path;
+int port;
+int ds3port;
+string xml_file;
+string name;
+string address;
+
+void parseOptions(int argc, char** argv);
+
+
+int main(int argc, char* argv[]) {
+ parseOptions(argc,argv);
+
+ if(!InitVisionFilter("file="+dsp_file)) {
+ exit(1);
+ }
+
+ FrameworkManager *manager;
+ manager= new FrameworkManager(name);
+ manager->SetupConnection(address,port);
+ manager->SetupUserInterface(xml_file);
+ manager->SetupLogger(log_path);
+
+ Uav* drone=CreateUav(name,uav_type);
+ TargetEthController *controller=new TargetEthController("Dualshock3",ds3port);
+ DemoOpticalFlow* demo=new DemoOpticalFlow(controller);
+
+ demo->Start();
+ demo->Join();
+
+ delete manager;
+
+ CloseVisionFilter();
+}
+
+void parseOptions(int argc, char** argv)
+{
+ try
+ {
+
+ CmdLine cmd("Command description message", ' ', "0.1");
+
+ ValueArg nameArg("n","name","uav name",true,"x4","string");
+ cmd.add( nameArg );
+
+ ValueArg typeArg("t","type","uav type: ardrone2, hds_x4, hds_x8, hds_xufo, hds_simu or hds_simux (with x the number of the simulated uav)",true,"hds_x4","string");
+ cmd.add( typeArg );
+
+ ValueArg dspArg("v","dsp","executable dsp",true,"./dspcv_dsp.out","string");
+ cmd.add( dspArg );
+
+ ValueArg xmlArg("x","xml","fichier xml",true,"./reglages.xml","string");
+ cmd.add( xmlArg );
+
+ ValueArg logsArg("l","logs","repertoire des logs",true,"/media/ram","string");
+ cmd.add( logsArg );
+
+ ValueArg portArg("p","port","port pour station sol",true,9000,"int");
+ cmd.add( portArg );
+
+ ValueArg addressArg("a","address","addresse station sol",true,"127.0.0.1","string");
+ cmd.add( addressArg );
+
+ ValueArg ds3portArg("d","ds3_port","port pour ds3",false,20000,"int");
+ cmd.add( ds3portArg );
+
+ cmd.parse( argc, argv );
+
+ // Get the value parsed by each arg.
+ uav_type=typeArg.getValue();
+ dsp_file = dspArg.getValue();
+ log_path = logsArg.getValue();
+ port=portArg.getValue();
+ ds3port=ds3portArg.getValue();
+ xml_file = xmlArg.getValue();
+ name=nameArg.getValue();
+ address=addressArg.getValue();
+
+ } catch (ArgException &e) {
+ cerr << "error: " << e.error() << " for arg " << e.argId() << endl;
+ exit(EXIT_FAILURE);
+ }
+}
Index: trunk/demos/Sinus/resources/armv5te/Sinus.xml
===================================================================
--- trunk/demos/Sinus/resources/armv5te/Sinus.xml (revision 404)
+++ (revision )
@@ -1,43 +1,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
Index: trunk/demos/Sinus/resources/armv5te/sinus.xml
===================================================================
--- trunk/demos/Sinus/resources/armv5te/sinus.xml (revision 405)
+++ trunk/demos/Sinus/resources/armv5te/sinus.xml (revision 405)
@@ -0,0 +1,43 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Index: trunk/demos/Sinus/resources/armv7a-neon/Sinus.xml
===================================================================
--- trunk/demos/Sinus/resources/armv7a-neon/Sinus.xml (revision 404)
+++ (revision )
@@ -1,43 +1,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
Index: trunk/demos/Sinus/resources/armv7a-neon/sinus.xml
===================================================================
--- trunk/demos/Sinus/resources/armv7a-neon/sinus.xml (revision 405)
+++ trunk/demos/Sinus/resources/armv7a-neon/sinus.xml (revision 405)
@@ -0,0 +1,43 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Index: trunk/demos/Sinus/resources/core2-64/Sinus.xml
===================================================================
--- trunk/demos/Sinus/resources/core2-64/Sinus.xml (revision 404)
+++ (revision )
@@ -1,43 +1,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
Index: trunk/demos/Sinus/resources/core2-64/sinus.xml
===================================================================
--- trunk/demos/Sinus/resources/core2-64/sinus.xml (revision 405)
+++ trunk/demos/Sinus/resources/core2-64/sinus.xml (revision 405)
@@ -0,0 +1,43 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Index: trunk/demos/Sinus/resources/cortexa7thf-neon-vfpv4/Sinus.xml
===================================================================
--- trunk/demos/Sinus/resources/cortexa7thf-neon-vfpv4/Sinus.xml (revision 404)
+++ (revision )
@@ -1,43 +1,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
Index: trunk/demos/Sinus/resources/cortexa7thf-neon-vfpv4/sinus.xml
===================================================================
--- trunk/demos/Sinus/resources/cortexa7thf-neon-vfpv4/sinus.xml (revision 405)
+++ trunk/demos/Sinus/resources/cortexa7thf-neon-vfpv4/sinus.xml (revision 405)
@@ -0,0 +1,43 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Index: trunk/demos/Sinus/src/main.cpp
===================================================================
--- trunk/demos/Sinus/src/main.cpp (revision 404)
+++ trunk/demos/Sinus/src/main.cpp (revision 405)
@@ -65,5 +65,5 @@
// setup xml file, ./Sinus.xml by default
- ValueArg xmlArg("x", "xml", "xml file", false, "./Sinus.xml",
+ ValueArg xmlArg("x", "xml", "xml file", false, "./sinus.xml",
"string");
cmd.add(xmlArg);