Skip to content

Commit

Permalink
many
Browse files Browse the repository at this point in the history
  • Loading branch information
@TienPoly
TienPoly committed Oct 11, 2018
1 parent 431eb7d commit 249c9fc
Show file tree
Hide file tree
Showing 18 changed files with 583 additions and 436 deletions.
1 change: 1 addition & 0 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -101,6 +101,7 @@ generate_messages(
## Generate dynamic reconfigure parameters in the 'cfg' folder
generate_dynamic_reconfigure_options(
cfg/controllerDyn.cfg
cfg/controllerGS1Dyn.cfg
)

###################################
Expand Down
40 changes: 40 additions & 0 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,3 +36,43 @@ doi: 10.1109/ICUAS.2017.7991516
month={June}
}
```

## Scenario de test
* Gazebo test
- Code
+ Simulink Code Generation: 200Hz => Ts = 1/200
+ Node: ros::Rate r(200);
- Test
+ Launch
- roslaunch rotors_gazebo mav.launch
- roslaunch gsft_control tuning_nominal_Gazebo.launch
+ Gazebo: click run
- first odometry mesage
- rqt dynamic reconfigure "gazebo": max_update_rate from 100Hz to 200
- dynamic reconfigure "firefly/controller": mode, gain, LOE, ... => enable_take_off
* Compare GS and Baseline
- Code
+ Simulink Code Generation: 200Hz => Ts = 1/200
+ Node: ros::Rate r(200);
+ Cfg
- controller: enable_take_off = True
-
- Test
+ Launch
- roslaunch rotors_gazebo mav.launch
- roslaunch gsft_control tuning_nominal_Gazebo.launch
+ Gazebo: click run => first odometry mesage and rqt dynamic reconfigure "gazebo"
+ rqt
- dynamic reconfigure "gazebo": max_update_rate = 200
- dynamic reconfigure "firefly/controller": mode, gain, LOE, ... then click enable_take_off
* Experimental test
- Code
+ Simulink Code Generation: 1KHz => Ts = 1/1000
+ Node: ros::Rate r(1000);
+ git status, git add, git commit and git push
- Vicon server: firefly object, 200Hz
- Ground Station: roslaunch vicon, rqt, rosbag record
- Asctec Firefly
+ git pull and build
+ roslaunch: fcu, msf, controller
+ check frequencies: Vicon 200HZ, other (imu, msf, controller) 1KHz
15 changes: 9 additions & 6 deletions cfg/controllerDyn.cfg
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,8 @@
#! /usr/bin/env python

# cd ~/cfg/
# chmod +x your_file.cfg

PACKAGE='gsft_control'
import roslib
roslib.load_manifest(PACKAGE)
Expand Down Expand Up @@ -48,12 +51,12 @@ group5.add("LOE_3", double_t, 0.0, "lost of control
group5.add("LOE_4", double_t, 0.0, "lost of control 4", 0.0, 0.0, 1.0)
group5.add("LOE_5", double_t, 0.0, "lost of control 5", 0.0, 0.0, 1.0)
group5.add("LOE_6", double_t, 0.0, "lost of control 6", 0.0, 0.0, 1.0)
group5.add("LOE_t1", double_t, 0.0, "occurs time 1", 0.0, 0.0, 100.0)
group5.add("LOE_t2", double_t, 0.0, "occurs time 2", 0.0, 0.0, 100.0)
group5.add("LOE_t3", double_t, 0.0, "occurs time 3", 0.0, 0.0, 100.0)
group5.add("LOE_t4", double_t, 0.0, "occurs time 4", 0.0, 0.0, 100.0)
group5.add("LOE_t5", double_t, 0.0, "occurs time 5", 0.0, 0.0, 100.0)
group5.add("LOE_t6", double_t, 0.0, "occurs time 6", 0.0, 0.0, 100.0)
group5.add("LOE_t1", double_t, 0.0, "fault time 1", 0.0, 0.0, 100.0)
group5.add("LOE_t2", double_t, 0.0, "fault time 2", 0.0, 0.0, 100.0)
group5.add("LOE_t3", double_t, 0.0, "fault time 3", 0.0, 0.0, 100.0)
group5.add("LOE_t4", double_t, 0.0, "fault time 4", 0.0, 0.0, 100.0)
group5.add("LOE_t5", double_t, 0.0, "fault time 5", 0.0, 0.0, 100.0)
group5.add("LOE_t6", double_t, 0.0, "fault time 6", 0.0, 0.0, 100.0)

#group3.add("kT", double_t, NEW_PARAMETERS["value"], "thrust constant", 8.54858e-6, 1e-7, 1e-5)
#group3.add("factor", double_t, NEW_PARAMETERS["value"], "scaling factor", 0.035, 0.0, 0.05)
Expand Down
109 changes: 109 additions & 0 deletions cfg/controllerGS1Dyn.cfg
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,109 @@
#! /usr/bin/env python

# cd ~/cfg/
# chmod +x your_file.cfg

PACKAGE='gsft_control'
import roslib
roslib.load_manifest(PACKAGE)

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

ENABLE_CTRL = gen.const("ENABLE_CTRL", int_t, 0x00000001, "enable_ctrl")
NEW_REFERENCES = gen.const("NEW_REFERENCES", int_t, 0x00000002, "new_references")
#NEW_PARAMETERS = gen.const("NEW_PARAMETERS", int_t, 0x00000004, "drone_parameter")
CONTROLLER_GAIN = gen.const("CONTROLLER_GAIN", int_t, 0x00000008, "controller_gain")
#LOE = gen.const("LOE", int_t, 0x01000000, "lost_of_control_effectiveness")

group1 = gen.add_group("group_control")
group2 = gen.add_group("group_waypoint_references")
#group3 = gen.add_group("group3_drone_parameters")
group4 = gen.add_group("group_controller_gains")
group5 = gen.add_group("group_LOE")

# Name Type Reconfiguration level Description Default Min Max
gen.add("RESET", bool_t, 0, "reset parameters, gains", False)

test_mode_enum = gen.enum([ gen.const("TEST_MANUAL", int_t, 0, "Manual test"),
gen.const("TEST_WAYPOINT", int_t, 1, "Waypoint test"),
gen.const("TEST_TRACKING", int_t, 2, "Circular tracking test")],
"An enum to chose test mode")
gen.add("test_mode", int_t, 0, "test mode", 0, 0, 2, edit_method=test_mode_enum)

LOE_est_enum = gen.enum([ gen.const("NO", int_t, 0, "Nominal test"),
gen.const("LOE_FICTIVE", int_t, 1, "LOE fictive"),
gen.const("LOE_CALCUL", int_t, 2, "LOE calcul"),
gen.const("LOE_FDD", int_t, 3, "LOE fdd")],
"An enum to chose LOE estimation mode")
gen.add("LOE_estimation", int_t, 0, "LOE estimation", 0, 0, 3, edit_method=LOE_est_enum)

group1.add("enable_take_off", bool_t, ENABLE_CTRL["value"], "enable take off", False) # ENABLE_CTRL["value"] = 0x00000001
group1.add("enable_landing", bool_t, ENABLE_CTRL["value"], "enable landing", False)
group1.add("send_waypoint", bool_t, ENABLE_CTRL["value"], "send new waypoint", False)
group1.add("new_controller_gains", bool_t, ENABLE_CTRL["value"], "change controller gains", False)
#group1.add("new_drone_parameters", bool_t, ENABLE_CTRL["value"], "change drone parameters", False) # to do later
#group1.add("enable_yaw", bool_t, ENABLE_CTRL["value"], "enable yaw-control", True) # to do later
#group1.add("enable_gohome", bool_t, ENABLE_CTRL["value"], "enable go home", False) # to do later

group2.add("ref_x", double_t, NEW_REFERENCES["value"], "X position reference", 0.0, -2.0, 2.0)
group2.add("ref_y", double_t, NEW_REFERENCES["value"], "Y position reference", 0.0, -2.0, 2.0)
group2.add("ref_z", double_t, NEW_REFERENCES["value"], "Z position reference", 0.5, 0.0, 1.5)
group2.add("ref_yaw_deg", double_t, NEW_REFERENCES["value"], "yaw angle reference", 0.0, -180.0, 180.0)

group5.add("LOE_1", double_t, 0.0, "lost of control 1", 0.0, 0.0, 1.0)
group5.add("LOE_2", double_t, 0.0, "lost of control 2", 0.0, 0.0, 1.0)
group5.add("LOE_3", double_t, 0.0, "lost of control 3", 0.0, 0.0, 1.0)
group5.add("LOE_4", double_t, 0.0, "lost of control 4", 0.0, 0.0, 1.0)
group5.add("LOE_5", double_t, 0.0, "lost of control 5", 0.0, 0.0, 1.0)
group5.add("LOE_6", double_t, 0.0, "lost of control 6", 0.0, 0.0, 1.0)
group5.add("LOE_t1", double_t, 0.0, "fault time 1", 0.0, 0.0, 100.0)
group5.add("LOE_t2", double_t, 0.0, "fault time 2", 0.0, 0.0, 100.0)
group5.add("LOE_t3", double_t, 0.0, "fault time 3", 0.0, 0.0, 100.0)
group5.add("LOE_t4", double_t, 0.0, "fault time 4", 0.0, 0.0, 100.0)
group5.add("LOE_t5", double_t, 0.0, "fault time 5", 0.0, 0.0, 100.0)
group5.add("LOE_t6", double_t, 0.0, "fault time 6", 0.0, 0.0, 100.0)

#group3.add("kT", double_t, NEW_PARAMETERS["value"], "thrust constant", 8.54858e-6, 1e-7, 1e-5)
#group3.add("factor", double_t, NEW_PARAMETERS["value"], "scaling factor", 0.035, 0.0, 0.05)

group4.add("kz", double_t, CONTROLLER_GAIN["value"],"k-z", 14.7226, 0.0, 23.2925)
group4.add("kvz", double_t, CONTROLLER_GAIN["value"],"k-zd", 6.8078, 0.0, 8.47)
group4.add("kiz", double_t, CONTROLLER_GAIN["value"],"k-iz", 12.2474, 0.0, 25.025)

group4.add("kx", double_t, CONTROLLER_GAIN["value"],"k-x", 1.2426, 0.0, 2.0)
group4.add("kvx", double_t, CONTROLLER_GAIN["value"],"k-xd", 0.7085, 0.0, 2.0)
group4.add("kix", double_t, CONTROLLER_GAIN["value"],"k-ix", 0.9913, 0.0, 2.0)
group4.add("ky", double_t, CONTROLLER_GAIN["value"],"k-y", -0.9479, -2.0, 0.0)
group4.add("kvy", double_t, CONTROLLER_GAIN["value"],"k-yd", -0.5505, -2.0, 0.0)
group4.add("kiy", double_t, CONTROLLER_GAIN["value"],"k-iy", -0.7657, -2.0, 0.0)

#group4.add("kx", double_t, CONTROLLER_GAIN["value"],"k-x", 1.6651, 0.0, 2.0)
#group4.add("kvx", double_t, CONTROLLER_GAIN["value"],"k-xd", 0.9236, 0.0, 2.0)
#group4.add("kix", double_t, CONTROLLER_GAIN["value"],"k-ix", 1.3486, 0.0, 2.0)
#group4.add("ky", double_t, CONTROLLER_GAIN["value"],"k-y", -1.5145, -2.0, 0.0)
#group4.add("kvy", double_t, CONTROLLER_GAIN["value"],"k-yd", -0.8551, -2.0, 0.0)
#group4.add("kiy", double_t, CONTROLLER_GAIN["value"],"k-iy", -1.1930, -2.0, 0.0)

group4.add("kphi", double_t, CONTROLLER_GAIN["value"],"k-phi", 1.7378, 0.0, 10.0)
group4.add("kp", double_t, CONTROLLER_GAIN["value"],"k-p", 0.3476, 0.0, 1.0)
group4.add("kiphi", double_t, CONTROLLER_GAIN["value"],"k-iphi", 0.0, 0.0, 20.0)
group4.add("ktheta", double_t, CONTROLLER_GAIN["value"],"k-theta", 2.2946, 0.0, 10.0)
group4.add("kq", double_t, CONTROLLER_GAIN["value"],"k-q", 0.4589, 0.0, 1.0)
group4.add("kitheta", double_t, CONTROLLER_GAIN["value"],"k-itheta", 0.0, 0.0, 20.0)
group4.add("kpsi", double_t, CONTROLLER_GAIN["value"],"k-psi", 0.3036, 0.0, 0.498)
group4.add("kr", double_t, CONTROLLER_GAIN["value"],"k-p", 0.2440, 0.0, 0.3130)
group4.add("kipsi", double_t, CONTROLLER_GAIN["value"],"k-ipsi", 0.1581, 0.0, 0.3162)

#group4.add("kphi", double_t, CONTROLLER_GAIN["value"],"k-phi", 2.6042, 0.0, 10.0)
#group4.add("kp", double_t, CONTROLLER_GAIN["value"],"k-p", 0.4431, 0.0, 1.0)
#group4.add("kiphi", double_t, CONTROLLER_GAIN["value"],"k-iphi", 0.0, 0.0, 20.0)
#group4.add("ktheta", double_t, CONTROLLER_GAIN["value"],"k-theta", 2.8913 , 0.0, 10.0)
#group4.add("kq", double_t, CONTROLLER_GAIN["value"],"k-q", 0.5163, 0.0, 1.0)
#group4.add("kitheta", double_t, CONTROLLER_GAIN["value"],"k-itheta", 0.0, 0.0, 20.0)
#group4.add("kpsi", double_t, CONTROLLER_GAIN["value"],"k-psi", 0.7660, 0.0, 1.0)
#group4.add("kr", double_t, CONTROLLER_GAIN["value"],"k-p", 0.4006, 0.0, 1.0)
#group4.add("kipsi", double_t, CONTROLLER_GAIN["value"],"k-ipsi", 0.5774, 0.0, 1.0)

exit(gen.generate(PACKAGE, "gsft_control", "controllerGS1Dyn"))
2 changes: 1 addition & 1 deletion include/parameters.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@ static constexpr double kDefaultPi = 3.14159;
// Default constraints
static constexpr double kDefaultXmax = 2.5;
static constexpr double kDefaultYmax = 2.5;
static constexpr double kDefaultZmax = 1.5;
static constexpr double kDefaultZmax = 1.75;

static constexpr double kDefaultCutoffAltitude = 0.175; // CutOff motor (landing)

Expand Down
4 changes: 2 additions & 2 deletions launch/gs1_Gazebo.launch → launch/tuning_compare_Gazebo.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,14 +6,14 @@

<group ns="firefly">
<node name="tuning_GS1" pkg="gsft_control" type="tuning_GS1_node" output="screen">
<param name="~scenario" value="gazebo" type="str"/>
<param name="~scenario" value="compare" type="str"/>
<remap from="odometry" to="odometry_sensor1/odometry" />
</node>
</group>

<group ns="firefly1">
<node name="tuning_nominal" pkg="gsft_control" type="tuning_nominal_node" output="screen">
<param name="~scenario" value="gazebo" type="str"/>
<param name="~scenario" value="compare" type="str"/>
<remap from="odometry" to="odometry_sensor1/odometry" />
</node>
</group>
Expand Down
0 launch/gs1.launch → launch/tuning_gs1.launch
View file
File renamed without changes.
13 changes: 13 additions & 0 deletions launch/tuning_gs1_Gazebo.launch
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,13 @@
<launch>
<arg name="world_name" default="basic"/>
<arg name="enable_logging" default="false" />
<arg name="enable_ground_truth" default="true" />
<arg name="debug" default="false"/>

<group ns="firefly">
<node name="tuning_GS1" pkg="gsft_control" type="tuning_GS1_node" output="screen">
<param name="~scenario" value="gazebo" type="str"/>
<remap from="odometry" to="odometry_sensor1/odometry" />
</node>
</group>
</launch>
Loading

0 comments on commit 249c9fc

Please sign in to comment.