From d0c6636b7ff0d1e83a973a32e67d7ed1bd75a28c Mon Sep 17 00:00:00 2001 From: Marcel Stuettgen Date: Sat, 28 Feb 2015 17:25:51 +0100 Subject: [PATCH 001/268] added debug code to check if the values are really changing --- src/examples/rover_steering_control/main.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/src/examples/rover_steering_control/main.cpp b/src/examples/rover_steering_control/main.cpp index 7ef9140980..67ff489166 100644 --- a/src/examples/rover_steering_control/main.cpp +++ b/src/examples/rover_steering_control/main.cpp @@ -156,6 +156,23 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st struct actuator_controls_s *actuators) { + //double r,p,y,t; + + + + //printf("vehicle_attitude_setpoint:\n"); + //print values only when they change to get less spam in console + if((double)att_sp->roll_body > 0.0) { printf("att_sp->roll_body: %8.4f\n" , (double)att_sp->roll_body); } + if((double)att_sp->pitch_body > 0.0) {printf("att_sp->pitch_body: %8.4f\n" , (double)att_sp->pitch_body); } + if((double)att_sp->yaw_body > 0.0) { printf("att_sp->yaw_body: %8.4f\n" , (double)att_sp->yaw_body); } + if((double)att_sp->thrust > 0.0) { printf("att_sp->throttle: %8.4f\n" , (double)att_sp->thrust); } + + // r = (double)att_sp->roll_body; + // p = (double)att_sp->pitch_body; + // y = (double)att_sp->yaw_body; + // t = (double)att_sp->thrust; + + /* * The PX4 architecture provides a mixer outside of the controller. * The mixer is fed with a default vector of actuator controls, representing From 77142f2da77adcda9d5882b96ec3f2da3c8cda17 Mon Sep 17 00:00:00 2001 From: Marcel Stuettgen Date: Tue, 3 Mar 2015 18:54:16 +0100 Subject: [PATCH 002/268] renaming of rover file to match the model's name --- .../init.d/{50001_rover => 50001_axialracing_ax10} | 0 ROMFS/px4fmu_common/init.d/rc.autostart | 4 ++-- 2 files changed, 2 insertions(+), 2 deletions(-) rename ROMFS/px4fmu_common/init.d/{50001_rover => 50001_axialracing_ax10} (100%) diff --git a/ROMFS/px4fmu_common/init.d/50001_rover b/ROMFS/px4fmu_common/init.d/50001_axialracing_ax10 similarity index 100% rename from ROMFS/px4fmu_common/init.d/50001_rover rename to ROMFS/px4fmu_common/init.d/50001_axialracing_ax10 diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 434e7870b8..de81795b44 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -287,10 +287,10 @@ fi # -# Ground Rover +# Ground Rover AxialRacing AX10 # if param compare SYS_AUTOSTART 50001 then - sh /etc/init.d/50001_rover + sh /etc/init.d/50001_axialracing_ax10 fi From 9f99f7a79b2fb1bbbedcb6c1cbfa4089c860a1a0 Mon Sep 17 00:00:00 2001 From: Marcel Stuettgen Date: Tue, 3 Mar 2015 19:00:14 +0100 Subject: [PATCH 003/268] renamed apps and default config files too to match the model's name --- ROMFS/px4fmu_common/init.d/50001_axialracing_ax10 | 4 ++-- .../init.d/{rc.rover_apps => rc.axialracing_ax10_apps} | 0 .../{rc.rover_defaults => rc.axialracing_ax10_defaults} | 4 ++-- ROMFS/px4fmu_common/init.d/rcS | 4 ++-- 4 files changed, 6 insertions(+), 6 deletions(-) rename ROMFS/px4fmu_common/init.d/{rc.rover_apps => rc.axialracing_ax10_apps} (100%) rename ROMFS/px4fmu_common/init.d/{rc.rover_defaults => rc.axialracing_ax10_defaults} (95%) diff --git a/ROMFS/px4fmu_common/init.d/50001_axialracing_ax10 b/ROMFS/px4fmu_common/init.d/50001_axialracing_ax10 index c2f94705cd..a36bac5cc8 100644 --- a/ROMFS/px4fmu_common/init.d/50001_axialracing_ax10 +++ b/ROMFS/px4fmu_common/init.d/50001_axialracing_ax10 @@ -1,10 +1,10 @@ #!nsh # -# Generic rover +# loading default values for the axialracing ax10 # #load some defaults e.g. PWM values -sh /etc/init.d/rc.rover_defaults +sh /etc/init.d/rc.axialracing_ax10_defaults #choose a mixer, for rover control we need a plain passthrough to the servos set MIXER IO_pass diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_apps b/ROMFS/px4fmu_common/init.d/rc.axialracing_ax10_apps similarity index 100% rename from ROMFS/px4fmu_common/init.d/rc.rover_apps rename to ROMFS/px4fmu_common/init.d/rc.axialracing_ax10_apps diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_defaults b/ROMFS/px4fmu_common/init.d/rc.axialracing_ax10_defaults similarity index 95% rename from ROMFS/px4fmu_common/init.d/rc.rover_defaults rename to ROMFS/px4fmu_common/init.d/rc.axialracing_ax10_defaults index 23bf47df16..e5d400abe4 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.axialracing_ax10_defaults @@ -19,8 +19,8 @@ set PWM_RATE 50 set PWM_DISARMED 1500 # PWM range -set PWM_MIN 1200 -set PWM_MAX 1800 +set PWM_MIN 1100 +set PWM_MAX 1900 # Enable servo output on pins 3 and 4 (steering and thrust) # but also include 1+2 as they form together one output group diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 9118f26013..1f70f96600 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -18,7 +18,7 @@ set MODE autostart set FRC /fs/microsd/etc/rc.txt set FCONFIG /fs/microsd/etc/config.txt -set TUNE_ERR ML< Date: Tue, 3 Mar 2015 19:03:50 +0100 Subject: [PATCH 004/268] disabled start of rover steering app --- ROMFS/px4fmu_common/init.d/rc.axialracing_ax10_apps | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.axialracing_ax10_apps b/ROMFS/px4fmu_common/init.d/rc.axialracing_ax10_apps index 1d15b98351..625febd7a4 100644 --- a/ROMFS/px4fmu_common/init.d/rc.axialracing_ax10_apps +++ b/ROMFS/px4fmu_common/init.d/rc.axialracing_ax10_apps @@ -6,4 +6,5 @@ ekf_att_pos_estimator start -rover_steering_control start +# disabled the start of steering control app due to use of offboard mode only +# rover_steering_control start From f23b1d23501c9c56c5fc371a17a3537972088461 Mon Sep 17 00:00:00 2001 From: Marcel Stuettgen Date: Tue, 3 Mar 2015 19:06:08 +0100 Subject: [PATCH 005/268] commented set tuner back in (was commented out by accident) --- ROMFS/px4fmu_common/init.d/rcS | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 1f70f96600..b6db3a0e49 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -18,7 +18,7 @@ set MODE autostart set FRC /fs/microsd/etc/rc.txt set FCONFIG /fs/microsd/etc/config.txt -#set TUNE_ERR ML< Date: Sat, 28 Feb 2015 18:15:51 +0100 Subject: [PATCH 006/268] update ros launch files and nodes for update of rotors_simulator --- launch/ardrone.launch | 7 +++++-- launch/gazebo_ardrone_empty_world.launch | 9 ++++++--- ...o_ardrone_empty_world_offboard_attitudedemo.launch | 11 +++++++---- ...o_ardrone_empty_world_offboard_positiondemo.launch | 11 +++++++---- launch/gazebo_ardrone_house_world.launch | 9 ++++++--- launch/gazebo_iris_empty_world.launch | 9 ++++++--- launch/gazebo_iris_house_world.launch | 7 +++++-- launch/gazebo_iris_outdoor_world.launch | 7 +++++-- launch/iris.launch | 7 +++++-- launch/multicopter.launch | 3 ++- launch/multicopter_w.launch | 7 +++++-- launch/multicopter_x.launch | 7 +++++-- src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 6 +++--- 13 files changed, 67 insertions(+), 33 deletions(-) diff --git a/launch/ardrone.launch b/launch/ardrone.launch index 3173e7cf1a..809eab2924 100644 --- a/launch/ardrone.launch +++ b/launch/ardrone.launch @@ -1,8 +1,11 @@ + - + + + - + diff --git a/launch/gazebo_ardrone_empty_world.launch b/launch/gazebo_ardrone_empty_world.launch index 22bfb0c799..122a27867d 100644 --- a/launch/gazebo_ardrone_empty_world.launch +++ b/launch/gazebo_ardrone_empty_world.launch @@ -4,15 +4,18 @@ - + + - + - + + + diff --git a/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch b/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch index 717244abf5..596496194a 100644 --- a/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch +++ b/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch @@ -1,9 +1,12 @@ - + - + + + - - + + + diff --git a/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch b/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch index 9ff7f7d1fa..46f427f936 100644 --- a/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch +++ b/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch @@ -1,9 +1,12 @@ - + - + + + - - + + + diff --git a/launch/gazebo_ardrone_house_world.launch b/launch/gazebo_ardrone_house_world.launch index c636868eb2..89f27c901f 100644 --- a/launch/gazebo_ardrone_house_world.launch +++ b/launch/gazebo_ardrone_house_world.launch @@ -4,15 +4,18 @@ - + + - + - + + + diff --git a/launch/gazebo_iris_empty_world.launch b/launch/gazebo_iris_empty_world.launch index 8a4c128b90..0e80208e67 100644 --- a/launch/gazebo_iris_empty_world.launch +++ b/launch/gazebo_iris_empty_world.launch @@ -4,15 +4,18 @@ - + + - + - + + + diff --git a/launch/gazebo_iris_house_world.launch b/launch/gazebo_iris_house_world.launch index e4f9f6532c..d0e824d4b5 100644 --- a/launch/gazebo_iris_house_world.launch +++ b/launch/gazebo_iris_house_world.launch @@ -4,7 +4,8 @@ - + + @@ -13,6 +14,8 @@ - + + + diff --git a/launch/gazebo_iris_outdoor_world.launch b/launch/gazebo_iris_outdoor_world.launch index 22d55502d3..e75e57b877 100644 --- a/launch/gazebo_iris_outdoor_world.launch +++ b/launch/gazebo_iris_outdoor_world.launch @@ -4,7 +4,8 @@ - + + @@ -13,6 +14,8 @@ - + + + diff --git a/launch/iris.launch b/launch/iris.launch index be33cb85f4..fb9c0e6976 100644 --- a/launch/iris.launch +++ b/launch/iris.launch @@ -1,8 +1,11 @@ + - + + + - + diff --git a/launch/multicopter.launch b/launch/multicopter.launch index bc0e377715..6882f24137 100644 --- a/launch/multicopter.launch +++ b/launch/multicopter.launch @@ -1,6 +1,7 @@ + - + diff --git a/launch/multicopter_w.launch b/launch/multicopter_w.launch index f124082aa9..66c30d186f 100644 --- a/launch/multicopter_w.launch +++ b/launch/multicopter_w.launch @@ -1,8 +1,11 @@ + - + + + - + diff --git a/launch/multicopter_x.launch b/launch/multicopter_x.launch index 6355b87be9..c686eba390 100644 --- a/launch/multicopter_x.launch +++ b/launch/multicopter_x.launch @@ -1,8 +1,11 @@ + - + + + - + diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index 1098ec73b0..9b48294b64 100644 --- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -40,7 +40,7 @@ #include #include #include -#include +#include #include class MultirotorMixer @@ -129,7 +129,7 @@ MultirotorMixer::MultirotorMixer(): _rotors(_config_index[0]) { _sub = _n.subscribe("actuator_controls_0", 1, &MultirotorMixer::actuatorControlsCallback, this); - _pub = _n.advertise("/mixed_motor_commands", 10); + _pub = _n.advertise("command/motor_speed", 10); if (!_n.hasParam("motor_scaling_radps")) { _n.setParam("motor_scaling_radps", 150.0); @@ -237,7 +237,7 @@ void MultirotorMixer::actuatorControlsCallback(const px4::actuator_controls_0 &m mix(); // publish message - mav_msgs::MotorSpeed rotor_vel_msg; + mav_msgs::CommandMotorSpeed rotor_vel_msg; double scaling; double offset; _n.getParamCached("motor_scaling_radps", scaling); From df4f8259026bb7882659303558f5fb5b0d99e5f6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 4 Mar 2015 20:58:54 +0100 Subject: [PATCH 007/268] ros offboard demo: move mavros into model namespace --- ...gazebo_ardrone_empty_world_offboard_attitudedemo.launch | 4 +++- ...gazebo_ardrone_empty_world_offboard_positiondemo.launch | 4 +++- launch/mavros_sitl.launch | 7 +++++-- 3 files changed, 11 insertions(+), 4 deletions(-) diff --git a/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch b/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch index 596496194a..1309529b63 100644 --- a/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch +++ b/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch @@ -4,7 +4,9 @@ - + + + diff --git a/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch b/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch index 46f427f936..0371306ce5 100644 --- a/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch +++ b/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch @@ -4,7 +4,9 @@ - + + + diff --git a/launch/mavros_sitl.launch b/launch/mavros_sitl.launch index 40010a2735..7752c682d1 100644 --- a/launch/mavros_sitl.launch +++ b/launch/mavros_sitl.launch @@ -1,7 +1,9 @@ - - + + + + @@ -18,4 +20,5 @@ + From 9a9ca184e80622f2e217b4397be78a74cb1dad7f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 7 Mar 2015 19:27:36 +0100 Subject: [PATCH 008/268] update namespace in mavros tests --- .../demo_tests/direct_manual_input_test.py | 4 ++-- .../demo_tests/direct_offboard_posctl_test.py | 6 +++--- integrationtests/demo_tests/manual_input.py | 6 +++--- .../demo_tests/mavros_offboard_attctl_test.py | 18 +++++++++--------- .../demo_tests/mavros_offboard_posctl_test.py | 18 +++++++++--------- .../demo_tests/mavros_tests.launch | 8 ++++++-- 6 files changed, 32 insertions(+), 28 deletions(-) diff --git a/integrationtests/demo_tests/direct_manual_input_test.py b/integrationtests/demo_tests/direct_manual_input_test.py index 6d115316b2..b037d842f8 100755 --- a/integrationtests/demo_tests/direct_manual_input_test.py +++ b/integrationtests/demo_tests/direct_manual_input_test.py @@ -64,8 +64,8 @@ class ManualInputTest(unittest.TestCase): # def test_manual_input(self): rospy.init_node('test_node', anonymous=True) - rospy.Subscriber('px4_multicopter/actuator_armed', actuator_armed, self.actuator_armed_callback) - rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + rospy.Subscriber('iris/actuator_armed', actuator_armed, self.actuator_armed_callback) + rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) man = ManualInput() diff --git a/integrationtests/demo_tests/direct_offboard_posctl_test.py b/integrationtests/demo_tests/direct_offboard_posctl_test.py index e09550bbc2..18239e9264 100755 --- a/integrationtests/demo_tests/direct_offboard_posctl_test.py +++ b/integrationtests/demo_tests/direct_offboard_posctl_test.py @@ -66,9 +66,9 @@ class DirectOffboardPosctlTest(unittest.TestCase): def setUp(self): rospy.init_node('test_node', anonymous=True) - rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) - rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback) - self.pubSpt = rospy.Publisher('px4_multicopter/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) + rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + rospy.Subscriber("iris/vehicle_local_position", vehicle_local_position, self.position_callback) + self.pubSpt = rospy.Publisher('iris/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) self.rate = rospy.Rate(10) # 10hz def tearDown(self): diff --git a/integrationtests/demo_tests/manual_input.py b/integrationtests/demo_tests/manual_input.py index 9b2471a00e..7072464dec 100755 --- a/integrationtests/demo_tests/manual_input.py +++ b/integrationtests/demo_tests/manual_input.py @@ -54,8 +54,8 @@ class ManualInput: def __init__(self): rospy.init_node('test_node', anonymous=True) - self.pubMcsp = rospy.Publisher('px4_multicopter/manual_control_setpoint', manual_control_setpoint, queue_size=10) - self.pubOff = rospy.Publisher('px4_multicopter/offboard_control_mode', offboard_control_mode, queue_size=10) + self.pubMcsp = rospy.Publisher('iris/manual_control_setpoint', manual_control_setpoint, queue_size=10) + self.pubOff = rospy.Publisher('iris/offboard_control_mode', offboard_control_mode, queue_size=10) self.pubAtt = rospy.Publisher('iris/command/attitude', CommandAttitudeThrust, queue_size=10) def arm(self): @@ -85,7 +85,7 @@ class ManualInput: # Fake input to iris commander self.pubAtt.publish(att) rate.sleep() - count = count + 1 + count = count + 1 pos.r = 1 count = 0 diff --git a/integrationtests/demo_tests/mavros_offboard_attctl_test.py b/integrationtests/demo_tests/mavros_offboard_attctl_test.py index a52f7ffc1b..92cdf1e0ad 100755 --- a/integrationtests/demo_tests/mavros_offboard_attctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_attctl_test.py @@ -65,12 +65,12 @@ class MavrosOffboardAttctlTest(unittest.TestCase): def setUp(self): rospy.init_node('test_node', anonymous=True) - rospy.wait_for_service('mavros/cmd/arming', 30) - rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) - rospy.Subscriber("mavros/position/local", PoseStamped, self.position_callback) - self.pubAtt = rospy.Publisher('mavros/setpoint/attitude', PoseStamped, queue_size=10) - self.pubThr = rospy.Publisher('mavros/setpoint/att_throttle', Float64, queue_size=10) - self.cmdArm = rospy.ServiceProxy("mavros/cmd/arming", CommandBool) + rospy.wait_for_service('iris/mavros/cmd/arming', 30) + rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + rospy.Subscriber("iris/mavros/position/local", PoseStamped, self.position_callback) + self.pubAtt = rospy.Publisher('iris/mavros/setpoint/attitude', PoseStamped, queue_size=10) + self.pubThr = rospy.Publisher('iris/mavros/setpoint/att_throttle', Float64, queue_size=10) + self.cmdArm = rospy.ServiceProxy("iris/mavros/cmd/arming", CommandBool) self.rate = rospy.Rate(10) # 10hz self.rateSec = rospy.Rate(1) self.hasPos = False @@ -109,7 +109,7 @@ class MavrosOffboardAttctlTest(unittest.TestCase): # set some attitude and thrust att = PoseStamped() - att.header = Header() + att.header = Header() att.header.frame_id = "base_footprint" att.header.stamp = rospy.Time.now() quaternion = quaternion_from_euler(0.15, 0.15, 0) @@ -126,7 +126,7 @@ class MavrosOffboardAttctlTest(unittest.TestCase): att.header.stamp = rospy.Time.now() self.pubAtt.publish(att) self.pubThr.publish(throttle) - + if (self.localPosition.pose.position.x > 5 and self.localPosition.pose.position.z > 5 and self.localPosition.pose.position.y < -5): @@ -135,7 +135,7 @@ class MavrosOffboardAttctlTest(unittest.TestCase): self.rate.sleep() self.assertTrue(count < timeout, "took too long to cross boundaries") - + if __name__ == '__main__': import rostest diff --git a/integrationtests/demo_tests/mavros_offboard_posctl_test.py b/integrationtests/demo_tests/mavros_offboard_posctl_test.py index a3739ae5ce..6d26015e7b 100755 --- a/integrationtests/demo_tests/mavros_offboard_posctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_posctl_test.py @@ -64,11 +64,11 @@ class MavrosOffboardPosctlTest(unittest.TestCase): def setUp(self): rospy.init_node('test_node', anonymous=True) - rospy.wait_for_service('mavros/cmd/arming', 30) - rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) - rospy.Subscriber("mavros/position/local", PoseStamped, self.position_callback) - self.pubSpt = rospy.Publisher('mavros/setpoint/local_position', PoseStamped, queue_size=10) - self.cmdArm = rospy.ServiceProxy("mavros/cmd/arming", CommandBool) + rospy.wait_for_service('iris/mavros/cmd/arming', 30) + rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + rospy.Subscriber("iris/mavros/position/local", PoseStamped, self.position_callback) + self.pubSpt = rospy.Publisher('iris/mavros/setpoint/local_position', PoseStamped, queue_size=10) + self.cmdArm = rospy.ServiceProxy("iris/mavros/cmd/arming", CommandBool) self.rate = rospy.Rate(10) # 10hz self.rateSec = rospy.Rate(1) self.hasPos = False @@ -100,7 +100,7 @@ class MavrosOffboardPosctlTest(unittest.TestCase): def reach_position(self, x, y, z, timeout): # set a position setpoint pos = PoseStamped() - pos.header = Header() + pos.header = Header() pos.header.frame_id = "base_footprint" pos.pose.position.x = x pos.pose.position.y = y @@ -118,7 +118,7 @@ class MavrosOffboardPosctlTest(unittest.TestCase): # update timestamp for each published SP pos.header.stamp = rospy.Time.now() self.pubSpt.publish(pos) - + if(self.is_at_position(pos.pose.position.x, pos.pose.position.y, pos.pose.position.z, 0.5)): break count = count + 1 @@ -153,7 +153,7 @@ class MavrosOffboardPosctlTest(unittest.TestCase): for i in range(0, len(positions)): self.reach_position(positions[i][0], positions[i][1], positions[i][2], 120) - + # does it hold the position for Y seconds? positionHeld = True count = 0 @@ -166,7 +166,7 @@ class MavrosOffboardPosctlTest(unittest.TestCase): self.rate.sleep() self.assertTrue(count == timeout, "position could not be held") - + if __name__ == '__main__': import rostest diff --git a/integrationtests/demo_tests/mavros_tests.launch b/integrationtests/demo_tests/mavros_tests.launch index 4651f0dc9c..e42db60434 100644 --- a/integrationtests/demo_tests/mavros_tests.launch +++ b/integrationtests/demo_tests/mavros_tests.launch @@ -3,7 +3,8 @@ - + + @@ -11,8 +12,11 @@ + + + + - From 2883edaecd442e3049ad3224989cb384096e637a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 7 Mar 2015 23:38:48 +0100 Subject: [PATCH 009/268] ros sitl: increase Z gains for ardrone and iris --- launch/ardrone.launch | 4 +++- launch/iris.launch | 2 ++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/launch/ardrone.launch b/launch/ardrone.launch index 809eab2924..56030dd1f4 100644 --- a/launch/ardrone.launch +++ b/launch/ardrone.launch @@ -7,11 +7,13 @@ - + + + diff --git a/launch/iris.launch b/launch/iris.launch index fb9c0e6976..bf5b099b64 100644 --- a/launch/iris.launch +++ b/launch/iris.launch @@ -13,6 +13,8 @@ + + From b311f7302b508d1ed7fd3cfecd0bfbe47b324bde Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 10 Mar 2015 21:24:07 +0100 Subject: [PATCH 010/268] mavros sitl launch file: add default namespace --- launch/mavros_sitl.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/mavros_sitl.launch b/launch/mavros_sitl.launch index 7752c682d1..7362fa4644 100644 --- a/launch/mavros_sitl.launch +++ b/launch/mavros_sitl.launch @@ -2,7 +2,7 @@ - + From 7236f22f12dc5609f2775b846d12a62175c11d80 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 10 Mar 2015 21:37:26 +0100 Subject: [PATCH 011/268] added feed-forward for rates --- .../mc_att_control/mc_att_control_main.cpp | 16 +++++++++- .../mc_att_control/mc_att_control_params.c | 30 +++++++++++++++++++ 2 files changed, 45 insertions(+), 1 deletion(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 401cf05f17..85a0dda9e3 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -166,14 +166,17 @@ private: param_t roll_rate_p; param_t roll_rate_i; param_t roll_rate_d; + param_t roll_rate_ff; param_t pitch_p; param_t pitch_rate_p; param_t pitch_rate_i; param_t pitch_rate_d; + param_t pitch_rate_ff; param_t yaw_p; param_t yaw_rate_p; param_t yaw_rate_i; param_t yaw_rate_d; + param_t yaw_rate_ff; param_t yaw_ff; param_t yaw_rate_max; @@ -191,6 +194,7 @@ private: math::Vector<3> rate_p; /**< P gain for angular rate error */ math::Vector<3> rate_i; /**< I gain for angular rate error */ math::Vector<3> rate_d; /**< D gain for angular rate error */ + math::Vector<3> rate_ff; /**< Feedforward gain for desired rates */ float yaw_ff; /**< yaw control feed-forward */ float yaw_rate_max; /**< max yaw rate */ @@ -316,6 +320,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params.rate_p.zero(); _params.rate_i.zero(); _params.rate_d.zero(); + _params.rate_ff.zero(); _params.yaw_ff = 0.0f; _params.yaw_rate_max = 0.0f; _params.man_roll_max = 0.0f; @@ -335,14 +340,17 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); _params_handles.roll_rate_d = param_find("MC_ROLLRATE_D"); + _params_handles.roll_rate_ff = param_find("MC_ROLLRATE_FF"); _params_handles.pitch_p = param_find("MC_PITCH_P"); _params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P"); _params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I"); _params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D"); + _params_handles.pitch_rate_ff = param_find("MC_PITCHRATE_FF"); _params_handles.yaw_p = param_find("MC_YAW_P"); _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); + _params_handles.yaw_rate_ff = param_find("MC_YAWRATE_FF"); _params_handles.yaw_ff = param_find("MC_YAW_FF"); _params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX"); _params_handles.man_roll_max = param_find("MC_MAN_R_MAX"); @@ -394,6 +402,8 @@ MulticopterAttitudeControl::parameters_update() _params.rate_i(0) = v; param_get(_params_handles.roll_rate_d, &v); _params.rate_d(0) = v; + param_get(_params_handles.roll_rate_ff, &v); + _params.rate_ff(0) = v; /* pitch gains */ param_get(_params_handles.pitch_p, &v); @@ -404,6 +414,8 @@ MulticopterAttitudeControl::parameters_update() _params.rate_i(1) = v; param_get(_params_handles.pitch_rate_d, &v); _params.rate_d(1) = v; + param_get(_params_handles.pitch_rate_ff, &v); + _params.rate_ff(1) = v; /* yaw gains */ param_get(_params_handles.yaw_p, &v); @@ -414,6 +426,8 @@ MulticopterAttitudeControl::parameters_update() _params.rate_i(2) = v; param_get(_params_handles.yaw_rate_d, &v); _params.rate_d(2) = v; + param_get(_params_handles.yaw_rate_ff, &v); + _params.rate_ff(2) = v; param_get(_params_handles.yaw_ff, &_params.yaw_ff); param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max); @@ -653,7 +667,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) /* angular rates error */ math::Vector<3> rates_err = _rates_sp - rates; - _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int; + _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int + _params.rate_ff.emult(_rates_sp); _rates_prev = rates; /* update integral only if not saturated on low limit */ diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 302551959b..6a21f9046f 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -82,6 +82,16 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f); */ PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f); +/** + * Roll rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_ROLLRATE_FF, 0.0f); + /** * Pitch P gain * @@ -123,6 +133,16 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f); */ PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f); +/** + * Pitch rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_PITCHRATE_FF, 0.0f); + /** * Yaw P gain * @@ -164,6 +184,16 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); */ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); +/** + * Yaw rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_FF, 0.0f); + /** * Yaw feed forward * From 86970eead7919193a62022e9a9f0efe05d660dc6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 11 Mar 2015 09:24:30 +0100 Subject: [PATCH 012/268] Matlab tools: Motor transfer function curves --- Tools/Matlab/motors.m | 58 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 58 insertions(+) create mode 100644 Tools/Matlab/motors.m diff --git a/Tools/Matlab/motors.m b/Tools/Matlab/motors.m new file mode 100644 index 0000000000..6d688a3077 --- /dev/null +++ b/Tools/Matlab/motors.m @@ -0,0 +1,58 @@ +clear all; +close all; + +% Measurement data +% 1045 propeller +% Robbe Roxxy Motor (1100 kV, data collected in 2010) +data = [ 45, 7.4;... + 38, 5.6;... + 33, 4.3;... + 26, 3.0;... + 18, 2.0;... + 10, 1.0 ]; + +% Normalize the data, as we're operating later +% anyways in normalized units +data(:,1) = data(:,1) ./ max(data(:,1)); +data(:,2) = data(:,2) ./ max(data(:,2)); + +% Fit a 2nd degree polygon to the data and +% print the x2, x1, x0 coefficients +p = polyfit(data(:,2), data(:,1),2) + +% Override the first coffefficient for testing +% purposes +pf = 0.62; + +% Generate plotting data +px1 = linspace(0, max(data(:,2))); +py1 = polyval(p, px1); + +pyt = zeros(size(data, 1), 1); +corr = zeros(size(data, 1), 1); + +% Actual code test +% the two lines below are the ones needed to be ported to C: +% pf: Power factor parameter. +% px1(i): The current normalized motor command (-1..1) +% corr(i): The required correction. The motor speed is: +% px1(i) +for i=1:size(px1, 2) + + % The actual output throttle + pyt(i) = -pf * (px1(i) * px1(i)) + (1 + pf) * px1(i); + + % Solve for input throttle + % y = -p * x^2 + (1+p) * x; + % +end + +plot(data(:,2), data(:,1), '*r'); +hold on; +plot(px1, py1, '*b'); +hold on; +plot([0 px1(end)], [0 py1(end)], '-k'); +hold on; +plot(px1, pyt, '-b'); +hold on; +plot(px1, corr, '-m'); From 588edd794dea5fb0e4361a7bb8b79344552df5c6 Mon Sep 17 00:00:00 2001 From: zefz Date: Wed, 11 Mar 2015 12:47:11 +0100 Subject: [PATCH 013/268] AttPosEKF: Reset covariance calculation on state reset --- .../estimator_22states.cpp | 37 ++++++++++++++++--- .../estimator_22states.h | 8 +++- 2 files changed, 38 insertions(+), 7 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 5c01286e30..958ed8a18a 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -2592,25 +2592,40 @@ void AttPosEKF::CovarianceInit() P[1][1] = 0.25f * sq(1.0f*deg2rad); P[2][2] = 0.25f * sq(1.0f*deg2rad); P[3][3] = 0.25f * sq(10.0f*deg2rad); + + //velocities P[4][4] = sq(0.7f); P[5][5] = P[4][4]; P[6][6] = sq(0.7f); + + //positions P[7][7] = sq(15.0f); P[8][8] = P[7][7]; P[9][9] = sq(5.0f); + + //delta angle biases P[10][10] = sq(0.1f*deg2rad*dtIMU); P[11][11] = P[10][10]; P[12][12] = P[10][10]; + + //Z delta velocity bias P[13][13] = sq(0.2f*dtIMU); - P[14][14] = sq(0.0f); + + //Wind velocities + P[14][14] = 0.0f; P[15][15] = P[14][14]; + + //Earth magnetic field P[16][16] = sq(0.02f); P[17][17] = P[16][16]; P[18][18] = P[16][16]; + + //Body magnetic field P[19][19] = sq(0.02f); P[20][20] = P[19][19]; P[21][21] = P[19][19]; + //Optical flow fScaleFactorVar = 0.001f; // focal length scale factor variance Popt[0][0] = 0.001f; } @@ -2863,8 +2878,12 @@ void AttPosEKF::ResetPosition(void) for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){ storedStates[7][i] = states[7]; storedStates[8][i] = states[8]; - } + } } + + //reset position covariance + P[7][7] = sq(15.0f); + P[8][8] = P[7][7]; } void AttPosEKF::ResetHeight(void) @@ -2876,6 +2895,10 @@ void AttPosEKF::ResetHeight(void) for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){ storedStates[9][i] = states[9]; } + + //reset altitude covariance + P[9][9] = sq(5.0f); + P[6][6] = sq(0.7f); } void AttPosEKF::ResetVelocity(void) @@ -2884,7 +2907,8 @@ void AttPosEKF::ResetVelocity(void) states[4] = 0.0f; states[5] = 0.0f; states[6] = 0.0f; - } else if (GPSstatus >= GPS_FIX_3D) { + } + else if (GPSstatus >= GPS_FIX_3D) { //Do not use Z velocity, we trust the Barometer history more states[4] = velNED[0]; // north velocity from last reading @@ -2894,8 +2918,12 @@ void AttPosEKF::ResetVelocity(void) for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){ storedStates[4][i] = states[4]; storedStates[5][i] = states[5]; - } + } } + + //reset velocities covariance + P[4][4] = sq(0.7f); + P[5][5] = P[4][4]; } bool AttPosEKF::StatesNaN() { @@ -3293,7 +3321,6 @@ void AttPosEKF::ZeroVariables() magstate.DCM.identity(); memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); - } void AttPosEKF::GetFilterState(struct ekf_status_report *err) diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index c5517e38b6..89cf96245f 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -362,8 +362,6 @@ public: */ void ResetVelocity(); - void ZeroVariables(); - void GetFilterState(struct ekf_status_report *state); void GetLastErrorState(struct ekf_status_report *last_error); @@ -410,6 +408,12 @@ protected: void ResetStoredStates(); + /** + * @brief + * Reset internal filter states and clear all variables to zero value + */ + void ZeroVariables(); + private: bool _isFixedWing; ///< True if the vehicle is a fixed-wing frame type bool _onGround; ///< boolean true when the flight vehicle is on the ground (not flying) From 5f5a6e841f4ab287ee06b255e97bed4090b66068 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 12 Mar 2015 09:41:38 +0100 Subject: [PATCH 014/268] AttPosEKF: Reset states to current state --- .../estimator_22states.cpp | 33 +++++++++---------- 1 file changed, 15 insertions(+), 18 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 958ed8a18a..d5995f84da 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -2329,15 +2329,20 @@ void AttPosEKF::zeroCols(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATE // Store states in a history array along with time stamp void AttPosEKF::StoreStates(uint64_t timestamp_ms) { - for (size_t i=0; i= EKF_DATA_BUFFER_SIZE) { storeIndex = 0; + } } void AttPosEKF::ResetStoredStates() @@ -2350,10 +2355,8 @@ void AttPosEKF::ResetStoredStates() // reset store index to first storeIndex = 0; - statetimeStamp[storeIndex] = millis(); - - // increment to next storage index - storeIndex++; + //Reset stored state to current state + StoreStates(millis()); } // Output the state vector stored at the time that best matches that specified by msec @@ -2643,9 +2646,11 @@ float AttPosEKF::ConstrainFloat(float val, float min_val, float max_val) ret = val; } +#if 0 if (!isfinite(val)) { - //ekf_debug("constrain: non-finite!"); + ekf_debug("constrain: non-finite!"); } +#endif return ret; } @@ -3040,10 +3045,10 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error) // Fill error report GetFilterState(&last_ekf_error); - ResetStoredStates(); ResetVelocity(); ResetPosition(); ResetHeight(); + ResetStoredStates(); // Timeout cleared with this reset current_ekf_state.imuTimeout = false; @@ -3057,10 +3062,10 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error) // Fill error report, but not setting error flag GetFilterState(&last_ekf_error); - ResetStoredStates(); ResetVelocity(); ResetPosition(); ResetHeight(); + ResetStoredStates(); ret = 0; } @@ -3230,10 +3235,10 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) states[20] = magBias.y; // Magnetic Field Bias Y states[21] = magBias.z; // Magnetic Field Bias Z - ResetStoredStates(); ResetVelocity(); ResetPosition(); ResetHeight(); + ResetStoredStates(); // initialise focal length scale factor estimator states flowStates[0] = 1.0f; @@ -3245,7 +3250,6 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) //Define Earth rotation vector in the NED navigation frame calcEarthRateNED(earthRateNED, latRef); - } void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination) @@ -3337,13 +3341,6 @@ void AttPosEKF::GetFilterState(struct ekf_status_report *err) current_ekf_state.useAirspeed = useAirspeed; memcpy(err, ¤t_ekf_state, sizeof(*err)); - - // err->velHealth = current_ekf_state.velHealth; - // err->posHealth = current_ekf_state.posHealth; - // err->hgtHealth = current_ekf_state.hgtHealth; - // err->velTimeout = current_ekf_state.velTimeout; - // err->posTimeout = current_ekf_state.posTimeout; - // err->hgtTimeout = current_ekf_state.hgtTimeout; } void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error) From 20592ce4d81ff533510698391d163b9ec5819c9d Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 12 Mar 2015 10:29:16 +0100 Subject: [PATCH 015/268] AttPosEKF: Compile fix for protected HIL function --- .../ekf_att_pos_estimator/estimator_22states.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index 89cf96245f..6d3076da79 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -379,6 +379,12 @@ public: * true if the vehicle moves like a Fixed Wing, false otherwise **/ void setIsFixedWing(const bool fixedWing); + + /** + * @brief + * Reset internal filter states and clear all variables to zero value + */ + void ZeroVariables(); protected: @@ -407,12 +413,6 @@ protected: void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat); void ResetStoredStates(); - - /** - * @brief - * Reset internal filter states and clear all variables to zero value - */ - void ZeroVariables(); private: bool _isFixedWing; ///< True if the vehicle is a fixed-wing frame type From 67695f191ea6f528971c6d2da3dcac66540d7be9 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 12 Mar 2015 13:17:51 +0100 Subject: [PATCH 016/268] AttPosEKF: Use Geolib lat/lon position projection --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 3bb395a870..dfe3f5357c 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -908,8 +908,7 @@ void AttitudePositionEstimatorEKF::publishWindEstimate() } void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const bool fuseMag, - const bool fuseRangeSensor, - const bool fuseBaro, const bool fuseAirSpeed) + const bool fuseRangeSensor, const bool fuseBaro, const bool fuseAirSpeed) { // Run the strapdown INS equations every IMU update _ekf->UpdateStrapdownEquationsNED(); @@ -1324,10 +1323,7 @@ void AttitudePositionEstimatorEKF::pollData() if (_gps_initialized) { //Convert from global frame to local frame - float posNED[3] = {0.0f, 0.0f, 0.0f}; - _ekf->calcposNED(posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef); - _ekf->posNE[0] = posNED[0]; - _ekf->posNE[1] = posNED[1]; + map_projection_project(&_pos_ref, (_gps.lat / 1.0e7), (_gps.lon / 1.0e7), &_ekf->posNE[0], &_ekf->posNE[1]); if (dtLastGoodGPS > POS_RESET_THRESHOLD) { _ekf->ResetPosition(); From 211760e3e3e5af3ca8c9d0f96368d39d156fbad3 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 12 Mar 2015 17:28:21 +0100 Subject: [PATCH 017/268] AttPosEKF: Fix 5Hz sawtooth oscilation in XY position estimate --- .../AttitudePositionEstimatorEKF.h | 1 - .../ekf_att_pos_estimator_main.cpp | 14 ++++---------- 2 files changed, 4 insertions(+), 11 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index ec9efe8eea..712b4a17ef 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -208,7 +208,6 @@ private: bool _ekf_logging; ///< log EKF state unsigned _debug; ///< debug level - default 0 - bool _newDataGps; bool _newHgtData; bool _newAdsData; bool _newDataMag; diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index dfe3f5357c..3e007a9f94 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -188,7 +188,6 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _ekf_logging(true), _debug(0), - _newDataGps(false), _newHgtData(false), _newAdsData(false), _newDataMag(false), @@ -668,7 +667,7 @@ void AttitudePositionEstimatorEKF::task_main() } //Run EKF data fusion steps - updateSensorFusion(_newDataGps, _newDataMag, _newRangeData, _newHgtData, _newAdsData); + updateSensorFusion(_gpsIsGood, _newDataMag, _newRangeData, _newHgtData, _newAdsData); //Publish attitude estimations publishAttitude(); @@ -1267,10 +1266,10 @@ void AttitudePositionEstimatorEKF::pollData() } - orb_check(_gps_sub, &_newDataGps); - - if (_newDataGps) { + bool gps_update; + orb_check(_gps_sub, &gps_update); + if (gps_update) { orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); perf_count(_perf_gps); @@ -1349,9 +1348,6 @@ void AttitudePositionEstimatorEKF::pollData() _previousGPSTimestamp = _gps.timestamp_position; - } else { - //Too poor GPS fix to use - _newDataGps = false; } } @@ -1415,8 +1411,6 @@ void AttitudePositionEstimatorEKF::pollData() } perf_count(_perf_baro); - - _newHgtData = true; } //Update Magnetometer From 435d82dee216e73fbb648218d3f16dd7acab87f6 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 12 Mar 2015 18:17:39 +0100 Subject: [PATCH 018/268] AttPosEKF: Remove barometer reference altitude --- .../AttitudePositionEstimatorEKF.h | 1 - .../ekf_att_pos_estimator_main.cpp | 30 ++++++++----------- 2 files changed, 13 insertions(+), 18 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index 712b4a17ef..8d036261eb 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -174,7 +174,6 @@ private: struct map_projection_reference_s _pos_ref; - float _baro_ref; /**< barometer reference altitude */ float _baro_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */ float _baro_gps_offset; /**< offset between baro altitude (at GPS init time) and GPS altitude */ hrt_abstime _last_debug_print = 0; diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 3e007a9f94..686ec64f9f 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -153,7 +153,6 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _sensor_combined {}, _pos_ref {}, - _baro_ref(0.0f), _baro_ref_offset(0.0f), _baro_gps_offset(0.0f), @@ -635,24 +634,26 @@ void AttitudePositionEstimatorEKF::task_main() // } /* Initialize the filter first */ - if (!_gps_initialized && _gpsIsGood) { - initializeGPS(); - - } else if (!_ekf->statesInitialised) { + if (!_ekf->statesInitialised) { // North, East Down position (m) float initVelNED[3] = {0.0f, 0.0f, 0.0f}; _ekf->posNE[0] = 0.0f; _ekf->posNE[1] = 0.0f; - _local_pos.ref_alt = _baro_ref; + _local_pos.ref_alt = 0.0f; _baro_ref_offset = 0.0f; _baro_gps_offset = 0.0f; _baro_alt_filt = _baro.altitude; _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); - } else if (_ekf->statesInitialised) { + } else { + + if (!_gps_initialized && _gpsIsGood) { + initializeGPS(); + continue; + } // Check if on ground - status is used by covariance prediction _ekf->setOnGround(_landDetector.landed); @@ -716,7 +717,7 @@ void AttitudePositionEstimatorEKF::initializeGPS() _baro_alt_filt = _baro.altitude; _ekf->baroHgt = _baro.altitude; - _ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref)); + _ekf->hgtMea = _ekf->baroHgt; // Set up position variables correctly _ekf->GPSstatus = _gps.fix_type; @@ -857,7 +858,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() } /* local pos alt is negative, change sign and add alt offsets */ - _global_pos.alt = _baro_ref + (-_local_pos.z) - _baro_gps_offset; + _global_pos.alt = (-_local_pos.z) - _baro_gps_offset; if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; @@ -976,7 +977,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const if (fuseBaro) { // Could use a blend of GPS and baro alt data if desired - _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); + _ekf->hgtMea = _ekf->baroHgt; _ekf->fuseHgtData = true; // recall states stored at time of measurement after adjusting for delays @@ -1069,7 +1070,7 @@ void AttitudePositionEstimatorEKF::print_status() printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec); printf("baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)(_gps.alt / 1e3f)); - printf("ref alt: %8.4f baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref, (double)_baro_ref_offset, + printf("baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref_offset, (double)_baro_gps_offset); printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); @@ -1398,18 +1399,13 @@ void AttitudePositionEstimatorEKF::pollData() } baro_last = _baro.timestamp; + _baro_init = true; _ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f)); _ekf->baroHgt = _baro.altitude; _baro_alt_filt += (baro_elapsed / (rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt); - if (!_baro_init) { - _baro_ref = _baro.altitude; - _baro_init = true; - warnx("ALT REF INIT"); - } - perf_count(_perf_baro); } From e566cff28b0cabc0058f0d0dbede4709df470242 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 13 Mar 2015 10:20:06 +0100 Subject: [PATCH 019/268] ignore mount commands in commander --- src/modules/commander/commander.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 710ed85a19..4e119a110e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -727,6 +727,9 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case VEHICLE_CMD_CUSTOM_2: case VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY: case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY: + case VEHICLE_CMD_DO_MOUNT_CONTROL: + case VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT: + case VEHICLE_CMD_DO_MOUNT_CONFIGURE: /* ignore commands that handled in low prio loop */ break; From 4a8f799e9e0c862018360db808793e77759a5336 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Fri, 13 Mar 2015 15:27:02 +0100 Subject: [PATCH 020/268] AttPosEKF: Make local_pos output Z ref pos relative --- .../AttitudePositionEstimatorEKF.h | 1 + .../ekf_att_pos_estimator_main.cpp | 122 +++++++++--------- 2 files changed, 64 insertions(+), 59 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index 8d036261eb..4d5e56a961 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -193,6 +193,7 @@ private: bool _gpsIsGood; ///< True if the current GPS fix is good enough for us to use uint64_t _previousGPSTimestamp; ///< Timestamp of last good GPS fix we have received bool _baro_init; + float _baroAltRef; bool _gps_initialized; hrt_abstime _filter_start_time; hrt_abstime _last_sensor_timestamp; diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 686ec64f9f..6ca10e56ae 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -132,70 +132,71 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _wind_pub(-1), _att({}), - _gyro({}), - _accel({}), - _mag({}), - _airspeed({}), - _baro({}), - _vstatus({}), - _global_pos({}), - _local_pos({}), - _gps({}), - _wind({}), - _distance {}, - _landDetector {}, - _armed {}, + _gyro({}), + _accel({}), + _mag({}), + _airspeed({}), + _baro({}), + _vstatus({}), + _global_pos({}), + _local_pos({}), + _gps({}), + _wind({}), + _distance {}, + _landDetector {}, + _armed {}, - _gyro_offsets({}), - _accel_offsets({}), - _mag_offsets({}), + _gyro_offsets({}), + _accel_offsets({}), + _mag_offsets({}), - _sensor_combined {}, + _sensor_combined {}, - _pos_ref {}, - _baro_ref_offset(0.0f), - _baro_gps_offset(0.0f), + _pos_ref{}, + _baro_ref_offset(0.0f), + _baro_gps_offset(0.0f), - /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")), - _loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")), - _perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")), - _perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")), - _perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")), - _perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")), - _perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")), - _perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")), + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")), + _loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")), + _perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")), + _perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")), + _perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")), + _perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")), + _perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")), + _perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")), - /* states */ - _gps_alt_filt(0.0f), - _baro_alt_filt(0.0f), - _covariancePredictionDt(0.0f), - _gpsIsGood(false), - _previousGPSTimestamp(0), - _baro_init(false), - _gps_initialized(false), - _filter_start_time(0), - _last_sensor_timestamp(0), - _last_run(0), - _distance_last_valid(0), - _gyro_valid(false), - _accel_valid(false), - _mag_valid(false), - _gyro_main(0), - _accel_main(0), - _mag_main(0), - _ekf_logging(true), - _debug(0), + /* states */ + _gps_alt_filt(0.0f), + _baro_alt_filt(0.0f), + _covariancePredictionDt(0.0f), + _gpsIsGood(false), + _previousGPSTimestamp(0), + _baro_init(false), + _baroAltRef(0.0f), + _gps_initialized(false), + _filter_start_time(0), + _last_sensor_timestamp(0), + _last_run(0), + _distance_last_valid(0), + _gyro_valid(false), + _accel_valid(false), + _mag_valid(false), + _gyro_main(0), + _accel_main(0), + _mag_main(0), + _ekf_logging(true), + _debug(0), - _newHgtData(false), - _newAdsData(false), - _newDataMag(false), - _newRangeData(false), + _newHgtData(false), + _newAdsData(false), + _newDataMag(false), + _newRangeData(false), - _mavlink_fd(-1), - _parameters {}, - _parameter_handles {}, - _ekf(nullptr) + _mavlink_fd(-1), + _parameters {}, + _parameter_handles {}, + _ekf(nullptr) { _last_run = hrt_absolute_time(); @@ -810,7 +811,7 @@ void AttitudePositionEstimatorEKF::publishLocalPosition() _local_pos.y = _ekf->states[8]; // XXX need to announce change of Z reference somehow elegantly - _local_pos.z = _ekf->states[9] - _baro_ref_offset; + _local_pos.z = _ekf->states[9] - _baro_ref_offset - _baroAltRef; _local_pos.vx = _ekf->states[4]; _local_pos.vy = _ekf->states[5]; @@ -1399,7 +1400,10 @@ void AttitudePositionEstimatorEKF::pollData() } baro_last = _baro.timestamp; - _baro_init = true; + if(!_baro_init) { + _baro_init = true; + _baroAltRef = _baro.altitude; + } _ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f)); From 85082875906ffca937dbaaf3839188b17abd951e Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Fri, 13 Mar 2015 15:59:01 +0100 Subject: [PATCH 021/268] AttPosEKF: Remove unused code --- .../estimator_22states.cpp | 21 ------------------- .../estimator_22states.h | 7 ------- 2 files changed, 28 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index d5995f84da..c313e83af9 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -2516,27 +2516,6 @@ void AttPosEKF::quat2eul(float (&y)[3], const float (&u)[4]) y[2] = atan2f((2.0f*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3])); } -void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD) -{ - velNED[0] = gpsGndSpd*cosf(gpsCourse); - velNED[1] = gpsGndSpd*sinf(gpsCourse); - velNED[2] = gpsVelD; -} - -void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latReference, double lonReference, float hgtReference) -{ - posNED[0] = earthRadius * (lat - latReference); - posNED[1] = earthRadius * cos(latReference) * (lon - lonReference); - posNED[2] = -(hgt - hgtReference); -} - -void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef) -{ - lat = latRef + (double)posNED[0] * earthRadiusInv; - lon = lonRef + (double)posNED[1] * earthRadiusInv / cos(latRef); - hgt = hgtRef - posNED[2]; -} - void AttPosEKF::setOnGround(const bool isLanded) { _onGround = isLanded; diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index 6d3076da79..9b23f4df44 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -288,7 +288,6 @@ public: * Recall the state vector. * * Recalls the vector stored at closest time to the one specified by msec - *FuseOptFlow * @return zero on success, integer indicating the number of invalid states on failure. * Does only copy valid states, if the statesForFusion vector was initialized * correctly by the caller, the result can be safely used, but is a mixture @@ -307,12 +306,6 @@ public: static void quat2eul(float (&eul)[3], const float (&quat)[4]); - static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); - - static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef); - - static void calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef); - //static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]); static inline float sq(float valIn) {return valIn * valIn;} From 76aed9e58dcf7345e41b8e0e43d7af5f76418075 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 13 Mar 2015 19:00:16 +0100 Subject: [PATCH 022/268] - implemented configuration with mount config cmd - remember recieved mount control values --- src/drivers/gimbal/gimbal.cpp | 91 ++++++++++++++++++++++++++--------- 1 file changed, 68 insertions(+), 23 deletions(-) diff --git a/src/drivers/gimbal/gimbal.cpp b/src/drivers/gimbal/gimbal.cpp index ccda4c0a56..2e254c45da 100644 --- a/src/drivers/gimbal/gimbal.cpp +++ b/src/drivers/gimbal/gimbal.cpp @@ -121,8 +121,12 @@ private: int _vehicle_command_sub; int _att_sub; - bool _attitude_compensation; + bool _attitude_compensation_roll; + bool _attitude_compensation_pitch; + bool _attitude_compensation_yaw; bool _initialized; + bool _control_cmd_set; + bool _config_cmd_set; orb_advert_t _actuator_controls_2_topic; @@ -130,6 +134,9 @@ private: perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; + struct vehicle_command_s _control_cmd; + struct vehicle_command_s _config_cmd; + /** * Initialise the automatic measurement state machine and start it. * @@ -169,7 +176,9 @@ Gimbal::Gimbal() : CDev("Gimbal", GIMBAL_DEVICE_PATH), _vehicle_command_sub(-1), _att_sub(-1), - _attitude_compensation(true), + _attitude_compensation_pitch(true), + _attitude_compensation_yaw(true), + _attitude_compensation_roll(true), _initialized(false), _actuator_controls_2_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "gimbal_read")), @@ -221,7 +230,9 @@ Gimbal::ioctl(struct file *filp, int cmd, unsigned long arg) switch (cmd) { case GIMBALIOCATTCOMPENSATE: - _attitude_compensation = (arg != 0); + _attitude_compensation_roll = (arg != 0); + _attitude_compensation_pitch = (arg != 0); + _attitude_compensation_yaw = (arg != 0); return OK; default: @@ -286,22 +297,30 @@ Gimbal::cycle() float yaw = 0.0f; - if (_attitude_compensation) { - if (_att_sub < 0) { - _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - } + if (_att_sub < 0) { + _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + } - vehicle_attitude_s att; + vehicle_attitude_s att; - orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att); + orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att); + if (_attitude_compensation_roll) { roll = -att.roll; - pitch = -att.pitch; - yaw = att.yaw; - updated = true; } + if (_attitude_compensation_pitch) { + pitch = -att.pitch; + updated = true; + } + + if (_attitude_compensation_yaw) { + yaw = att.yaw; + updated = true; + } + + struct vehicle_command_s cmd; bool cmd_updated; @@ -312,22 +331,47 @@ Gimbal::cycle() orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd); - VEHICLE_MOUNT_MODE mountMode = (VEHICLE_MOUNT_MODE)cmd.param7; - debug("cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", cmd.command, mountMode, (double)cmd.param1, (double)cmd.param2); + if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL + || cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT) { - if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL && mountMode == VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { + _control_cmd = cmd; + _control_cmd_set = true; - /* Convert to range -1 ... 1, which corresponds to -180deg ... 180deg */ - roll += 1.0f / M_PI_F * M_DEG_TO_RAD_F * cmd.param1; - pitch += 1.0f / M_PI_F * M_DEG_TO_RAD_F * cmd.param2; - yaw += 1.0f / M_PI_F * M_DEG_TO_RAD_F * cmd.param3; - - updated = true; + } else if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONFIGURE) { + + _config_cmd = cmd; + _config_cmd_set = true; } - if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT && mountMode == VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { - float gimbalDirectionQuat[] = {cmd.param1, cmd.param2, cmd.param3, cmd.param4}; + } + + if (_config_cmd_set) { + + _config_cmd_set = false; + + _attitude_compensation_roll = _config_cmd.param2 == 1; + _attitude_compensation_pitch = _config_cmd.param3 == 1; + _attitude_compensation_yaw = _config_cmd.param4 == 1; + + } + + if (_control_cmd_set) { + + VEHICLE_MOUNT_MODE mountMode = (VEHICLE_MOUNT_MODE)_control_cmd.param7; + debug("control_cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", _control_cmd.command, mountMode, (double)_control_cmd.param1, (double)_control_cmd.param2); + + if (_control_cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL && mountMode == VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { + /* Convert to range -1 ... 1, which corresponds to -180deg ... 180deg */ + roll += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param1; + pitch += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param2; + yaw += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param3; + + updated = true; + } + + if (_control_cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT && mountMode == VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { + float gimbalDirectionQuat[] = {_control_cmd.param1, _control_cmd.param2, _control_cmd.param3, _control_cmd.param4}; math::Vector<3> gimablDirectionEuler = math::Quaternion(gimbalDirectionQuat).to_dcm().to_euler(); roll += gimablDirectionEuler(0); @@ -336,6 +380,7 @@ Gimbal::cycle() updated = true; } + } if (updated) { From 9efeb4cf0bab9cf3aaba2b618455f342f749b3ad Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 14 Mar 2015 13:56:28 +1100 Subject: [PATCH 023/268] FMUv2: added bootloader delay signature to text this allows for a configurable bootloader delay --- nuttx-configs/px4fmu-v2/scripts/ld.script | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/nuttx-configs/px4fmu-v2/scripts/ld.script b/nuttx-configs/px4fmu-v2/scripts/ld.script index bec896d1ce..b04ad89a6c 100644 --- a/nuttx-configs/px4fmu-v2/scripts/ld.script +++ b/nuttx-configs/px4fmu-v2/scripts/ld.script @@ -66,12 +66,20 @@ EXTERN(_vectors) /* force the vectors to be included in the output */ * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). */ EXTERN(abort) +EXTERN(_bootdelay_signature) SECTIONS { .text : { _stext = ABSOLUTE(.); *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; *(.text .text.*) *(.fixup) *(.gnu.warning) From 2c2359dcf02325a04502dc3bf917ef9c3906763a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 14 Mar 2015 13:56:54 +1100 Subject: [PATCH 024/268] px_uploader.py: added --boot-delay option this sets the bootloader delay --- Tools/px_uploader.py | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 43a1167451..3cdd9d1a6f 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -161,6 +161,7 @@ class uploader(object): GET_OTP = b'\x2a' # rev4+ , get a word from OTP area GET_SN = b'\x2b' # rev4+ , get a word from SN area GET_CHIP = b'\x2c' # rev5+ , get chip version + SET_BOOT_DELAY = b'\x2d' # rev5+ , set boot delay REBOOT = b'\x30' INFO_BL_REV = b'\x01' # bootloader protocol revision @@ -405,6 +406,12 @@ class uploader(object): raise RuntimeError("Program CRC failed") self.__drawProgressBar(label, 100, 100) + def __set_boot_delay(self, boot_delay): + self.__send(uploader.SET_BOOT_DELAY + + struct.pack("b", boot_delay) + + uploader.EOC) + self.__getSync() + # get basic data about the board def identify(self): # make sure we are in sync before starting @@ -472,6 +479,9 @@ class uploader(object): else: self.__verify_v3("Verify ", fw) + if args.boot_delay is not None: + self.__set_boot_delay(args.boot_delay) + print("\nRebooting.\n") self.__reboot() self.port.close() @@ -501,6 +511,7 @@ parser = argparse.ArgumentParser(description="Firmware uploader for the PX autop parser.add_argument('--port', action="store", required=True, help="Serial port(s) to which the FMU may be attached") parser.add_argument('--baud', action="store", type=int, default=115200, help="Baud rate of the serial port (default is 115200), only required for true serial ports.") parser.add_argument('--force', action='store_true', default=False, help='Override board type check and continue loading') +parser.add_argument('--boot-delay', type=int, default=None, help='minimum boot delay to store in flash') parser.add_argument('firmware', action="store", help="Firmware file to be uploaded") args = parser.parse_args() From 9d8931328b1c4423f747c08d115aec1387d5e597 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 14 Mar 2015 14:17:21 +1100 Subject: [PATCH 025/268] Tools: added boot_now.py script --- Tools/boot_now.py | 59 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 59 insertions(+) create mode 100755 Tools/boot_now.py diff --git a/Tools/boot_now.py b/Tools/boot_now.py new file mode 100755 index 0000000000..5a9e608dad --- /dev/null +++ b/Tools/boot_now.py @@ -0,0 +1,59 @@ +#!/usr/bin/env python +############################################################################ +# +# Copyright (C) 2012-2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# send BOOT command to a device + +import argparse +import serial, sys + +from sys import platform as _platform + +# Parse commandline arguments +parser = argparse.ArgumentParser(description="Send boot command to a device") +parser.add_argument('--baud', action="store", type=int, default=115200, help="Baud rate of the serial port") +parser.add_argument('port', action="store", help="Serial port(s) to which the FMU may be attached") +args = parser.parse_args() + +REBOOT = b'\x30' +EOC = b'\x20' + +print("Sending reboot to %s" % args.port) +try: + port = serial.Serial(args.port, args.baud, timeout=0.5) +except Exception: + print("Unable to open %s" % args.port) + sys.exit(1) +port.write(REBOOT + EOC) +port.close() +sys.exit(0) From aceca6b2a929482ba1c32c553dc98478ea40dfb8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 15 Mar 2015 10:39:21 +0100 Subject: [PATCH 026/268] Fix gyro offset calculation --- .../ekf_att_pos_estimator_main.cpp | 14 ++++++------- .../estimator_22states.cpp | 20 ++++++++++++------- 2 files changed, 20 insertions(+), 14 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 6ca10e56ae..c16e72ea0b 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -784,14 +784,14 @@ void AttitudePositionEstimatorEKF::publishAttitude() _att.pitch = euler(1); _att.yaw = euler(2); - _att.rollspeed = _ekf->angRate.x - _ekf->states[10] / _ekf->dtIMU; - _att.pitchspeed = _ekf->angRate.y - _ekf->states[11] / _ekf->dtIMU; - _att.yawspeed = _ekf->angRate.z - _ekf->states[12] / _ekf->dtIMU; + _att.rollspeed = _ekf->angRate.x - _ekf->states[10] / _ekf->dtIMUfilt; + _att.pitchspeed = _ekf->angRate.y - _ekf->states[11] / _ekf->dtIMUfilt; + _att.yawspeed = _ekf->angRate.z - _ekf->states[12] / _ekf->dtIMUfilt; // gyro offsets - _att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMU; - _att.rate_offsets[1] = _ekf->states[11] / _ekf->dtIMU; - _att.rate_offsets[2] = _ekf->states[12] / _ekf->dtIMU; + _att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt; + _att.rate_offsets[1] = _ekf->states[11] / _ekf->dtIMUfilt; + _att.rate_offsets[2] = _ekf->states[12] / _ekf->dtIMUfilt; /* lazily publish the attitude only once available */ if (_att_pub > 0) { @@ -1069,7 +1069,7 @@ void AttitudePositionEstimatorEKF::print_status() // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) - printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec); + printf("dtIMU: %8.6f filt: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (double)_ekf->dtIMUfilt, (int)IMUmsec); printf("baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)(_gps.alt / 1e3f)); printf("baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref_offset, (double)_baro_gps_offset); diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index c313e83af9..4fac83db20 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -210,7 +210,7 @@ void AttPosEKF::InitialiseParameters() yawVarScale = 1.0f; windVelSigma = 0.1f; - dAngBiasSigma = 5.0e-7f; + dAngBiasSigma = 5.0e-7; dVelBiasSigma = 1e-4f; magEarthSigma = 3.0e-4f; magBodySigma = 3.0e-4f; @@ -414,9 +414,10 @@ void AttPosEKF::CovariancePrediction(float dt) // calculate covariance prediction process noise for (uint8_t i= 0; i<4; i++) processNoise[i] = 1.0e-9f; for (uint8_t i= 4; i<10; i++) processNoise[i] = 1.0e-9f; - for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma; // scale gyro bias noise when on ground to allow for faster bias estimation - for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma; + float gyroBiasScale = (_onGround) ? 2.0f : 1.0f; + + for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma * gyroBiasScale; processNoise[13] = dVelBiasSigma; if (!inhibitWindStates) { for (uint8_t i=14; i<=15; i++) processNoise[i] = dt * windVelSigma; @@ -2706,6 +2707,11 @@ void AttPosEKF::ConstrainStates() // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) + // Constrain dtIMUfilt + if (!isfinite(dtIMUfilt) || (fabsf(dtIMU - dtIMUfilt) > 0.01f)) { + dtIMUfilt = dtIMU; + } + // Constrain quaternion for (size_t i = 0; i <= 3; i++) { states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); @@ -2727,11 +2733,11 @@ void AttPosEKF::ConstrainStates() // Angle bias limit - set to 8 degrees / sec for (size_t i = 10; i <= 12; i++) { - states[i] = ConstrainFloat(states[i], -0.12f * dtIMU, 0.12f * dtIMU); + states[i] = ConstrainFloat(states[i], -0.16f * dtIMUfilt, 0.16f * dtIMUfilt); } // Constrain delta velocity bias - states[13] = ConstrainFloat(states[13], -1.0f * dtIMU, 1.0f * dtIMU); + states[13] = ConstrainFloat(states[13], -1.0f * dtIMUfilt, 1.0f * dtIMUfilt); // Wind velocity limits - assume 120 m/s max velocity for (size_t i = 14; i <= 15; i++) { @@ -2795,8 +2801,8 @@ bool AttPosEKF::GyroOffsetsDiverged() // Protect against division by zero if (delta_len > 0.0f) { - float cov_mag = ConstrainFloat((P[10][10] + P[11][11] + P[12][12]), 1e-12f, 1e-8f); - delta_len_scaled = (5e-7 / (double)cov_mag) * (double)delta_len / (double)dtIMU; + float cov_mag = ConstrainFloat((P[10][10] + P[11][11] + P[12][12]), 1e-12f, 1e-2f); + delta_len_scaled = (5e-7 / (double)cov_mag) * (double)delta_len / (double)dtIMUfilt; } bool diverged = (delta_len_scaled > 1.0f); From bab79ece49692e61c042ebc384f222a01fa4bb14 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 15 Mar 2015 12:00:37 +0100 Subject: [PATCH 027/268] Better defaults for filter noise params --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_params.c | 4 ++-- src/modules/ekf_att_pos_estimator/estimator_22states.cpp | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c index 8c82dd6c14..f8cca6c0dd 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c @@ -218,7 +218,7 @@ PARAM_DEFINE_FLOAT(PE_ACC_PNOISE, 0.25f); * @max 0.00001 * @group Position Estimator */ -PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-07f); +PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-06f); /** * Accelerometer bias estimate process noise @@ -230,7 +230,7 @@ PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-07f); * @max 0.001 * @group Position Estimator */ -PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 0.00005f); +PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 0.0002f); /** * Magnetometer earth frame offsets process noise diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 4fac83db20..817590babf 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -210,10 +210,10 @@ void AttPosEKF::InitialiseParameters() yawVarScale = 1.0f; windVelSigma = 0.1f; - dAngBiasSigma = 5.0e-7; - dVelBiasSigma = 1e-4f; - magEarthSigma = 3.0e-4f; - magBodySigma = 3.0e-4f; + dAngBiasSigma = 1.0e-6; + dVelBiasSigma = 0.0002f; + magEarthSigma = 0.0003f; + magBodySigma = 0.0003f; vneSigma = 0.2f; vdSigma = 0.3f; From cf56db21a3e2987569e03edca8ef41b6f6f272b5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 15 Mar 2015 12:00:48 +0100 Subject: [PATCH 028/268] Better defaults for filter tuning params --- ROMFS/px4fmu_common/init.d/rc.fw_defaults | 7 +++++++ ROMFS/px4fmu_common/init.d/rc.mc_defaults | 2 ++ ROMFS/px4fmu_common/init.d/rc.vtol_defaults | 7 +++++++ 3 files changed, 16 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index e6de64054b..156711c26d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -15,4 +15,11 @@ then param set FW_T_RLL2THR 15 param set FW_T_SRATE_P 0.01 param set FW_T_TIME_CONST 5 + + param set PE_VELNE_NOISE 0.3 + param set PE_VELD_NOISE 0.5 + param set PE_POSNE_NOISE 0.5 + param set PE_POSD_NOISE 0.5 + param set PE_GBIAS_PNOISE 0.000001 + param set PE_ABIAS_PNOISE 0.0002 fi diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index 24d9650a0c..1f34282aec 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -40,6 +40,8 @@ then param set PE_VELD_NOISE 0.7 param set PE_POSNE_NOISE 0.5 param set PE_POSD_NOISE 1.0 + param set PE_GBIAS_PNOISE 0.000001 + param set PE_ABIAS_PNOISE 0.0001 param set NAV_ACC_RAD 2.0 param set RTL_RETURN_ALT 30.0 diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults index 6dc5a65db7..844d540bf0 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults @@ -32,6 +32,13 @@ then param set FW_RR_I 0.00 param set FW_RR_IMAX 0.2 param set FW_RR_P 0.02 + + param set PE_VELNE_NOISE 0.5 + param set PE_VELD_NOISE 0.7 + param set PE_POSNE_NOISE 0.5 + param set PE_POSD_NOISE 1.0 + param set PE_GBIAS_PNOISE 0.000001 + param set PE_ABIAS_PNOISE 0.0001 fi set PWM_DISARMED 900 From 9db48df3d63836c5cca4480d847777c166bb31e8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 15 Mar 2015 12:03:20 +0100 Subject: [PATCH 029/268] Slightly increase commander stack size to accomodate any additional printf calls --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 4e119a110e..aa7a17a22c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -278,7 +278,7 @@ int commander_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 3200, + 3400, commander_thread_main, (argv) ? (char * const *)&argv[2] : (char * const *)NULL); From 6f22cd07da4bf9295fb18abab43589ad790cb4a8 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sun, 15 Mar 2015 12:10:53 +0100 Subject: [PATCH 030/268] fixing handling of attitude setpoints --- src/platforms/ros/nodes/mavlink/mavlink.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index 5459fcffdc..c8188f9375 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -162,16 +162,19 @@ void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) */ att_sp.timestamp = get_time_micros(); - mavlink_quaternion_to_euler(set_attitude_target.q, &att_sp.roll_body, &att_sp.pitch_body, - &att_sp.yaw_body); - mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body.data()); - att_sp.R_valid = true; + if (!ignore_attitude) { + mavlink_quaternion_to_euler(set_attitude_target.q, &att_sp.roll_body, &att_sp.pitch_body, + &att_sp.yaw_body); + mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body.data()); + att_sp.R_valid = true; + } + if (!offboard_control_mode.ignore_thrust) { att_sp.thrust = set_attitude_target.thrust; } - if (!offboard_control_mode.ignore_attitude) { + if (!ignore_attitude) { for (ssize_t i = 0; i < 4; i++) { att_sp.q_d[i] = set_attitude_target.q[i]; } From 050473b3b52993dd01b44e1b5580f0ffd74f3b9e Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sun, 15 Mar 2015 12:25:52 +0100 Subject: [PATCH 031/268] added member vars for att_sp and offboard_control_mode --- src/platforms/ros/nodes/mavlink/mavlink.cpp | 49 ++++++++++----------- src/platforms/ros/nodes/mavlink/mavlink.h | 2 + 2 files changed, 25 insertions(+), 26 deletions(-) diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index c8188f9375..2758979a29 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -57,7 +57,8 @@ Mavlink::Mavlink() : { _link = mavconn::MAVConnInterface::open_url("udp://localhost:14565@localhost:14560"); _link->message_received.connect(boost::bind(&Mavlink::handle_msg, this, _1, _2, _3)); - + _att_sp = {}; + _offboard_control_mode = {}; } int main(int argc, char **argv) @@ -127,10 +128,8 @@ void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) mavlink_set_attitude_target_t set_attitude_target; mavlink_msg_set_attitude_target_decode(mmsg, &set_attitude_target); - static offboard_control_mode offboard_control_mode; - /* set correct ignore flags for thrust field: copy from mavlink message */ - offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6)); + _offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6)); /* * if attitude or body rate have been used (not ignored) previously and this message only sends @@ -140,48 +139,46 @@ void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) bool ignore_bodyrate = (bool)(set_attitude_target.type_mask & 0x7); bool ignore_attitude = (bool)(set_attitude_target.type_mask & (1 << 7)); - if (ignore_bodyrate && ignore_attitude && !offboard_control_mode.ignore_thrust) { + if (ignore_bodyrate && ignore_attitude && !_offboard_control_mode.ignore_thrust) { /* Message want's us to ignore everything except thrust: only ignore if previously ignored */ - offboard_control_mode.ignore_bodyrate = ignore_bodyrate && offboard_control_mode.ignore_bodyrate; - offboard_control_mode.ignore_attitude = ignore_attitude && offboard_control_mode.ignore_attitude; + _offboard_control_mode.ignore_bodyrate = ignore_bodyrate && _offboard_control_mode.ignore_bodyrate; + _offboard_control_mode.ignore_attitude = ignore_attitude && _offboard_control_mode.ignore_attitude; } else { - offboard_control_mode.ignore_bodyrate = ignore_bodyrate; - offboard_control_mode.ignore_attitude = ignore_attitude; + _offboard_control_mode.ignore_bodyrate = ignore_bodyrate; + _offboard_control_mode.ignore_attitude = ignore_attitude; } - offboard_control_mode.ignore_position = true; - offboard_control_mode.ignore_velocity = true; - offboard_control_mode.ignore_acceleration_force = true; + _offboard_control_mode.ignore_position = true; + _offboard_control_mode.ignore_velocity = true; + _offboard_control_mode.ignore_acceleration_force = true; - offboard_control_mode.timestamp = get_time_micros(); - _offboard_control_mode_pub.publish(offboard_control_mode); - - static vehicle_attitude_setpoint att_sp = {}; + _offboard_control_mode.timestamp = get_time_micros(); + _offboard_control_mode_pub.publish(_offboard_control_mode); /* The real mavlink app has a ckeck at this location which makes sure that the attitude setpoint * gets published only if in offboard mode. We leave that out for now. */ - att_sp.timestamp = get_time_micros(); + _att_sp.timestamp = get_time_micros(); if (!ignore_attitude) { - mavlink_quaternion_to_euler(set_attitude_target.q, &att_sp.roll_body, &att_sp.pitch_body, - &att_sp.yaw_body); - mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body.data()); - att_sp.R_valid = true; + mavlink_quaternion_to_euler(set_attitude_target.q, &_att_sp.roll_body, &_att_sp.pitch_body, + &_att_sp.yaw_body); + mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])_att_sp.R_body.data()); + _att_sp.R_valid = true; } - if (!offboard_control_mode.ignore_thrust) { - att_sp.thrust = set_attitude_target.thrust; + if (!_offboard_control_mode.ignore_thrust) { + _att_sp.thrust = set_attitude_target.thrust; } if (!ignore_attitude) { for (ssize_t i = 0; i < 4; i++) { - att_sp.q_d[i] = set_attitude_target.q[i]; + _att_sp.q_d[i] = set_attitude_target.q[i]; } - att_sp.q_d_valid = true; + _att_sp.q_d_valid = true; } - _v_att_sp_pub.publish(att_sp); + _v_att_sp_pub.publish(_att_sp); //XXX real mavlink publishes rate sp here diff --git a/src/platforms/ros/nodes/mavlink/mavlink.h b/src/platforms/ros/nodes/mavlink/mavlink.h index acb2408f30..8b7d08d242 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.h +++ b/src/platforms/ros/nodes/mavlink/mavlink.h @@ -69,6 +69,8 @@ protected: ros::Publisher _pos_sp_triplet_pub; ros::Publisher _offboard_control_mode_pub; ros::Publisher _force_sp_pub; + vehicle_attitude_setpoint _att_sp; + offboard_control_mode _offboard_control_mode; /** * From 3e02ab3cd72bcca6c2fe1160fa044ab687f86191 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 13 Mar 2015 20:19:46 +0100 Subject: [PATCH 032/268] fixed some lint errors and removed arming --- .../demo_tests/mavros_offboard_attctl_test.py | 49 +++++++------------ 1 file changed, 17 insertions(+), 32 deletions(-) diff --git a/integrationtests/demo_tests/mavros_offboard_attctl_test.py b/integrationtests/demo_tests/mavros_offboard_attctl_test.py index 92cdf1e0ad..4c850315b3 100755 --- a/integrationtests/demo_tests/mavros_offboard_attctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_attctl_test.py @@ -68,45 +68,27 @@ class MavrosOffboardAttctlTest(unittest.TestCase): rospy.wait_for_service('iris/mavros/cmd/arming', 30) rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) rospy.Subscriber("iris/mavros/position/local", PoseStamped, self.position_callback) - self.pubAtt = rospy.Publisher('iris/mavros/setpoint/attitude', PoseStamped, queue_size=10) - self.pubThr = rospy.Publisher('iris/mavros/setpoint/att_throttle', Float64, queue_size=10) - self.cmdArm = rospy.ServiceProxy("iris/mavros/cmd/arming", CommandBool) + self.pub_att = rospy.Publisher('iris/mavros/setpoint/attitude', PoseStamped, queue_size=10) + self.pub_thr = rospy.Publisher('iris/mavros/setpoint/att_throttle', Float64, queue_size=10) self.rate = rospy.Rate(10) # 10hz - self.rateSec = rospy.Rate(1) - self.hasPos = False - self.controlMode = vehicle_control_mode() + self.has_pos = False + self.control_mode = vehicle_control_mode() + self.local_position = PoseStamped() # # General callback functions used in tests # def position_callback(self, data): - self.hasPos = True - self.localPosition = data + self.has_pos = True + self.local_position = data def vehicle_control_mode_callback(self, data): - self.controlMode = data - - - # - # Helper methods - # - def arm(self): - return self.cmdArm(value=True) + self.control_mode = data # # Test offboard position control # def test_attctl(self): - # FIXME: this must go ASAP when arming is implemented - manIn = ManualInput() - manIn.arm() - manIn.offboard_attctl() - - self.assertTrue(self.arm(), "Could not arm") - self.rateSec.sleep() - self.rateSec.sleep() - self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set after 2 seconds") - # set some attitude and thrust att = PoseStamped() att.header = Header() @@ -121,19 +103,22 @@ class MavrosOffboardAttctlTest(unittest.TestCase): # does it cross expected boundaries in X seconds? count = 0 timeout = 120 - while(count < timeout): + while count < timeout: # update timestamp for each published SP att.header.stamp = rospy.Time.now() - self.pubAtt.publish(att) - self.pubThr.publish(throttle) + self.pub_att.publish(att) + self.pub_thr.publish(throttle) - if (self.localPosition.pose.position.x > 5 - and self.localPosition.pose.position.z > 5 - and self.localPosition.pose.position.y < -5): + if (self.local_position.pose.position.x > 5 + and self.local_position.pose.position.z > 5 + and self.local_position.pose.position.y < -5): break count = count + 1 self.rate.sleep() + self.assertTrue(self.control_mode.flag_armed, "flag_armed is not set") + self.assertTrue(self.control_mode.flag_control_attitude_enabled, "flag_control_attitude_enabled is not set") + self.assertTrue(self.control_mode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") self.assertTrue(count < timeout, "took too long to cross boundaries") From 8ada8dc648e730c6dd4efefada1f77e8dd732e68 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 13 Mar 2015 20:36:45 +0100 Subject: [PATCH 033/268] trying to fix timing issue --- integrationtests/demo_tests/mavros_offboard_attctl_test.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/integrationtests/demo_tests/mavros_offboard_attctl_test.py b/integrationtests/demo_tests/mavros_offboard_attctl_test.py index 4c850315b3..73cfe5a819 100755 --- a/integrationtests/demo_tests/mavros_offboard_attctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_attctl_test.py @@ -70,7 +70,7 @@ class MavrosOffboardAttctlTest(unittest.TestCase): rospy.Subscriber("iris/mavros/position/local", PoseStamped, self.position_callback) self.pub_att = rospy.Publisher('iris/mavros/setpoint/attitude', PoseStamped, queue_size=10) self.pub_thr = rospy.Publisher('iris/mavros/setpoint/att_throttle', Float64, queue_size=10) - self.rate = rospy.Rate(10) # 10hz + self.rate = rospy.Rate(20) # 10hz self.has_pos = False self.control_mode = vehicle_control_mode() self.local_position = PoseStamped() @@ -107,14 +107,15 @@ class MavrosOffboardAttctlTest(unittest.TestCase): # update timestamp for each published SP att.header.stamp = rospy.Time.now() self.pub_att.publish(att) + self.rate.sleep() self.pub_thr.publish(throttle) + self.rate.sleep() if (self.local_position.pose.position.x > 5 and self.local_position.pose.position.z > 5 and self.local_position.pose.position.y < -5): break count = count + 1 - self.rate.sleep() self.assertTrue(self.control_mode.flag_armed, "flag_armed is not set") self.assertTrue(self.control_mode.flag_control_attitude_enabled, "flag_control_attitude_enabled is not set") From 2bbad206961ec4bf1e78ac8b6689634fa3e482b8 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sun, 15 Mar 2015 09:51:38 +0100 Subject: [PATCH 034/268] fixed some warnings and updated comments --- .../demo_tests/mavros_offboard_attctl_test.py | 20 ++++++------------- 1 file changed, 6 insertions(+), 14 deletions(-) diff --git a/integrationtests/demo_tests/mavros_offboard_attctl_test.py b/integrationtests/demo_tests/mavros_offboard_attctl_test.py index 73cfe5a819..ecad9df4dd 100755 --- a/integrationtests/demo_tests/mavros_offboard_attctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_attctl_test.py @@ -37,29 +37,20 @@ # PKG = 'px4' -import sys import unittest import rospy -import math - -from numpy import linalg -import numpy as np from px4.msg import vehicle_control_mode from std_msgs.msg import Header from std_msgs.msg import Float64 from geometry_msgs.msg import PoseStamped, Quaternion from tf.transformations import quaternion_from_euler -from mavros.srv import CommandBool - -from manual_input import ManualInput # -# Tests flying a path in offboard control by sending position setpoints +# Tests flying a path in offboard control by sending attitude and thrust setpoints # over MAVROS. # -# For the test to be successful it needs to reach all setpoints in a certain time. -# FIXME: add flight path assertion (needs transformation from ROS frame to NED) +# For the test to be successful it needs to cross a certain boundary in time. # class MavrosOffboardAttctlTest(unittest.TestCase): @@ -106,14 +97,15 @@ class MavrosOffboardAttctlTest(unittest.TestCase): while count < timeout: # update timestamp for each published SP att.header.stamp = rospy.Time.now() + self.pub_att.publish(att) - self.rate.sleep() + self.rate.sleep() # I'm guessing this is necessary to prevent timing issues self.pub_thr.publish(throttle) self.rate.sleep() if (self.local_position.pose.position.x > 5 - and self.local_position.pose.position.z > 5 - and self.local_position.pose.position.y < -5): + and self.local_position.pose.position.z > 5 + and self.local_position.pose.position.y < -5): break count = count + 1 From 7018ff298ea3174d6764a1e35196521ee31f4dd3 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sun, 15 Mar 2015 12:07:35 +0100 Subject: [PATCH 035/268] removing manual input and unused code from posctl test --- .../demo_tests/mavros_offboard_posctl_test.py | 71 ++++++++----------- 1 file changed, 29 insertions(+), 42 deletions(-) diff --git a/integrationtests/demo_tests/mavros_offboard_posctl_test.py b/integrationtests/demo_tests/mavros_offboard_posctl_test.py index 6d26015e7b..568c2cbd80 100755 --- a/integrationtests/demo_tests/mavros_offboard_posctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_posctl_test.py @@ -37,7 +37,6 @@ # PKG = 'px4' -import sys import unittest import rospy import math @@ -49,9 +48,6 @@ from px4.msg import vehicle_control_mode from std_msgs.msg import Header from geometry_msgs.msg import PoseStamped, Quaternion from tf.transformations import quaternion_from_euler -from mavros.srv import CommandBool - -from manual_input import ManualInput # # Tests flying a path in offboard control by sending position setpoints @@ -64,37 +60,41 @@ class MavrosOffboardPosctlTest(unittest.TestCase): def setUp(self): rospy.init_node('test_node', anonymous=True) - rospy.wait_for_service('iris/mavros/cmd/arming', 30) rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) rospy.Subscriber("iris/mavros/position/local", PoseStamped, self.position_callback) - self.pubSpt = rospy.Publisher('iris/mavros/setpoint/local_position', PoseStamped, queue_size=10) - self.cmdArm = rospy.ServiceProxy("iris/mavros/cmd/arming", CommandBool) + self.pub_spt = rospy.Publisher('iris/mavros/setpoint/local_position', PoseStamped, queue_size=10) self.rate = rospy.Rate(10) # 10hz - self.rateSec = rospy.Rate(1) - self.hasPos = False - self.controlMode = vehicle_control_mode() + self.has_pos = False + self.local_position = PoseStamped() + self.control_mode = vehicle_control_mode() # # General callback functions used in tests # def position_callback(self, data): - self.hasPos = True - self.localPosition = data + self.has_pos = True + self.local_position = data def vehicle_control_mode_callback(self, data): - self.controlMode = data + self.control_mode = data # # Helper methods # def is_at_position(self, x, y, z, offset): - if(not self.hasPos): + if not self.has_pos: return False - rospy.logdebug("current position %f, %f, %f" % (self.localPosition.pose.position.x, self.localPosition.pose.position.y, self.localPosition.pose.position.z)) + rospy.logdebug("current position %f, %f, %f" % + (self.local_position.pose.position.x, + self.local_position.pose.position.y, + self.local_position.pose.position.z)) + desired = np.array((x, y, z)) - pos = np.array((self.localPosition.pose.position.x, self.localPosition.pose.position.y, self.localPosition.pose.position.z)) + pos = np.array((self.local_position.pose.position.x, + self.local_position.pose.position.y, + self.local_position.pose.position.z)) return linalg.norm(desired - pos) < offset def reach_position(self, x, y, z, timeout): @@ -114,57 +114,44 @@ class MavrosOffboardPosctlTest(unittest.TestCase): # does it reach the position in X seconds? count = 0 - while(count < timeout): + while count < timeout: # update timestamp for each published SP pos.header.stamp = rospy.Time.now() - self.pubSpt.publish(pos) + self.pub_spt.publish(pos) - if(self.is_at_position(pos.pose.position.x, pos.pose.position.y, pos.pose.position.z, 0.5)): + if self.is_at_position(pos.pose.position.x, pos.pose.position.y, pos.pose.position.z, 0.5): break count = count + 1 self.rate.sleep() self.assertTrue(count < timeout, "took too long to get to position") - def arm(self): - return self.cmdArm(value=True) - # # Test offboard position control # def test_posctl(self): - # FIXME: this must go ASAP when arming is implemented - manIn = ManualInput() - manIn.arm() - manIn.offboard_posctl() - - self.assertTrue(self.arm(), "Could not arm") - self.rateSec.sleep() - self.rateSec.sleep() - self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set after 2 seconds") - # prepare flight path positions = ( - (0,0,0), - (2,2,2), - (2,-2,2), - (-2,-2,2), - (2,2,2)) + (0, 0, 0), + (2, 2, 2), + (2, -2, 2), + (-2, -2, 2), + (2, 2, 2)) for i in range(0, len(positions)): self.reach_position(positions[i][0], positions[i][1], positions[i][2], 120) - # does it hold the position for Y seconds? - positionHeld = True count = 0 timeout = 50 - while(count < timeout): - if(not self.is_at_position(2, 2, 2, 0.5)): - positionHeld = False + while count < timeout: + if not self.is_at_position(2, 2, 2, 0.5): break count = count + 1 self.rate.sleep() + self.assertTrue(self.control_mode.flag_armed, "flag_armed is not set") + self.assertTrue(self.control_mode.flag_control_position_enabled, "flag_control_position_enabled is not set") + self.assertTrue(self.control_mode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") self.assertTrue(count == timeout, "position could not be held") From 4b65e6259595f1f6dfb70fe8b44e229b49896f28 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sun, 15 Mar 2015 12:08:36 +0100 Subject: [PATCH 036/268] removing sleep between publishments again, issue should be fixed in mavlink node --- integrationtests/demo_tests/mavros_offboard_attctl_test.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/integrationtests/demo_tests/mavros_offboard_attctl_test.py b/integrationtests/demo_tests/mavros_offboard_attctl_test.py index ecad9df4dd..30e9fe9baf 100755 --- a/integrationtests/demo_tests/mavros_offboard_attctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_attctl_test.py @@ -61,7 +61,7 @@ class MavrosOffboardAttctlTest(unittest.TestCase): rospy.Subscriber("iris/mavros/position/local", PoseStamped, self.position_callback) self.pub_att = rospy.Publisher('iris/mavros/setpoint/attitude', PoseStamped, queue_size=10) self.pub_thr = rospy.Publisher('iris/mavros/setpoint/att_throttle', Float64, queue_size=10) - self.rate = rospy.Rate(20) # 10hz + self.rate = rospy.Rate(10) # 10hz self.has_pos = False self.control_mode = vehicle_control_mode() self.local_position = PoseStamped() @@ -99,7 +99,6 @@ class MavrosOffboardAttctlTest(unittest.TestCase): att.header.stamp = rospy.Time.now() self.pub_att.publish(att) - self.rate.sleep() # I'm guessing this is necessary to prevent timing issues self.pub_thr.publish(throttle) self.rate.sleep() From 49b3906b7827a7f2b5b4e947d080db7496d044da Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 15 Mar 2015 14:33:22 +0100 Subject: [PATCH 037/268] commander: Fix status checks for leds and adjust stack size based on actual use --- src/modules/commander/commander.cpp | 27 +++++++++--------- src/modules/commander/commander_helper.cpp | 32 ++++++++++++++++------ 2 files changed, 37 insertions(+), 22 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index aa7a17a22c..763c617bf9 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -967,19 +967,6 @@ int commander_thread_main(int argc, char *argv[]) int ret; - pthread_attr_t commander_low_prio_attr; - pthread_attr_init(&commander_low_prio_attr); - pthread_attr_setstacksize(&commander_low_prio_attr, 2000); - - struct sched_param param; - (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); - - /* low priority */ - param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; - (void)pthread_attr_setschedparam(&commander_low_prio_attr, ¶m); - pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL); - pthread_attr_destroy(&commander_low_prio_attr); - /* Start monitoring loop */ unsigned counter = 0; unsigned stick_off_counter = 0; @@ -1144,6 +1131,20 @@ int commander_thread_main(int argc, char *argv[]) bool main_state_changed = false; bool failsafe_old = false; + /* initialize low priority thread */ + pthread_attr_t commander_low_prio_attr; + pthread_attr_init(&commander_low_prio_attr); + pthread_attr_setstacksize(&commander_low_prio_attr, 1600); + + struct sched_param param; + (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); + + /* low priority */ + param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; + (void)pthread_attr_setschedparam(&commander_low_prio_attr, ¶m); + pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL); + pthread_attr_destroy(&commander_low_prio_attr); + while (!thread_should_exit) { if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) { diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index b5ec6c4e6f..a2e827a15e 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -231,7 +231,7 @@ int led_init() /* then try RGB LEDs, this can fail on FMUv1*/ rgbleds = open(RGBLED0_DEVICE_PATH, 0); - if (rgbleds == -1) { + if (rgbleds < 0) { warnx("No RGB LED found at " RGBLED0_DEVICE_PATH); } @@ -240,50 +240,64 @@ int led_init() void led_deinit() { - close(leds); + if (leds >= 0) { + close(leds); + } - if (rgbleds != -1) { + if (rgbleds >= 0) { close(rgbleds); } } int led_toggle(int led) { + if (leds < 0) { + return leds; + } return ioctl(leds, LED_TOGGLE, led); } int led_on(int led) { + if (leds < 0) { + return leds; + } return ioctl(leds, LED_ON, led); } int led_off(int led) { + if (leds < 0) { + return leds; + } return ioctl(leds, LED_OFF, led); } void rgbled_set_color(rgbled_color_t color) { - if (rgbleds != -1) { - ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color); + if (rgbleds < 0) { + return; } + ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color); } void rgbled_set_mode(rgbled_mode_t mode) { - if (rgbleds != -1) { - ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode); + if (rgbleds < 0) { + return; } + ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode); } void rgbled_set_pattern(rgbled_pattern_t *pattern) { - if (rgbleds != -1) { - ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); + if (rgbleds < 0) { + return; } + ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); } float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized) From 2bddaa9573f0160ad944e1610c87ad48182511d6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 15 Mar 2015 14:33:57 +0100 Subject: [PATCH 038/268] commander: Mag calibration: Use c++ syntax for array initialization --- src/modules/commander/mag_calibration.cpp | 36 ++++++++++------------- 1 file changed, 16 insertions(+), 20 deletions(-) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index e0786db79d..a0aadab39b 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -164,9 +164,9 @@ int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id) const unsigned int calibration_maxcount = 240; unsigned int calibration_counter; - float *x = NULL; - float *y = NULL; - float *z = NULL; + float *x = new float[calibration_maxcount]; + float *y = new float[calibration_maxcount]; + float *z = new float[calibration_maxcount]; char str[30]; int res = OK; @@ -174,24 +174,20 @@ int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id) /* allocate memory */ mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20); - x = reinterpret_cast(malloc(sizeof(float) * calibration_maxcount)); - y = reinterpret_cast(malloc(sizeof(float) * calibration_maxcount)); - z = reinterpret_cast(malloc(sizeof(float) * calibration_maxcount)); - - if (x == NULL || y == NULL || z == NULL) { + if (x == nullptr || y == nullptr || z == nullptr) { mavlink_and_console_log_critical(mavlink_fd, "ERROR: out of memory"); /* clean up */ - if (x != NULL) { - free(x); + if (x != nullptr) { + delete x; } - if (y != NULL) { - free(y); + if (y != nullptr) { + delete y; } - if (z != NULL) { - free(z); + if (z != nullptr) { + delete z; } res = ERROR; @@ -274,16 +270,16 @@ int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id) } } - if (x != NULL) { - free(x); + if (x != nullptr) { + delete x; } - if (y != NULL) { - free(y); + if (y != nullptr) { + delete y; } - if (z != NULL) { - free(z); + if (z != nullptr) { + delete z; } if (res == OK) { From 6f44b4d3543ff702dd9cf465212b20d7c3ae8d2e Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sun, 15 Mar 2015 15:44:01 +0100 Subject: [PATCH 039/268] remove gazebo plugin command fake, fix some lint warnings --- integrationtests/demo_tests/manual_input.py | 24 +++++++-------------- 1 file changed, 8 insertions(+), 16 deletions(-) diff --git a/integrationtests/demo_tests/manual_input.py b/integrationtests/demo_tests/manual_input.py index 7072464dec..167221285a 100755 --- a/integrationtests/demo_tests/manual_input.py +++ b/integrationtests/demo_tests/manual_input.py @@ -35,7 +35,6 @@ # # @author Andreas Antener # -import sys import rospy from px4.msg import manual_control_setpoint @@ -46,17 +45,12 @@ from std_msgs.msg import Header # # Manual input control helper # -# FIXME: this is not the way to do it! ATM it fakes input to iris/command/attitude because else -# the simulator does not instantiate our controller. Probably this whole class will disappear once -# arming works correctly. -# -class ManualInput: +class ManualInput(object): def __init__(self): rospy.init_node('test_node', anonymous=True) - self.pubMcsp = rospy.Publisher('iris/manual_control_setpoint', manual_control_setpoint, queue_size=10) - self.pubOff = rospy.Publisher('iris/offboard_control_mode', offboard_control_mode, queue_size=10) - self.pubAtt = rospy.Publisher('iris/command/attitude', CommandAttitudeThrust, queue_size=10) + self.pub_mcsp = rospy.Publisher('iris/manual_control_setpoint', manual_control_setpoint, queue_size=10) + self.pub_off = rospy.Publisher('iris/offboard_control_mode', offboard_control_mode, queue_size=10) def arm(self): rate = rospy.Rate(10) # 10hz @@ -81,9 +75,7 @@ class ManualInput: rospy.loginfo("zeroing") time = rospy.get_rostime().now() pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 - self.pubMcsp.publish(pos) - # Fake input to iris commander - self.pubAtt.publish(att) + self.pub_mcsp.publish(pos) rate.sleep() count = count + 1 @@ -93,7 +85,7 @@ class ManualInput: rospy.loginfo("arming") time = rospy.get_rostime().now() pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 - self.pubMcsp.publish(pos) + self.pub_mcsp.publish(pos) rate.sleep() count = count + 1 @@ -118,7 +110,7 @@ class ManualInput: rospy.loginfo("triggering posctl") time = rospy.get_rostime().now() pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 - self.pubMcsp.publish(pos) + self.pub_mcsp.publish(pos) rate.sleep() count = count + 1 @@ -147,7 +139,7 @@ class ManualInput: rospy.loginfo("setting offboard mode") time = rospy.get_rostime().now() mode.timestamp = time.secs * 1e6 + time.nsecs / 1000 - self.pubOff.publish(mode) + self.pub_off.publish(mode) rate.sleep() count = count + 1 @@ -169,7 +161,7 @@ class ManualInput: rospy.loginfo("triggering offboard") time = rospy.get_rostime().now() pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 - self.pubMcsp.publish(pos) + self.pub_mcsp.publish(pos) rate.sleep() count = count + 1 From c4a81f66cbe6714f28b3a9853d428bde827d4a03 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sun, 15 Mar 2015 15:50:25 +0100 Subject: [PATCH 040/268] set ground truth to true again, needs latest master from euroc_simulator --- integrationtests/demo_tests/direct_tests.launch | 2 +- integrationtests/demo_tests/mavros_tests.launch | 2 +- launch/gazebo_ardrone_empty_world.launch | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/integrationtests/demo_tests/direct_tests.launch b/integrationtests/demo_tests/direct_tests.launch index 1a7d843fdc..f332332cfb 100644 --- a/integrationtests/demo_tests/direct_tests.launch +++ b/integrationtests/demo_tests/direct_tests.launch @@ -2,7 +2,7 @@ - + diff --git a/integrationtests/demo_tests/mavros_tests.launch b/integrationtests/demo_tests/mavros_tests.launch index e42db60434..cc49183076 100644 --- a/integrationtests/demo_tests/mavros_tests.launch +++ b/integrationtests/demo_tests/mavros_tests.launch @@ -2,7 +2,7 @@ - + diff --git a/launch/gazebo_ardrone_empty_world.launch b/launch/gazebo_ardrone_empty_world.launch index 122a27867d..7243e7476e 100644 --- a/launch/gazebo_ardrone_empty_world.launch +++ b/launch/gazebo_ardrone_empty_world.launch @@ -3,7 +3,7 @@ - + From 15a5c0a9bebf113ddcb6f1b95a8672fa882dcdcc Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sun, 15 Mar 2015 16:12:17 +0100 Subject: [PATCH 041/268] cleaned up direct tests, fixed various lint warnings and removed unused code --- .../demo_tests/direct_manual_input_test.py | 23 ++-- .../demo_tests/direct_offboard_posctl_test.py | 55 +++++----- .../demo_tests/flight_path_assertion.py | 100 +++++++++++------- 3 files changed, 99 insertions(+), 79 deletions(-) diff --git a/integrationtests/demo_tests/direct_manual_input_test.py b/integrationtests/demo_tests/direct_manual_input_test.py index b037d842f8..394500e941 100755 --- a/integrationtests/demo_tests/direct_manual_input_test.py +++ b/integrationtests/demo_tests/direct_manual_input_test.py @@ -37,7 +37,6 @@ # PKG = 'px4' -import sys import unittest import rospy @@ -50,14 +49,18 @@ from manual_input import ManualInput # class ManualInputTest(unittest.TestCase): + def setUp(self): + self.actuator_status = actuator_armed() + self.control_mode = vehicle_control_mode() + # # General callback functions used in tests # def actuator_armed_callback(self, data): - self.actuatorStatus = data + self.actuator_status = data def vehicle_control_mode_callback(self, data): - self.controlMode = data + self.control_mode = data # # Test arming @@ -67,19 +70,19 @@ class ManualInputTest(unittest.TestCase): rospy.Subscriber('iris/actuator_armed', actuator_armed, self.actuator_armed_callback) rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) - man = ManualInput() + man_in = ManualInput() # Test arming - man.arm() - self.assertEquals(self.actuatorStatus.armed, True, "did not arm") + man_in.arm() + self.assertEquals(self.actuator_status.armed, True, "did not arm") # Test posctl - man.posctl() - self.assertTrue(self.controlMode.flag_control_position_enabled, "flag_control_position_enabled is not set") + man_in.posctl() + self.assertTrue(self.control_mode.flag_control_position_enabled, "flag_control_position_enabled is not set") # Test offboard - man.offboard() - self.assertTrue(self.controlMode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") + man_in.offboard() + self.assertTrue(self.control_mode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") if __name__ == '__main__': diff --git a/integrationtests/demo_tests/direct_offboard_posctl_test.py b/integrationtests/demo_tests/direct_offboard_posctl_test.py index 18239e9264..225760d6c0 100755 --- a/integrationtests/demo_tests/direct_offboard_posctl_test.py +++ b/integrationtests/demo_tests/direct_offboard_posctl_test.py @@ -37,7 +37,6 @@ # PKG = 'px4' -import sys import unittest import rospy @@ -46,11 +45,8 @@ import numpy as np from px4.msg import vehicle_local_position from px4.msg import vehicle_control_mode -from px4.msg import actuator_armed from px4.msg import position_setpoint_triplet from px4.msg import position_setpoint -from sensor_msgs.msg import Joy -from std_msgs.msg import Header from manual_input import ManualInput from flight_path_assertion import FlightPathAssertion @@ -68,31 +64,34 @@ class DirectOffboardPosctlTest(unittest.TestCase): rospy.init_node('test_node', anonymous=True) rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) rospy.Subscriber("iris/vehicle_local_position", vehicle_local_position, self.position_callback) - self.pubSpt = rospy.Publisher('iris/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) + self.pub_spt = rospy.Publisher('iris/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) self.rate = rospy.Rate(10) # 10hz + self.has_pos = False + self.local_position = vehicle_local_position() + self.control_mode = vehicle_control_mode() def tearDown(self): - if (self.fpa): + if self.fpa: self.fpa.stop() # # General callback functions used in tests # def position_callback(self, data): - self.hasPos = True - self.localPosition = data + self.has_pos = True + self.local_position = data def vehicle_control_mode_callback(self, data): - self.controlMode = data + self.control_mode = data # # Helper methods # def is_at_position(self, x, y, z, offset): - rospy.logdebug("current position %f, %f, %f" % (self.localPosition.x, self.localPosition.y, self.localPosition.z)) + rospy.logdebug("current position %f, %f, %f" % (self.local_position.x, self.local_position.y, self.local_position.z)) desired = np.array((x, y, z)) - pos = np.array((self.localPosition.x, self.localPosition.y, self.localPosition.z)) + pos = np.array((self.local_position.x, self.local_position.y, self.local_position.z)) return linalg.norm(desired - pos) < offset def reach_position(self, x, y, z, timeout): @@ -105,12 +104,12 @@ class DirectOffboardPosctlTest(unittest.TestCase): pos.position_valid = True stp = position_setpoint_triplet() stp.current = pos - self.pubSpt.publish(stp) + self.pub_spt.publish(stp) # does it reach the position in X seconds? count = 0 - while(count < timeout): - if(self.is_at_position(pos.x, pos.y, pos.z, 0.5)): + while count < timeout: + if self.is_at_position(pos.x, pos.y, pos.z, 0.5): break count = count + 1 self.rate.sleep() @@ -121,22 +120,22 @@ class DirectOffboardPosctlTest(unittest.TestCase): # Test offboard position control # def test_posctl(self): - manIn = ManualInput() + man_in = ManualInput() # arm and go into offboard - manIn.arm() - manIn.offboard() - self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set") - self.assertTrue(self.controlMode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") - self.assertTrue(self.controlMode.flag_control_position_enabled, "flag_control_position_enabled is not set") + man_in.arm() + man_in.offboard() + self.assertTrue(self.control_mode.flag_armed, "flag_armed is not set") + self.assertTrue(self.control_mode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") + self.assertTrue(self.control_mode.flag_control_position_enabled, "flag_control_position_enabled is not set") # prepare flight path positions = ( - (0,0,0), - (2,2,-2), - (2,-2,-2), - (-2,-2,-2), - (2,2,-2)) + (0, 0, 0), + (2, 2, -2), + (2, -2, -2), + (-2, -2, -2), + (2, 2, -2)) # flight path assertion self.fpa = FlightPathAssertion(positions, 1, 0) @@ -147,12 +146,10 @@ class DirectOffboardPosctlTest(unittest.TestCase): self.assertFalse(self.fpa.failed, "breached flight path tunnel (%d)" % i) # does it hold the position for Y seconds? - positionHeld = True count = 0 timeout = 50 - while(count < timeout): - if(not self.is_at_position(2, 2, -2, 0.5)): - positionHeld = False + while count < timeout: + if not self.is_at_position(2, 2, -2, 0.5): break count = count + 1 self.rate.sleep() diff --git a/integrationtests/demo_tests/flight_path_assertion.py b/integrationtests/demo_tests/flight_path_assertion.py index 485de8c416..ccadd1ab9b 100644 --- a/integrationtests/demo_tests/flight_path_assertion.py +++ b/integrationtests/demo_tests/flight_path_assertion.py @@ -35,7 +35,6 @@ # # @author Andreas Antener # -import sys import rospy import threading @@ -48,7 +47,6 @@ from geometry_msgs.msg import Twist from numpy import linalg import numpy as np -import math # # Helper to test if vehicle stays on expected flight path. @@ -62,30 +60,52 @@ class FlightPathAssertion(threading.Thread): # TODO: yaw validation # TODO: fail main test thread # - def __init__(self, positions, tunnelRadius = 1, yawOffset = 0.2): + def __init__(self, positions, tunnelRadius=1, yaw_offset=0.2): threading.Thread.__init__(self) rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback) - self.spawnModel = rospy.ServiceProxy('gazebo/spawn_sdf_model', SpawnModel) - self.setModelState = rospy.ServiceProxy('gazebo/set_model_state', SetModelState) - self.deleteModel = rospy.ServiceProxy('gazebo/delete_model', DeleteModel) + self.spawn_model = rospy.ServiceProxy('gazebo/spawn_sdf_model', SpawnModel) + self.set_model_state = rospy.ServiceProxy('gazebo/set_model_state', SetModelState) + self.delete_model = rospy.ServiceProxy('gazebo/delete_model', DeleteModel) self.positions = positions - self.tunnelRadius = tunnelRadius - self.yawOffset = yawOffset - self.hasPos = False - self.shouldStop = False + self.tunnel_radius = tunnelRadius + self.yaw_offset = yaw_offset + self.has_pos = False + self.should_stop = False self.center = positions[0] - self.endOfSegment = False + self.end_of_segment = False self.failed = False + self.local_position = vehicle_local_position def position_callback(self, data): - self.hasPos = True - self.localPosition = data + self.has_pos = True + self.local_position = data def spawn_indicator(self): - self.deleteModel("indicator") - xml = "true0.7%f1 0 0 0.51 0 0 0.5" % self.tunnelRadius - self.spawnModel("indicator", xml, "", Pose(), "") + self.delete_model("indicator") + xml = ( + "" + + "" + + "" + + "true" + + "" + + "" + + "0.7" + + "" + + "" + + "%f" + + "" + + "" + + "" + + "1 0 0 0.5" + + "1 0 0 0.5" + + "" + + "" + + "" + + "" + + "" % self.tunnel_radius) + + self.spawn_model("indicator", xml, "", Pose(), "") def position_indicator(self): state = SetModelState() @@ -97,7 +117,7 @@ class FlightPathAssertion(threading.Thread): state.pose = pose state.twist = Twist() state.reference_frame = "" - self.setModelState(state) + self.set_model_state(state) def distance_to_line(self, a, b, pos): v = b - a @@ -111,7 +131,7 @@ class FlightPathAssertion(threading.Thread): c2 = np.dot(v, v) if c2 <= c1: # after b self.center = b - self.endOfSegment = True + self.end_of_segment = True return linalg.norm(pos - b) x = c1 / c2 @@ -120,7 +140,7 @@ class FlightPathAssertion(threading.Thread): return linalg.norm(pos - l) def stop(self): - self.shouldStop = True + self.should_stop = True def run(self): rate = rospy.Rate(10) # 10hz @@ -128,43 +148,43 @@ class FlightPathAssertion(threading.Thread): current = 0 - while not self.shouldStop: - if (self.hasPos): + while not self.should_stop: + if self.has_pos: # calculate distance to line segment between first two points - # if distances > tunnelRadius + # if distances > tunnel_radius # exit with error - # advance current pos if not on the line anymore or distance to next point < tunnelRadius + # advance current pos if not on the line anymore or distance to next point < tunnel_radius # exit if current pos is now the last position self.position_indicator() - pos = np.array((self.localPosition.x, - self.localPosition.y, - self.localPosition.z)) - aPos = np.array((self.positions[current][0], - self.positions[current][1], - self.positions[current][2])) - bPos = np.array((self.positions[current + 1][0], - self.positions[current + 1][1], - self.positions[current + 1][2])) + pos = np.array((self.local_position.x, + self.local_position.y, + self.local_position.z)) + a_pos = np.array((self.positions[current][0], + self.positions[current][1], + self.positions[current][2])) + b_pos = np.array((self.positions[current + 1][0], + self.positions[current + 1][1], + self.positions[current + 1][2])) - dist = self.distance_to_line(aPos, bPos, pos) - bDist = linalg.norm(pos - bPos) + dist = self.distance_to_line(a_pos, b_pos, pos) + b_dist = linalg.norm(pos - b_pos) - rospy.logdebug("distance to line: %f, distance to end: %f" % (dist, bDist)) + rospy.logdebug("distance to line: %f, distance to end: %f" % (dist, b_dist)) - if (dist > self.tunnelRadius): - msg = "left tunnel at position (%f, %f, %f)" % (self.localPosition.x, self.localPosition.y, self.localPosition.z) + if dist > self.tunnel_radius: + msg = "left tunnel at position (%f, %f, %f)" % (self.local_position.x, self.local_position.y, self.local_position.z) rospy.logerr(msg) self.failed = True break - if (self.endOfSegment or bDist < self.tunnelRadius): + if self.end_of_segment or b_dist < self.tunnel_radius: rospy.loginfo("next segment") - self.endOfSegment = False + self.end_of_segment = False current = current + 1 - if (current == len(self.positions) - 1): + if current == len(self.positions) - 1: rospy.loginfo("no more positions") break From 1d5beb1edf63fef52a8c68993bcfe80b51a1aba8 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sun, 15 Mar 2015 16:39:35 +0100 Subject: [PATCH 042/268] - updated flight path assertion position subscription and added check if it does not get a position after 1 sec - modified test so it works iwth new _Z_P parameter --- .../demo_tests/direct_offboard_posctl_test.py | 1 + integrationtests/demo_tests/flight_path_assertion.py | 12 +++++++++--- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/integrationtests/demo_tests/direct_offboard_posctl_test.py b/integrationtests/demo_tests/direct_offboard_posctl_test.py index 225760d6c0..d61eec0632 100755 --- a/integrationtests/demo_tests/direct_offboard_posctl_test.py +++ b/integrationtests/demo_tests/direct_offboard_posctl_test.py @@ -132,6 +132,7 @@ class DirectOffboardPosctlTest(unittest.TestCase): # prepare flight path positions = ( (0, 0, 0), + (0, 0, -2), (2, 2, -2), (2, -2, -2), (-2, -2, -2), diff --git a/integrationtests/demo_tests/flight_path_assertion.py b/integrationtests/demo_tests/flight_path_assertion.py index ccadd1ab9b..244324cd09 100644 --- a/integrationtests/demo_tests/flight_path_assertion.py +++ b/integrationtests/demo_tests/flight_path_assertion.py @@ -62,7 +62,7 @@ class FlightPathAssertion(threading.Thread): # def __init__(self, positions, tunnelRadius=1, yaw_offset=0.2): threading.Thread.__init__(self) - rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback) + rospy.Subscriber("iris/vehicle_local_position", vehicle_local_position, self.position_callback) self.spawn_model = rospy.ServiceProxy('gazebo/spawn_sdf_model', SpawnModel) self.set_model_state = rospy.ServiceProxy('gazebo/set_model_state', SetModelState) self.delete_model = rospy.ServiceProxy('gazebo/delete_model', DeleteModel) @@ -103,7 +103,7 @@ class FlightPathAssertion(threading.Thread): "" + "" + "" + - "" % self.tunnel_radius) + "") % self.tunnel_radius self.spawn_model("indicator", xml, "", Pose(), "") @@ -147,7 +147,7 @@ class FlightPathAssertion(threading.Thread): self.spawn_indicator() current = 0 - + count = 0 while not self.should_stop: if self.has_pos: # calculate distance to line segment between first two points @@ -189,3 +189,9 @@ class FlightPathAssertion(threading.Thread): break rate.sleep() + count = count + 1 + + if count > 10 and not self.has_pos: # no position after 1 sec + rospy.logerr("no position") + self.failed = True + break From 1e54dc4409df700b8b4c4a4480238db27b270dfc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 15 Mar 2015 17:23:48 +0100 Subject: [PATCH 043/268] commander: Accel calibration: Reduce memory footprint, be more responsive --- .../commander/accelerometer_calibration.cpp | 19 +++---------------- 1 file changed, 3 insertions(+), 16 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index d70e050006..526b135d8b 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -164,11 +164,6 @@ int do_accel_calibration(int mavlink_fd) mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); - mavlink_and_console_log_info(mavlink_fd, "You need to put the system on all six sides"); - sleep(3); - mavlink_and_console_log_info(mavlink_fd, "Follow the instructions on the screen"); - sleep(5); - struct accel_scale accel_scale = { 0.0f, 1.0f, @@ -352,7 +347,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_se (!data_collected[4]) ? "up " : ""); /* allow user enough time to read the message */ - sleep(3); + sleep(1); int orient = detect_orientation(mavlink_fd, subs); @@ -365,7 +360,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_se /* inform user about already handled side */ if (data_collected[orient]) { mavlink_and_console_log_info(mavlink_fd, "%s side done, rotate to a different side", orientation_strs[orient]); - sleep(3); + sleep(1); continue; } @@ -374,7 +369,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_se read_accelerometer_avg(subs, accel_ref, orient, samples_num); mavlink_and_console_log_info(mavlink_fd, "%s side done, rotate to a different side", orientation_strs[orient]); usleep(100000); - mavlink_and_console_log_info(mavlink_fd, "result for %s side: [ %.2f %.2f %.2f ]", orientation_strs[orient], + mavlink_and_console_log_info(mavlink_fd, "result for %s side: [ %8.4f %8.4f %8.4f ]", orientation_strs[orient], (double)accel_ref[0][orient][0], (double)accel_ref[0][orient][1], (double)accel_ref[0][orient][2]); @@ -399,13 +394,6 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_se for (unsigned i = 0; i < (*active_sensors); i++) { res = calculate_calibration_values(i, accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); - /* verbose output on the console */ - printf("accel_T transformation matrix:\n"); - for (unsigned j = 0; j < 3; j++) { - printf(" %8.4f %8.4f %8.4f\n", (double)accel_T[i][j][0], (double)accel_T[i][j][1], (double)accel_T[i][j][2]); - } - printf("\n"); - if (res != OK) { mavlink_and_console_log_critical(mavlink_fd, "ERROR: calibration values calculation error"); break; @@ -635,7 +623,6 @@ int read_accelerometer_avg(int (&subs)[max_sens], float (&accel_avg)[max_sens][6 for (unsigned s = 0; s < max_sens; s++) { for (unsigned i = 0; i < 3; i++) { accel_avg[s][orient][i] = accel_sum[s][i] / counts[s]; - warnx("input: s:%u, axis: %u, orient: %u cnt: %u -> %8.4f", s, i, orient, counts[s], (double)accel_avg[s][orient][i]); } } From 37de377dcffb07ef49bacc0ec6ff722dadba1154 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 15 Mar 2015 17:24:11 +0100 Subject: [PATCH 044/268] commander: Increase stack size for low prio task to accomodate accel cal. --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 763c617bf9..b8638c6a18 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1134,7 +1134,7 @@ int commander_thread_main(int argc, char *argv[]) /* initialize low priority thread */ pthread_attr_t commander_low_prio_attr; pthread_attr_init(&commander_low_prio_attr); - pthread_attr_setstacksize(&commander_low_prio_attr, 1600); + pthread_attr_setstacksize(&commander_low_prio_attr, 2100); struct sched_param param; (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); From 904fc9b21c4ad20a78b83273cea667110045de1a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 15 Mar 2015 17:24:29 +0100 Subject: [PATCH 045/268] commander: Improve gyro calibration --- src/modules/commander/gyro_calibration.cpp | 37 +++++++++++++++++----- 1 file changed, 29 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index dfd1657c5b..8a70cf66e7 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -73,16 +73,17 @@ int do_gyro_calibration(int mavlink_fd) /* wait for the user to respond */ sleep(2); - struct gyro_scale gyro_scale[max_gyros] = { { + struct gyro_scale gyro_scale_zero = { 0.0f, 1.0f, 0.0f, 1.0f, 0.0f, 1.0f, - } }; + struct gyro_scale gyro_scale[max_gyros]; + int res = OK; /* store board ID */ @@ -97,7 +98,7 @@ int do_gyro_calibration(int mavlink_fd) for (unsigned s = 0; s < max_gyros; s++) { /* ensure all scale fields are initialized tha same as the first struct */ - (void)memcpy(&gyro_scale[s], &gyro_scale[0], sizeof(gyro_scale[0])); + (void)memcpy(&gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale[0])); sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); /* reset all offsets to zero and all scales to one */ @@ -109,7 +110,7 @@ int do_gyro_calibration(int mavlink_fd) device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0); - res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale); + res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero); close(fd); if (res != OK) { @@ -120,6 +121,8 @@ int do_gyro_calibration(int mavlink_fd) unsigned calibration_counter[max_gyros] = { 0 }; const unsigned calibration_count = 5000; + struct gyro_report gyro_report_0 = {}; + if (res == OK) { /* determine gyro mean values */ unsigned poll_errcount = 0; @@ -140,7 +143,7 @@ int do_gyro_calibration(int mavlink_fd) while (calibration_counter[0] < calibration_count) { /* wait blocking for new data */ - int poll_ret = poll(fds, 1, 1000); + int poll_ret = poll(&fds[0], max_gyros, 1000); if (poll_ret > 0) { @@ -150,6 +153,11 @@ int do_gyro_calibration(int mavlink_fd) if (changed) { orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report); + + if (s == 0) { + orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report_0); + } + gyro_scale[s].x_offset += gyro_report.x; gyro_scale[s].y_offset += gyro_report.y; gyro_scale[s].z_offset += gyro_report.z; @@ -183,8 +191,20 @@ int do_gyro_calibration(int mavlink_fd) if (res == OK) { /* check offsets */ - if (!isfinite(gyro_scale[0].x_offset) || !isfinite(gyro_scale[0].y_offset) || !isfinite(gyro_scale[0].z_offset)) { - mavlink_log_critical(mavlink_fd, "ERROR: offset is NaN"); + float xdiff = gyro_report_0.x - gyro_scale[0].x_offset; + float ydiff = gyro_report_0.y - gyro_scale[0].y_offset; + float zdiff = gyro_report_0.z - gyro_scale[0].z_offset; + + /* maximum allowable calibration error in radians */ + const float maxoff = 0.01f; + + if (!isfinite(gyro_scale[0].x_offset) || + !isfinite(gyro_scale[0].y_offset) || + !isfinite(gyro_scale[0].z_offset) || + fabsf(xdiff) > maxoff || + fabsf(ydiff) > maxoff || + fabsf(zdiff) > maxoff) { + mavlink_log_critical(mavlink_fd, "ERROR: Calibration failed"); res = ERROR; } } @@ -214,6 +234,7 @@ int do_gyro_calibration(int mavlink_fd) int fd = open(str, 0); if (fd < 0) { + failed = true; continue; } @@ -226,7 +247,7 @@ int do_gyro_calibration(int mavlink_fd) } if (failed) { - mavlink_log_critical(mavlink_fd, "ERROR: failed to set offset params"); + mavlink_and_console_log_critical(mavlink_fd, "ERROR: failed to set offset params"); res = ERROR; } } From ad54ff616dc83a703fe51c2a80a0662618116782 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 15 Mar 2015 17:24:50 +0100 Subject: [PATCH 046/268] commander: Increase frame size limit --- src/modules/commander/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index 4a1369aba9..4ee8732fcf 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -52,5 +52,5 @@ MODULE_STACKSIZE = 5000 MAXOPTIMIZATION = -Os -EXTRACXXFLAGS = -Wframe-larger-than=2000 +EXTRACXXFLAGS = -Wframe-larger-than=2200 From 7e9984b1d26fd0c1ded8fc7dd9165d83e3683316 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 15 Mar 2015 17:25:19 +0100 Subject: [PATCH 047/268] HMC5883 driver: Rotate before applying offsets. --- src/drivers/hmc5883/hmc5883.cpp | 22 +++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 7560ef20be..2b3520fc80 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -848,6 +848,10 @@ HMC5883::collect() struct mag_report new_report; bool sensor_is_onboard = false; + float xraw_f; + float yraw_f; + float zraw_f; + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ new_report.timestamp = hrt_absolute_time(); new_report.error_count = perf_event_count(_comms_errors); @@ -907,17 +911,21 @@ HMC5883::collect() report.x = -report.x; } - /* the standard external mag by 3DR has x pointing to the + /* the standard external mag by 3DR has x pointing to the * right, y pointing backwards, and z down, therefore switch x * and y and invert y */ - new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; - /* flip axes and negate value for y */ - new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; - /* z remains z */ - new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; + xraw_f = -report.y; + yraw_f = report.x; + zraw_f = report.z; // apply user specified rotation - rotate_3f(_rotation, new_report.x, new_report.y, new_report.z); + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + new_report.x = ((xraw_f * _range_scale) - _scale.x_offset) * _scale.x_scale; + /* flip axes and negate value for y */ + new_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale; + /* z remains z */ + new_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale; if (!(_pub_blocked)) { From f36f43db6f2ad545c5b102d6010ecc8176f151a7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 15 Mar 2015 17:25:35 +0100 Subject: [PATCH 048/268] L3GD20(H): driver: Rotate before applying offsets. --- src/drivers/l3gd20/l3gd20.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 4c41491a8b..7687236408 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -1029,9 +1029,16 @@ L3GD20::measure() report.temperature_raw = raw_report.temp; - report.x = ((report.x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; - report.y = ((report.y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; - report.z = ((report.z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; + float xraw_f = report.x_raw; + float yraw_f = report.y_raw; + float zraw_f = report.z_raw; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + report.x = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; + report.y = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; + report.z = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; report.x = _gyro_filter_x.apply(report.x); report.y = _gyro_filter_y.apply(report.y); @@ -1039,9 +1046,6 @@ L3GD20::measure() report.temperature = L3GD20_TEMP_OFFSET_CELSIUS - raw_report.temp; - // apply user specified rotation - rotate_3f(_rotation, report.x, report.y, report.z); - report.scaling = _gyro_range_scale; report.range_rad_s = _gyro_range_rad_s; From 3436f089768c31f0d925faf36b41626b78ddb6f5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 15 Mar 2015 17:25:55 +0100 Subject: [PATCH 049/268] LSM303D driver: Rotate before applying offsets. --- src/drivers/lsm303d/lsm303d.cpp | 33 +++++++++++++++++++++------------ 1 file changed, 21 insertions(+), 12 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index b5f01b942f..84f7fb5c81 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1517,9 +1517,16 @@ LSM303D::measure() accel_report.y_raw = raw_accel_report.y; accel_report.z_raw = raw_accel_report.z; - float x_in_new = ((accel_report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - float y_in_new = ((accel_report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - float z_in_new = ((accel_report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + float xraw_f = raw_accel_report.x; + float yraw_f = raw_accel_report.y; + float zraw_f = raw_accel_report.z; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; /* we have logs where the accelerometers get stuck at a fixed @@ -1555,9 +1562,6 @@ LSM303D::measure() accel_report.y = _accel_filter_y.apply(y_in_new); accel_report.z = _accel_filter_z.apply(z_in_new); - // apply user specified rotation - rotate_3f(_rotation, accel_report.x, accel_report.y, accel_report.z); - accel_report.scaling = _accel_range_scale; accel_report.range_m_s2 = _accel_range_m_s2; @@ -1623,16 +1627,21 @@ LSM303D::mag_measure() mag_report.x_raw = raw_mag_report.x; mag_report.y_raw = raw_mag_report.y; mag_report.z_raw = raw_mag_report.z; - mag_report.x = ((mag_report.x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; - mag_report.y = ((mag_report.y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; - mag_report.z = ((mag_report.z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; + + float xraw_f = mag_report.x_raw; + float yraw_f = mag_report.y_raw; + float zraw_f = mag_report.z_raw; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + mag_report.x = ((xraw_f * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; + mag_report.y = ((yraw_f * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; + mag_report.z = ((zraw_f * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; mag_report.scaling = _mag_range_scale; mag_report.range_ga = (float)_mag_range_ga; mag_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); - // apply user specified rotation - rotate_3f(_rotation, mag_report.x, mag_report.y, mag_report.z); - _mag_reports->force(&mag_report); /* XXX please check this poll_notify, is it the right one? */ From 92a52ea1953e0e17e7a55512b484adf7048e619c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 15 Mar 2015 17:26:11 +0100 Subject: [PATCH 050/268] MPU6000 driver: Rotate before applying offsets. --- src/drivers/mpu6000/mpu6000.cpp | 32 ++++++++++++++++++++------------ 1 file changed, 20 insertions(+), 12 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index b48ea85776..eaf25a9f3c 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1711,17 +1711,21 @@ MPU6000::measure() arb.y_raw = report.accel_y; arb.z_raw = report.accel_z; - float x_in_new = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - float y_in_new = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - float z_in_new = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + float xraw_f = report.accel_x; + float yraw_f = report.accel_y; + float zraw_f = report.accel_z; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; arb.x = _accel_filter_x.apply(x_in_new); arb.y = _accel_filter_y.apply(y_in_new); arb.z = _accel_filter_z.apply(z_in_new); - // apply user specified rotation - rotate_3f(_rotation, arb.x, arb.y, arb.z); - arb.scaling = _accel_range_scale; arb.range_m_s2 = _accel_range_m_s2; @@ -1732,17 +1736,21 @@ MPU6000::measure() grb.y_raw = report.gyro_y; grb.z_raw = report.gyro_z; - float x_gyro_in_new = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; - float y_gyro_in_new = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; - float z_gyro_in_new = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; + xraw_f = report.gyro_x; + yraw_f = report.gyro_y; + zraw_f = report.gyro_z; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; + float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; + float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; grb.x = _gyro_filter_x.apply(x_gyro_in_new); grb.y = _gyro_filter_y.apply(y_gyro_in_new); grb.z = _gyro_filter_z.apply(z_gyro_in_new); - // apply user specified rotation - rotate_3f(_rotation, grb.x, grb.y, grb.z); - grb.scaling = _gyro_range_scale; grb.range_rad_s = _gyro_range_rad_s; From c94755c847ce122e2da727ce58c3171a11896879 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 21 Feb 2015 01:14:29 +0100 Subject: [PATCH 051/268] PX4IO Firmware: Support 16 output channels --- src/modules/px4iofirmware/mixer.cpp | 6 ++++-- src/modules/px4iofirmware/px4io.h | 3 ++- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 66f0969de1..835dfc0f5e 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -272,8 +272,9 @@ mixer_tick(void) if (mixer_servos_armed && should_arm) { /* update the servo outputs. */ - for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) + for (unsigned i = 0; i < PX4IO_SERVO_HARDWARE_COUNT; i++) { up_pwm_servo_set(i, r_page_servos[i]); + } /* set S.BUS1 or S.BUS2 outputs */ @@ -285,8 +286,9 @@ mixer_tick(void) } else if (mixer_servos_armed && should_always_enable_pwm) { /* set the disarmed servo outputs. */ - for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) + for (unsigned i = 0; i < PX4IO_SERVO_HARDWARE_COUNT; i++) { up_pwm_servo_set(i, r_page_servo_disarmed[i]); + } /* set S.BUS1 or S.BUS2 outputs */ if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 93a33490fa..df2633cc5a 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -51,7 +51,8 @@ /* * Constants and limits. */ -#define PX4IO_SERVO_COUNT 8 +#define PX4IO_SERVO_COUNT 16 +#define PX4IO_SERVO_HARDWARE_COUNT 8 #define PX4IO_CONTROL_CHANNELS 8 #define PX4IO_CONTROL_GROUPS 4 #define PX4IO_RC_INPUT_CHANNELS 18 From ea5293b6fe977835b6f601b1e17a5d7f5e4ca248 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 21 Feb 2015 01:14:50 +0100 Subject: [PATCH 052/268] PX4IO driver: Support 16 output channels --- src/drivers/px4io/px4io.cpp | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index f62df54f6c..1c4cc18384 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1675,14 +1675,20 @@ PX4IO::io_publish_pwm_outputs() uint16_t ctl[_max_actuators]; int ret = io_reg_get(PX4IO_PAGE_SERVOS, 0, ctl, _max_actuators); - if (ret != OK) + if (ret != OK){ return ret; + } + + unsigned maxouts = sizeof(outputs.output) / sizeof(outputs.output[0]); + unsigned actuator_max = (_max_actuators > maxouts) ? maxouts : _max_actuators; + /* convert from register format to float */ - for (unsigned i = 0; i < _max_actuators; i++) + for (unsigned i = 0; i < actuator_max; i++){ outputs.output[i] = ctl[i]; + } - outputs.noutputs = _max_actuators; + outputs.noutputs = actuator_max; /* lazily advertise on first publication */ if (_to_outputs == 0) { @@ -1998,13 +2004,13 @@ PX4IO::print_status(bool extended_status) printf("vrssi %u\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VRSSI)); } - printf("actuators"); + printf("actuators (including S.BUS)"); for (unsigned i = 0; i < _max_actuators; i++) printf(" %hi", int16_t(io_reg_get(PX4IO_PAGE_ACTUATORS, i))); printf("\n"); - printf("servos"); + printf("hardware servo ports"); for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_SERVOS, i)); From 649fcd7cc77aa9ebf4595bd706b8f862b67c5b1e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 21 Feb 2015 01:18:36 +0100 Subject: [PATCH 053/268] PX4IO Firmware: Fall back to S.BUS1 for S.BUS2 requested --- src/modules/px4iofirmware/sbus.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index d76ec55f05..82701f0b1a 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -163,8 +163,8 @@ sbus1_output(uint16_t *values, uint16_t num_values) void sbus2_output(uint16_t *values, uint16_t num_values) { - char b = 'B'; - write(sbus_fd, &b, 1); + // XXX S.BUS2 is not implemented, fall back to S.BUS1 + sbus1_output(values, num_values); } bool From 6bd94f15a89c8252e1d04350e043c764483647b7 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Thu, 12 Mar 2015 17:43:19 +0100 Subject: [PATCH 054/268] PX4IO Firmware: fix 16ch output --- src/modules/px4iofirmware/registers.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index f0c2cfd26d..ae7aec34e4 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -284,7 +284,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num case PX4IO_PAGE_DIRECT_PWM: /* copy channel data */ - while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { + while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { /* XXX range-check value? */ if (*values != PWM_IGNORE_THIS_CHANNEL) { From 6a7f12496e8f2b28d58c1fab21653ff7748e7398 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 16 Mar 2015 10:28:42 +0100 Subject: [PATCH 055/268] fix errors and flag comparison --- src/drivers/gimbal/gimbal.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drivers/gimbal/gimbal.cpp b/src/drivers/gimbal/gimbal.cpp index 2e254c45da..1e27309d83 100644 --- a/src/drivers/gimbal/gimbal.cpp +++ b/src/drivers/gimbal/gimbal.cpp @@ -176,9 +176,9 @@ Gimbal::Gimbal() : CDev("Gimbal", GIMBAL_DEVICE_PATH), _vehicle_command_sub(-1), _att_sub(-1), + _attitude_compensation_roll(true), _attitude_compensation_pitch(true), _attitude_compensation_yaw(true), - _attitude_compensation_roll(true), _initialized(false), _actuator_controls_2_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "gimbal_read")), @@ -350,9 +350,9 @@ Gimbal::cycle() _config_cmd_set = false; - _attitude_compensation_roll = _config_cmd.param2 == 1; - _attitude_compensation_pitch = _config_cmd.param3 == 1; - _attitude_compensation_yaw = _config_cmd.param4 == 1; + _attitude_compensation_roll = (int)_config_cmd.param2 == 1; + _attitude_compensation_pitch = (int)_config_cmd.param3 == 1; + _attitude_compensation_yaw = (int)_config_cmd.param4 == 1; } From 374c50cab89c0173de267d23fcdc4b1d9430973c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 17 Mar 2015 13:39:39 +1100 Subject: [PATCH 056/268] lsm303d: support temperature readings in lsm303d these are read at the same rate as the mag --- src/drivers/lsm303d/lsm303d.cpp | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 84f7fb5c81..11a046a8dc 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -305,6 +305,9 @@ private: float _last_accel[3]; uint8_t _constant_accel_count; + // last temperature value + float _last_temperature; + // this is used to support runtime checking of key // configuration registers to detect SPI bus errors and sensor // reset @@ -567,6 +570,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _rotation(rotation), _constant_accel_count(0), + _last_temperature(0), _checked_next(0) { @@ -711,7 +715,7 @@ LSM303D::reset() /* enable mag */ write_checked_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); - write_checked_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); + write_checked_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M | REG5_ENABLE_T); write_checked_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1 write_checked_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2 @@ -1507,6 +1511,9 @@ LSM303D::measure() accel_report.timestamp = hrt_absolute_time(); + // use the temperature from the last mag reading + accel_report.temperature = _last_temperature; + // report the error count as the sum of the number of bad // register reads and bad values. This allows the higher level // code to decide if it should use this sensor based on @@ -1588,6 +1595,7 @@ LSM303D::mag_measure() #pragma pack(push, 1) struct { uint8_t cmd; + int16_t temperature; uint8_t status; int16_t x; int16_t y; @@ -1603,7 +1611,7 @@ LSM303D::mag_measure() /* fetch data from the sensor */ memset(&raw_mag_report, 0, sizeof(raw_mag_report)); - raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT; + raw_mag_report.cmd = ADDR_OUT_TEMP_L | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report)); /* @@ -1642,6 +1650,10 @@ LSM303D::mag_measure() mag_report.range_ga = (float)_mag_range_ga; mag_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); + // remember the temperature. The datasheet isn't clear, but it + // seems to be a signed offset from 25 degrees C in units of 0.125C + _last_temperature = 25 + (raw_mag_report.temperature*0.125f); + _mag_reports->force(&mag_report); /* XXX please check this poll_notify, is it the right one? */ @@ -1681,6 +1693,7 @@ LSM303D::print_info() (unsigned)_checked_values[i]); } } + ::printf("temperature: %.2f\n", _last_temperature); } void From 9442fc13e4a93ba43300aa4648e9c9033075a70e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 17 Mar 2015 13:25:27 +1100 Subject: [PATCH 057/268] mpu6000: show temperature in "mpu6000 info" --- src/drivers/mpu6000/mpu6000.cpp | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index eaf25a9f3c..727132a646 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -284,6 +284,9 @@ private: // self test volatile bool _in_factory_test; + // last temperature reading for print_info() + float _last_temperature; + /** * Start automatic measurement. */ @@ -518,7 +521,8 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _rotation(rotation), _checked_next(0), - _in_factory_test(false) + _in_factory_test(false), + _last_temperature(0) { // disable debug() calls _debug_enabled = false; @@ -1729,8 +1733,10 @@ MPU6000::measure() arb.scaling = _accel_range_scale; arb.range_m_s2 = _accel_range_m_s2; + _last_temperature = (report.temp) / 361.0f + 35.0f; + arb.temperature_raw = report.temp; - arb.temperature = (report.temp) / 361.0f + 35.0f; + arb.temperature = _last_temperature; grb.x_raw = report.gyro_x; grb.y_raw = report.gyro_y; @@ -1755,7 +1761,7 @@ MPU6000::measure() grb.range_rad_s = _gyro_range_rad_s; grb.temperature_raw = report.temp; - grb.temperature = (report.temp) / 361.0f + 35.0f; + grb.temperature = _last_temperature; _accel_reports->force(&arb); _gyro_reports->force(&grb); @@ -1803,6 +1809,7 @@ MPU6000::print_info() (unsigned)_checked_values[i]); } } + ::printf("temperature: %.1f\n", _last_temperature); } void From b3e6d911becc087c26fd4c40a6b2dd5381371e02 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 17 Mar 2015 11:34:04 +0100 Subject: [PATCH 058/268] Eigen: Add eigen as a submodule --- .gitmodules | 3 +++ src/lib/eigen | 1 + 2 files changed, 4 insertions(+) create mode 160000 src/lib/eigen diff --git a/.gitmodules b/.gitmodules index c869803b7a..6a4f815d42 100644 --- a/.gitmodules +++ b/.gitmodules @@ -16,3 +16,6 @@ [submodule "unittests/gtest"] path = unittests/gtest url = https://github.com/sjwilks/gtest.git +[submodule "src/lib/eigen"] + path = src/lib/eigen + url = https://github.com/PX4/eigen.git diff --git a/src/lib/eigen b/src/lib/eigen new file mode 160000 index 0000000000..e7850ed81f --- /dev/null +++ b/src/lib/eigen @@ -0,0 +1 @@ +Subproject commit e7850ed81f9c469e02df496ef09ae32ec0379b71 From 0be253003725b86a559795be78011aae6f2e41c7 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 17 Mar 2015 13:27:50 +0100 Subject: [PATCH 059/268] tests: Added test_eigen to verify correctness of eigen calculations --- src/systemcmds/tests/module.mk | 3 +- src/systemcmds/tests/test_eigen.cpp | 410 ++++++++++++++++++++++++++++ src/systemcmds/tests/tests.h | 1 + src/systemcmds/tests/tests_main.c | 1 + 4 files changed, 414 insertions(+), 1 deletion(-) create mode 100644 src/systemcmds/tests/test_eigen.cpp diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 1d728e8579..4b7b73ac65 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -32,7 +32,8 @@ SRCS = test_adc.c \ test_ppm_loopback.c \ test_rc.c \ test_conv.cpp \ - test_mount.c + test_mount.c \ + test_eigen.cpp EXTRACXXFLAGS = -Wframe-larger-than=2500 -Wno-float-equal -Wno-double-promotion diff --git a/src/systemcmds/tests/test_eigen.cpp b/src/systemcmds/tests/test_eigen.cpp new file mode 100644 index 0000000000..0c9ee810aa --- /dev/null +++ b/src/systemcmds/tests/test_eigen.cpp @@ -0,0 +1,410 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file test_eigen.cpp + * + * Eigen test (based of mathlib test) + * @author Johan Jansen + */ + +//Hacks to make Eigen compile on NuttX +#pragma GCC diagnostic push +#define RAND_MAX __RAND_MAX +#pragma GCC diagnostic ignored "-Wshadow" +#pragma GCC diagnostic ignored "-Wfloat-equal" +#define _GLIBCXX_USE_C99_FP_MACROS_DYNAMIC 1 + +#include +#include +#pragma GCC diagnostic pop + +#include +#include +#include +#include +#include +#include +#include + +#include "tests.h" + +namespace Eigen +{ + typedef Matrix Vector10f; +} + +#define TEST_OP(_title, _op) { size_t n = 60000; hrt_abstime t0, t1; t0 = hrt_absolute_time(); for (size_t j = 0; j < n; j++) { _op; }; t1 = hrt_absolute_time(); warnx(_title ": %.6fus", (double)(t1 - t0) / n); } + +/** +* @brief +* Prints an Eigen::Matrix to stdout +**/ +template +void printEigen(const Eigen::MatrixBase& b) +{ + for(int i = 0; i < b.rows(); ++i) { + printf("("); + for(int j = 0; j < b.cols(); ++i) { + if(j > 0) { + printf(","); + } + + printf("%.3f", static_cast(b(i, j))); + } + printf(")%s\n", i+1 < b.rows() ? "," : ""); + } +} + +/** +* @brief +* Construct new Eigen::Quaternion from euler angles +**/ +template +Eigen::Quaternion quatFromEuler(const T roll, const T pitch, const T yaw) +{ + Eigen::AngleAxis rollAngle(roll, Eigen::Matrix::UnitZ()); + Eigen::AngleAxis yawAngle(yaw, Eigen::Matrix::UnitY()); + Eigen::AngleAxis pitchAngle(pitch, Eigen::Matrix::UnitX()); + return rollAngle * yawAngle * pitchAngle; +} + + +int test_eigen(int argc, char *argv[]) +{ + int rc = 0; + warnx("testing eigen"); + + { + Eigen::Vector2f v; + Eigen::Vector2f v1(1.0f, 2.0f); + Eigen::Vector2f v2(1.0f, -1.0f); + float data[2] = {1.0f, 2.0f}; + TEST_OP("Constructor Vector2f()", Eigen::Vector2f v3); + TEST_OP("Constructor Vector2f(Vector2f)", Eigen::Vector2f v3(v1)); + TEST_OP("Constructor Vector2f(float[])", Eigen::Vector2f v3(data)); + TEST_OP("Constructor Vector2f(float, float)", Eigen::Vector2f v3(1.0f, 2.0f)); + TEST_OP("Vector2f = Vector2f", v = v1); + TEST_OP("Vector2f + Vector2f", v + v1); + TEST_OP("Vector2f - Vector2f", v - v1); + TEST_OP("Vector2f += Vector2f", v += v1); + TEST_OP("Vector2f -= Vector2f", v -= v1); + TEST_OP("Vector2f dot Vector2f", v.dot(v1)); + //TEST_OP("Vector2f cross Vector2f", v1.cross(v2)); //cross product for 2d array? + } + + { + Eigen::Vector3f v; + Eigen::Vector3f v1(1.0f, 2.0f, 0.0f); + Eigen::Vector3f v2(1.0f, -1.0f, 2.0f); + float data[3] = {1.0f, 2.0f, 3.0f}; + TEST_OP("Constructor Vector3f()", Eigen::Vector3f v3); + TEST_OP("Constructor Vector3f(Vector3f)", Eigen::Vector3f v3(v1)); + TEST_OP("Constructor Vector3f(float[])", Eigen::Vector3f v3(data)); + TEST_OP("Constructor Vector3f(float, float, float)", Eigen::Vector3f v3(1.0f, 2.0f, 3.0f)); + TEST_OP("Vector3f = Vector3f", v = v1); + TEST_OP("Vector3f + Vector3f", v + v1); + TEST_OP("Vector3f - Vector3f", v - v1); + TEST_OP("Vector3f += Vector3f", v += v1); + TEST_OP("Vector3f -= Vector3f", v -= v1); + TEST_OP("Vector3f * float", v1 * 2.0f); + TEST_OP("Vector3f / float", v1 / 2.0f); + TEST_OP("Vector3f *= float", v1 *= 2.0f); + TEST_OP("Vector3f /= float", v1 /= 2.0f); + TEST_OP("Vector3f dot Vector3f", v.dot(v1)); + TEST_OP("Vector3f cross Vector3f", v1.cross(v2)); + TEST_OP("Vector3f length", v1.norm()); + TEST_OP("Vector3f length squared", v1.squaredNorm()); +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" + // Need pragma here intead of moving variable out of TEST_OP and just reference because + // TEST_OP measures performance of vector operations. + TEST_OP("Vector<3> element read", volatile float a = v1(0)); + TEST_OP("Vector<3> element read direct", volatile float a = v1.data()[0]); +#pragma GCC diagnostic pop + TEST_OP("Vector<3> element write", v1(0) = 1.0f); + TEST_OP("Vector<3> element write direct", v1.data()[0] = 1.0f); + } + + { + Eigen::Vector4f v; + Eigen::Vector4f v1(1.0f, 2.0f, 0.0f, -1.0f); + Eigen::Vector4f v2(1.0f, -1.0f, 2.0f, 0.0f); + float data[4] = {1.0f, 2.0f, 3.0f, 4.0f}; + TEST_OP("Constructor Vector<4>()", Eigen::Vector4f v3); + TEST_OP("Constructor Vector<4>(Vector<4>)", Eigen::Vector4f v3(v1)); + TEST_OP("Constructor Vector<4>(float[])", Eigen::Vector4f v3(data)); + TEST_OP("Constructor Vector<4>(float, float, float, float)", Eigen::Vector4f v3(1.0f, 2.0f, 3.0f, 4.0f)); + TEST_OP("Vector<4> = Vector<4>", v = v1); + TEST_OP("Vector<4> + Vector<4>", v + v1); + TEST_OP("Vector<4> - Vector<4>", v - v1); + TEST_OP("Vector<4> += Vector<4>", v += v1); + TEST_OP("Vector<4> -= Vector<4>", v -= v1); + TEST_OP("Vector<4> * Vector<4>", v.dot(v1)); + } + + { + Eigen::Vector10f v1; + v1.Zero(); + float data[10]; + TEST_OP("Constructor Vector<10>()", Eigen::Vector10f v3); + TEST_OP("Constructor Vector<10>(Vector<10>)", Eigen::Vector10f v3(v1)); + TEST_OP("Constructor Vector<10>(float[])", Eigen::Vector10f v3(data)); + } + + { + Eigen::Matrix3f m1; + m1.setIdentity(); + Eigen::Matrix3f m2; + m2.setIdentity(); + Eigen::Vector3f v1(1.0f, 2.0f, 0.0f); + TEST_OP("Matrix3f * Vector3f", m1 * v1); + TEST_OP("Matrix3f + Matrix3f", m1 + m2); + TEST_OP("Matrix3f * Matrix3f", m1 * m2); + } + + { + Eigen::Matrix m1; + m1.setIdentity(); + Eigen::Matrix m2; + m2.setIdentity(); + Eigen::Vector10f v1; + v1.Zero(); + TEST_OP("Matrix<10, 10> * Vector<10>", m1 * v1); + TEST_OP("Matrix<10, 10> + Matrix<10, 10>", m1 + m2); + TEST_OP("Matrix<10, 10> * Matrix<10, 10>", m1 * m2); + } + + { + warnx("Nonsymmetric matrix operations test"); + // test nonsymmetric +, -, +=, -= + + const Eigen::Matrix m1_orig = + (Eigen::Matrix() << 1, 3, 3, + 4, 6, 6).finished(); + + Eigen::Matrix m1(m1_orig); + + Eigen::Matrix m2; + m2 << 2, 4, 6, + 8, 10, 12; + + Eigen::Matrix m3; + m3 << 3, 6, 9, + 12, 15, 18; + + if (m1 + m2 != m3) { + warnx("Matrix<2, 3> + Matrix<2, 3> failed!"); + printEigen(m1 + m2); + printf("!=\n"); + printEigen(m3); + rc = 1; + } + + if (m3 - m2 != m1) { + warnx("Matrix<2, 3> - Matrix<2, 3> failed!"); + printEigen(m3 - m2); + printf("!=\n"); + printEigen(m1); + rc = 1; + } + + m1 += m2; + + if (m1 != m3) { + warnx("Matrix<2, 3> += Matrix<2, 3> failed!"); + printEigen(m1); + printf("!=\n"); + printEigen(m3); + rc = 1; + } + + m1 -= m2; + + if (m1 != m1_orig) { + warnx("Matrix<2, 3> -= Matrix<2, 3> failed!"); + printEigen(m1); + printf("!=\n"); + printEigen(m1_orig); + rc = 1; + } + + } + + { + // test conversion rotation matrix to quaternion and back + Eigen::Matrix3f R_orig; + Eigen::Matrix3f R; + Eigen::Quaternionf q; + float diff = 0.1f; + float tol = 0.00001f; + + warnx("Quaternion transformation methods test."); + + for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) { + for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) { + for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) { + R_orig.eulerAngles(roll, pitch, yaw); + q = R_orig; //from_dcm + R = q.toRotationMatrix(); + + for (size_t i = 0; i < 3; i++) { + for (size_t j = 0; j < 3; j++) { + if (fabsf(R_orig(i, j) - R(i, j)) > 0.00001f) { + warnx("Quaternion method 'from_dcm' or 'toRotationMatrix' outside tolerance!"); + rc = 1; + } + } + } + } + } + } + + // test against some known values + tol = 0.0001f; + Eigen::Quaternionf q_true = {1.0f, 0.0f, 0.0f, 0.0f}; + R_orig.setIdentity(); + q = R_orig; + + for (size_t i = 0; i < 4; i++) { + if (!q.isApprox(q_true, tol)) { + warnx("Quaternion method 'from_dcm()' outside tolerance!"); + rc = 1; + } + } + + q_true = quatFromEuler(0.3f, 0.2f, 0.1f); + q = {0.9833f, 0.1436f, 0.1060f, 0.0343f}; + + for (size_t i = 0; i < 4; i++) { + if (!q.isApprox(q_true, tol)) { + warnx("Quaternion method 'eulerAngles()' outside tolerance!"); + rc = 1; + } + } + + q_true = quatFromEuler(-1.5f, -0.2f, 0.5f); + q = {0.7222f, -0.6391f, -0.2386f, 0.1142f}; + + for (size_t i = 0; i < 4; i++) { + if (!q.isApprox(q_true, tol)) { + warnx("Quaternion method 'eulerAngles()' outside tolerance!"); + rc = 1; + } + } + + q_true = quatFromEuler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3); + q = {0.6830f, 0.1830f, -0.6830f, 0.1830f}; + + for (size_t i = 0; i < 4; i++) { + if (!q.isApprox(q_true, tol)) { + warnx("Quaternion method 'eulerAngles()' outside tolerance!"); + rc = 1; + } + } + + } + + { + // test quaternion method "rotate" (rotate vector by quaternion) + Eigen::Vector3f vector = {1.0f,1.0f,1.0f}; + Eigen::Vector3f vector_q; + Eigen::Vector3f vector_r; + Eigen::Quaternionf q; + Eigen::Matrix3f R; + float diff = 0.1f; + float tol = 0.00001f; + + warnx("Quaternion vector rotation method test."); + + for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) { + for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) { + for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) { + R.eulerAngles(roll, pitch, yaw); + q = quatFromEuler(roll,pitch,yaw); + vector_r = R*vector; + vector_q = q._transformVector(vector); + for (int i = 0; i < 3; i++) { + if(fabsf(vector_r(i) - vector_q(i)) > tol) { + warnx("Quaternion method 'rotate' outside tolerance"); + rc = 1; + } + } + } + } + } + + // test some values calculated with matlab + tol = 0.0001f; + q = quatFromEuler(M_PI_2_F,0.0f,0.0f); + vector_q = q._transformVector(vector); + Eigen::Vector3f vector_true = {1.00f,-1.00f,1.00f}; + for(size_t i = 0;i<3;i++) { + if(fabsf(vector_true(i) - vector_q(i)) > tol) { + warnx("Quaternion method 'rotate' outside tolerance"); + rc = 1; + } + } + + q = quatFromEuler(0.3f,0.2f,0.1f); + vector_q = q._transformVector(vector); + vector_true = {1.1566,0.7792,1.0273}; + for(size_t i = 0;i<3;i++) { + if(fabsf(vector_true(i) - vector_q(i)) > tol) { + warnx("Quaternion method 'rotate' outside tolerance"); + rc = 1; + } + } + + q = quatFromEuler(-1.5f,-0.2f,0.5f); + vector_q = q._transformVector(vector); + vector_true = {0.5095,1.4956,-0.7096}; + for(size_t i = 0;i<3;i++) { + if(fabsf(vector_true(i) - vector_q(i)) > tol) { + warnx("Quaternion method 'rotate' outside tolerance"); + rc = 1; + } + } + + q = quatFromEuler(M_PI_2_F,-M_PI_2_F,-M_PI_F/3.0f); + vector_q = q._transformVector(vector); + vector_true = {-1.3660,0.3660,1.0000}; + for(size_t i = 0;i<3;i++) { + if(fabsf(vector_true(i) - vector_q(i)) > tol) { + warnx("Quaternion method 'rotate' outside tolerance"); + rc = 1; + } + } + } + return rc; +} \ No newline at end of file diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index 69fb0e57bd..9d13d50d77 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -113,6 +113,7 @@ extern int test_rc(int argc, char *argv[]); extern int test_conv(int argc, char *argv[]); extern int test_mount(int argc, char *argv[]); extern int test_mathlib(int argc, char *argv[]); +extern int test_eigen(int argc, char *argv[]); __END_DECLS diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 0768c1d5aa..bad8d9cc3b 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -112,6 +112,7 @@ const struct { #ifndef TESTS_MATHLIB_DISABLE {"mathlib", test_mathlib, 0}, #endif + {"eigen", test_eigen, 0}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} }; From 008a36003f4bf3ca031bd4473597a4892727dba2 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 17 Mar 2015 13:58:03 +0100 Subject: [PATCH 060/268] Eigen: Add general purpose Eigen compatability header for PX4 --- src/lib/px4_eigen.h | 51 +++++++++++++++++++++++++++++ src/systemcmds/tests/test_eigen.cpp | 12 +------ 2 files changed, 52 insertions(+), 11 deletions(-) create mode 100644 src/lib/px4_eigen.h diff --git a/src/lib/px4_eigen.h b/src/lib/px4_eigen.h new file mode 100644 index 0000000000..2cd98e59aa --- /dev/null +++ b/src/lib/px4_eigen.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4_eigen.h + * + * Compatability header to make Eigen compile on the PX4 stack + * @author Johan Jansen + */ + +#pragma once + +#pragma GCC diagnostic push +#define RAND_MAX __RAND_MAX +#pragma GCC diagnostic ignored "-Wshadow" +#pragma GCC diagnostic ignored "-Wfloat-equal" +#define _GLIBCXX_USE_C99_FP_MACROS_DYNAMIC 1 + +#include +#include +#pragma GCC diagnostic pop diff --git a/src/systemcmds/tests/test_eigen.cpp b/src/systemcmds/tests/test_eigen.cpp index 0c9ee810aa..ed04d39a1b 100644 --- a/src/systemcmds/tests/test_eigen.cpp +++ b/src/systemcmds/tests/test_eigen.cpp @@ -38,17 +38,7 @@ * @author Johan Jansen */ -//Hacks to make Eigen compile on NuttX -#pragma GCC diagnostic push -#define RAND_MAX __RAND_MAX -#pragma GCC diagnostic ignored "-Wshadow" -#pragma GCC diagnostic ignored "-Wfloat-equal" -#define _GLIBCXX_USE_C99_FP_MACROS_DYNAMIC 1 - -#include -#include -#pragma GCC diagnostic pop - +#include #include #include #include From 3451a4686af81b1ef7fde21e9ef79b21163de09c Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 17 Mar 2015 14:08:20 +0100 Subject: [PATCH 061/268] test_eigen: Make TEST_OP macro more readable --- src/systemcmds/tests/test_eigen.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/tests/test_eigen.cpp b/src/systemcmds/tests/test_eigen.cpp index ed04d39a1b..767ea2d87a 100644 --- a/src/systemcmds/tests/test_eigen.cpp +++ b/src/systemcmds/tests/test_eigen.cpp @@ -54,7 +54,16 @@ namespace Eigen typedef Matrix Vector10f; } -#define TEST_OP(_title, _op) { size_t n = 60000; hrt_abstime t0, t1; t0 = hrt_absolute_time(); for (size_t j = 0; j < n; j++) { _op; }; t1 = hrt_absolute_time(); warnx(_title ": %.6fus", (double)(t1 - t0) / n); } +static constexpr size_t OPERATOR_ITERATIONS = 60000; + +#define TEST_OP(_title, _op) \ +{ \ + const hrt_abstime t0 = hrt_absolute_time(); \ + for (size_t j = 0; j < OPERATOR_ITERATIONS; j++) { \ + _op; \ + } \ + printf(_title ": %.6fus", static_cast(hrt_absolute_time() - t0) / OPERATOR_ITERATIONS); \ +} /** * @brief @@ -160,7 +169,7 @@ int test_eigen(int argc, char *argv[]) TEST_OP("Vector<4> - Vector<4>", v - v1); TEST_OP("Vector<4> += Vector<4>", v += v1); TEST_OP("Vector<4> -= Vector<4>", v -= v1); - TEST_OP("Vector<4> * Vector<4>", v.dot(v1)); + TEST_OP("Vector<4> dot Vector<4>", v.dot(v1)); } { From c0c3e153ec5b8fdea6a69a97e17ff149e483c8dd Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 17 Mar 2015 14:51:18 +0100 Subject: [PATCH 062/268] Eigen: Add verify macro to check if math operations are correct --- src/systemcmds/tests/test_eigen.cpp | 33 ++++++++++++++++++++--------- 1 file changed, 23 insertions(+), 10 deletions(-) diff --git a/src/systemcmds/tests/test_eigen.cpp b/src/systemcmds/tests/test_eigen.cpp index 767ea2d87a..de5630cc3e 100644 --- a/src/systemcmds/tests/test_eigen.cpp +++ b/src/systemcmds/tests/test_eigen.cpp @@ -39,6 +39,7 @@ */ #include +#include #include #include #include @@ -56,7 +57,7 @@ namespace Eigen static constexpr size_t OPERATOR_ITERATIONS = 60000; -#define TEST_OP(_title, _op) \ +#define TEST_OP(_title, _op) \ { \ const hrt_abstime t0 = hrt_absolute_time(); \ for (size_t j = 0; j < OPERATOR_ITERATIONS; j++) { \ @@ -65,6 +66,18 @@ static constexpr size_t OPERATOR_ITERATIONS = 60000; printf(_title ": %.6fus", static_cast(hrt_absolute_time() - t0) / OPERATOR_ITERATIONS); \ } +#define VERIFY_OP(_title, _op, __OP_TEST__) \ +{ \ + _op; \ + if(!(__OP_TEST__)) { \ + printf(_title " Failed! ("#__OP_TEST__")"); \ + } \ +} + +#define TEST_OP_VERIFY(_title, _op, __OP_TEST__) \ + VERIFY_OP(_title, _op, __OP_TEST__) \ + TEST_OP(_title, _op) + /** * @brief * Prints an Eigen::Matrix to stdout @@ -110,15 +123,15 @@ int test_eigen(int argc, char *argv[]) Eigen::Vector2f v2(1.0f, -1.0f); float data[2] = {1.0f, 2.0f}; TEST_OP("Constructor Vector2f()", Eigen::Vector2f v3); - TEST_OP("Constructor Vector2f(Vector2f)", Eigen::Vector2f v3(v1)); - TEST_OP("Constructor Vector2f(float[])", Eigen::Vector2f v3(data)); - TEST_OP("Constructor Vector2f(float, float)", Eigen::Vector2f v3(1.0f, 2.0f)); - TEST_OP("Vector2f = Vector2f", v = v1); - TEST_OP("Vector2f + Vector2f", v + v1); - TEST_OP("Vector2f - Vector2f", v - v1); - TEST_OP("Vector2f += Vector2f", v += v1); - TEST_OP("Vector2f -= Vector2f", v -= v1); - TEST_OP("Vector2f dot Vector2f", v.dot(v1)); + TEST_OP_VERIFY("Constructor Vector2f(Vector2f)", Eigen::Vector2f v3(v1), v3.isApprox(v1)); + TEST_OP_VERIFY("Constructor Vector2f(float[])", Eigen::Vector2f v3(data), v3[0] == data[0] && v3[1] == data[1]); + TEST_OP_VERIFY("Constructor Vector2f(float, float)", Eigen::Vector2f v3(1.0f, 2.0f), v3(0) == 1.0f && v3(1) == 2.0f); + TEST_OP_VERIFY("Vector2f = Vector2f", v = v1, v.isApprox(v1)); + TEST_OP_VERIFY("Vector2f + Vector2f", v + v1, v.isApprox(v1+v1)); + TEST_OP_VERIFY("Vector2f - Vector2f", v - v1, v.isApprox(v1)); + TEST_OP_VERIFY("Vector2f += Vector2f", v += v1, v.isApprox(v1+v1)); + TEST_OP_VERIFY("Vector2f -= Vector2f", v -= v1, v.isApprox(v1)); + TEST_OP_VERIFY("Vector2f dot Vector2f", v.dot(v1), fabs(v.dot(v1) - 5.0f) <= FLT_EPSILON); //TEST_OP("Vector2f cross Vector2f", v1.cross(v2)); //cross product for 2d array? } From 1cc8b6eefc3fa3526b017aad9d9c37a25e69165a Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Tue, 17 Mar 2015 15:19:45 -0600 Subject: [PATCH 063/268] add cpu load to STAT log message --- src/modules/sdlog2/sdlog2.c | 9 +++++---- src/modules/sdlog2/sdlog2_messages.h | 5 +++-- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 7017a65add..04bf71c178 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1216,12 +1216,13 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- VEHICLE STATUS --- */ if (status_updated) { log_msg.msg_type = LOG_STAT_MSG; - log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; - log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; - log_msg.body.log_STAT.failsafe_state = (uint8_t) buf_status.failsafe; + log_msg.body.log_STAT.main_state = buf_status.main_state; + log_msg.body.log_STAT.arming_state = buf_status.arming_state; + log_msg.body.log_STAT.failsafe = (uint8_t) buf_status.failsafe; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; - log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; + log_msg.body.log_STAT.battery_warning = buf_status.battery_warning; log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed; + log_msg.body.log_STAT.load = buf_status.load; LOGBUFFER_WRITE_AND_COUNT(STAT); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 87b4795bca..060dcf62fb 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -171,10 +171,11 @@ struct log_ATTC_s { struct log_STAT_s { uint8_t main_state; uint8_t arming_state; - uint8_t failsafe_state; + uint8_t failsafe; float battery_remaining; uint8_t battery_warning; uint8_t landed; + float load; }; /* --- RC - RC INPUT CHANNELS --- */ @@ -480,7 +481,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"), LOG_FORMAT_S(ATTC, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), - LOG_FORMAT(STAT, "BBBfBB", "MainState,ArmState,FailsafeState,BatRem,BatWarn,Landed"), + LOG_FORMAT(STAT, "BBBfBBf", "MainS,ArmS,Failsafe,BatRem,BatWarn,Landed,Load"), LOG_FORMAT(VTOL, "f", "Arsp"), LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), From f3a2c66e89aa2da0e9f736b89b4da9e4e03283f0 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sun, 15 Mar 2015 20:39:57 +0100 Subject: [PATCH 064/268] write rosbags --- .../demo_tests/direct_offboard_posctl_test.py | 9 ++++++++- .../demo_tests/mavros_offboard_attctl_test.py | 12 ++++++++++++ .../demo_tests/mavros_offboard_posctl_test.py | 12 ++++++++++++ 3 files changed, 32 insertions(+), 1 deletion(-) diff --git a/integrationtests/demo_tests/direct_offboard_posctl_test.py b/integrationtests/demo_tests/direct_offboard_posctl_test.py index d61eec0632..d71354059a 100755 --- a/integrationtests/demo_tests/direct_offboard_posctl_test.py +++ b/integrationtests/demo_tests/direct_offboard_posctl_test.py @@ -39,6 +39,7 @@ PKG = 'px4' import unittest import rospy +import rosbag from numpy import linalg import numpy as np @@ -62,8 +63,9 @@ class DirectOffboardPosctlTest(unittest.TestCase): def setUp(self): rospy.init_node('test_node', anonymous=True) + self.bag = rosbag.Bag('direct_offboard_posctl_test.bag', 'w', compression="lz4") rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) - rospy.Subscriber("iris/vehicle_local_position", vehicle_local_position, self.position_callback) + self.sub_vlp = rospy.Subscriber("iris/vehicle_local_position", vehicle_local_position, self.position_callback) self.pub_spt = rospy.Publisher('iris/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) self.rate = rospy.Rate(10) # 10hz self.has_pos = False @@ -74,12 +76,17 @@ class DirectOffboardPosctlTest(unittest.TestCase): if self.fpa: self.fpa.stop() + self.sub_vlp.unregister() + self.rate.sleep() + self.bag.close() + # # General callback functions used in tests # def position_callback(self, data): self.has_pos = True self.local_position = data + self.bag.write('vehicle_local_position', data) def vehicle_control_mode_callback(self, data): self.control_mode = data diff --git a/integrationtests/demo_tests/mavros_offboard_attctl_test.py b/integrationtests/demo_tests/mavros_offboard_attctl_test.py index 30e9fe9baf..aa6bd8d9f2 100755 --- a/integrationtests/demo_tests/mavros_offboard_attctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_attctl_test.py @@ -39,8 +39,10 @@ PKG = 'px4' import unittest import rospy +import rosbag from px4.msg import vehicle_control_mode +from px4.msg import vehicle_local_position from std_msgs.msg import Header from std_msgs.msg import Float64 from geometry_msgs.msg import PoseStamped, Quaternion @@ -56,8 +58,10 @@ class MavrosOffboardAttctlTest(unittest.TestCase): def setUp(self): rospy.init_node('test_node', anonymous=True) + self.bag = rosbag.Bag('mavros_offboard_attctl_test.bag', 'w', compression="lz4") rospy.wait_for_service('iris/mavros/cmd/arming', 30) rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + self.sub_vlp = rospy.Subscriber("iris/vehicle_local_position", vehicle_local_position, self.vehicle_position_callback) rospy.Subscriber("iris/mavros/position/local", PoseStamped, self.position_callback) self.pub_att = rospy.Publisher('iris/mavros/setpoint/attitude', PoseStamped, queue_size=10) self.pub_thr = rospy.Publisher('iris/mavros/setpoint/att_throttle', Float64, queue_size=10) @@ -66,9 +70,17 @@ class MavrosOffboardAttctlTest(unittest.TestCase): self.control_mode = vehicle_control_mode() self.local_position = PoseStamped() + def tearDown(self): + self.sub_vlp.unregister() + self.rate.sleep() + self.bag.close() + # # General callback functions used in tests # + def vehicle_position_callback(self, data): + self.bag.write('vehicle_local_position', data) + def position_callback(self, data): self.has_pos = True self.local_position = data diff --git a/integrationtests/demo_tests/mavros_offboard_posctl_test.py b/integrationtests/demo_tests/mavros_offboard_posctl_test.py index 568c2cbd80..f244d9d077 100755 --- a/integrationtests/demo_tests/mavros_offboard_posctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_posctl_test.py @@ -40,11 +40,13 @@ PKG = 'px4' import unittest import rospy import math +import rosbag from numpy import linalg import numpy as np from px4.msg import vehicle_control_mode +from px4.msg import vehicle_local_position from std_msgs.msg import Header from geometry_msgs.msg import PoseStamped, Quaternion from tf.transformations import quaternion_from_euler @@ -60,7 +62,9 @@ class MavrosOffboardPosctlTest(unittest.TestCase): def setUp(self): rospy.init_node('test_node', anonymous=True) + self.bag = rosbag.Bag('mavros_offboard_posctl_test.bag', 'w', compression="lz4") rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + self.sub_vlp = rospy.Subscriber("iris/vehicle_local_position", vehicle_local_position, self.vehicle_position_callback) rospy.Subscriber("iris/mavros/position/local", PoseStamped, self.position_callback) self.pub_spt = rospy.Publisher('iris/mavros/setpoint/local_position', PoseStamped, queue_size=10) self.rate = rospy.Rate(10) # 10hz @@ -68,9 +72,17 @@ class MavrosOffboardPosctlTest(unittest.TestCase): self.local_position = PoseStamped() self.control_mode = vehicle_control_mode() + def tearDown(self): + self.sub_vlp.unregister() + self.rate.sleep() + self.bag.close() + # # General callback functions used in tests # + def vehicle_position_callback(self, data): + self.bag.write('vehicle_local_position', data) + def position_callback(self, data): self.has_pos = True self.local_position = data From f1fa57ff4297f50ccfbee86344de02554f641a7f Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 16 Mar 2015 21:45:29 +0100 Subject: [PATCH 065/268] write more bags, use helper class to log default topics --- .../demo_tests/direct_offboard_posctl_test.py | 13 +- .../demo_tests/mavros_offboard_attctl_test.py | 17 +-- .../demo_tests/mavros_offboard_posctl_test.py | 15 ++- .../demo_tests/px4_test_helper.py | 111 ++++++++++++++++++ 4 files changed, 135 insertions(+), 21 deletions(-) create mode 100644 integrationtests/demo_tests/px4_test_helper.py diff --git a/integrationtests/demo_tests/direct_offboard_posctl_test.py b/integrationtests/demo_tests/direct_offboard_posctl_test.py index d71354059a..734bcf5909 100755 --- a/integrationtests/demo_tests/direct_offboard_posctl_test.py +++ b/integrationtests/demo_tests/direct_offboard_posctl_test.py @@ -48,9 +48,11 @@ from px4.msg import vehicle_local_position from px4.msg import vehicle_control_mode from px4.msg import position_setpoint_triplet from px4.msg import position_setpoint +from px4.msg import vehicle_local_position_setpoint from manual_input import ManualInput from flight_path_assertion import FlightPathAssertion +from px4_test_helper import PX4TestHelper # # Tests flying a path in offboard control by directly sending setpoints @@ -63,7 +65,9 @@ class DirectOffboardPosctlTest(unittest.TestCase): def setUp(self): rospy.init_node('test_node', anonymous=True) - self.bag = rosbag.Bag('direct_offboard_posctl_test.bag', 'w', compression="lz4") + self.helper = PX4TestHelper("direct_offboard_posctl_test") + self.helper.setUp() + rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) self.sub_vlp = rospy.Subscriber("iris/vehicle_local_position", vehicle_local_position, self.position_callback) self.pub_spt = rospy.Publisher('iris/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) @@ -76,17 +80,15 @@ class DirectOffboardPosctlTest(unittest.TestCase): if self.fpa: self.fpa.stop() - self.sub_vlp.unregister() - self.rate.sleep() - self.bag.close() + self.helper.tearDown() # # General callback functions used in tests # + def position_callback(self, data): self.has_pos = True self.local_position = data - self.bag.write('vehicle_local_position', data) def vehicle_control_mode_callback(self, data): self.control_mode = data @@ -112,6 +114,7 @@ class DirectOffboardPosctlTest(unittest.TestCase): stp = position_setpoint_triplet() stp.current = pos self.pub_spt.publish(stp) + self.helper.bag_write('px4/position_setpoint_triplet', stp) # does it reach the position in X seconds? count = 0 diff --git a/integrationtests/demo_tests/mavros_offboard_attctl_test.py b/integrationtests/demo_tests/mavros_offboard_attctl_test.py index aa6bd8d9f2..ef875ce61e 100755 --- a/integrationtests/demo_tests/mavros_offboard_attctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_attctl_test.py @@ -43,10 +43,13 @@ import rosbag from px4.msg import vehicle_control_mode from px4.msg import vehicle_local_position +from px4.msg import vehicle_attitude_setpoint +from px4.msg import vehicle_attitude from std_msgs.msg import Header from std_msgs.msg import Float64 from geometry_msgs.msg import PoseStamped, Quaternion from tf.transformations import quaternion_from_euler +from px4_test_helper import PX4TestHelper # # Tests flying a path in offboard control by sending attitude and thrust setpoints @@ -58,10 +61,11 @@ class MavrosOffboardAttctlTest(unittest.TestCase): def setUp(self): rospy.init_node('test_node', anonymous=True) - self.bag = rosbag.Bag('mavros_offboard_attctl_test.bag', 'w', compression="lz4") rospy.wait_for_service('iris/mavros/cmd/arming', 30) + self.helper = PX4TestHelper("mavros_offboard_attctl_test") + self.helper.setUp() + rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) - self.sub_vlp = rospy.Subscriber("iris/vehicle_local_position", vehicle_local_position, self.vehicle_position_callback) rospy.Subscriber("iris/mavros/position/local", PoseStamped, self.position_callback) self.pub_att = rospy.Publisher('iris/mavros/setpoint/attitude', PoseStamped, queue_size=10) self.pub_thr = rospy.Publisher('iris/mavros/setpoint/att_throttle', Float64, queue_size=10) @@ -71,16 +75,11 @@ class MavrosOffboardAttctlTest(unittest.TestCase): self.local_position = PoseStamped() def tearDown(self): - self.sub_vlp.unregister() - self.rate.sleep() - self.bag.close() + self.helper.tearDown() # # General callback functions used in tests # - def vehicle_position_callback(self, data): - self.bag.write('vehicle_local_position', data) - def position_callback(self, data): self.has_pos = True self.local_position = data @@ -111,7 +110,9 @@ class MavrosOffboardAttctlTest(unittest.TestCase): att.header.stamp = rospy.Time.now() self.pub_att.publish(att) + self.helper.bag_write('mavros/setpoint/attitude', att) self.pub_thr.publish(throttle) + self.helper.bag_write('mavros/setpoint/att_throttle', throttle) self.rate.sleep() if (self.local_position.pose.position.x > 5 diff --git a/integrationtests/demo_tests/mavros_offboard_posctl_test.py b/integrationtests/demo_tests/mavros_offboard_posctl_test.py index f244d9d077..1383cd04cd 100755 --- a/integrationtests/demo_tests/mavros_offboard_posctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_posctl_test.py @@ -47,9 +47,11 @@ import numpy as np from px4.msg import vehicle_control_mode from px4.msg import vehicle_local_position +from px4.msg import vehicle_local_position_setpoint from std_msgs.msg import Header from geometry_msgs.msg import PoseStamped, Quaternion from tf.transformations import quaternion_from_euler +from px4_test_helper import PX4TestHelper # # Tests flying a path in offboard control by sending position setpoints @@ -62,9 +64,10 @@ class MavrosOffboardPosctlTest(unittest.TestCase): def setUp(self): rospy.init_node('test_node', anonymous=True) - self.bag = rosbag.Bag('mavros_offboard_posctl_test.bag', 'w', compression="lz4") + self.helper = PX4TestHelper("mavros_offboard_posctl_test") + self.helper.setUp() + rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) - self.sub_vlp = rospy.Subscriber("iris/vehicle_local_position", vehicle_local_position, self.vehicle_position_callback) rospy.Subscriber("iris/mavros/position/local", PoseStamped, self.position_callback) self.pub_spt = rospy.Publisher('iris/mavros/setpoint/local_position', PoseStamped, queue_size=10) self.rate = rospy.Rate(10) # 10hz @@ -73,16 +76,11 @@ class MavrosOffboardPosctlTest(unittest.TestCase): self.control_mode = vehicle_control_mode() def tearDown(self): - self.sub_vlp.unregister() - self.rate.sleep() - self.bag.close() + self.helper.tearDown() # # General callback functions used in tests # - def vehicle_position_callback(self, data): - self.bag.write('vehicle_local_position', data) - def position_callback(self, data): self.has_pos = True self.local_position = data @@ -130,6 +128,7 @@ class MavrosOffboardPosctlTest(unittest.TestCase): # update timestamp for each published SP pos.header.stamp = rospy.Time.now() self.pub_spt.publish(pos) + self.helper.bag_write('mavros/setpoint/local_position', pos) if self.is_at_position(pos.pose.position.x, pos.pose.position.y, pos.pose.position.z, 0.5): break diff --git a/integrationtests/demo_tests/px4_test_helper.py b/integrationtests/demo_tests/px4_test_helper.py new file mode 100644 index 0000000000..4dc8866341 --- /dev/null +++ b/integrationtests/demo_tests/px4_test_helper.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python +#*************************************************************************** +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +#***************************************************************************/ + +# +# @author Andreas Antener +# +PKG = 'px4' + +import unittest +import rospy +import rosbag + +from px4.msg import vehicle_local_position +from px4.msg import vehicle_attitude_setpoint +from px4.msg import vehicle_attitude +from px4.msg import vehicle_local_position_setpoint + +from threading import Condition + +# +# Test helper +# +class PX4TestHelper(object): + + def __init__(self, test_name): + self.test_name = test_name + + def setUp(self): + self.condition = Condition() + self.closed = False + + rospy.init_node('test_node', anonymous=True) + self.bag = rosbag.Bag(self.test_name + '.bag', 'w', compression="lz4") + + self.sub_vlp = rospy.Subscriber("iris/vehicle_local_position", + vehicle_local_position, self.vehicle_position_callback) + self.sub_vasp = rospy.Subscriber("iris/vehicle_attitude_setpoint", + vehicle_attitude_setpoint, self.vehicle_attitude_setpoint_callback) + self.sub_va = rospy.Subscriber("iris/vehicle_attitude", + vehicle_attitude, self.vehicle_attitude_callback) + self.sub_vlps = rospy.Subscriber("iris/vehicle_local_position_setpoint", + vehicle_local_position_setpoint, self.vehicle_local_position_setpoint_callback) + + + def tearDown(self): + try: + self.condition.acquire() + self.closed = True + + self.sub_vlp.unregister() + self.sub_vasp.unregister() + self.sub_va.unregister() + self.sub_vlps.unregister() + self.bag.close() + + finally: + self.condition.release() + + def vehicle_position_callback(self, data): + self.bag_write('px4/vehicle_local_position', data) + + def vehicle_attitude_setpoint_callback(self, data): + self.bag_write('px4/vehicle_attitude_setpoint', data) + + def vehicle_attitude_callback(self, data): + self.bag_write('px4/vehicle_attitude', data) + + def vehicle_local_position_setpoint_callback(self, data): + self.bag_write('px4/vehicle_local_position_setpoint', data) + + def bag_write(self, topic, data): + try: + self.condition.acquire() + if not self.closed: + self.bag.write(topic, data) + else: + rospy.logwarn("Trying to write to bag but it's already closed") + finally: + self.condition.release() + From 23075f6dab4fc3584cd17eed44196169488af626 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 18 Mar 2015 20:05:45 +0900 Subject: [PATCH 066/268] batt_smbus: calculate current discharged --- src/drivers/batt_smbus/batt_smbus.cpp | 25 +++++++++++++++++++++++-- 1 file changed, 23 insertions(+), 2 deletions(-) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index a958fc60d3..4fd82de49c 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -84,6 +84,7 @@ #define BATT_SMBUS_ADDR 0x0B ///< I2C address #define BATT_SMBUS_TEMP 0x08 ///< temperature register #define BATT_SMBUS_VOLTAGE 0x09 ///< voltage register +#define BATT_SMBUS_REMAINING_CAPACITY 0x0f ///< predicted remaining battery capacity as a percentage #define BATT_SMBUS_DESIGN_CAPACITY 0x18 ///< design capacity register #define BATT_SMBUS_DESIGN_VOLTAGE 0x19 ///< design voltage register #define BATT_SMBUS_SERIALNUM 0x1c ///< serial number register @@ -179,6 +180,7 @@ private: orb_advert_t _batt_topic; ///< uORB battery topic orb_id_t _batt_orb_id; ///< uORB battery topic ID uint64_t _start_time; ///< system time we first attempt to communicate with battery + uint16_t _batt_design_capacity; ///< battery's design capacity in mAh (0 means unknown) }; namespace @@ -197,7 +199,8 @@ BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) : _reports(nullptr), _batt_topic(-1), _batt_orb_id(nullptr), - _start_time(0) + _start_time(0), + _batt_design_capacity(0) { // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); @@ -263,7 +266,7 @@ BATT_SMBUS::test() if (updated) { if (orb_copy(ORB_ID(battery_status), sub, &status) == OK) { - warnx("V=%4.2f C=%4.2f", status.voltage_v, status.current_a); + warnx("V=%4.2f C=%4.2f DismAh=%4.2f", (float)status.voltage_v, (float)status.current_a, (float)status.discharged_mah); } } @@ -371,6 +374,24 @@ BATT_SMBUS::cycle() new_report.current_a = -(float)((int32_t)((uint32_t)buff[3] << 24 | (uint32_t)buff[2] << 16 | (uint32_t)buff[1] << 8 | (uint32_t)buff[0])) / 1000.0f; } + // read battery design capacity + if (_batt_design_capacity == 0) { + usleep(1); + if (read_reg(BATT_SMBUS_DESIGN_CAPACITY, tmp) == OK) { + _batt_design_capacity = tmp; + } + } + + // read remaining capacity + if (_batt_design_capacity > 0) { + usleep(1); + if (read_reg(BATT_SMBUS_REMAINING_CAPACITY, tmp) == OK) { + if (tmp < _batt_design_capacity) { + new_report.discharged_mah = _batt_design_capacity - tmp; + } + } + } + // publish to orb if (_batt_topic != -1) { orb_publish(_batt_orb_id, _batt_topic, &new_report); From 4e83f43ea3f66c02d3f982948728f611f2daa220 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 19 Mar 2015 11:54:21 +0900 Subject: [PATCH 067/268] batt_smbus: remove usleep Also restore I2C address after performing command line search for devices --- src/drivers/batt_smbus/batt_smbus.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 4fd82de49c..9f72bd56fc 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -282,6 +282,7 @@ BATT_SMBUS::search() { bool found_slave = false; uint16_t tmp; + int16_t orig_addr = get_address(); // search through all valid SMBus addresses for (uint8_t i = BATT_SMBUS_ADDR_MIN; i <= BATT_SMBUS_ADDR_MAX; i++) { @@ -296,6 +297,9 @@ BATT_SMBUS::search() usleep(1); } + // restore original i2c address + set_address(orig_addr); + // display completion message if (found_slave) { warnx("Done."); @@ -367,7 +371,6 @@ BATT_SMBUS::cycle() new_report.voltage_v = ((float)tmp) / 1000.0f; // read current - usleep(1); uint8_t buff[4]; if (read_block(BATT_SMBUS_CURRENT, buff, 4, false) == 4) { @@ -376,7 +379,6 @@ BATT_SMBUS::cycle() // read battery design capacity if (_batt_design_capacity == 0) { - usleep(1); if (read_reg(BATT_SMBUS_DESIGN_CAPACITY, tmp) == OK) { _batt_design_capacity = tmp; } @@ -384,7 +386,6 @@ BATT_SMBUS::cycle() // read remaining capacity if (_batt_design_capacity > 0) { - usleep(1); if (read_reg(BATT_SMBUS_REMAINING_CAPACITY, tmp) == OK) { if (tmp < _batt_design_capacity) { new_report.discharged_mah = _batt_design_capacity - tmp; @@ -451,8 +452,6 @@ BATT_SMBUS::read_block(uint8_t reg, uint8_t *data, uint8_t max_len, bool append_ { uint8_t buff[max_len + 2]; // buffer to hold results - usleep(1); - // read bytes including PEC int ret = transfer(®, 1, buff, max_len + 2); From b78a021fb9bfc32612f0a5095b2bba62f9e8a996 Mon Sep 17 00:00:00 2001 From: dogmaphobic Date: Thu, 19 Mar 2015 12:07:15 -0400 Subject: [PATCH 068/268] Adding a few missing units to the Battery Calibration group. --- src/modules/commander/commander_params.c | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 1b0c4258b2..94eeb6f71f 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -55,6 +55,7 @@ PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); * Defines the voltage where a single cell of the battery is considered empty. * * @group Battery Calibration + * @unit V */ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); @@ -64,6 +65,7 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); * Defines the voltage where a single cell of the battery is considered full. * * @group Battery Calibration + * @unit V */ PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f); @@ -74,6 +76,7 @@ PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f); * to maximum current ratio and assumes linearity. * * @group Battery Calibration + * @unit V */ PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f); @@ -83,6 +86,7 @@ PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f); * Defines the number of cells the attached battery consists of. * * @group Battery Calibration + * @unit S */ PARAM_DEFINE_INT32(BAT_N_CELLS, 3); @@ -92,6 +96,7 @@ PARAM_DEFINE_INT32(BAT_N_CELLS, 3); * Defines the capacity of the attached battery. * * @group Battery Calibration + * @unit mA */ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); From e6688826212aec1c33a042593206fbcea45f112d Mon Sep 17 00:00:00 2001 From: orlando3d Date: Thu, 19 Mar 2015 09:39:44 -0800 Subject: [PATCH 069/268] Turn on PWM output for PPM loopback test --- src/systemcmds/tests/test_ppm_loopback.c | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/src/systemcmds/tests/test_ppm_loopback.c b/src/systemcmds/tests/test_ppm_loopback.c index a753f45c23..6866531d78 100644 --- a/src/systemcmds/tests/test_ppm_loopback.c +++ b/src/systemcmds/tests/test_ppm_loopback.c @@ -105,16 +105,15 @@ int test_ppm_loopback(int argc, char *argv[]) int pwm_values[] = {1200, 1300, 1900, 1700, 1500, 1250, 1800, 1400}; - // for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) { - // result = ioctl(servo_fd, PWM_SERVO_SET(i), pwm_values[i]); - - // if (result) { - // (void)close(servo_fd); - // return ERROR; - // } else { - // warnx("channel %d set to %d", i, pwm_values[i]); - // } - // } + for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) { + result = ioctl(servo_fd, PWM_SERVO_SET(i), pwm_values[i]); + if (result) { + (void)close(servo_fd); + return ERROR; + } else { + warnx("channel %d set to %d", i, pwm_values[i]); + } + } warnx("servo count: %d", servo_count); From c5ae02deda839919841dee5c68db564c0b74c690 Mon Sep 17 00:00:00 2001 From: Michal Ulianko Date: Thu, 19 Mar 2015 19:50:05 +0100 Subject: [PATCH 070/268] Fix generating $3_len in BIN_TO_OBJ makefile function --- makefiles/toolchain_gnu-arm-eabi.mk | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 2c53028124..93ee235c4b 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -296,6 +296,9 @@ endef # # - compile an empty file to generate a suitable object file # - relink the object and insert the binary file +# - extract the length +# - create const unsigned $3_len with the extracted length as its value and compile it to an object file +# - link the two generated object files together # - edit symbol names to suit # # NOTE: exercise caution using this with absolute pathnames; it looks @@ -320,11 +323,14 @@ define BIN_TO_OBJ @$(MKDIR) -p $(dir $2) $(Q) $(ECHO) > $2.c $(call COMPILE,$2.c,$2.c.o) - $(Q) $(LD) -r -o $2 $2.c.o -b binary $1 + $(Q) $(LD) -r -o $2.bin.o $2.c.o -b binary $1 + $(Q) $(ECHO) "const unsigned int $3_len = 0x`$(NM) $2.bin.o | egrep $(call BIN_SYM_PREFIX,$1)_size$$ | cut -d' ' -f1`;" > $2.c + $(call COMPILE,$2.c,$2.c.o) + $(Q) $(LD) -r -o $2 $2.c.o $2.bin.o $(Q) $(OBJCOPY) $2 \ --redefine-sym $(call BIN_SYM_PREFIX,$1)_start=$3 \ - --redefine-sym $(call BIN_SYM_PREFIX,$1)_size=$3_len \ + --strip-symbol $(call BIN_SYM_PREFIX,$1)_size \ --strip-symbol $(call BIN_SYM_PREFIX,$1)_end \ --rename-section .data=.rodata - $(Q) $(REMOVE) $2.c $2.c.o + $(Q) $(REMOVE) $2.c $2.c.o $2.bin.o endef From b55fe24161ef60c7329494ab741263b9b01fe19c Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 2 Mar 2015 14:32:39 -0500 Subject: [PATCH 071/268] unittests fix code style --- unittests/autodeclination_test.cpp | 5 ++-- unittests/conversion_test.cpp | 5 ++-- unittests/hrt.cpp | 16 ++++++------ unittests/mixer_test.cpp | 5 ++-- unittests/param_test.cpp | 39 ++++++++++++++++++------------ unittests/queue.h | 32 +++++++++++------------- unittests/sbus2_test.cpp | 10 +++++--- unittests/sf0x_test.cpp | 3 ++- unittests/st24_test.cpp | 5 ++-- unittests/uorb_stub.cpp | 6 +++-- 10 files changed, 71 insertions(+), 55 deletions(-) diff --git a/unittests/autodeclination_test.cpp b/unittests/autodeclination_test.cpp index a27d5f1008..8c89243f6e 100644 --- a/unittests/autodeclination_test.cpp +++ b/unittests/autodeclination_test.cpp @@ -1,6 +1,6 @@ #include #include -#include +#include #include #include @@ -12,6 +12,7 @@ #include "gtest/gtest.h" -TEST(AutoDeclinationTest, AutoDeclination) { +TEST(AutoDeclinationTest, AutoDeclination) +{ ASSERT_NEAR(get_mag_declination(47.0, 8.0), 0.6, 0.5) << "declination differs more than 1 degree"; } diff --git a/unittests/conversion_test.cpp b/unittests/conversion_test.cpp index 12d2213e2c..7a2909dc34 100644 --- a/unittests/conversion_test.cpp +++ b/unittests/conversion_test.cpp @@ -4,6 +4,7 @@ #include "gtest/gtest.h" -TEST(ConversionTest, quad_w_main) { - ASSERT_EQ(test_conv(0, NULL), 0) << "Conversion test failed"; +TEST(ConversionTest, quad_w_main) +{ + ASSERT_EQ(test_conv(0, NULL), 0) << "Conversion test failed"; } diff --git a/unittests/hrt.cpp b/unittests/hrt.cpp index 01b5958b7c..d7b4670db1 100644 --- a/unittests/hrt.cpp +++ b/unittests/hrt.cpp @@ -3,14 +3,16 @@ #include #include -hrt_abstime hrt_absolute_time() { - struct timeval te; - gettimeofday(&te, NULL); // get current time - hrt_abstime us = static_cast(te.tv_sec) * 1e6 + te.tv_usec; // caculate us - return us; +hrt_abstime hrt_absolute_time() +{ + struct timeval te; + gettimeofday(&te, NULL); // get current time + hrt_abstime us = static_cast(te.tv_sec) * 1e6 + te.tv_usec; // caculate us + return us; } -hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then) { +hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then) +{ // not thread safe - return hrt_absolute_time() - *then; + return hrt_absolute_time() - *then; } diff --git a/unittests/mixer_test.cpp b/unittests/mixer_test.cpp index 1271dab5e0..5be4cb032b 100644 --- a/unittests/mixer_test.cpp +++ b/unittests/mixer_test.cpp @@ -5,7 +5,8 @@ #include "gtest/gtest.h" -TEST(MixerTest, Mixer) { - char* args[] = {"empty", "../ROMFS/px4fmu_common/mixers/IO_pass.mix", "../ROMFS/px4fmu_common/mixers/quad_w.main.mix"}; +TEST(MixerTest, Mixer) +{ + char *args[] = {"empty", "../ROMFS/px4fmu_common/mixers/IO_pass.mix", "../ROMFS/px4fmu_common/mixers/quad_w.main.mix"}; ASSERT_EQ(test_mixer(3, args), 0) << "IO_pass.mix failed"; } diff --git a/unittests/param_test.cpp b/unittests/param_test.cpp index 5ea6bcc602..44ae4df068 100644 --- a/unittests/param_test.cpp +++ b/unittests/param_test.cpp @@ -13,7 +13,8 @@ struct param_info_s *param_info_limit; /* * Adds test parameters */ -void _add_parameters() { +void _add_parameters() +{ struct param_info_s test_1 = { "TEST_1", PARAM_TYPE_INT32 @@ -44,17 +45,19 @@ void _add_parameters() { param_array[3] = rc2_x; param_info_base = (struct param_info_s *) ¶m_array[0]; param_info_limit = (struct param_info_s *) ¶m_array[4]; // needs to point at the end of the data, - // therefore number of params + 1 + // therefore number of params + 1 } -void _assert_parameter_int_value(param_t param, int32_t expected) { +void _assert_parameter_int_value(param_t param, int32_t expected) +{ int32_t value; int result = param_get(param, &value); ASSERT_EQ(0, result) << printf("param_get (%i) did not return parameter\n", param); ASSERT_EQ(expected, value) << printf("value for param (%i) doesn't match default value\n", param); } -void _set_all_int_parameters_to(int32_t value) { +void _set_all_int_parameters_to(int32_t value) +{ param_set((param_t)0, &value); param_set((param_t)1, &value); param_set((param_t)2, &value); @@ -66,9 +69,10 @@ void _set_all_int_parameters_to(int32_t value) { _assert_parameter_int_value((param_t)3, value); } -TEST(ParamTest, SimpleFind) { +TEST(ParamTest, SimpleFind) +{ _add_parameters(); - + param_t param = param_find("TEST_2"); ASSERT_NE(PARAM_INVALID, param) << "param_find did not find parameter"; @@ -78,7 +82,8 @@ TEST(ParamTest, SimpleFind) { ASSERT_EQ(4, value) << "value of returned parameter does not match"; } -TEST(ParamTest, ResetAll) { +TEST(ParamTest, ResetAll) +{ _add_parameters(); _set_all_int_parameters_to(50); @@ -90,11 +95,12 @@ TEST(ParamTest, ResetAll) { _assert_parameter_int_value((param_t)3, 16); } -TEST(ParamTest, ResetAllExcludesOne) { +TEST(ParamTest, ResetAllExcludesOne) +{ _add_parameters(); _set_all_int_parameters_to(50); - const char* excludes[] = {"RC_X"}; + const char *excludes[] = {"RC_X"}; param_reset_excludes(excludes, 1); _assert_parameter_int_value((param_t)0, 2); @@ -103,11 +109,12 @@ TEST(ParamTest, ResetAllExcludesOne) { _assert_parameter_int_value((param_t)3, 16); } -TEST(ParamTest, ResetAllExcludesTwo) { +TEST(ParamTest, ResetAllExcludesTwo) +{ _add_parameters(); _set_all_int_parameters_to(50); - const char* excludes[] = {"RC_X", "TEST_1"}; + const char *excludes[] = {"RC_X", "TEST_1"}; param_reset_excludes(excludes, 2); _assert_parameter_int_value((param_t)0, 50); @@ -116,11 +123,12 @@ TEST(ParamTest, ResetAllExcludesTwo) { _assert_parameter_int_value((param_t)3, 16); } -TEST(ParamTest, ResetAllExcludesBoundaryCheck) { +TEST(ParamTest, ResetAllExcludesBoundaryCheck) +{ _add_parameters(); _set_all_int_parameters_to(50); - const char* excludes[] = {"RC_X", "TEST_1"}; + const char *excludes[] = {"RC_X", "TEST_1"}; param_reset_excludes(excludes, 1); _assert_parameter_int_value((param_t)0, 2); @@ -129,11 +137,12 @@ TEST(ParamTest, ResetAllExcludesBoundaryCheck) { _assert_parameter_int_value((param_t)3, 16); } -TEST(ParamTest, ResetAllExcludesWildcard) { +TEST(ParamTest, ResetAllExcludesWildcard) +{ _add_parameters(); _set_all_int_parameters_to(50); - const char* excludes[] = {"RC*"}; + const char *excludes[] = {"RC*"}; param_reset_excludes(excludes, 1); _assert_parameter_int_value((param_t)0, 2); diff --git a/unittests/queue.h b/unittests/queue.h index 0fdb170db6..e056bc263e 100644 --- a/unittests/queue.h +++ b/unittests/queue.h @@ -67,30 +67,26 @@ * Global Type Declarations ************************************************************************/ -struct sq_entry_s -{ - FAR struct sq_entry_s *flink; +struct sq_entry_s { + FAR struct sq_entry_s *flink; }; typedef struct sq_entry_s sq_entry_t; -struct dq_entry_s -{ - FAR struct dq_entry_s *flink; - FAR struct dq_entry_s *blink; +struct dq_entry_s { + FAR struct dq_entry_s *flink; + FAR struct dq_entry_s *blink; }; typedef struct dq_entry_s dq_entry_t; -struct sq_queue_s -{ - FAR sq_entry_t *head; - FAR sq_entry_t *tail; +struct sq_queue_s { + FAR sq_entry_t *head; + FAR sq_entry_t *tail; }; typedef struct sq_queue_s sq_queue_t; -struct dq_queue_s -{ - FAR dq_entry_t *head; - FAR dq_entry_t *tail; +struct dq_queue_s { + FAR dq_entry_t *head; + FAR dq_entry_t *tail; }; typedef struct dq_queue_s dq_queue_t; @@ -110,11 +106,11 @@ EXTERN void dq_addfirst(FAR dq_entry_t *node, dq_queue_t *queue); EXTERN void sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue); EXTERN void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue); EXTERN void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node, - sq_queue_t *queue); + sq_queue_t *queue); EXTERN void dq_addafter(FAR dq_entry_t *prev, FAR dq_entry_t *node, - dq_queue_t *queue); + dq_queue_t *queue); EXTERN void dq_addbefore(FAR dq_entry_t *next, FAR dq_entry_t *node, - dq_queue_t *queue); + dq_queue_t *queue); EXTERN FAR sq_entry_t *sq_remafter(FAR sq_entry_t *node, sq_queue_t *queue); EXTERN void sq_rem(FAR sq_entry_t *node, sq_queue_t *queue); diff --git a/unittests/sbus2_test.cpp b/unittests/sbus2_test.cpp index 15a21c86b7..41f49627d4 100644 --- a/unittests/sbus2_test.cpp +++ b/unittests/sbus2_test.cpp @@ -10,11 +10,12 @@ #include "gtest/gtest.h" -TEST(SBUS2Test, SBUS2) { +TEST(SBUS2Test, SBUS2) +{ const char *filepath = "testdata/sbus2_r7008SB.txt"; FILE *fp; - fp = fopen(filepath,"rt"); + fp = fopen(filepath, "rt"); ASSERT_TRUE(fp); warnx("loading data from: %s", filepath); @@ -51,8 +52,9 @@ TEST(SBUS2Test, SBUS2) { //warnx("%f: 0x%02x, first: 0x%02x, last: 0x%02x, pcount: %u", (double)f, x, frame[0], frame[24], partial_frame_count); - if (partial_frame_count == sizeof(frame)) + if (partial_frame_count == sizeof(frame)) { partial_frame_count = 0; + } last_time = f; @@ -60,7 +62,7 @@ TEST(SBUS2Test, SBUS2) { hrt_abstime now = hrt_absolute_time(); //if (partial_frame_count % 25 == 0) - //sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels); + //sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels); } ASSERT_EQ(ret, EOF); diff --git a/unittests/sf0x_test.cpp b/unittests/sf0x_test.cpp index b161f1ffd0..56e673a06d 100644 --- a/unittests/sf0x_test.cpp +++ b/unittests/sf0x_test.cpp @@ -9,7 +9,8 @@ #include "gtest/gtest.h" -TEST(SF0XTest, SF0X) { +TEST(SF0XTest, SF0X) +{ const char _LINE_MAX = 20; char _linebuf[_LINE_MAX]; _linebuf[0] = '\0'; diff --git a/unittests/st24_test.cpp b/unittests/st24_test.cpp index 89c7ffb1c3..42fa07ae74 100644 --- a/unittests/st24_test.cpp +++ b/unittests/st24_test.cpp @@ -8,8 +8,9 @@ #include "gtest/gtest.h" -TEST(ST24Test, ST24) { - const char* filepath = "testdata/st24_data.txt"; +TEST(ST24Test, ST24) +{ + const char *filepath = "testdata/st24_data.txt"; warnx("loading data from: %s", filepath); diff --git a/unittests/uorb_stub.cpp b/unittests/uorb_stub.cpp index fc039a4086..e4269afc32 100644 --- a/unittests/uorb_stub.cpp +++ b/unittests/uorb_stub.cpp @@ -10,11 +10,13 @@ * TODO: use googlemock ******************************************/ -orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) { +orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) +{ return (orb_advert_t)0; } -int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) { +int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) +{ return 0; } From c147424fe735d92c5271ba3b5bc830bb33fb6097 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 2 Mar 2015 14:34:08 -0500 Subject: [PATCH 072/268] nuttx-configs fix code style --- nuttx-configs/aerocore/include/board.h | 4 ++-- nuttx-configs/aerocore/src/empty.c | 2 +- nuttx-configs/px4fmu-v1/include/board.h | 2 +- nuttx-configs/px4fmu-v1/src/empty.c | 2 +- nuttx-configs/px4fmu-v2/include/board.h | 14 +++++++------- nuttx-configs/px4fmu-v2/src/empty.c | 2 +- nuttx-configs/px4io-v1/include/board.h | 2 +- nuttx-configs/px4io-v1/src/empty.c | 2 +- nuttx-configs/px4io-v2/include/board.h | 2 +- nuttx-configs/px4io-v2/src/empty.c | 2 +- 10 files changed, 17 insertions(+), 17 deletions(-) diff --git a/nuttx-configs/aerocore/include/board.h b/nuttx-configs/aerocore/include/board.h index 8705c1bc25..6dc85735a5 100644 --- a/nuttx-configs/aerocore/include/board.h +++ b/nuttx-configs/aerocore/include/board.h @@ -145,7 +145,7 @@ #define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY) /* Timer Frequencies, if APBx is set to 1, frequency is same to APBx - * otherwise frequency is 2xAPBx. + * otherwise frequency is 2xAPBx. * Note: TIM1,8 are on APB2, others on APB1 */ @@ -203,7 +203,7 @@ /* * SPI */ -/* PA[4-7] SPI1 broken out on J12 */ +/* PA[4-7] SPI1 broken out on J12 */ #define GPIO_SPI1_NSS (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) /* should be GPIO_SPI1_NSS_2 but use as a GPIO */ #define GPIO_SPI1_SCK (GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz) #define GPIO_SPI1_MISO (GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz) diff --git a/nuttx-configs/aerocore/src/empty.c b/nuttx-configs/aerocore/src/empty.c index ace900866c..5de10699fb 100644 --- a/nuttx-configs/aerocore/src/empty.c +++ b/nuttx-configs/aerocore/src/empty.c @@ -1,4 +1,4 @@ /* - * There are no source files here, but libboard.a can't be empty, so + * There are no source files here, but libboard.a can't be empty, so * we have this empty source file to keep it company. */ diff --git a/nuttx-configs/px4fmu-v1/include/board.h b/nuttx-configs/px4fmu-v1/include/board.h index ff1c634244..9e6d50a8b9 100644 --- a/nuttx-configs/px4fmu-v1/include/board.h +++ b/nuttx-configs/px4fmu-v1/include/board.h @@ -209,7 +209,7 @@ /* UART DMA configuration for USART1/6 */ #define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 #define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 - + /* * CAN * diff --git a/nuttx-configs/px4fmu-v1/src/empty.c b/nuttx-configs/px4fmu-v1/src/empty.c index ace900866c..5de10699fb 100644 --- a/nuttx-configs/px4fmu-v1/src/empty.c +++ b/nuttx-configs/px4fmu-v1/src/empty.c @@ -1,4 +1,4 @@ /* - * There are no source files here, but libboard.a can't be empty, so + * There are no source files here, but libboard.a can't be empty, so * we have this empty source file to keep it company. */ diff --git a/nuttx-configs/px4fmu-v2/include/board.h b/nuttx-configs/px4fmu-v2/include/board.h index 3b3c6fa707..52668cacd2 100755 --- a/nuttx-configs/px4fmu-v2/include/board.h +++ b/nuttx-configs/px4fmu-v2/include/board.h @@ -146,21 +146,21 @@ #define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY) /* Timer Frequencies, if APBx is set to 1, frequency is same to APBx - * otherwise frequency is 2xAPBx. + * otherwise frequency is 2xAPBx. * Note: TIM1,8 are on APB2, others on APB1 */ #define STM32_TIM18_FREQUENCY (2*STM32_PCLK2_FREQUENCY) #define STM32_TIM27_FREQUENCY (2*STM32_PCLK1_FREQUENCY) -/* SDIO dividers. Note that slower clocking is required when DMA is disabled +/* SDIO dividers. Note that slower clocking is required when DMA is disabled * in order to avoid RX overrun/TX underrun errors due to delayed responses * to service FIFOs in interrupt driven mode. These values have not been * tuned!!! * * HCLK=72MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(178+2)=400 KHz */ - + #define SDIO_INIT_CLKDIV (178 << SDIO_CLKCR_CLKDIV_SHIFT) /* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(2+2)=18 MHz @@ -168,9 +168,9 @@ */ #ifdef CONFIG_SDIO_DMA -# define SDIO_MMCXFR_CLKDIV (2 << SDIO_CLKCR_CLKDIV_SHIFT) +# define SDIO_MMCXFR_CLKDIV (2 << SDIO_CLKCR_CLKDIV_SHIFT) #else -# define SDIO_MMCXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT) +# define SDIO_MMCXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT) #endif /* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(1+2)=24 MHz @@ -227,10 +227,10 @@ #define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 #define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 -/* +/* * CAN * - * CAN1 is routed to the onboard transceiver. + * CAN1 is routed to the onboard transceiver. * CAN2 is routed to the expansion connector. */ #define GPIO_CAN1_RX GPIO_CAN1_RX_3 diff --git a/nuttx-configs/px4fmu-v2/src/empty.c b/nuttx-configs/px4fmu-v2/src/empty.c index ace900866c..5de10699fb 100644 --- a/nuttx-configs/px4fmu-v2/src/empty.c +++ b/nuttx-configs/px4fmu-v2/src/empty.c @@ -1,4 +1,4 @@ /* - * There are no source files here, but libboard.a can't be empty, so + * There are no source files here, but libboard.a can't be empty, so * we have this empty source file to keep it company. */ diff --git a/nuttx-configs/px4io-v1/include/board.h b/nuttx-configs/px4io-v1/include/board.h index 8150792665..ee8a9d1ba7 100755 --- a/nuttx-configs/px4io-v1/include/board.h +++ b/nuttx-configs/px4io-v1/include/board.h @@ -143,7 +143,7 @@ extern "C" { ************************************************************************************/ EXTERN void stm32_boardinitialize(void); - + #if defined(__cplusplus) } #endif diff --git a/nuttx-configs/px4io-v1/src/empty.c b/nuttx-configs/px4io-v1/src/empty.c index ace900866c..5de10699fb 100644 --- a/nuttx-configs/px4io-v1/src/empty.c +++ b/nuttx-configs/px4io-v1/src/empty.c @@ -1,4 +1,4 @@ /* - * There are no source files here, but libboard.a can't be empty, so + * There are no source files here, but libboard.a can't be empty, so * we have this empty source file to keep it company. */ diff --git a/nuttx-configs/px4io-v2/include/board.h b/nuttx-configs/px4io-v2/include/board.h index 17e77918b4..b70056f828 100755 --- a/nuttx-configs/px4io-v2/include/board.h +++ b/nuttx-configs/px4io-v2/include/board.h @@ -138,7 +138,7 @@ extern "C" { ************************************************************************************/ EXTERN void stm32_boardinitialize(void); - + #if defined(__cplusplus) } #endif diff --git a/nuttx-configs/px4io-v2/src/empty.c b/nuttx-configs/px4io-v2/src/empty.c index ace900866c..5de10699fb 100644 --- a/nuttx-configs/px4io-v2/src/empty.c +++ b/nuttx-configs/px4io-v2/src/empty.c @@ -1,4 +1,4 @@ /* - * There are no source files here, but libboard.a can't be empty, so + * There are no source files here, but libboard.a can't be empty, so * we have this empty source file to keep it company. */ From c2abb0f82ac23aab3295522ade4c255323191931 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 2 Mar 2015 14:36:04 -0500 Subject: [PATCH 073/268] fix code style if trivial one line difference --- src/drivers/boards/aerocore/board_config.h | 2 +- src/drivers/boards/px4fmu-v1/board_config.h | 2 +- src/drivers/boards/px4fmu-v2/board_config.h | 2 +- src/drivers/boards/px4io-v1/px4io_init.c | 2 +- src/drivers/boards/px4io-v2/px4iov2_init.c | 2 +- src/drivers/drv_airspeed.h | 2 +- src/drivers/drv_blinkm.h | 2 +- src/drivers/drv_orb_dev.h | 2 +- src/drivers/drv_oreoled.h | 2 +- src/drivers/drv_pwm_input.h | 2 +- src/drivers/hmc5883/hmc5883.h | 2 +- src/drivers/ms5611/ms5611.h | 2 +- src/examples/fixedwing_control/params.c | 2 +- src/examples/fixedwing_control/params.h | 2 +- .../flow_position_estimator/flow_position_estimator_params.c | 2 +- .../flow_position_estimator/flow_position_estimator_params.h | 2 +- src/lib/conversion/rotation.h | 2 +- src/lib/launchdetection/LaunchDetector.h | 2 +- src/lib/rc/st24.h | 2 +- .../attitude_estimator_ekf/attitude_estimator_ekf_params.c | 1 + src/modules/commander/calibration_routines.h | 3 ++- src/modules/fixedwing_backside/params.c | 2 +- src/modules/mavlink/mavlink_stream.cpp | 1 + src/modules/navigator/geofence.h | 2 +- src/modules/position_estimator_inav/inertial_filter.c | 1 + src/modules/px4iofirmware/sbus.c | 2 +- src/modules/sensors/sensor_params.c | 2 +- src/modules/systemlib/circuit_breaker.cpp | 2 +- src/modules/systemlib/circuit_breaker.h | 2 +- src/modules/systemlib/rc_check.h | 2 +- src/modules/uORB/topics/esc_status.h | 2 +- src/modules/uORB/topics/fence.h | 2 +- .../demo_offboard_position_setpoints.cpp | 1 + src/systemcmds/tests/test_jig_voltages.c | 2 +- src/systemcmds/tests/test_rc.c | 2 +- src/systemcmds/tests/test_uart_send.c | 2 +- src/systemcmds/usb_connected/usb_connected.c | 2 +- 37 files changed, 38 insertions(+), 33 deletions(-) diff --git a/src/drivers/boards/aerocore/board_config.h b/src/drivers/boards/aerocore/board_config.h index 776a2071ed..19518d73d9 100644 --- a/src/drivers/boards/aerocore/board_config.h +++ b/src/drivers/boards/aerocore/board_config.h @@ -54,7 +54,7 @@ __BEGIN_DECLS #include #define UDID_START 0x1FFF7A10 - + /**************************************************************************************************** * Definitions ****************************************************************************************************/ diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h index 882ec6aa2d..17fa9c542e 100644 --- a/src/drivers/boards/px4fmu-v1/board_config.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -52,7 +52,7 @@ __BEGIN_DECLS /* these headers are not C++ safe */ #include #include - + /**************************************************************************************************** * Definitions ****************************************************************************************************/ diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 273af1023a..6188e29ae1 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -54,7 +54,7 @@ __BEGIN_DECLS #include #define UDID_START 0x1FFF7A10 - + /**************************************************************************************************** * Definitions ****************************************************************************************************/ diff --git a/src/drivers/boards/px4io-v1/px4io_init.c b/src/drivers/boards/px4io-v1/px4io_init.c index 8292da9e1c..5a02a97164 100644 --- a/src/drivers/boards/px4io-v1/px4io_init.c +++ b/src/drivers/boards/px4io-v1/px4io_init.c @@ -87,7 +87,7 @@ __EXPORT void stm32_boardinitialize(void) stm32_configgpio(GPIO_RELAY1_EN); stm32_configgpio(GPIO_RELAY2_EN); - /* turn off - all leds are active low */ + /* turn off - all leds are active low */ stm32_gpiowrite(GPIO_LED1, true); stm32_gpiowrite(GPIO_LED2, true); stm32_gpiowrite(GPIO_LED3, true); diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c index 5c3343ccca..fd7eb33c3e 100644 --- a/src/drivers/boards/px4io-v2/px4iov2_init.c +++ b/src/drivers/boards/px4io-v2/px4iov2_init.c @@ -126,7 +126,7 @@ __EXPORT void stm32_boardinitialize(void) stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */ stm32_configgpio(GPIO_SBUS_OUTPUT); - + /* sbus output enable is active low - disable it by default */ stm32_gpiowrite(GPIO_SBUS_OENABLE, true); stm32_configgpio(GPIO_SBUS_OENABLE); diff --git a/src/drivers/drv_airspeed.h b/src/drivers/drv_airspeed.h index 2ff91d5d00..fddef36263 100644 --- a/src/drivers/drv_airspeed.h +++ b/src/drivers/drv_airspeed.h @@ -35,7 +35,7 @@ * @file drv_airspeed.h * * Airspeed driver interface. - * + * * @author Simon Wilks */ diff --git a/src/drivers/drv_blinkm.h b/src/drivers/drv_blinkm.h index 7258c9e841..8568436d35 100644 --- a/src/drivers/drv_blinkm.h +++ b/src/drivers/drv_blinkm.h @@ -60,7 +60,7 @@ /** play the numbered script in (arg), repeating forever */ #define BLINKM_PLAY_SCRIPT _BLINKMIOC(2) -/** +/** * Set the user script; (arg) is a pointer to an array of script lines, * where each line is an array of four bytes giving , , arg[0-2] * diff --git a/src/drivers/drv_orb_dev.h b/src/drivers/drv_orb_dev.h index e0b16fa5cd..c1db6b534b 100644 --- a/src/drivers/drv_orb_dev.h +++ b/src/drivers/drv_orb_dev.h @@ -36,7 +36,7 @@ /** * @file drv_orb_dev.h - * + * * uORB published object driver. */ diff --git a/src/drivers/drv_oreoled.h b/src/drivers/drv_oreoled.h index 0dcb10a7b1..00e3683184 100644 --- a/src/drivers/drv_oreoled.h +++ b/src/drivers/drv_oreoled.h @@ -117,7 +117,7 @@ enum oreoled_macro { OREOLED_PARAM_MACRO_ENUM_COUNT }; -/* +/* structure passed to OREOLED_SET_RGB ioctl() Note that the driver scales the brightness to 0 to 255, regardless of the hardware scaling diff --git a/src/drivers/drv_pwm_input.h b/src/drivers/drv_pwm_input.h index 778d9fcf5b..47b84e6e1e 100644 --- a/src/drivers/drv_pwm_input.h +++ b/src/drivers/drv_pwm_input.h @@ -46,6 +46,6 @@ __BEGIN_DECLS /* * Initialise the timer */ -__EXPORT extern int pwm_input_main(int argc, char * argv[]); +__EXPORT extern int pwm_input_main(int argc, char *argv[]); __END_DECLS diff --git a/src/drivers/hmc5883/hmc5883.h b/src/drivers/hmc5883/hmc5883.h index e91e91fc09..9607fe6149 100644 --- a/src/drivers/hmc5883/hmc5883.h +++ b/src/drivers/hmc5883/hmc5883.h @@ -50,4 +50,4 @@ /* interface factories */ extern device::Device *HMC5883_SPI_interface(int bus) weak_function; extern device::Device *HMC5883_I2C_interface(int bus) weak_function; -typedef device::Device* (*HMC5883_constructor)(int); +typedef device::Device *(*HMC5883_constructor)(int); diff --git a/src/drivers/ms5611/ms5611.h b/src/drivers/ms5611/ms5611.h index c8211b8dd1..c946e38cb8 100644 --- a/src/drivers/ms5611/ms5611.h +++ b/src/drivers/ms5611/ms5611.h @@ -82,4 +82,4 @@ extern bool crc4(uint16_t *n_prom); /* interface factories */ extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum) weak_function; extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint8_t busnum) weak_function; -typedef device::Device* (*MS5611_constructor)(ms5611::prom_u &prom_buf, uint8_t busnum); +typedef device::Device *(*MS5611_constructor)(ms5611::prom_u &prom_buf, uint8_t busnum); diff --git a/src/examples/fixedwing_control/params.c b/src/examples/fixedwing_control/params.c index c2e94ff3d3..fbbd30201e 100644 --- a/src/examples/fixedwing_control/params.c +++ b/src/examples/fixedwing_control/params.c @@ -34,7 +34,7 @@ /* * @file params.c - * + * * Parameters for fixedwing demo */ diff --git a/src/examples/fixedwing_control/params.h b/src/examples/fixedwing_control/params.h index 4ae8e90ec4..49ffff4467 100644 --- a/src/examples/fixedwing_control/params.h +++ b/src/examples/fixedwing_control/params.h @@ -34,7 +34,7 @@ /* * @file params.h - * + * * Definition of parameters for fixedwing example */ diff --git a/src/examples/flow_position_estimator/flow_position_estimator_params.c b/src/examples/flow_position_estimator/flow_position_estimator_params.c index ec3c3352d0..b565797874 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_params.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_params.c @@ -35,7 +35,7 @@ /* * @file flow_position_estimator_params.c - * + * * Parameters for position estimator */ diff --git a/src/examples/flow_position_estimator/flow_position_estimator_params.h b/src/examples/flow_position_estimator/flow_position_estimator_params.h index f9a9bb303b..3ab4e560fc 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_params.h +++ b/src/examples/flow_position_estimator/flow_position_estimator_params.h @@ -35,7 +35,7 @@ /* * @file flow_position_estimator_params.h - * + * * Parameters for position estimator */ diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 917c7f830e..13fe701dff 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -118,7 +118,7 @@ const rot_lookup_t rot_lookup[] = { * Get the rotation matrix */ __EXPORT void -get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix); +get_rot_matrix(enum Rotation rot, math::Matrix<3, 3> *rot_matrix); /** diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h index 4215b49d28..3b276c5563 100644 --- a/src/lib/launchdetection/LaunchDetector.h +++ b/src/lib/launchdetection/LaunchDetector.h @@ -76,7 +76,7 @@ private: method is checked for further adavancing in the state machine (e.g. when to power up the motors) */ - LaunchMethod* launchMethods[1]; + LaunchMethod *launchMethods[1]; control::BlockParamInt launchdetection_on; control::BlockParamFloat throttlePreTakeoff; diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h index 454234601d..0cf732824b 100644 --- a/src/lib/rc/st24.h +++ b/src/lib/rc/st24.h @@ -158,6 +158,6 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len); * @return 0 for success (a decoded packet), 1 for no packet yet (accumulating), 2 for unknown packet, 3 for out of sync, 4 for checksum error */ __EXPORT int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, - uint16_t *channels, uint16_t max_chan_count); + uint16_t *channels, uint16_t max_chan_count); __END_DECLS diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 5637ec102f..e143f37b9b 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -177,6 +177,7 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru for (int i = 0; i < 3; i++) { param_get(h->moment_inertia_J[i], &(p->moment_inertia_J[3 * i + i])); } + param_get(h->use_moment_inertia, &(p->use_moment_inertia)); return OK; diff --git a/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h index 3c8e49ee9e..ba4ca07ccb 100644 --- a/src/modules/commander/calibration_routines.h +++ b/src/modules/commander/calibration_routines.h @@ -57,4 +57,5 @@ * @return 0 on success, 1 on failure */ int sphere_fit_least_squares(const float x[], const float y[], const float z[], - unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius); \ No newline at end of file + unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, + float *sphere_radius); \ No newline at end of file diff --git a/src/modules/fixedwing_backside/params.c b/src/modules/fixedwing_backside/params.c index db30af4160..aa82a4f596 100644 --- a/src/modules/fixedwing_backside/params.c +++ b/src/modules/fixedwing_backside/params.c @@ -57,7 +57,7 @@ PARAM_DEFINE_FLOAT(FWB_V_CMD, 12.0f); // commanded velocity PARAM_DEFINE_FLOAT(FWB_V_MAX, 16.0f); // maximum commanded velocity // rate of climb -// this is what rate of climb is commanded (in m/s) +// this is what rate of climb is commanded (in m/s) // when the pitch stick is fully defelcted in simple mode PARAM_DEFINE_FLOAT(FWB_CR_MAX, 1.0f); diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 5b9e45d3ec..57a92c8b58 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -80,6 +80,7 @@ MavlinkStream::update(const hrt_abstime t) if (dt > 0 && dt >= interval) { /* interval expired, send message */ send(t); + if (const_rate()) { _last_sent = (t / _interval) * _interval; diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 9d647cb68a..37a41b68a3 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -76,7 +76,7 @@ public: * @return true: system is inside fence, false: system is outside fence */ bool inside(const struct vehicle_global_position_s &global_position, - const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl); + const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl); bool inside_polygon(double lat, double lon, float altitude); int clearDm(); diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c index a36a4688d0..c70cbeb0ea 100644 --- a/src/modules/position_estimator_inav/inertial_filter.c +++ b/src/modules/position_estimator_inav/inertial_filter.c @@ -15,6 +15,7 @@ void inertial_filter_predict(float dt, float x[2], float acc) if (!isfinite(acc)) { acc = 0.0f; } + x[0] += x[1] * dt + acc * dt * dt / 2.0f; x[1] += acc * dt; } diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index d76ec55f05..9d28490907 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -142,7 +142,7 @@ sbus1_output(uint16_t *values, uint16_t num_values) value = (uint16_t)(((values[i] - SBUS_SCALE_OFFSET) / SBUS_SCALE_FACTOR) + .5f); /*protect from out of bounds values and limit to 11 bits*/ - if (value > 0x07ff ) { + if (value > 0x07ff) { value = 0x07ff; } diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 272e4b14f3..1acc14fc6c 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -682,7 +682,7 @@ PARAM_DEFINE_INT32(SENS_FLOW_ROT, 0); * * @group Sensor Calibration */ - PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f); +PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f); /** * Board rotation X (Roll) offset diff --git a/src/modules/systemlib/circuit_breaker.cpp b/src/modules/systemlib/circuit_breaker.cpp index ea478a60fa..f5ff0afd4b 100644 --- a/src/modules/systemlib/circuit_breaker.cpp +++ b/src/modules/systemlib/circuit_breaker.cpp @@ -45,7 +45,7 @@ #include #include -bool circuit_breaker_enabled(const char* breaker, int32_t magic) +bool circuit_breaker_enabled(const char *breaker, int32_t magic) { int32_t val; (void)PX4_PARAM_GET_BYNAME(breaker, &val); diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index c97dbc26ff..c76e6c37f3 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -61,7 +61,7 @@ __BEGIN_DECLS -extern "C" __EXPORT bool circuit_breaker_enabled(const char* breaker, int32_t magic); +extern "C" __EXPORT bool circuit_breaker_enabled(const char *breaker, int32_t magic); __END_DECLS diff --git a/src/modules/systemlib/rc_check.h b/src/modules/systemlib/rc_check.h index e70b83ccee..035f63bef4 100644 --- a/src/modules/systemlib/rc_check.h +++ b/src/modules/systemlib/rc_check.h @@ -39,7 +39,7 @@ #pragma once - __BEGIN_DECLS +__BEGIN_DECLS /** * Check the RC calibration diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h index b45c49788d..73fe0f9361 100644 --- a/src/modules/uORB/topics/esc_status.h +++ b/src/modules/uORB/topics/esc_status.h @@ -92,7 +92,7 @@ struct esc_status_s { struct { enum ESC_VENDOR esc_vendor; /**< Vendor of current ESC */ uint32_t esc_errorcount; /**< Number of reported errors by ESC - if supported */ - int32_t esc_rpm; /**< Motor RPM, negative for reverse rotation [RPM] - if supported */ + int32_t esc_rpm; /**< Motor RPM, negative for reverse rotation [RPM] - if supported */ float esc_voltage; /**< Voltage measured from current ESC [V] - if supported */ float esc_current; /**< Current measured from current ESC [A] - if supported */ float esc_temperature; /**< Temperature measured from current ESC [degC] - if supported */ diff --git a/src/modules/uORB/topics/fence.h b/src/modules/uORB/topics/fence.h index a61f078ba1..43cee89fe8 100644 --- a/src/modules/uORB/topics/fence.h +++ b/src/modules/uORB/topics/fence.h @@ -65,7 +65,7 @@ struct fence_vertex_s { * This is the position of a geofence * */ -struct fence_s { +struct fence_s { unsigned count; /**< number of actual vertices */ struct fence_vertex_s vertices[GEOFENCE_MAX_VERTICES]; }; diff --git a/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp index 7366d7fc61..125ceaddc7 100644 --- a/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp +++ b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp @@ -66,6 +66,7 @@ int DemoOffboardPositionSetpoints::main() pose.pose.position.z = 1; _local_position_sp_pub.publish(pose); } + return 0; } diff --git a/src/systemcmds/tests/test_jig_voltages.c b/src/systemcmds/tests/test_jig_voltages.c index 98a105cb3e..554125302a 100644 --- a/src/systemcmds/tests/test_jig_voltages.c +++ b/src/systemcmds/tests/test_jig_voltages.c @@ -162,7 +162,7 @@ int test_jig_voltages(int argc, char *argv[]) errout_with_dev: - if (fd != 0) close(fd); + if (fd != 0) { close(fd); } return ret; } diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c index c9f9ef4398..3a8144077c 100644 --- a/src/systemcmds/tests/test_rc.c +++ b/src/systemcmds/tests/test_rc.c @@ -126,7 +126,7 @@ int test_rc(int argc, char *argv[]) warnx("TIMEOUT, less than 10 Hz updates"); (void)close(_rc_sub); return ERROR; - } + } } else { /* key pressed, bye bye */ diff --git a/src/systemcmds/tests/test_uart_send.c b/src/systemcmds/tests/test_uart_send.c index 7e1e8d307d..70a9c719e1 100644 --- a/src/systemcmds/tests/test_uart_send.c +++ b/src/systemcmds/tests/test_uart_send.c @@ -95,7 +95,7 @@ int test_uart_send(int argc, char *argv[]) /* input handling */ char *uart_name = "/dev/ttyS3"; - if (argc > 1) uart_name = argv[1]; + if (argc > 1) { uart_name = argv[1]; } /* assuming NuttShell is on UART1 (/dev/ttyS0) */ int test_uart = open(uart_name, O_RDWR | O_NONBLOCK | O_NOCTTY); // diff --git a/src/systemcmds/usb_connected/usb_connected.c b/src/systemcmds/usb_connected/usb_connected.c index 98bbbc4be9..530fee3405 100644 --- a/src/systemcmds/usb_connected/usb_connected.c +++ b/src/systemcmds/usb_connected/usb_connected.c @@ -52,5 +52,5 @@ __EXPORT int usb_connected_main(int argc, char *argv[]); int usb_connected_main(int argc, char *argv[]) { - return stm32_gpioread(GPIO_OTGFS_VBUS) ? 0:1; + return stm32_gpioread(GPIO_OTGFS_VBUS) ? 0 : 1; } From 0eaf41a048074d5ebf6b84094cfd69ef10451180 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 2 Mar 2015 20:49:27 -0500 Subject: [PATCH 074/268] check_code_style.sh properly ignore src/modules/attitude_estimator_ekf/codegen/* --- Tools/check_code_style.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/check_code_style.sh b/Tools/check_code_style.sh index 8d1ab6363b..2353a52326 100755 --- a/Tools/check_code_style.sh +++ b/Tools/check_code_style.sh @@ -3,7 +3,7 @@ set -eu failed=0 for fn in $(find . -path './src/lib/uavcan' -prune -o \ -path './src/lib/mathlib/CMSIS' -prune -o \ - -path './src/modules/attitude_estimator_ekf/codegen/' -prune -o \ + -path './src/modules/attitude_estimator_ekf/codegen' -prune -o \ -path './NuttX' -prune -o \ -path './Build' -prune -o \ -path './mavlink' -prune -o \ From 662ec3f62f6c271d7746204888c2d55e3c3bd86e Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 3 Mar 2015 11:47:27 -0500 Subject: [PATCH 075/268] examples fix code style --- src/examples/fixedwing_control/main.c | 43 +++-- .../flow_position_estimator_main.c | 181 ++++++++---------- .../matlab_csv_serial/matlab_csv_serial.c | 57 +++--- src/examples/publisher/publisher_example.h | 9 +- .../publisher/publisher_start_nuttx.cpp | 10 +- src/examples/px4_daemon_app/px4_daemon_app.c | 26 ++- src/examples/px4_simple_app/px4_simple_app.c | 16 +- src/examples/rover_steering_control/main.cpp | 12 +- .../subscriber/subscriber_example.cpp | 20 +- src/examples/subscriber/subscriber_example.h | 5 +- .../subscriber/subscriber_start_nuttx.cpp | 10 +- 11 files changed, 201 insertions(+), 188 deletions(-) diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index d8b777b91b..17b27290c2 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -112,8 +112,9 @@ static void usage(const char *reason); * @param att The current attitude. The controller should make the attitude match the setpoint * @param rates_sp The angular rate setpoint. This is the output of the controller. */ -void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, - struct actuator_controls_s *actuators); +void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, + struct vehicle_rates_setpoint_s *rates_sp, + struct actuator_controls_s *actuators); /** * Control heading. @@ -128,7 +129,7 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st * @param att_sp The attitude setpoint. This is the output of the controller */ void control_heading(const struct vehicle_global_position_s *pos, const struct position_setpoint_s *sp, - const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp); + const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp); /* Variables */ static bool thread_should_exit = false; /**< Daemon exit flag */ @@ -137,8 +138,9 @@ static int deamon_task; /**< Handle of deamon task / thread */ static struct params p; static struct param_handles ph; -void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, - struct actuator_controls_s *actuators) +void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, + struct vehicle_rates_setpoint_s *rates_sp, + struct actuator_controls_s *actuators) { /* @@ -175,7 +177,7 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st } void control_heading(const struct vehicle_global_position_s *pos, const struct position_setpoint_s *sp, - const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp) + const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp) { /* @@ -192,6 +194,7 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct p /* limit output, this commonly is a tuning parameter, too */ if (att_sp->roll_body < -0.6f) { att_sp->roll_body = -0.6f; + } else if (att_sp->roll_body > 0.6f) { att_sp->roll_body = 0.6f; } @@ -285,7 +288,8 @@ int fixedwing_control_thread_main(int argc, char *argv[]) /* Setup of loop */ struct pollfd fds[2] = {{ .fd = param_sub, .events = POLLIN }, - { .fd = att_sub, .events = POLLIN }}; + { .fd = att_sub, .events = POLLIN } + }; while (!thread_should_exit) { @@ -345,12 +349,14 @@ int fixedwing_control_thread_main(int argc, char *argv[]) if (manual_sp_updated) /* get the RC (or otherwise user based) input */ + { orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp); + } /* check if the throttle was ever more than 50% - go later only to failsafe if yes */ if (isfinite(manual_sp.z) && - (manual_sp.z >= 0.6f) && - (manual_sp.z <= 1.0f)) { + (manual_sp.z >= 0.6f) && + (manual_sp.z <= 1.0f)) { } /* get the system status and the flight mode we're in */ @@ -378,8 +384,9 @@ int fixedwing_control_thread_main(int argc, char *argv[]) thread_running = false; /* kill all outputs */ - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { actuators.control[i] = 0.0f; + } orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); @@ -393,8 +400,9 @@ int fixedwing_control_thread_main(int argc, char *argv[]) static void usage(const char *reason) { - if (reason) + if (reason) { fprintf(stderr, "%s\n", reason); + } fprintf(stderr, "usage: ex_fixedwing_control {start|stop|status}\n\n"); exit(1); @@ -410,8 +418,9 @@ usage(const char *reason) */ int ex_fixedwing_control_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 1) { usage("missing command"); + } if (!strcmp(argv[1], "start")) { @@ -423,11 +432,11 @@ int ex_fixedwing_control_main(int argc, char *argv[]) thread_should_exit = false; deamon_task = task_spawn_cmd("ex_fixedwing_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 20, - 2048, - fixedwing_control_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 20, + 2048, + fixedwing_control_thread_main, + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); thread_running = true; exit(0); } diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index da4ea825b0..f8e3f2dc5f 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -81,8 +81,10 @@ static void usage(const char *reason); static void usage(const char *reason) { - if (reason) + if (reason) { fprintf(stderr, "%s\n", reason); + } + fprintf(stderr, "usage: daemon {start|stop|status} [-p ]\n\n"); exit(1); } @@ -97,13 +99,12 @@ static void usage(const char *reason) */ int flow_position_estimator_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 1) { usage("missing command"); + } - if (!strcmp(argv[1], "start")) - { - if (thread_running) - { + if (!strcmp(argv[1], "start")) { + if (thread_running) { printf("flow position estimator already running\n"); /* this is not an error */ exit(0); @@ -111,26 +112,26 @@ int flow_position_estimator_main(int argc, char *argv[]) thread_should_exit = false; daemon_task = task_spawn_cmd("flow_position_estimator", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 4000, - flow_position_estimator_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 4000, + flow_position_estimator_thread_main, + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } - if (!strcmp(argv[1], "stop")) - { + if (!strcmp(argv[1], "stop")) { thread_should_exit = true; exit(0); } - if (!strcmp(argv[1], "status")) - { - if (thread_running) + if (!strcmp(argv[1], "status")) { + if (thread_running) { printf("\tflow position estimator is running\n"); - else + + } else { printf("\tflow position estimator not started\n"); + } exit(0); } @@ -147,9 +148,10 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) /* rotation matrix for transformation of optical flow speed vectors */ static const int8_t rotM_flow_sensor[3][3] = {{ 0, -1, 0 }, - { 1, 0, 0 }, - { 0, 0, 1 }}; // 90deg rotated - const float time_scale = powf(10.0f,-6.0f); + { 1, 0, 0 }, + { 0, 0, 1 } + }; // 90deg rotated + const float time_scale = powf(10.0f, -6.0f); static float speed[3] = {0.0f, 0.0f, 0.0f}; static float flow_speed[3] = {0.0f, 0.0f, 0.0f}; static float global_speed[3] = {0.0f, 0.0f, 0.0f}; @@ -192,18 +194,18 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) /* init local position and filtered flow struct */ struct vehicle_local_position_s local_pos = { - .x = 0.0f, - .y = 0.0f, - .z = 0.0f, - .vx = 0.0f, - .vy = 0.0f, - .vz = 0.0f + .x = 0.0f, + .y = 0.0f, + .z = 0.0f, + .vx = 0.0f, + .vy = 0.0f, + .vz = 0.0f }; struct filtered_bottom_flow_s filtered_flow = { - .sumx = 0.0f, - .sumy = 0.0f, - .vx = 0.0f, - .vy = 0.0f + .sumx = 0.0f, + .sumy = 0.0f, + .vx = 0.0f, + .vy = 0.0f }; /* advert pub messages */ @@ -224,37 +226,29 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_position_estimator_interval"); perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_position_estimator_err"); - while (!thread_should_exit) - { + while (!thread_should_exit) { - if (sensors_ready) - { + if (sensors_ready) { /*This runs at the rate of the sensors */ struct pollfd fds[2] = { - { .fd = optical_flow_sub, .events = POLLIN }, - { .fd = parameter_update_sub, .events = POLLIN } + { .fd = optical_flow_sub, .events = POLLIN }, + { .fd = parameter_update_sub, .events = POLLIN } }; /* wait for a sensor update, check for exit condition every 500 ms */ int ret = poll(fds, 2, 500); - if (ret < 0) - { + if (ret < 0) { /* poll error, count it in perf */ perf_count(mc_err_perf); - } - else if (ret == 0) - { + } else if (ret == 0) { /* no return value, ignore */ // printf("[flow position estimator] no bottom flow.\n"); - } - else - { + } else { /* parameter update available? */ - if (fds[1].revents & POLLIN) - { + if (fds[1].revents & POLLIN) { /* read from param to clear updated flag */ struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); @@ -264,8 +258,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) } /* only if flow data changed */ - if (fds[0].revents & POLLIN) - { + if (fds[0].revents & POLLIN) { perf_begin(mc_loop_perf); orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); @@ -284,46 +277,48 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) * -> minimum sonar value 0.3m */ - if (!vehicle_liftoff) - { - if (armed.armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f) + if (!vehicle_liftoff) { + if (armed.armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f) { vehicle_liftoff = true; - } - else - { - if (!armed.armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f)) + } + + } else { + if (!armed.armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f)) { vehicle_liftoff = false; + } } /* calc dt between flow timestamps */ /* ignore first flow msg */ - if(time_last_flow == 0) - { + if (time_last_flow == 0) { time_last_flow = flow.timestamp; continue; } + dt = (float)(flow.timestamp - time_last_flow) * time_scale ; time_last_flow = flow.timestamp; /* only make position update if vehicle is lift off or DEBUG is activated*/ - if (vehicle_liftoff || params.debug) - { + if (vehicle_liftoff || params.debug) { /* copy flow */ if (flow.integration_timespan > 0) { flow_speed[0] = flow.pixel_flow_x_integral / (flow.integration_timespan / 1e6f) * flow.ground_distance_m; flow_speed[1] = flow.pixel_flow_y_integral / (flow.integration_timespan / 1e6f) * flow.ground_distance_m; + } else { flow_speed[0] = 0; flow_speed[1] = 0; } + flow_speed[2] = 0.0f; /* convert to bodyframe velocity */ - for(uint8_t i = 0; i < 3; i++) - { + for (uint8_t i = 0; i < 3; i++) { float sum = 0.0f; - for(uint8_t j = 0; j < 3; j++) + + for (uint8_t j = 0; j < 3; j++) { sum = sum + flow_speed[j] * rotM_flow_sensor[j][i]; + } speed[i] = sum; } @@ -339,11 +334,12 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) /* convert to globalframe velocity * -> local position is currently not used for position control */ - for(uint8_t i = 0; i < 3; i++) - { + for (uint8_t i = 0; i < 3; i++) { float sum = 0.0f; - for(uint8_t j = 0; j < 3; j++) + + for (uint8_t j = 0; j < 3; j++) { sum = sum + speed[j] * PX4_R(att.R, i, j); + } global_speed[i] = sum; } @@ -354,9 +350,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) local_pos.vy = global_speed[1]; local_pos.xy_valid = true; local_pos.v_xy_valid = true; - } - else - { + + } else { /* set speed to zero and let position as it is */ filtered_flow.vx = 0; filtered_flow.vy = 0; @@ -367,24 +362,20 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) } /* filtering ground distance */ - if (!vehicle_liftoff || !armed.armed) - { + if (!vehicle_liftoff || !armed.armed) { /* not possible to fly */ sonar_valid = false; local_pos.z = 0.0f; local_pos.z_valid = false; - } - else - { + + } else { sonar_valid = true; } - if (sonar_valid || params.debug) - { + if (sonar_valid || params.debug) { /* simple lowpass sonar filtering */ /* if new value or with sonar update frequency */ - if (sonar_new != sonar_last || counter % 10 == 0) - { + if (sonar_new != sonar_last || counter % 10 == 0) { sonar_lp = 0.05f * sonar_new + 0.95f * sonar_lp; sonar_last = sonar_new; } @@ -392,12 +383,10 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) float height_diff = sonar_new - sonar_lp; /* if over 1/2m spike follow lowpass */ - if (height_diff < -params.sonar_lower_lp_threshold || height_diff > params.sonar_upper_lp_threshold) - { + if (height_diff < -params.sonar_lower_lp_threshold || height_diff > params.sonar_upper_lp_threshold) { local_pos.z = -sonar_lp; - } - else - { + + } else { local_pos.z = -sonar_new; } @@ -408,15 +397,14 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) local_pos.timestamp = hrt_absolute_time(); /* publish local position */ - if(isfinite(local_pos.x) && isfinite(local_pos.y) && isfinite(local_pos.z) - && isfinite(local_pos.vx) && isfinite(local_pos.vy)) - { + if (isfinite(local_pos.x) && isfinite(local_pos.y) && isfinite(local_pos.z) + && isfinite(local_pos.vx) && isfinite(local_pos.vy)) { orb_publish(ORB_ID(vehicle_local_position), local_pos_pub, &local_pos); } /* publish filtered flow */ - if(isfinite(filtered_flow.sumx) && isfinite(filtered_flow.sumy) && isfinite(filtered_flow.vx) && isfinite(filtered_flow.vy)) - { + if (isfinite(filtered_flow.sumx) && isfinite(filtered_flow.sumy) && isfinite(filtered_flow.vx) + && isfinite(filtered_flow.vy)) { orb_publish(ORB_ID(filtered_bottom_flow), filtered_flow_pub, &filtered_flow); } @@ -427,9 +415,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) } } - } - else - { + } else { /* sensors not ready waiting for first attitude msg */ /* polling */ @@ -440,19 +426,16 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) /* wait for a attitude message, check for exit condition every 5 s */ int ret = poll(fds, 1, 5000); - if (ret < 0) - { + if (ret < 0) { /* poll error, count it in perf */ perf_count(mc_err_perf); - } - else if (ret == 0) - { + + } else if (ret == 0) { /* no return value, ignore */ printf("[flow position estimator] no attitude received.\n"); - } - else - { - if (fds[0].revents & POLLIN){ + + } else { + if (fds[0].revents & POLLIN) { sensors_ready = true; printf("[flow position estimator] initialized.\n"); } diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c index 145cf99cc4..8e439bdac5 100644 --- a/src/examples/matlab_csv_serial/matlab_csv_serial.c +++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -73,8 +73,10 @@ static void usage(const char *reason); static void usage(const char *reason) { - if (reason) + if (reason) { fprintf(stderr, "%s\n", reason); + } + fprintf(stderr, "usage: daemon {start|stop|status} [-p ]\n\n"); exit(1); } @@ -89,13 +91,12 @@ static void usage(const char *reason) */ int matlab_csv_serial_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 1) { usage("missing command"); + } - if (!strcmp(argv[1], "start")) - { - if (thread_running) - { + if (!strcmp(argv[1], "start")) { + if (thread_running) { warnx("already running\n"); /* this is not an error */ exit(0); @@ -103,24 +104,23 @@ int matlab_csv_serial_main(int argc, char *argv[]) thread_should_exit = false; daemon_task = task_spawn_cmd("matlab_csv_serial", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - matlab_csv_serial_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + matlab_csv_serial_thread_main, + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } - if (!strcmp(argv[1], "stop")) - { + if (!strcmp(argv[1], "stop")) { thread_should_exit = true; exit(0); } - if (!strcmp(argv[1], "status")) - { + if (!strcmp(argv[1], "status")) { if (thread_running) { warnx("running"); + } else { warnx("stopped"); } @@ -139,7 +139,7 @@ int matlab_csv_serial_thread_main(int argc, char *argv[]) errx(1, "need a serial port name as argument"); } - const char* uart_name = argv[1]; + const char *uart_name = argv[1]; warnx("opening port %s", uart_name); @@ -197,40 +197,35 @@ int matlab_csv_serial_thread_main(int argc, char *argv[]) thread_running = true; - while (!thread_should_exit) - { + while (!thread_should_exit) { /*This runs at the rate of the sensors */ struct pollfd fds[] = { - { .fd = accel0_sub, .events = POLLIN } + { .fd = accel0_sub, .events = POLLIN } }; /* wait for a sensor update, check for exit condition every 500 ms */ int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 500); - if (ret < 0) - { + if (ret < 0) { /* poll error, ignore */ - } - else if (ret == 0) - { + } else if (ret == 0) { /* no return value, ignore */ warnx("no sensor data"); - } - else - { + + } else { /* accel0 update available? */ - if (fds[0].revents & POLLIN) - { + if (fds[0].revents & POLLIN) { orb_copy(ORB_ID(sensor_accel), accel0_sub, &accel0); orb_copy(ORB_ID(sensor_accel), accel1_sub, &accel1); orb_copy(ORB_ID(sensor_gyro), gyro0_sub, &gyro0); orb_copy(ORB_ID(sensor_gyro), gyro1_sub, &gyro1); // write out on accel 0, but collect for all other sensors as they have updates - dprintf(serial_fd, "%llu,%d,%d,%d,%d,%d,%d\n", accel0.timestamp, (int)accel0.x_raw, (int)accel0.y_raw, (int)accel0.z_raw, + dprintf(serial_fd, "%llu,%d,%d,%d,%d,%d,%d\n", accel0.timestamp, (int)accel0.x_raw, (int)accel0.y_raw, + (int)accel0.z_raw, (int)accel1.x_raw, (int)accel1.y_raw, (int)accel1.z_raw); } diff --git a/src/examples/publisher/publisher_example.h b/src/examples/publisher/publisher_example.h index 8165b0ffce..0a66d3edca 100644 --- a/src/examples/publisher/publisher_example.h +++ b/src/examples/publisher/publisher_example.h @@ -40,7 +40,8 @@ #pragma once #include -class PublisherExample { +class PublisherExample +{ public: PublisherExample(); @@ -49,7 +50,7 @@ public: int main(); protected: px4::NodeHandle _n; - px4::Publisher * _rc_channels_pub; - px4::Publisher * _v_att_pub; - px4::Publisher * _parameter_update_pub; + px4::Publisher *_rc_channels_pub; + px4::Publisher *_v_att_pub; + px4::Publisher *_parameter_update_pub; }; diff --git a/src/examples/publisher/publisher_start_nuttx.cpp b/src/examples/publisher/publisher_start_nuttx.cpp index db9d85269b..405b3c987f 100644 --- a/src/examples/publisher/publisher_start_nuttx.cpp +++ b/src/examples/publisher/publisher_start_nuttx.cpp @@ -69,11 +69,11 @@ int publisher_main(int argc, char *argv[]) task_should_exit = false; daemon_task = task_spawn_cmd("publisher", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - main, - (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + main, + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/examples/px4_daemon_app/px4_daemon_app.c b/src/examples/px4_daemon_app/px4_daemon_app.c index 45d541137a..860b1af787 100644 --- a/src/examples/px4_daemon_app/px4_daemon_app.c +++ b/src/examples/px4_daemon_app/px4_daemon_app.c @@ -34,7 +34,7 @@ /** * @file px4_daemon_app.c * daemon application example for PX4 autopilot - * + * * @author Example User */ @@ -71,8 +71,10 @@ static void usage(const char *reason); static void usage(const char *reason) { - if (reason) + if (reason) { warnx("%s\n", reason); + } + errx(1, "usage: daemon {start|stop|status} [-p ]\n\n"); } @@ -80,14 +82,15 @@ usage(const char *reason) * The daemon app only briefly exists to start * the background job. The stack size assigned in the * Makefile does only apply to this management task. - * + * * The actual stack size should be set in the call * to task_create(). */ int px4_daemon_app_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 1) { usage("missing command"); + } if (!strcmp(argv[1], "start")) { @@ -99,11 +102,11 @@ int px4_daemon_app_main(int argc, char *argv[]) thread_should_exit = false; daemon_task = task_spawn_cmd("daemon", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2000, - px4_daemon_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2000, + px4_daemon_thread_main, + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } @@ -115,9 +118,11 @@ int px4_daemon_app_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { warnx("\trunning\n"); + } else { warnx("\tnot started\n"); } + exit(0); } @@ -125,7 +130,8 @@ int px4_daemon_app_main(int argc, char *argv[]) exit(1); } -int px4_daemon_thread_main(int argc, char *argv[]) { +int px4_daemon_thread_main(int argc, char *argv[]) +{ warnx("[daemon] starting\n"); diff --git a/src/examples/px4_simple_app/px4_simple_app.c b/src/examples/px4_simple_app/px4_simple_app.c index 4e9f099ed7..e22c59fa22 100644 --- a/src/examples/px4_simple_app/px4_simple_app.c +++ b/src/examples/px4_simple_app/px4_simple_app.c @@ -75,30 +75,33 @@ int px4_simple_app_main(int argc, char *argv[]) for (int i = 0; i < 5; i++) { /* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */ int poll_ret = poll(fds, 1, 1000); - + /* handle the poll result */ if (poll_ret == 0) { /* this means none of our providers is giving us data */ printf("[px4_simple_app] Got no data within a second\n"); + } else if (poll_ret < 0) { /* this is seriously bad - should be an emergency */ if (error_counter < 10 || error_counter % 50 == 0) { /* use a counter to prevent flooding (and slowing us down) */ printf("[px4_simple_app] ERROR return value from poll(): %d\n" - , poll_ret); + , poll_ret); } + error_counter++; + } else { - + if (fds[0].revents & POLLIN) { /* obtained data for the first file descriptor */ struct sensor_combined_s raw; /* copy sensors raw data into local buffer */ orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw); printf("[px4_simple_app] Accelerometer:\t%8.4f\t%8.4f\t%8.4f\n", - (double)raw.accelerometer_m_s2[0], - (double)raw.accelerometer_m_s2[1], - (double)raw.accelerometer_m_s2[2]); + (double)raw.accelerometer_m_s2[0], + (double)raw.accelerometer_m_s2[1], + (double)raw.accelerometer_m_s2[2]); /* set att and publish this information for other apps */ att.roll = raw.accelerometer_m_s2[0]; @@ -106,6 +109,7 @@ int px4_simple_app_main(int argc, char *argv[]) att.yaw = raw.accelerometer_m_s2[2]; orb_publish(ORB_ID(vehicle_attitude), att_pub, &att); } + /* there could be more file descriptors here, in the form like: * if (fds[1..n].revents & POLLIN) {} */ diff --git a/src/examples/rover_steering_control/main.cpp b/src/examples/rover_steering_control/main.cpp index 7ef9140980..0f8eb800e0 100644 --- a/src/examples/rover_steering_control/main.cpp +++ b/src/examples/rover_steering_control/main.cpp @@ -267,18 +267,27 @@ int rover_steering_control_thread_main(int argc, char *argv[]) /* subscribe to topics. */ int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + int param_sub = orb_subscribe(ORB_ID(parameter_update)); /* Setup of loop */ struct pollfd fds[2]; + fds[0].fd = param_sub; + fds[0].events = POLLIN; + fds[1].fd = att_sub; + fds[1].events = POLLIN; while (!thread_should_exit) { @@ -371,6 +380,7 @@ int rover_steering_control_thread_main(int argc, char *argv[]) for (unsigned i = 0; i < (sizeof(actuators.control) / sizeof(actuators.control[0])); i++) { actuators.control[i] = 0.0f; } + actuators.timestamp = hrt_absolute_time(); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); @@ -421,7 +431,7 @@ int rover_steering_control_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 20, 2048, rover_steering_control_thread_main, - (argv) ? (char *const *)&argv[2] : (char *const *)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); thread_running = true; exit(0); } diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index 496e1dcd8f..5a23dc80b8 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -43,7 +43,8 @@ using namespace px4; -void rc_channels_callback_function(const px4_rc_channels &msg) { +void rc_channels_callback_function(const px4_rc_channels &msg) +{ PX4_INFO("I heard: [%llu]", msg.data().timestamp_last_valid); } @@ -81,21 +82,24 @@ SubscriberExample::SubscriberExample() : * This tutorial demonstrates simple receipt of messages over the PX4 middleware system. * Also the current value of the _sub_rc_chan subscription is printed */ -void SubscriberExample::rc_channels_callback(const px4_rc_channels &msg) { +void SubscriberExample::rc_channels_callback(const px4_rc_channels &msg) +{ PX4_INFO("rc_channels_callback (method): [%llu]", - msg.data().timestamp_last_valid); + msg.data().timestamp_last_valid); PX4_INFO("rc_channels_callback (method): value of _sub_rc_chan: [%llu]", - _sub_rc_chan->data().timestamp_last_valid); + _sub_rc_chan->data().timestamp_last_valid); } -void SubscriberExample::vehicle_attitude_callback(const px4_vehicle_attitude &msg) { +void SubscriberExample::vehicle_attitude_callback(const px4_vehicle_attitude &msg) +{ PX4_INFO("vehicle_attitude_callback (method): [%llu]", - msg.data().timestamp); + msg.data().timestamp); } -void SubscriberExample::parameter_update_callback(const px4_parameter_update &msg) { +void SubscriberExample::parameter_update_callback(const px4_parameter_update &msg) +{ PX4_INFO("parameter_update_callback (method): [%llu]", - msg.data().timestamp); + msg.data().timestamp); _p_sub_interv.update(); PX4_INFO("Param SUB_INTERV = %d", _p_sub_interv.get()); _p_test_float.update(); diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h index 686fed023e..9eb5dd2a0a 100644 --- a/src/examples/subscriber/subscriber_example.h +++ b/src/examples/subscriber/subscriber_example.h @@ -43,7 +43,8 @@ using namespace px4; void rc_channels_callback_function(const px4_rc_channels &msg); -class SubscriberExample { +class SubscriberExample +{ public: SubscriberExample(); @@ -54,7 +55,7 @@ protected: px4::NodeHandle _n; px4::ParameterInt _p_sub_interv; px4::ParameterFloat _p_test_float; - px4::Subscriber * _sub_rc_chan; + px4::Subscriber *_sub_rc_chan; void rc_channels_callback(const px4_rc_channels &msg); diff --git a/src/examples/subscriber/subscriber_start_nuttx.cpp b/src/examples/subscriber/subscriber_start_nuttx.cpp index 6129b19acc..b450230c1a 100644 --- a/src/examples/subscriber/subscriber_start_nuttx.cpp +++ b/src/examples/subscriber/subscriber_start_nuttx.cpp @@ -69,11 +69,11 @@ int subscriber_main(int argc, char *argv[]) task_should_exit = false; daemon_task = task_spawn_cmd("subscriber", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - main, - (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + main, + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } From 14247d95dd0b0d1ebc9716d4c94e6dcf9dcb5933 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 20 Mar 2015 00:00:54 +0100 Subject: [PATCH 076/268] Ensure Eigen is installed as submodule --- Tools/check_submodules.sh | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index 5e6e571644..4b251642ce 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -74,6 +74,28 @@ else git submodule update; fi +if [ -d src/lib/eigen ] +then + STATUSRETVAL=$(git submodule summary | grep -A20 -i eigen | grep "<") + if [ -z "$STATUSRETVAL" ] + then + echo "Checked Eigen submodule, correct version found" + else + echo "" + echo "" + echo "New commits required:" + echo "$(git submodule summary)" + echo "" + echo "" + echo "eigen sub repo not at correct version. Try 'git submodule update'" + echo "or follow instructions on http://pixhawk.org/dev/git/submodules" + exit 1 + fi +else + git submodule init; + git submodule update; +fi + if [ -d Tools/gencpp ] then STATUSRETVAL=$(git submodule summary | grep -A20 -i gencpp | grep "<") From b80908d95677fa927152ed0cf6e49e27e1780543 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 20 Mar 2015 00:05:32 +0100 Subject: [PATCH 077/268] Disable eigen test until fixed --- src/systemcmds/tests/tests_main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index bad8d9cc3b..7ffd478925 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -112,7 +112,7 @@ const struct { #ifndef TESTS_MATHLIB_DISABLE {"mathlib", test_mathlib, 0}, #endif - {"eigen", test_eigen, 0}, + {"eigen", test_eigen, OPT_NOALLTEST}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} }; From 6b494ee0f034e0e61deaee2dc7b8a84185cd4533 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 20 Mar 2015 09:43:45 +0100 Subject: [PATCH 078/268] Remove boardinfo ccommand --- makefiles/config_aerocore_default.mk | 2 +- makefiles/config_px4fmu-v2_default.mk | 1 - makefiles/config_px4fmu-v2_multiplatform.mk | 1 - makefiles/config_px4fmu-v2_test.mk | 1 - src/systemcmds/boardinfo/boardinfo.c | 409 -------------------- src/systemcmds/boardinfo/module.mk | 41 -- 6 files changed, 1 insertion(+), 454 deletions(-) delete mode 100644 src/systemcmds/boardinfo/boardinfo.c delete mode 100644 src/systemcmds/boardinfo/module.mk diff --git a/makefiles/config_aerocore_default.mk b/makefiles/config_aerocore_default.mk index 0d3934bd01..9ab63a9734 100644 --- a/makefiles/config_aerocore_default.mk +++ b/makefiles/config_aerocore_default.mk @@ -27,7 +27,7 @@ MODULES += modules/sensors # # System commands # -MODULES += systemcmds/boardinfo +MODULES += systemcmds/ver MODULES += systemcmds/mixer MODULES += systemcmds/param MODULES += systemcmds/perf diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index f414186d15..324e12696a 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -50,7 +50,6 @@ MODULES += drivers/gimbal # System commands # MODULES += systemcmds/bl_update -MODULES += systemcmds/boardinfo MODULES += systemcmds/mixer MODULES += systemcmds/param MODULES += systemcmds/perf diff --git a/makefiles/config_px4fmu-v2_multiplatform.mk b/makefiles/config_px4fmu-v2_multiplatform.mk index 29eb680960..568f940eea 100644 --- a/makefiles/config_px4fmu-v2_multiplatform.mk +++ b/makefiles/config_px4fmu-v2_multiplatform.mk @@ -48,7 +48,6 @@ MODULES += drivers/px4flow # System commands # MODULES += systemcmds/bl_update -MODULES += systemcmds/boardinfo MODULES += systemcmds/mixer MODULES += systemcmds/param MODULES += systemcmds/perf diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 22563838bd..0d04a1f19d 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -42,7 +42,6 @@ MODULES += modules/sensors # System commands # MODULES += systemcmds/bl_update -MODULES += systemcmds/boardinfo MODULES += systemcmds/mixer MODULES += systemcmds/param MODULES += systemcmds/perf diff --git a/src/systemcmds/boardinfo/boardinfo.c b/src/systemcmds/boardinfo/boardinfo.c deleted file mode 100644 index 3ff5a066c1..0000000000 --- a/src/systemcmds/boardinfo/boardinfo.c +++ /dev/null @@ -1,409 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file boardinfo.c - * autopilot and carrier board information app - */ - -#include -#include -#include -#include -#include - -#include - -#include -#include -#include - -__EXPORT int boardinfo_main(int argc, char *argv[]); - -struct eeprom_info_s -{ - unsigned bus; - unsigned address; - unsigned page_size; - unsigned page_count; - unsigned page_write_delay; -}; - -/* XXX currently code below only supports 8-bit addressing */ -const struct eeprom_info_s eeprom_info[] = { - {3, 0x57, 8, 16, 3300}, -}; - -struct board_parameter_s { - const char *name; - bson_type_t type; -}; - -const struct board_parameter_s board_parameters[] = { - {"name", BSON_STRING}, /* ascii board name */ - {"vers", BSON_INT32}, /* board version (major << 8) | minor */ - {"date", BSON_INT32}, /* manufacture date */ - {"build", BSON_INT32} /* build code (fab << 8) | tester */ -}; - -const unsigned num_parameters = sizeof(board_parameters) / sizeof(board_parameters[0]); - -static int -eeprom_write(const struct eeprom_info_s *eeprom, uint8_t *buf, unsigned size) -{ - int result = -1; - - struct i2c_dev_s *dev = up_i2cinitialize(eeprom->bus); - if (dev == NULL) { - warnx("failed to init bus %d for EEPROM", eeprom->bus); - goto out; - } - I2C_SETFREQUENCY(dev, 400000); - - /* loop until all data has been transferred */ - for (unsigned address = 0; address < size; ) { - - uint8_t pagebuf[eeprom->page_size + 1]; - - /* how many bytes available to transfer? */ - unsigned count = size - address; - - /* constrain writes to the page size */ - if (count > eeprom->page_size) - count = eeprom->page_size; - - pagebuf[0] = address & 0xff; - memcpy(pagebuf + 1, buf + address, count); - - struct i2c_msg_s msgv[1] = { - { - .addr = eeprom->address, - .flags = 0, - .buffer = pagebuf, - .length = count + 1 - } - }; - - result = I2C_TRANSFER(dev, msgv, 1); - if (result != OK) { - warnx("EEPROM write failed: %d", result); - goto out; - } - usleep(eeprom->page_write_delay); - address += count; - } - -out: - if (dev != NULL) - up_i2cuninitialize(dev); - return result; -} - -static int -eeprom_read(const struct eeprom_info_s *eeprom, uint8_t *buf, unsigned size) -{ - int result = -1; - - struct i2c_dev_s *dev = up_i2cinitialize(eeprom->bus); - if (dev == NULL) { - warnx("failed to init bus %d for EEPROM", eeprom->bus); - goto out; - } - I2C_SETFREQUENCY(dev, 400000); - - /* loop until all data has been transferred */ - for (unsigned address = 0; address < size; ) { - - /* how many bytes available to transfer? */ - unsigned count = size - address; - - /* constrain transfers to the page size (bus anti-hog) */ - if (count > eeprom->page_size) - count = eeprom->page_size; - - uint8_t addr = address; - struct i2c_msg_s msgv[2] = { - { - .addr = eeprom->address, - .flags = 0, - .buffer = &addr, - .length = 1 - }, - { - .addr = eeprom->address, - .flags = I2C_M_READ, - .buffer = buf + address, - .length = count - } - }; - - result = I2C_TRANSFER(dev, msgv, 2); - if (result != OK) { - warnx("EEPROM read failed: %d", result); - goto out; - } - address += count; - } - -out: - if (dev != NULL) - up_i2cuninitialize(dev); - return result; -} - -static void * -idrom_read(const struct eeprom_info_s *eeprom) -{ - uint32_t size = 0xffffffff; - int result; - void *buf = NULL; - - result = eeprom_read(eeprom, (uint8_t *)&size, sizeof(size)); - if (result != 0) { - warnx("failed reading ID ROM length"); - goto fail; - } - if (size > (eeprom->page_size * eeprom->page_count)) { - warnx("ID ROM not programmed"); - goto fail; - } - - buf = malloc(size); - if (buf == NULL) { - warnx("could not allocate %d bytes for ID ROM", size); - goto fail; - } - result = eeprom_read(eeprom, buf, size); - if (result != 0) { - warnx("failed reading ID ROM"); - goto fail; - } - return buf; - -fail: - if (buf != NULL) - free(buf); - return NULL; -} - -static void -boardinfo_set(const struct eeprom_info_s *eeprom, char *spec) -{ - struct bson_encoder_s encoder; - int result = 1; - char *state, *token; - unsigned i; - - /* create the encoder and make a writable copy of the spec */ - bson_encoder_init_buf(&encoder, NULL, 0); - - for (i = 0, token = strtok_r(spec, ",", &state); - token && (i < num_parameters); - i++, token = strtok_r(NULL, ",", &state)) { - - switch (board_parameters[i].type) { - case BSON_STRING: - result = bson_encoder_append_string(&encoder, board_parameters[i].name, token); - break; - case BSON_INT32: - result = bson_encoder_append_int(&encoder, board_parameters[i].name, strtoul(token, NULL, 0)); - break; - default: - result = 1; - } - if (result) { - warnx("bson append failed for %s<%s>", board_parameters[i].name, token); - goto out; - } - } - bson_encoder_fini(&encoder); - if (i != num_parameters) { - warnx("incorrect parameter list, expected: \",,\""); - result = 1; - goto out; - } - if (bson_encoder_buf_size(&encoder) > (int)(eeprom->page_size * eeprom->page_count)) { - warnx("data too large for EEPROM"); - result = 1; - goto out; - } - if ((int)*(uint32_t *)bson_encoder_buf_data(&encoder) != bson_encoder_buf_size(&encoder)) { - warnx("buffer length mismatch"); - result = 1; - goto out; - } - warnx("writing %p/%u", bson_encoder_buf_data(&encoder), bson_encoder_buf_size(&encoder)); - - result = eeprom_write(eeprom, (uint8_t *)bson_encoder_buf_data(&encoder), bson_encoder_buf_size(&encoder)); - if (result < 0) { - warnc(-result, "error writing EEPROM"); - result = 1; - } else { - result = 0; - } - -out: - free(bson_encoder_buf_data(&encoder)); - - exit(result); -} - -static int -boardinfo_print(bson_decoder_t decoder, void *private, bson_node_t node) -{ - switch (node->type) { - case BSON_INT32: - printf("%s: %d / 0x%08x\n", node->name, (int)node->i, (unsigned)node->i); - break; - case BSON_STRING: { - char buf[bson_decoder_data_pending(decoder)]; - bson_decoder_copy_data(decoder, buf); - printf("%s: %s\n", node->name, buf); - break; - } - case BSON_EOO: - break; - default: - warnx("unexpected node type %d", node->type); - break; - } - return 1; -} - -static void -boardinfo_show(const struct eeprom_info_s *eeprom) -{ - struct bson_decoder_s decoder; - void *buf; - - buf = idrom_read(eeprom); - if (buf == NULL) - errx(1, "ID ROM read failed"); - - if (bson_decoder_init_buf(&decoder, buf, 0, boardinfo_print, NULL) == 0) { - while (bson_decoder_next(&decoder) > 0) - ; - } else { - warnx("failed to init decoder"); - } - free(buf); - exit(0); -} - -struct { - const char *property; - const char *value; -} test_args; - -static int -boardinfo_test_callback(bson_decoder_t decoder, void *private, bson_node_t node) -{ - /* reject nodes with non-matching names */ - if (strcmp(node->name, test_args.property)) - return 1; - - /* compare node values to check for a match */ - switch (node->type) { - case BSON_STRING: { - char buf[bson_decoder_data_pending(decoder)]; - bson_decoder_copy_data(decoder, buf); - - /* check for a match */ - if (!strcmp(test_args.value, buf)) { - return 2; - } - break; - } - - case BSON_INT32: { - int32_t val = strtol(test_args.value, NULL, 0); - - /* check for a match */ - if (node->i == val) { - return 2; - } - break; - } - - default: - break; - } - - return 1; -} - -static void -boardinfo_test(const struct eeprom_info_s *eeprom, const char *property, const char *value) -{ - struct bson_decoder_s decoder; - void *buf; - int result = -1; - - if ((property == NULL) || (strlen(property) == 0) || - (value == NULL) || (strlen(value) == 0)) - errx(1, "missing property name or value"); - - test_args.property = property; - test_args.value = value; - - buf = idrom_read(eeprom); - if (buf == NULL) - errx(1, "ID ROM read failed"); - - if (bson_decoder_init_buf(&decoder, buf, 0, boardinfo_test_callback, NULL) == 0) { - do { - result = bson_decoder_next(&decoder); - } while (result == 1); - } else { - warnx("failed to init decoder"); - } - free(buf); - - /* if we matched, we exit with zero success */ - exit((result == 2) ? 0 : 1); -} - -int -boardinfo_main(int argc, char *argv[]) -{ - if (!strcmp(argv[1], "set")) - boardinfo_set(&eeprom_info[0], argv[2]); - - if (!strcmp(argv[1], "show")) - boardinfo_show(&eeprom_info[0]); - - if (!strcmp(argv[1], "test")) - boardinfo_test(&eeprom_info[0], argv[2], argv[3]); - - errx(1, "missing/unrecognised command, try one of 'set', 'show', 'test'"); -} diff --git a/src/systemcmds/boardinfo/module.mk b/src/systemcmds/boardinfo/module.mk deleted file mode 100644 index 6851d229b2..0000000000 --- a/src/systemcmds/boardinfo/module.mk +++ /dev/null @@ -1,41 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Information about FMU and IO boards connected -# - -MODULE_COMMAND = boardinfo -SRCS = boardinfo.c - -MAXOPTIMIZATION = -Os From 9130976d9e0e8d5fa27a78967eddfce2e0113b8a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marcel=20St=C3=BCttgen?= Date: Fri, 20 Mar 2015 17:01:52 +0100 Subject: [PATCH 079/268] Update main.cpp removed debug output code --- src/examples/rover_steering_control/main.cpp | 18 ------------------ 1 file changed, 18 deletions(-) diff --git a/src/examples/rover_steering_control/main.cpp b/src/examples/rover_steering_control/main.cpp index 67ff489166..edde5627de 100644 --- a/src/examples/rover_steering_control/main.cpp +++ b/src/examples/rover_steering_control/main.cpp @@ -155,24 +155,6 @@ int parameters_update(const struct param_handles *h, struct params *p) void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct actuator_controls_s *actuators) { - - //double r,p,y,t; - - - - //printf("vehicle_attitude_setpoint:\n"); - //print values only when they change to get less spam in console - if((double)att_sp->roll_body > 0.0) { printf("att_sp->roll_body: %8.4f\n" , (double)att_sp->roll_body); } - if((double)att_sp->pitch_body > 0.0) {printf("att_sp->pitch_body: %8.4f\n" , (double)att_sp->pitch_body); } - if((double)att_sp->yaw_body > 0.0) { printf("att_sp->yaw_body: %8.4f\n" , (double)att_sp->yaw_body); } - if((double)att_sp->thrust > 0.0) { printf("att_sp->throttle: %8.4f\n" , (double)att_sp->thrust); } - - // r = (double)att_sp->roll_body; - // p = (double)att_sp->pitch_body; - // y = (double)att_sp->yaw_body; - // t = (double)att_sp->thrust; - - /* * The PX4 architecture provides a mixer outside of the controller. * The mixer is fed with a default vector of actuator controls, representing From ecec2d76d9217713c800a48f1be0a886af5bb517 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 20 Mar 2015 21:57:31 +0100 Subject: [PATCH 080/268] Allow a bit more flexibility of PWM range --- src/drivers/drv_pwm_output.h | 4 ++-- src/systemcmds/esc_calib/esc_calib.c | 7 ++++++- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 35e9619f07..6271ad2086 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -78,7 +78,7 @@ __BEGIN_DECLS /** * Highest PWM allowed as the minimum PWM */ -#define PWM_HIGHEST_MIN 1300 +#define PWM_HIGHEST_MIN 1600 /** * Highest maximum PWM in us @@ -93,7 +93,7 @@ __BEGIN_DECLS /** * Lowest PWM allowed as the maximum PWM */ -#define PWM_LOWEST_MAX 1700 +#define PWM_LOWEST_MAX 1400 /** * Do not output a channel with this value diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index aee26680cc..d74010553a 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -168,8 +168,13 @@ esc_calib_main(int argc, char *argv[]) } } - if (set_mask == 0) + if (set_mask == 0) { usage("no channels chosen"); + } + + if (pwm_low > pwm_high) { + usage("low pwm is higher than high pwm"); + } /* make sure no other source is publishing control values now */ struct actuator_controls_s actuators; From 354809bff4da0ce2a74ca6a750fa15a3ff441847 Mon Sep 17 00:00:00 2001 From: Michal Ulianko Date: Fri, 20 Mar 2015 23:22:42 +0100 Subject: [PATCH 081/268] BIN_TO_OBJ: Switch egrep and cut to $(GREP). Use posix format and decimal radix with $(NM). --- makefiles/toolchain_gnu-arm-eabi.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 93ee235c4b..033d6e1fe1 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -324,7 +324,7 @@ define BIN_TO_OBJ $(Q) $(ECHO) > $2.c $(call COMPILE,$2.c,$2.c.o) $(Q) $(LD) -r -o $2.bin.o $2.c.o -b binary $1 - $(Q) $(ECHO) "const unsigned int $3_len = 0x`$(NM) $2.bin.o | egrep $(call BIN_SYM_PREFIX,$1)_size$$ | cut -d' ' -f1`;" > $2.c + $(Q) $(ECHO) "const unsigned int $3_len = `$(NM) -p --radix=d $2.bin.o | $(GREP) $(call BIN_SYM_PREFIX,$1)_size$$ | $(GREP) -o ^[0-9]*`;" > $2.c $(call COMPILE,$2.c,$2.c.o) $(Q) $(LD) -r -o $2 $2.c.o $2.bin.o $(Q) $(OBJCOPY) $2 \ From 631e518c45b054441cbe9eba5167d3075d5d2a9d Mon Sep 17 00:00:00 2001 From: Michal Ulianko Date: Sat, 21 Mar 2015 00:31:52 +0100 Subject: [PATCH 082/268] BIN_TO_OBJ: Change NM radix back to hex so multiple leading zeros in NM output won't generate octal constant in C. --- makefiles/toolchain_gnu-arm-eabi.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 033d6e1fe1..3b9fefb3ef 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -324,7 +324,7 @@ define BIN_TO_OBJ $(Q) $(ECHO) > $2.c $(call COMPILE,$2.c,$2.c.o) $(Q) $(LD) -r -o $2.bin.o $2.c.o -b binary $1 - $(Q) $(ECHO) "const unsigned int $3_len = `$(NM) -p --radix=d $2.bin.o | $(GREP) $(call BIN_SYM_PREFIX,$1)_size$$ | $(GREP) -o ^[0-9]*`;" > $2.c + $(Q) $(ECHO) "const unsigned int $3_len = 0x`$(NM) -p --radix=x $2.bin.o | $(GREP) $(call BIN_SYM_PREFIX,$1)_size$$ | $(GREP) -o ^[0-9a-fA-F]*`;" > $2.c $(call COMPILE,$2.c,$2.c.o) $(Q) $(LD) -r -o $2 $2.c.o $2.bin.o $(Q) $(OBJCOPY) $2 \ From 95be7c22896571ca7555ee54eae7fac43cc867a4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 21 Mar 2015 12:51:27 +0100 Subject: [PATCH 083/268] Fix limiting in mc mixer --- src/modules/systemlib/mixer/mixer_multirotor.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index b354eb518f..b753da5ce7 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -244,14 +244,23 @@ MultirotorMixer::mix(float *outputs, unsigned space) if (min_out < 0.0f) { float scale_in = thrust / (thrust - min_out); + max_out = 0.0f; + /* mix again with adjusted controls */ for (unsigned i = 0; i < _rotor_count; i++) { - outputs[i] = scale_in * (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) + thrust; + float out = scale_in * (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) + thrust; + + /* update max output value */ + if (out > max_out) { + max_out = out; + } + + outputs[i] = out; } _limits.roll_pitch = true; } else { - /* roll/pitch mixed without limiting, add yaw control */ + /* roll/pitch mixed without lower side limiting, add yaw control */ for (unsigned i = 0; i < _rotor_count; i++) { outputs[i] += yaw * _rotors[i].yaw_scale; } From 39d3b903e765abafeb31fc27af75ff94deb5618e Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Mon, 23 Mar 2015 21:13:07 +0100 Subject: [PATCH 084/268] Revert the MainS to MainState to keep LogMuncher happy. --- src/modules/sdlog2/sdlog2_messages.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 060dcf62fb..54b0fe73f6 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -481,7 +481,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"), LOG_FORMAT_S(ATTC, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), - LOG_FORMAT(STAT, "BBBfBBf", "MainS,ArmS,Failsafe,BatRem,BatWarn,Landed,Load"), + LOG_FORMAT(STAT, "BBBfBBf", "MainState,ArmS,Failsafe,BatRem,BatWarn,Landed,Load"), LOG_FORMAT(VTOL, "f", "Arsp"), LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), From 20f6fbd864dd2ad02650588ea107945b25c439f6 Mon Sep 17 00:00:00 2001 From: philipoe Date: Tue, 24 Mar 2015 12:48:00 +0100 Subject: [PATCH 085/268] PX4IO driver: Add mapping of AUX1-3 channels, in case these are also used as control surface inputs --- src/drivers/px4io/px4io.cpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index f62df54f6c..1512566fa2 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1291,6 +1291,24 @@ PX4IO::io_set_rc_config() input_map[ichan - 1] = 4; } + /* AUX 1*/ + param_get(param_find("RC_MAP_AUX1"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { + input_map[ichan - 1] = 5; + } + + /* AUX 2*/ + param_get(param_find("RC_MAP_AUX2"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { + input_map[ichan - 1] = 6; + } + + /* AUX 3*/ + param_get(param_find("RC_MAP_AUX3"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { + input_map[ichan - 1] = 7; + } + /* MAIN MODE SWITCH */ param_get(param_find("RC_MAP_MODE_SW"), &ichan); if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { From f26edd20f607e63a4377f23be5b8069e3bed74a4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 24 Mar 2015 14:50:22 -0700 Subject: [PATCH 086/268] Add missing gflags --- Tools/ros/px4_workspace_setup.sh | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/Tools/ros/px4_workspace_setup.sh b/Tools/ros/px4_workspace_setup.sh index 3e4bf06a57..1c97ec728f 100755 --- a/Tools/ros/px4_workspace_setup.sh +++ b/Tools/ros/px4_workspace_setup.sh @@ -17,14 +17,15 @@ cd .. # mav comm git clone https://github.com/PX4/mav_comm.git +# gflags catkin +git clone https://github.com/ethz-asl/gflags_catkin.git + # glog catkin git clone https://github.com/ethz-asl/glog_catkin.git # catkin simple git clone https://github.com/catkin/catkin_simple.git -# drcsim (for scenery and models) - cd .. catkin_make From 8a584be530cd13a2b56d7a2ff978316a72814d38 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 24 Mar 2015 14:55:17 -0700 Subject: [PATCH 087/268] Fix missing dependencies --- Tools/ros/px4_ros_installation_ubuntu.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/ros/px4_ros_installation_ubuntu.sh b/Tools/ros/px4_ros_installation_ubuntu.sh index 7efc400cdd..b65df8dcea 100755 --- a/Tools/ros/px4_ros_installation_ubuntu.sh +++ b/Tools/ros/px4_ros_installation_ubuntu.sh @@ -27,7 +27,7 @@ echo "source /opt/ros/indigo/setup.bash" >> ~/.bashrc sudo apt-get -y install python-rosinstall # additional dependencies -sudo apt-get -y install ros-indigo-octomap-msgs ros-indigo-joy +sudo apt-get -y install ros-indigo-octomap-msgs ros-indigo-joy ros-indigo-mavros ros-indigo-mavros-extras ## drcsim setup (for models) ### add osrf repository From 7554eb515a9b87658e860de3ceee552beff610ee Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Mar 2015 01:02:29 -0700 Subject: [PATCH 088/268] Fix ROS install instructions --- Tools/ros/px4_workspace_setup.sh | 3 --- 1 file changed, 3 deletions(-) diff --git a/Tools/ros/px4_workspace_setup.sh b/Tools/ros/px4_workspace_setup.sh index 1c97ec728f..5599ab9998 100755 --- a/Tools/ros/px4_workspace_setup.sh +++ b/Tools/ros/px4_workspace_setup.sh @@ -10,9 +10,6 @@ git clone https://github.com/PX4/Firmware.git # euroc simulator git clone https://github.com/PX4/euroc_simulator.git -cd euroc_simulator -git checkout px4_nodes -cd .. # mav comm git clone https://github.com/PX4/mav_comm.git From b361f7c81b07ebe3e75358b254eee492a1ef49da Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 25 Mar 2015 11:58:01 +0100 Subject: [PATCH 089/268] write trust setpoint to correct field for logging --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 4 ++-- src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 58d20ebef9..4910454bd9 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1308,8 +1308,8 @@ MulticopterPositionControl::task_main() /* save thrust setpoint for logging */ _local_pos_sp.acc_x = thrust_sp(0); - _local_pos_sp.acc_x = thrust_sp(1); - _local_pos_sp.acc_x = thrust_sp(2); + _local_pos_sp.acc_y = thrust_sp(1); + _local_pos_sp.acc_z = thrust_sp(2); _att_sp.timestamp = hrt_absolute_time(); diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp index 40268358ad..d135eecfbb 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -922,8 +922,8 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti /* save thrust setpoint for logging */ _local_pos_sp_msg.data().acc_x = thrust_sp(0); - _local_pos_sp_msg.data().acc_x = thrust_sp(1); - _local_pos_sp_msg.data().acc_x = thrust_sp(2); + _local_pos_sp_msg.data().acc_y = thrust_sp(1); + _local_pos_sp_msg.data().acc_z = thrust_sp(2); _att_sp_msg.data().timestamp = get_time_micros(); From 7be4298ba1fb0628e80b2ddf938f5c6937611e48 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 25 Mar 2015 08:11:32 -1000 Subject: [PATCH 090/268] Fixes dependency not being cleaned by keeping the uavcan artifacts in the BUILD_DIR --- src/modules/uavcan/module.mk | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 4f63629a0c..f7feeadabc 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -58,7 +58,8 @@ SRCS += sensors/sensor_bridge.cpp \ # libuavcan # include $(PX4_LIB_DIR)uavcan/libuavcan/include.mk -SRCS += $(LIBUAVCAN_SRC) +# Use the relitive path to keep the genrated files in the BUILD_DIR +SRCS += $(subst $(PX4_MODULE_SRC),../../,$(LIBUAVCAN_SRC)) INCLUDE_DIRS += $(LIBUAVCAN_INC) # Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile # because this platform lacks most of the standard library and STL. Hence we need to force C++03 mode. @@ -68,7 +69,8 @@ override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAV # libuavcan drivers for STM32 # include $(PX4_LIB_DIR)uavcan/libuavcan_drivers/stm32/driver/include.mk -SRCS += $(LIBUAVCAN_STM32_SRC) +# Use the relitive path to keep the genrated files in the BUILD_DIR +SRCS += $(subst $(PX4_MODULE_SRC),../../,$(LIBUAVCAN_STM32_SRC)) INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC) override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=2 From 89d5ae69d3f898324d8059e7ba97467dfffb17c9 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Mon, 16 Feb 2015 18:19:17 -0800 Subject: [PATCH 091/268] mpu6000: add set_accel_range --- src/drivers/mpu6000/mpu6000.cpp | 113 ++++++++++++-------------------- 1 file changed, 41 insertions(+), 72 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 727132a646..4461dc10b0 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -362,7 +362,7 @@ private: * @param max_g The maximum G value the range must support. * @return OK if the value can be supported, -ERANGE otherwise. */ - int set_range(unsigned max_g); + int set_accel_range(unsigned max_g); /** * Swap a 16-bit value read from the MPU6000 to native byte order. @@ -717,37 +717,7 @@ int MPU6000::reset() _gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F); _gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F; - // product-specific scaling - switch (_product) { - case MPU6000ES_REV_C4: - case MPU6000ES_REV_C5: - case MPU6000_REV_C4: - case MPU6000_REV_C5: - // Accel scale 8g (4096 LSB/g) - // Rev C has different scaling than rev D - write_checked_reg(MPUREG_ACCEL_CONFIG, 1 << 3); - break; - - case MPU6000ES_REV_D6: - case MPU6000ES_REV_D7: - case MPU6000ES_REV_D8: - case MPU6000_REV_D6: - case MPU6000_REV_D7: - case MPU6000_REV_D8: - case MPU6000_REV_D9: - case MPU6000_REV_D10: - // default case to cope with new chip revisions, which - // presumably won't have the accel scaling bug - default: - // Accel scale 8g (4096 LSB/g) - write_checked_reg(MPUREG_ACCEL_CONFIG, 2 << 3); - break; - } - - // Correct accel scale factors of 4096 LSB/g - // scale to m/s^2 ( 1g = 9.81 m/s^2) - _accel_range_scale = (MPU6000_ONE_G / 4096.0f); - _accel_range_m_s2 = 8.0f * MPU6000_ONE_G; + set_accel_range(8); usleep(1000); @@ -1301,11 +1271,8 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) return OK; case ACCELIOCSRANGE: - /* XXX not implemented */ - // XXX change these two values on set: - // _accel_range_scale = (9.81f / 4096.0f); - // _accel_range_m_s2 = 8.0f * 9.81f; - return -EINVAL; + return set_accel_range(arg); + case ACCELIOCGRANGE: return (unsigned long)((_accel_range_m_s2)/MPU6000_ONE_G + 0.5f); @@ -1455,44 +1422,46 @@ MPU6000::write_checked_reg(unsigned reg, uint8_t value) } int -MPU6000::set_range(unsigned max_g) +MPU6000::set_accel_range(unsigned max_g_in) { -#if 0 - uint8_t rangebits; - float rangescale; - - if (max_g > 16) { - return -ERANGE; - - } else if (max_g > 8) { /* 16G */ - rangebits = OFFSET_LSB1_RANGE_16G; - rangescale = 1.98; - - } else if (max_g > 4) { /* 8G */ - rangebits = OFFSET_LSB1_RANGE_8G; - rangescale = 0.99; - - } else if (max_g > 3) { /* 4G */ - rangebits = OFFSET_LSB1_RANGE_4G; - rangescale = 0.5; - - } else if (max_g > 2) { /* 3G */ - rangebits = OFFSET_LSB1_RANGE_3G; - rangescale = 0.38; - - } else if (max_g > 1) { /* 2G */ - rangebits = OFFSET_LSB1_RANGE_2G; - rangescale = 0.25; - - } else { /* 1G */ - rangebits = OFFSET_LSB1_RANGE_1G; - rangescale = 0.13; + // workaround for bugged versions of MPU6k (rev C) + switch (_product) { + case MPU6000ES_REV_C4: + case MPU6000ES_REV_C5: + case MPU6000_REV_C4: + case MPU6000_REV_C5: + write_checked_reg(MPUREG_ACCEL_CONFIG, 1 << 3); + _accel_range_scale = (MPU6000_ONE_G / 4096.0f); + _accel_range_m_s2 = 8.0f * MPU6000_ONE_G; + return OK; } - /* adjust sensor configuration */ - modify_reg(ADDR_OFFSET_LSB1, OFFSET_LSB1_RANGE_MASK, rangebits); - _range_scale = rangescale; -#endif + uint8_t afs_sel; + float lsb_per_g; + float max_accel_g; + + if (max_g_in > 8) { // 16g - AFS_SEL = 3 + afs_sel = 3; + lsb_per_g = 2048; + max_accel_g = 16; + } else if (max_g_in > 4) { // 8g - AFS_SEL = 2 + afs_sel = 2; + lsb_per_g = 4096; + max_accel_g = 8; + } else if (max_g_in > 2) { // 4g - AFS_SEL = 1 + afs_sel = 1; + lsb_per_g = 8192; + max_accel_g = 4; + } else { // 2g - AFS_SEL = 0 + afs_sel = 0; + lsb_per_g = 16384; + max_accel_g = 2; + } + + write_checked_reg(MPUREG_ACCEL_CONFIG, afs_sel << 3); + _accel_range_scale = (MPU6000_ONE_G / lsb_per_g); + _accel_range_m_s2 = max_accel_g * MPU6000_ONE_G; + return OK; } From cb8e32f8332957a6b0e9d46feb0f9b3589ccddae Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Mar 2015 22:57:18 -0700 Subject: [PATCH 092/268] MPU6K: Temp support --- src/drivers/mpu6000/mpu6000.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 4461dc10b0..b6642e2bbd 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1778,7 +1778,7 @@ MPU6000::print_info() (unsigned)_checked_values[i]); } } - ::printf("temperature: %.1f\n", _last_temperature); + ::printf("temperature: %.1f\n", (double)_last_temperature); } void From b3c45e7178e5a00195445f34a9c2ef35dfe90570 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Mar 2015 22:57:32 -0700 Subject: [PATCH 093/268] LSM303D: Temp support --- src/drivers/lsm303d/lsm303d.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 11a046a8dc..e594c92a11 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1640,7 +1640,7 @@ LSM303D::mag_measure() float yraw_f = mag_report.y_raw; float zraw_f = mag_report.z_raw; - // apply user specified rotation + /* apply user specified rotation */ rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); mag_report.x = ((xraw_f * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; @@ -1650,13 +1650,14 @@ LSM303D::mag_measure() mag_report.range_ga = (float)_mag_range_ga; mag_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); - // remember the temperature. The datasheet isn't clear, but it - // seems to be a signed offset from 25 degrees C in units of 0.125C - _last_temperature = 25 + (raw_mag_report.temperature*0.125f); + /* remember the temperature. The datasheet isn't clear, but it + * seems to be a signed offset from 25 degrees C in units of 0.125C + */ + _last_temperature = 25 + (raw_mag_report.temperature * 0.125f); + mag_report.temperature = _last_temperature; _mag_reports->force(&mag_report); - /* XXX please check this poll_notify, is it the right one? */ /* notify anyone waiting for data */ poll_notify(POLLIN); @@ -1693,7 +1694,7 @@ LSM303D::print_info() (unsigned)_checked_values[i]); } } - ::printf("temperature: %.2f\n", _last_temperature); + ::printf("temperature: %.2f\n", (double)_last_temperature); } void From 953cef9a9ec84cd54633362c3fb241144688544f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Mar 2015 22:57:47 -0700 Subject: [PATCH 094/268] HMC5883: Temp dummy value --- src/drivers/hmc5883/hmc5883.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 2b3520fc80..fd4d4cff5e 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -927,6 +927,9 @@ HMC5883::collect() /* z remains z */ new_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale; + /* this sensor does not measure temperature, output a constant value */ + new_report.temperature = 0.0f; + if (!(_pub_blocked)) { if (_mag_topic != -1) { From 0b26671d62d090eb46854eb251d2bf8c82bfb8e0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Mar 2015 22:58:00 -0700 Subject: [PATCH 095/268] Mag drivers: Temp support --- src/drivers/drv_mag.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index 7fcff9d382..ca33966b01 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -63,6 +63,7 @@ struct mag_report { float z; float range_ga; float scaling; + float temperature; int16_t x_raw; int16_t y_raw; From 8a1cbac683ac58643bc3f17319197ae927c6c23c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Mar 2015 22:58:25 -0700 Subject: [PATCH 096/268] Sensor combined uORB topic: Temperature for all sensors --- src/modules/uORB/topics/sensor_combined.h | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h index ded82adea2..a19f3dc107 100644 --- a/src/modules/uORB/topics/sensor_combined.h +++ b/src/modules/uORB/topics/sensor_combined.h @@ -83,6 +83,7 @@ struct sensor_combined_s { int16_t gyro_raw[3]; /**< Raw sensor values of angular velocity */ float gyro_rad_s[3]; /**< Angular velocity in radian per seconds */ unsigned gyro_errcount; /**< Error counter for gyro 0 */ + float gyro_temp; /**< Temperature of gyro 0 */ int16_t accelerometer_raw[3]; /**< Raw acceleration in NED body frame */ float accelerometer_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 */ @@ -90,6 +91,7 @@ struct sensor_combined_s { float accelerometer_range_m_s2; /**< Accelerometer measurement range in m/s^2 */ uint64_t accelerometer_timestamp; /**< Accelerometer timestamp */ unsigned accelerometer_errcount; /**< Error counter for accel 0 */ + float accelerometer_temp; /**< Temperature of accel 0 */ int16_t magnetometer_raw[3]; /**< Raw magnetic field in NED body frame */ float magnetometer_ga[3]; /**< Magnetic field in NED body frame, in Gauss */ @@ -98,36 +100,43 @@ struct sensor_combined_s { float magnetometer_cuttoff_freq_hz; /**< Internal analog low pass frequency of sensor */ uint64_t magnetometer_timestamp; /**< Magnetometer timestamp */ unsigned magnetometer_errcount; /**< Error counter for mag 0 */ + float magnetometer_temp; /**< Temperature of mag 0 */ int16_t gyro1_raw[3]; /**< Raw sensor values of angular velocity */ float gyro1_rad_s[3]; /**< Angular velocity in radian per seconds */ uint64_t gyro1_timestamp; /**< Gyro timestamp */ unsigned gyro1_errcount; /**< Error counter for gyro 1 */ + float gyro1_temp; /**< Temperature of gyro 1 */ int16_t accelerometer1_raw[3]; /**< Raw acceleration in NED body frame */ float accelerometer1_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 */ uint64_t accelerometer1_timestamp; /**< Accelerometer timestamp */ unsigned accelerometer1_errcount; /**< Error counter for accel 1 */ + float accelerometer1_temp; /**< Temperature of accel 1 */ int16_t magnetometer1_raw[3]; /**< Raw magnetic field in NED body frame */ float magnetometer1_ga[3]; /**< Magnetic field in NED body frame, in Gauss */ uint64_t magnetometer1_timestamp; /**< Magnetometer timestamp */ - unsigned magnetometer1_errcount; /**< Error counter for mag 0 */ + unsigned magnetometer1_errcount; /**< Error counter for mag 1 */ + float magnetometer1_temp; /**< Temperature of mag 1 */ int16_t gyro2_raw[3]; /**< Raw sensor values of angular velocity */ float gyro2_rad_s[3]; /**< Angular velocity in radian per seconds */ uint64_t gyro2_timestamp; /**< Gyro timestamp */ unsigned gyro2_errcount; /**< Error counter for gyro 1 */ + float gyro2_temp; /**< Temperature of gyro 1 */ int16_t accelerometer2_raw[3]; /**< Raw acceleration in NED body frame */ float accelerometer2_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 */ uint64_t accelerometer2_timestamp; /**< Accelerometer timestamp */ unsigned accelerometer2_errcount; /**< Error counter for accel 2 */ + float accelerometer2_temp; /**< Temperature of accel 2 */ int16_t magnetometer2_raw[3]; /**< Raw magnetic field in NED body frame */ float magnetometer2_ga[3]; /**< Magnetic field in NED body frame, in Gauss */ uint64_t magnetometer2_timestamp; /**< Magnetometer timestamp */ unsigned magnetometer2_errcount; /**< Error counter for mag 2 */ + float magnetometer2_temp; /**< Temperature of mag 2 */ float baro_pres_mbar; /**< Barometric pressure, already temp. comp. */ float baro_alt_meter; /**< Altitude, already temp. comp. */ From 81546e8f84f53446442b4b1014c5366e37e4d7d4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Mar 2015 22:58:54 -0700 Subject: [PATCH 097/268] sensors app: Log all temperature --- src/modules/sensors/sensors.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 527ca22100..3b93393d26 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1015,6 +1015,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) raw.accelerometer_timestamp = accel_report.timestamp; raw.accelerometer_errcount = accel_report.error_count; + raw.accelerometer_temp = accel_report.temperature; } orb_check(_accel1_sub, &accel_updated); @@ -1037,6 +1038,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) raw.accelerometer1_timestamp = accel_report.timestamp; raw.accelerometer1_errcount = accel_report.error_count; + raw.accelerometer1_temp = accel_report.temperature; } orb_check(_accel2_sub, &accel_updated); @@ -1059,6 +1061,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) raw.accelerometer2_timestamp = accel_report.timestamp; raw.accelerometer2_errcount = accel_report.error_count; + raw.accelerometer2_temp = accel_report.temperature; } } @@ -1086,6 +1089,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) raw.timestamp = gyro_report.timestamp; raw.gyro_errcount = gyro_report.error_count; + raw.gyro_temp = gyro_report.temperature; } orb_check(_gyro1_sub, &gyro_updated); @@ -1108,6 +1112,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) raw.gyro1_timestamp = gyro_report.timestamp; raw.gyro1_errcount = gyro_report.error_count; + raw.gyro1_temp = gyro_report.temperature; } orb_check(_gyro2_sub, &gyro_updated); @@ -1130,6 +1135,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) raw.gyro2_timestamp = gyro_report.timestamp; raw.gyro2_errcount = gyro_report.error_count; + raw.gyro2_temp = gyro_report.temperature; } } @@ -1158,6 +1164,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) raw.magnetometer_timestamp = mag_report.timestamp; raw.magnetometer_errcount = mag_report.error_count; + raw.magnetometer_temp = mag_report.temperature; } orb_check(_mag1_sub, &mag_updated); @@ -1181,6 +1188,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) raw.magnetometer1_timestamp = mag_report.timestamp; raw.magnetometer1_errcount = mag_report.error_count; + raw.magnetometer1_temp = mag_report.temperature; } orb_check(_mag2_sub, &mag_updated); @@ -1204,6 +1212,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) raw.magnetometer2_timestamp = mag_report.timestamp; raw.magnetometer2_errcount = mag_report.error_count; + raw.magnetometer2_temp = mag_report.temperature; } } From 4adb5ba276f502904b3e066146776c9297af905f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Mar 2015 22:59:08 -0700 Subject: [PATCH 098/268] sdlog2: Log all temperatures --- src/modules/sdlog2/module.mk | 2 +- src/modules/sdlog2/sdlog2.c | 68 ++++++++++++++++++++++------ src/modules/sdlog2/sdlog2_messages.h | 9 ++-- 3 files changed, 61 insertions(+), 18 deletions(-) diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk index 91a9611d45..8fded0bdb2 100644 --- a/src/modules/sdlog2/module.mk +++ b/src/modules/sdlog2/module.mk @@ -46,5 +46,5 @@ MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os -EXTRACFLAGS = -Wframe-larger-than=1300 +EXTRACFLAGS = -Wframe-larger-than=1400 diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 04bf71c178..1c8cdd4be0 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -340,15 +340,32 @@ int sdlog2_main(int argc, char *argv[]) exit(0); } + if (!thread_running) { + warnx("not started\n"); + return 1; + } + if (!strcmp(argv[1], "status")) { - if (thread_running) { - sdlog2_status(); + sdlog2_status(); + return 0; + } - } else { - warnx("not started\n"); - } + if (!strcmp(argv[1], "on")) { + struct vehicle_command_s cmd; + cmd.command = VEHICLE_CMD_PREFLIGHT_STORAGE; + cmd.param3 = 1; + int fd = orb_advertise(ORB_ID(vehicle_command), &cmd); + close(fd); + return 0; + } - exit(0); + if (!strcmp(argv[1], "off")) { + struct vehicle_command_s cmd; + cmd.command = VEHICLE_CMD_PREFLIGHT_STORAGE; + cmd.param3 = 0; + int fd = orb_advertise(ORB_ID(vehicle_command), &cmd); + close(fd); + return 0; } sdlog2_usage("unrecognized command"); @@ -627,13 +644,16 @@ static void *logwriter_thread(void *arg) void sdlog2_start_log() { + if (logging_enabled) { + return; + } + /* create log dir if needed */ if (create_log_dir() != 0) { mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] error creating log dir"); exit(1); } - /* initialize statistics counter */ log_bytes_written = 0; start_time = hrt_absolute_time(); @@ -674,6 +694,10 @@ void sdlog2_start_log() void sdlog2_stop_log() { + if (!logging_enabled) { + return; + } + logging_enabled = false; /* wake up write thread one last time */ @@ -1347,6 +1371,9 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0]; log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1]; log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2]; + log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro_temp; + log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer_temp; + log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer_temp; LOGBUFFER_WRITE_AND_COUNT(IMU); } @@ -1402,6 +1429,9 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer1_ga[0]; log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer1_ga[1]; log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer1_ga[2]; + log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro1_temp; + log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer1_temp; + log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer1_temp; LOGBUFFER_WRITE_AND_COUNT(IMU); } @@ -1431,6 +1461,9 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer2_ga[0]; log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer2_ga[1]; log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer2_ga[2]; + log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro2_temp; + log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer2_temp; + log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer2_temp; LOGBUFFER_WRITE_AND_COUNT(IMU); } @@ -1810,13 +1843,18 @@ int sdlog2_thread_main(int argc, char *argv[]) void sdlog2_status() { - float kibibytes = log_bytes_written / 1024.0f; - float mebibytes = kibibytes / 1024.0f; - float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f; - - warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped); warnx("extended logging: %s", (_extended_logging) ? "ON" : "OFF"); - mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped); + if (!logging_enabled) { + warnx("standing by"); + } else { + + float kibibytes = log_bytes_written / 1024.0f; + float mebibytes = kibibytes / 1024.0f; + float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f; + + warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped); + mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped); + } } /** @@ -1904,13 +1942,15 @@ void handle_command(struct vehicle_command_s *cmd) switch (cmd->command) { case VEHICLE_CMD_PREFLIGHT_STORAGE: - param = (int)(cmd->param3); + param = (int)(cmd->param3 + 0.5f); if (param == 1) { sdlog2_start_log(); } else if (param == 0) { sdlog2_stop_log(); + } else { + warnx("unknown storage cmd"); } break; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 54b0fe73f6..17401218ae 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -89,6 +89,9 @@ struct log_IMU_s { float mag_x; float mag_y; float mag_z; + float temp_acc; + float temp_gyro; + float temp_mag; }; /* --- SENS - OTHER SENSORS --- */ @@ -471,9 +474,9 @@ static const struct log_format_s log_formats[] = { /* business-level messages, ID < 0x80 */ LOG_FORMAT(ATT, "fffffffffffff", "qw,qx,qy,qz,Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), - LOG_FORMAT_S(IMU, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), - LOG_FORMAT_S(IMU1, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), - LOG_FORMAT_S(IMU2, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), + LOG_FORMAT_S(IMU, IMU, "ffffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ,tA,tG,tM"), + LOG_FORMAT_S(IMU1, IMU, "ffffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ,tA,tG,tM"), + LOG_FORMAT_S(IMU2, IMU, "ffffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ,tA,tG,tM"), LOG_FORMAT_S(SENS, SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), LOG_FORMAT_S(AIR1, SENS, "fffff", "BaroPa,BaroAlt,BaroTmp,DiffPres,DiffPresF"), LOG_FORMAT(LPOS, "ffffffffLLfBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,GFlg,EPH,EPV"), From 9e6d318d687dfdd4d549b5f56517006126032241 Mon Sep 17 00:00:00 2001 From: Matt Beall Date: Fri, 27 Mar 2015 10:39:18 -0600 Subject: [PATCH 099/268] Added flag to uorb publish wrapper...resolves no actuators in manual mode in fw_att_control --- src/modules/fw_att_control/fw_att_control_main.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 06df5fd974..8b41144f6f 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -1079,7 +1079,8 @@ FixedwingAttitudeControl::task_main() /* Only publish if any of the proper modes are enabled */ if(_vcontrol_mode.flag_control_rates_enabled || - _vcontrol_mode.flag_control_attitude_enabled) + _vcontrol_mode.flag_control_attitude_enabled || + _vcontrol_mode.flag_control_manual_enabled) { /* publish the actuator controls */ if (_actuators_0_pub > 0) { From e2499bbb238036856ee25fc72f9d8294090b4619 Mon Sep 17 00:00:00 2001 From: Hessel van der Molen Date: Fri, 27 Mar 2015 16:59:48 +0100 Subject: [PATCH 100/268] fixed missing float fields in IMU sdlog2 format --- .gitignore | 2 +- src/modules/sdlog2/sdlog2_messages.h | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.gitignore b/.gitignore index 0e553fa365..bd835e271f 100644 --- a/.gitignore +++ b/.gitignore @@ -47,4 +47,4 @@ unittests/build *.generated.h .vagrant *.pretty - +xcode diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 17401218ae..49483b15a2 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -474,9 +474,9 @@ static const struct log_format_s log_formats[] = { /* business-level messages, ID < 0x80 */ LOG_FORMAT(ATT, "fffffffffffff", "qw,qx,qy,qz,Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), - LOG_FORMAT_S(IMU, IMU, "ffffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ,tA,tG,tM"), - LOG_FORMAT_S(IMU1, IMU, "ffffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ,tA,tG,tM"), - LOG_FORMAT_S(IMU2, IMU, "ffffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ,tA,tG,tM"), + LOG_FORMAT_S(IMU, IMU, "ffffffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ,tA,tG,tM"), + LOG_FORMAT_S(IMU1, IMU, "ffffffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ,tA,tG,tM"), + LOG_FORMAT_S(IMU2, IMU, "ffffffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ,tA,tG,tM"), LOG_FORMAT_S(SENS, SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), LOG_FORMAT_S(AIR1, SENS, "fffff", "BaroPa,BaroAlt,BaroTmp,DiffPres,DiffPresF"), LOG_FORMAT(LPOS, "ffffffffLLfBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,GFlg,EPH,EPV"), From 5cc1a5dfdae53626c3cdf35e3d7b60f1a4ca68ed Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 27 Mar 2015 19:08:44 -0400 Subject: [PATCH 101/268] check_code_style exclude eigen --- Tools/check_code_style.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/check_code_style.sh b/Tools/check_code_style.sh index 2353a52326..b8806f3164 100755 --- a/Tools/check_code_style.sh +++ b/Tools/check_code_style.sh @@ -3,6 +3,7 @@ set -eu failed=0 for fn in $(find . -path './src/lib/uavcan' -prune -o \ -path './src/lib/mathlib/CMSIS' -prune -o \ + -path './src/lib/eigen' -prune -o \ -path './src/modules/attitude_estimator_ekf/codegen' -prune -o \ -path './NuttX' -prune -o \ -path './Build' -prune -o \ From 8aae66b893444c74a22ad7beb89e3828e0444108 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 27 Mar 2015 19:08:44 -0400 Subject: [PATCH 102/268] trivial code style cleanup round 2 --- src/drivers/airspeed/airspeed.h | 6 +++--- src/drivers/ardrone_interface/ardrone_motor_control.h | 3 ++- src/drivers/boards/aerocore/aerocore_spi.c | 4 ++-- src/drivers/boards/px4fmu-v1/px4fmu_spi.c | 4 ++-- src/drivers/boards/px4io-v2/board_config.h | 4 ++-- src/drivers/device/i2c.h | 6 +++--- src/drivers/drv_hrt.h | 3 ++- src/drivers/drv_rgbled.h | 4 ++-- src/drivers/frsky_telemetry/frsky_data.c | 3 ++- src/drivers/md25/BlockSysIdent.hpp | 3 ++- src/lib/mathlib/math/Limits.cpp | 5 +++-- src/lib/mathlib/math/Limits.hpp | 3 ++- src/lib/mathlib/math/test/test.cpp | 6 +++--- src/lib/rc/st24.c | 3 ++- src/modules/commander/calibration_routines.cpp | 3 ++- src/modules/commander/gyro_calibration.cpp | 4 ++-- src/modules/controllib/block/BlockParam.hpp | 4 ++-- src/modules/controllib/blocks.hpp | 5 +++-- src/modules/fw_att_control/fw_att_control_params.c | 3 ++- src/modules/fw_pos_control_l1/mtecs/mTecs.h | 6 +++--- src/modules/gpio_led/gpio_led.c | 2 ++ src/modules/land_detector/LandDetector.cpp | 4 ++-- src/modules/land_detector/MulticopterLandDetector.h | 4 ++-- src/modules/mavlink/mavlink_receiver.h | 4 ++-- src/modules/mavlink/mavlink_stream.h | 6 +++--- src/modules/mc_att_control_multiplatform/mc_att_control.h | 6 +++--- src/modules/navigator/mission_block.h | 6 +++--- src/modules/navigator/mission_feasibility_checker.h | 3 ++- src/modules/navigator/navigator_mode.cpp | 3 ++- src/modules/navigator/navigator_mode.h | 4 ++-- src/modules/px4iofirmware/px4io.h | 3 ++- src/modules/systemlib/cpuload.c | 3 ++- src/modules/systemlib/hx_stream.h | 4 ++-- src/modules/systemlib/mcu_version.h | 4 ++-- src/modules/systemlib/mixer/mixer_group.cpp | 4 +++- src/modules/systemlib/pwm_limit/pwm_limit.h | 3 ++- src/modules/systemlib/systemlib.c | 5 +++-- src/modules/uORB/topics/geofence_result.h | 3 +-- src/modules/uORB/topics/home_position.h | 3 +-- src/modules/uORB/topics/mission_result.h | 3 +-- src/modules/uORB/topics/tecs_status.h | 6 +++--- src/modules/uORB/uORB.h | 5 +++-- src/modules/uavcan/sensors/baro.hpp | 6 +++--- src/modules/uavcan/sensors/gnss.hpp | 6 +++--- src/modules/uavcan/sensors/mag.hpp | 6 +++--- src/modules/uavcan/uavcan_main.hpp | 6 +++--- src/modules/unit_test/unit_test.cpp | 5 +++-- src/platforms/px4_message.h | 6 +++--- src/platforms/ros/nodes/commander/commander.h | 6 +++--- .../ros/nodes/position_estimator/position_estimator.cpp | 4 +++- src/systemcmds/perf/perf.c | 2 ++ src/systemcmds/reboot/reboot.c | 3 ++- src/systemcmds/tests/test_adc.c | 5 +++-- src/systemcmds/tests/test_hott_telemetry.c | 3 ++- src/systemcmds/tests/test_uart_baudchange.c | 3 ++- unittests/sumd_test.cpp | 5 +++-- 56 files changed, 132 insertions(+), 104 deletions(-) diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index d6a88714b6..cdaf244dd1 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -90,7 +90,7 @@ static const int ERROR = -1; class __EXPORT Airspeed : public device::I2C { public: - Airspeed(int bus, int address, unsigned conversion_interval, const char* path); + Airspeed(int bus, int address, unsigned conversion_interval, const char *path); virtual ~Airspeed(); virtual int init(); @@ -108,8 +108,8 @@ private: perf_counter_t _buffer_overflows; /* this class has pointer data members and should not be copied */ - Airspeed(const Airspeed&); - Airspeed& operator=(const Airspeed&); + Airspeed(const Airspeed &); + Airspeed &operator=(const Airspeed &); protected: virtual int probe(); diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.h b/src/drivers/ardrone_interface/ardrone_motor_control.h index 78b603b63a..dcc49a4727 100644 --- a/src/drivers/ardrone_interface/ardrone_motor_control.h +++ b/src/drivers/ardrone_interface/ardrone_motor_control.h @@ -85,7 +85,8 @@ int ar_init_motors(int ardrone_uart, int gpio); /** * Set LED pattern. */ -void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green); +void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, + uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green); /** * Mix motors and output actuators diff --git a/src/drivers/boards/aerocore/aerocore_spi.c b/src/drivers/boards/aerocore/aerocore_spi.c index e329bd9d1a..0fa474a8c1 100644 --- a/src/drivers/boards/aerocore/aerocore_spi.c +++ b/src/drivers/boards/aerocore/aerocore_spi.c @@ -136,8 +136,8 @@ __EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, switch (devid) { case PX4_SPIDEV_GYRO: - /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); break; diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_spi.c b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c index 17e6862f7c..16f94a9f23 100644 --- a/src/drivers/boards/px4fmu-v1/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c @@ -90,8 +90,8 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, switch (devid) { case PX4_SPIDEV_GYRO: - /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1); break; diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index 10a93be0bf..99baee41df 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -50,7 +50,7 @@ /* these headers are not C++ safe */ #include #include - + /****************************************************************************** * Definitions ******************************************************************************/ @@ -117,7 +117,7 @@ #define GPIO_SBUS_OUTPUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) #define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4) -/* +/* * High-resolution timer */ #define HRT_TIMER 1 /* use timer1 for the HRT */ diff --git a/src/drivers/device/i2c.h b/src/drivers/device/i2c.h index 7bb4ae1aff..cb13fed83c 100644 --- a/src/drivers/device/i2c.h +++ b/src/drivers/device/i2c.h @@ -63,7 +63,7 @@ public: static int set_bus_clock(unsigned bus, unsigned clock_hz); static unsigned int _bus_clocks[3]; - + protected: /** * The number of times a read or write operation will be retried on @@ -144,8 +144,8 @@ private: uint32_t _frequency; struct i2c_dev_s *_dev; - I2C(const device::I2C&); - I2C operator=(const device::I2C&); + I2C(const device::I2C &); + I2C operator=(const device::I2C &); }; } // namespace device diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index 8bfc90c64b..a40943d3f2 100644 --- a/src/drivers/drv_hrt.h +++ b/src/drivers/drv_hrt.h @@ -127,7 +127,8 @@ __EXPORT extern void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, h * Note thet the interval is timed between scheduled, not actual, call times, so the call rate may * jitter but should not drift. */ -__EXPORT extern void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg); +__EXPORT extern void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, + hrt_callout callout, void *arg); /* * If this returns true, the entry has been invoked and removed from the callout list, diff --git a/src/drivers/drv_rgbled.h b/src/drivers/drv_rgbled.h index b10caf56a1..0633768684 100644 --- a/src/drivers/drv_rgbled.h +++ b/src/drivers/drv_rgbled.h @@ -58,7 +58,7 @@ /** play the numbered script in (arg), repeating forever */ #define RGBLED_PLAY_SCRIPT _RGBLEDIOC(2) -/** +/** * Set the user script; (arg) is a pointer to an array of script lines, * where each line is an array of four bytes giving , , arg[0-2] * @@ -79,7 +79,7 @@ #define RGBLED_SET_PATTERN _RGBLEDIOC(7) -/* +/* structure passed to RGBLED_SET_RGB ioctl() Note that the driver scales the brightness to 0 to 255, regardless of the hardware scaling diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index 890fada168..de22a888d3 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -222,6 +222,7 @@ void frsky_send_frame2(int uart) float course = 0, lat = 0, lon = 0, speed = 0, alt = 0; char lat_ns = 0, lon_ew = 0; int sec = 0; + if (global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { time_t time_gps = global_pos.time_utc_usec / 1000000ULL; struct tm *tm_gps = gmtime(&time_gps); @@ -232,7 +233,7 @@ void frsky_send_frame2(int uart) lon = frsky_format_gps(fabsf(global_pos.lon)); lon_ew = (global_pos.lon < 0) ? 'W' : 'E'; speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e) - * 25.0f / 46.0f; + * 25.0f / 46.0f; alt = global_pos.alt; sec = tm_gps->tm_sec; } diff --git a/src/drivers/md25/BlockSysIdent.hpp b/src/drivers/md25/BlockSysIdent.hpp index 270635f407..e8dd4ee9c5 100644 --- a/src/drivers/md25/BlockSysIdent.hpp +++ b/src/drivers/md25/BlockSysIdent.hpp @@ -1,7 +1,8 @@ #include #include -class BlockSysIdent : public control::Block { +class BlockSysIdent : public control::Block +{ public: BlockSysIdent(); private: diff --git a/src/lib/mathlib/math/Limits.cpp b/src/lib/mathlib/math/Limits.cpp index e16f33bd68..0ae545f1a5 100644 --- a/src/lib/mathlib/math/Limits.cpp +++ b/src/lib/mathlib/math/Limits.cpp @@ -44,9 +44,10 @@ #include "Limits.hpp" -namespace math { +namespace math +{ -#ifndef CONFIG_ARCH_ARM +#ifndef CONFIG_ARCH_ARM #define M_PI_F 3.14159265358979323846f #endif diff --git a/src/lib/mathlib/math/Limits.hpp b/src/lib/mathlib/math/Limits.hpp index fca4197b8d..f04d725078 100644 --- a/src/lib/mathlib/math/Limits.hpp +++ b/src/lib/mathlib/math/Limits.hpp @@ -42,7 +42,8 @@ #include #include -namespace math { +namespace math +{ float __EXPORT min(float val1, float val2); diff --git a/src/lib/mathlib/math/test/test.cpp b/src/lib/mathlib/math/test/test.cpp index 2fa2f7e7c4..c52771ab8c 100644 --- a/src/lib/mathlib/math/test/test.cpp +++ b/src/lib/mathlib/math/test/test.cpp @@ -51,7 +51,7 @@ bool __EXPORT equal(float a, float b, float epsilon) printf("not equal ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b)); return false; - } else return true; + } else { return true; } } void __EXPORT float2SigExp( @@ -84,10 +84,10 @@ void __EXPORT float2SigExp( // cheap power since it is integer if (exp > 0) { - for (int i = 0; i < abs(exp); i++) sig /= 10; + for (int i = 0; i < abs(exp); i++) { sig /= 10; } } else { - for (int i = 0; i < abs(exp); i++) sig *= 10; + for (int i = 0; i < abs(exp); i++) { sig *= 10; } } } diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index 5c53a1602f..f758279035 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -104,7 +104,7 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len) int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels, - uint16_t max_chan_count) + uint16_t max_chan_count) { int ret = 1; @@ -113,6 +113,7 @@ int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe case ST24_DECODE_STATE_UNSYNCED: if (byte == ST24_STX1) { _decode_state = ST24_DECODE_STATE_GOT_STX1; + } else { ret = 3; } diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 5796204bfd..290e83a0ba 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -45,7 +45,8 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[], - unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius) + unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, + float *sphere_radius) { float x_sumplain = 0.0f; diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 8a70cf66e7..d4a6d6801c 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -245,7 +245,7 @@ int do_gyro_calibration(int mavlink_fd) mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); } } - + if (failed) { mavlink_and_console_log_critical(mavlink_fd, "ERROR: failed to set offset params"); res = ERROR; @@ -376,7 +376,7 @@ int do_gyro_calibration(int mavlink_fd) } } - #endif +#endif if (res == OK) { /* auto-save to EEPROM */ diff --git a/src/modules/controllib/block/BlockParam.hpp b/src/modules/controllib/block/BlockParam.hpp index a64d0139e3..437e43bfb4 100644 --- a/src/modules/controllib/block/BlockParam.hpp +++ b/src/modules/controllib/block/BlockParam.hpp @@ -58,7 +58,7 @@ public: * * @param parent_prefix Set to true to include the parent name in the parameter name */ - BlockParamBase(Block *parent, const char *name, bool parent_prefix=true); + BlockParamBase(Block *parent, const char *name, bool parent_prefix = true); virtual ~BlockParamBase() {}; virtual void update() = 0; const char *getName() { return param_name(_handle); } @@ -75,7 +75,7 @@ class BlockParam : public BlockParamBase { public: BlockParam(Block *block, const char *name, - bool parent_prefix=true); + bool parent_prefix = true); T get(); void set(T val); void update(); diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index bffc355a82..65600190b2 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -114,7 +114,7 @@ public: // methods BlockLowPass(SuperBlock *parent, const char *name) : Block(parent, name), - _state(0.0f/0.0f /* initialize to invalid val, force into is_finite() check on first call */), + _state(0.0f / 0.0f /* initialize to invalid val, force into is_finite() check on first call */), _fCut(this, "") // only one parameter, no need to name {}; virtual ~BlockLowPass() {}; @@ -492,8 +492,9 @@ public: X = V1 * float(sqrt(-2 * float(log(S)) / S)); - } else + } else { X = V2 * float(sqrt(-2 * float(log(S)) / S)); + } phase = 1 - phase; return X * getStdDev() + getMean(); diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 6ab3ddbfc8..cfc5ae9654 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -136,7 +136,8 @@ PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.2f); * @max 2.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f); //xxx: set to 0 as default, see comment in ECL_PitchController::control_attitude (float turn_offset = ...) +PARAM_DEFINE_FLOAT(FW_P_ROLLFF, + 0.0f); //xxx: set to 0 as default, see comment in ECL_PitchController::control_attitude (float turn_offset = ...) /** * Roll rate proportional Gain diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index ae6867d382..4c2db0c64d 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -65,19 +65,19 @@ public: * Control in altitude setpoint and speed mode */ int updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, - float airspeedSp, tecs_mode mode, LimitOverride limitOverride); + float airspeedSp, tecs_mode mode, LimitOverride limitOverride); /* * Control in flightPathAngle setpoint (flollow a slope etc.) and speed mode */ int updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, - float airspeedSp, tecs_mode mode, LimitOverride limitOverride); + float airspeedSp, tecs_mode mode, LimitOverride limitOverride); /* * Control in flightPathAngle setpoint (flollow a slope etc.) and acceleration mode (base case) */ int updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeedFiltered, - float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride); + float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride); /* * Reset all integrators diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index b759b429ef..2ff3fc2767 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -184,11 +184,13 @@ int gpio_led_main(int argc, char *argv[]) warnx("start, using pin: %s", pin_name); exit(0); } + } else if (!strcmp(argv[1], "stop")) { if (gpio_led_started) { gpio_led_started = false; warnx("stop"); exit(0); + } else { errx(1, "not running"); } diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index a4fbb9861f..7c830fc072 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -46,8 +46,8 @@ LandDetector::LandDetector() : _landDetectedPub(-1), _landDetected({0, false}), - _taskShouldExit(false), - _taskIsRunning(false) + _taskShouldExit(false), + _taskIsRunning(false) { // ctor } diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 70df91f43c..8c57416b56 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -102,13 +102,13 @@ private: int _actuatorsSub; int _armingSub; int _parameterSub; - int _attitudeSub; + int _attitudeSub; struct vehicle_global_position_s _vehicleGlobalPosition; /**< the result from global position subscription */ struct vehicle_status_s _vehicleStatus; struct actuator_controls_s _actuators; struct actuator_armed_s _arming; - struct vehicle_attitude_s _vehicleAttitude; + struct vehicle_attitude_s _vehicleAttitude; uint64_t _landTimer; /**< timestamp in microseconds since a possible land was detected*/ }; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index b510dbbb77..1d214b948f 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -188,6 +188,6 @@ private: uint64_t _time_offset; /* do not allow copying this class */ - MavlinkReceiver(const MavlinkReceiver&); - MavlinkReceiver operator=(const MavlinkReceiver&); + MavlinkReceiver(const MavlinkReceiver &); + MavlinkReceiver operator=(const MavlinkReceiver &); }; diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index ef22dc443a..5e39bbbdf9 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -89,7 +89,7 @@ public: virtual unsigned get_size() = 0; protected: - Mavlink * _mavlink; + Mavlink *_mavlink; unsigned int _interval; virtual void send(const hrt_abstime t) = 0; @@ -98,8 +98,8 @@ private: hrt_abstime _last_sent; /* do not allow top copying this class */ - MavlinkStream(const MavlinkStream&); - MavlinkStream& operator=(const MavlinkStream&); + MavlinkStream(const MavlinkStream &); + MavlinkStream &operator=(const MavlinkStream &); }; diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.h b/src/modules/mc_att_control_multiplatform/mc_att_control.h index 52ba369c1a..1242c7fe37 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control.h +++ b/src/modules/mc_att_control_multiplatform/mc_att_control.h @@ -88,9 +88,9 @@ private: bool _task_should_exit; /**< if true, sensor task should exit */ bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ - px4::Publisher * _att_sp_pub; /**< attitude setpoint publication */ - px4::Publisher * _v_rates_sp_pub; /**< rate setpoint publication */ - px4::Publisher * _actuators_0_pub; /**< attitude actuator controls publication */ + px4::Publisher *_att_sp_pub; /**< attitude setpoint publication */ + px4::Publisher *_v_rates_sp_pub; /**< rate setpoint publication */ + px4::Publisher *_actuators_0_pub; /**< attitude actuator controls publication */ px4::NodeHandle _n; diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index adf50bc391..ec3e305825 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -83,9 +83,9 @@ protected: */ void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp); - /** - * Set previous position setpoint to current setpoint - */ + /** + * Set previous position setpoint to current setpoint + */ void set_previous_pos_setpoint(); /** diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 96c9209d3d..9a77a6dc25 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -78,7 +78,8 @@ public: /* * Returns true if mission is feasible and false otherwise */ - bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); + bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, + float home_alt); }; diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index 2f322031c3..f7ccda0cbd 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -58,7 +58,8 @@ NavigatorMode::~NavigatorMode() } void -NavigatorMode::run(bool active) { +NavigatorMode::run(bool active) +{ if (active) { if (_first_run) { /* first run */ diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h index de5545dcb1..0839d9be60 100644 --- a/src/modules/navigator/navigator_mode.h +++ b/src/modules/navigator/navigator_mode.h @@ -92,8 +92,8 @@ private: /* this class has ptr data members, so it should not be copied, * consequently the copy constructors are private. */ - NavigatorMode(const NavigatorMode&); - NavigatorMode operator=(const NavigatorMode&); + NavigatorMode(const NavigatorMode &); + NavigatorMode operator=(const NavigatorMode &); }; #endif diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 93a33490fa..8192c5b503 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -219,7 +219,8 @@ extern int dsm_init(const char *device); extern bool dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes); extern void dsm_bind(uint16_t cmd, int pulses); extern int sbus_init(const char *device); -extern bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); +extern bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, + uint16_t max_channels); extern void sbus1_output(uint16_t *values, uint16_t num_values); extern void sbus2_output(uint16_t *values, uint16_t num_values); diff --git a/src/modules/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c index 7aa2f3a5fa..b41896abd9 100644 --- a/src/modules/systemlib/cpuload.c +++ b/src/modules/systemlib/cpuload.c @@ -97,7 +97,8 @@ void cpuload_initialize_once() system_load.tasks[system_load.total_count].start_time = now; system_load.tasks[system_load.total_count].total_runtime = 0; system_load.tasks[system_load.total_count].curr_start_time = 0; - system_load.tasks[system_load.total_count].tcb = sched_gettcb(system_load.total_count); // it is assumed that these static threads have consecutive PIDs + system_load.tasks[system_load.total_count].tcb = sched_gettcb( + system_load.total_count); // it is assumed that these static threads have consecutive PIDs system_load.tasks[system_load.total_count].valid = true; } } diff --git a/src/modules/systemlib/hx_stream.h b/src/modules/systemlib/hx_stream.h index 1f3927222a..b674f192d5 100644 --- a/src/modules/systemlib/hx_stream.h +++ b/src/modules/systemlib/hx_stream.h @@ -103,7 +103,7 @@ __EXPORT extern void hx_stream_reset(hx_stream_t stream); /** * Prepare to send a frame. * - * Use this in conjunction with hx_stream_send_next to + * Use this in conjunction with hx_stream_send_next to * set the frame to be transmitted. * * Use hx_stream_send() to write to the stream fd directly. @@ -124,7 +124,7 @@ __EXPORT extern int hx_stream_start(hx_stream_t stream, * calling hx_stream_start first. * * @param stream A handle returned from hx_stream_init. - * @return The byte to send, or -1 if there is + * @return The byte to send, or -1 if there is * nothing left to send. */ __EXPORT extern int hx_stream_send_next(hx_stream_t stream); diff --git a/src/modules/systemlib/mcu_version.h b/src/modules/systemlib/mcu_version.h index e951e1e390..f40ac069e1 100644 --- a/src/modules/systemlib/mcu_version.h +++ b/src/modules/systemlib/mcu_version.h @@ -50,7 +50,7 @@ enum MCU_REV { /** * Reports the microcontroller unique id. * - * This ID is guaranteed to be unique for every mcu. + * This ID is guaranteed to be unique for every mcu. * @param uid_96_bit A uint32_t[3] array to copy the data to. */ __EXPORT void mcu_unique_id(uint32_t *uid_96_bit); @@ -62,6 +62,6 @@ __EXPORT void mcu_unique_id(uint32_t *uid_96_bit); * @param revstr The full chip name string * @return The silicon revision / version number as integer */ -__EXPORT int mcu_version(char* rev, char** revstr); +__EXPORT int mcu_version(char *rev, char **revstr); __END_DECLS diff --git a/src/modules/systemlib/mixer/mixer_group.cpp b/src/modules/systemlib/mixer/mixer_group.cpp index 3ed99fba09..4220b168d3 100644 --- a/src/modules/systemlib/mixer/mixer_group.cpp +++ b/src/modules/systemlib/mixer/mixer_group.cpp @@ -76,8 +76,9 @@ MixerGroup::add_mixer(Mixer *mixer) mpp = &_first; - while (*mpp != nullptr) + while (*mpp != nullptr) { mpp = &((*mpp)->_next); + } *mpp = mixer; mixer->_next = nullptr; @@ -185,6 +186,7 @@ MixerGroup::load_from_buf(const char *buf, unsigned &buflen) /* only adjust buflen if parsing was successful */ buflen = resid; debug("SUCCESS - buflen: %d", buflen); + } else { /* diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.h b/src/modules/systemlib/pwm_limit/pwm_limit.h index 6a667ac6fc..6ec9f4b7e2 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.h +++ b/src/modules/systemlib/pwm_limit/pwm_limit.h @@ -72,7 +72,8 @@ typedef struct { __EXPORT void pwm_limit_init(pwm_limit_t *limit); -__EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit); +__EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, + const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit); __END_DECLS diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c index 82183b0d72..280c30023d 100644 --- a/src/modules/systemlib/systemlib.c +++ b/src/modules/systemlib/systemlib.c @@ -66,10 +66,11 @@ systemreset(bool to_bootloader) /* XXX wow, this is evil - write a magic number into backup register zero */ *(uint32_t *)0x40002850 = 0xb007b007; } + up_systemreset(); /* lock up here */ - while(true); + while (true); } static void kill_task(FAR struct tcb_s *tcb, FAR void *arg); @@ -87,7 +88,7 @@ static void kill_task(FAR struct tcb_s *tcb, FAR void *arg) kill(tcb->pid, SIGUSR1); } -int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, char * const argv[]) +int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, char *const argv[]) { int pid; diff --git a/src/modules/uORB/topics/geofence_result.h b/src/modules/uORB/topics/geofence_result.h index b07e044993..8d32dfc0ae 100644 --- a/src/modules/uORB/topics/geofence_result.h +++ b/src/modules/uORB/topics/geofence_result.h @@ -50,8 +50,7 @@ * @{ */ -struct geofence_result_s -{ +struct geofence_result_s { bool geofence_violated; /**< true if the geofence is violated */ }; diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h index d747b5f435..02e17cdd7f 100644 --- a/src/modules/uORB/topics/home_position.h +++ b/src/modules/uORB/topics/home_position.h @@ -55,8 +55,7 @@ /** * GPS home position in WGS84 coordinates. */ -struct home_position_s -{ +struct home_position_s { uint64_t timestamp; /**< Timestamp (microseconds since system boot) */ double lat; /**< Latitude in degrees */ diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h index 2ddc529a3f..16e7f2f126 100644 --- a/src/modules/uORB/topics/mission_result.h +++ b/src/modules/uORB/topics/mission_result.h @@ -53,8 +53,7 @@ * @{ */ -struct mission_result_s -{ +struct mission_result_s { unsigned seq_reached; /**< Sequence of the mission item which has been reached */ unsigned seq_current; /**< Sequence of the current mission item */ bool reached; /**< true if mission has been reached */ diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h index ddca19d61e..3c858901c7 100644 --- a/src/modules/uORB/topics/tecs_status.h +++ b/src/modules/uORB/topics/tecs_status.h @@ -60,9 +60,9 @@ typedef enum { TECS_MODE_CLIMBOUT } tecs_mode; - /** - * Internal values of the (m)TECS fixed wing speed alnd altitude control system - */ +/** +* Internal values of the (m)TECS fixed wing speed alnd altitude control system +*/ struct tecs_status_s { uint64_t timestamp; /**< timestamp, in microseconds since system start */ diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index b54f0322b8..44dc6614fe 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -166,7 +166,7 @@ extern orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *d * node in /obj if required and publishes the initial data. * * Any number of advertisers may publish to a topic; publications are atomic - * but co-ordination between publishers is not provided by the ORB. + * but co-ordination between publishers is not provided by the ORB. * * @param meta The uORB metadata (usually from the ORB_ID() macro) * for the topic. @@ -184,7 +184,8 @@ extern orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *d * ORB_DEFINE with no corresponding ORB_DECLARE) * this function will return -1 and set errno to ENOENT. */ -extern orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority) __EXPORT; +extern orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, + int priority) __EXPORT; /** diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp index c7bbc5af8d..16cd476ea7 100644 --- a/src/modules/uavcan/sensors/baro.hpp +++ b/src/modules/uavcan/sensors/baro.hpp @@ -47,7 +47,7 @@ class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase public: static const char *const NAME; - UavcanBarometerBridge(uavcan::INode& node); + UavcanBarometerBridge(uavcan::INode &node); const char *get_name() const override { return NAME; } @@ -58,9 +58,9 @@ private: void air_data_sub_cb(const uavcan::ReceivedDataStructure &msg); - typedef uavcan::MethodBinder&)> + (const uavcan::ReceivedDataStructure &) > AirDataCbBinder; uavcan::Subscriber _sub_air_data; diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp index 96ff9404f5..4b94d9306f 100644 --- a/src/modules/uavcan/sensors/gnss.hpp +++ b/src/modules/uavcan/sensors/gnss.hpp @@ -56,7 +56,7 @@ class UavcanGnssBridge : public IUavcanSensorBridge public: static const char *const NAME; - UavcanGnssBridge(uavcan::INode& node); + UavcanGnssBridge(uavcan::INode &node); const char *get_name() const override { return NAME; } @@ -72,8 +72,8 @@ private: */ void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg); - typedef uavcan::MethodBinder&)> + typedef uavcan::MethodBinder < UavcanGnssBridge *, + void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure &) > FixCbBinder; uavcan::INode &_node; diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp index db38aee1d3..b74afeb0be 100644 --- a/src/modules/uavcan/sensors/mag.hpp +++ b/src/modules/uavcan/sensors/mag.hpp @@ -47,7 +47,7 @@ class UavcanMagnetometerBridge : public UavcanCDevSensorBridgeBase public: static const char *const NAME; - UavcanMagnetometerBridge(uavcan::INode& node); + UavcanMagnetometerBridge(uavcan::INode &node); const char *get_name() const override { return NAME; } @@ -59,9 +59,9 @@ private: void mag_sub_cb(const uavcan::ReceivedDataStructure &msg); - typedef uavcan::MethodBinder&)> + (const uavcan::ReceivedDataStructure &) > MagCbBinder; uavcan::Subscriber _sub_mag; diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 19b9b4b484..96b3ec1a60 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -82,7 +82,7 @@ public: static int start(uavcan::NodeID node_id, uint32_t bitrate); - Node& get_node() { return _node; } + Node &get_node() { return _node; } // TODO: move the actuator mixing stuff into the ESC controller class static int control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input); @@ -94,7 +94,7 @@ public: void print_info(); - static UavcanNode* instance() { return _instance; } + static UavcanNode *instance() { return _instance; } private: void fill_node_info(); @@ -122,7 +122,7 @@ private: UavcanEscController _esc_controller; - List _sensor_bridges; ///< List of active sensor bridges + List _sensor_bridges; ///< List of active sensor bridges MixerGroup *_mixers = nullptr; diff --git a/src/modules/unit_test/unit_test.cpp b/src/modules/unit_test/unit_test.cpp index b7568ce3a4..c45d7888ab 100644 --- a/src/modules/unit_test/unit_test.cpp +++ b/src/modules/unit_test/unit_test.cpp @@ -57,13 +57,14 @@ void UnitTest::print_results(void) } /// @brief Used internally to the ut_assert macro to print assert failures. -void UnitTest::_print_assert(const char* msg, const char* test, const char* file, int line) +void UnitTest::_print_assert(const char *msg, const char *test, const char *file, int line) { warnx("Assertion failed: %s - %s (%s:%d)", msg, test, file, line); } /// @brief Used internally to the ut_compare macro to print assert failures. -void UnitTest::_print_compare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line) +void UnitTest::_print_compare(const char *msg, const char *v1_text, int v1, const char *v2_text, int v2, + const char *file, int line) { warnx("Compare failed: %s - (%s:%d) (%s:%d) (%s:%d)", msg, v1_text, v1, v2_text, v2, file, line); } diff --git a/src/platforms/px4_message.h b/src/platforms/px4_message.h index bff7aa3133..499a5dd8b1 100644 --- a/src/platforms/px4_message.h +++ b/src/platforms/px4_message.h @@ -39,7 +39,7 @@ #pragma once #if defined(__PX4_ROS) -typedef const char* PX4TopicHandle; +typedef const char *PX4TopicHandle; #else #include typedef orb_id_t PX4TopicHandle; @@ -68,8 +68,8 @@ public: virtual ~PX4Message() {}; - virtual M& data() {return _data;} - virtual const M& data() const {return _data;} + virtual M &data() {return _data;} + virtual const M &data() const {return _data;} private: M _data; }; diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h index c537c84193..fc88260ba8 100644 --- a/src/platforms/ros/nodes/commander/commander.h +++ b/src/platforms/ros/nodes/commander/commander.h @@ -68,14 +68,14 @@ protected: * Set control mode flags based on stick positions (equiv to code in px4 commander) */ void EvalSwitches(const px4::manual_control_setpointConstPtr &msg, - px4::vehicle_status &msg_vehicle_status, - px4::vehicle_control_mode &msg_vehicle_control_mode); + px4::vehicle_status &msg_vehicle_status, + px4::vehicle_control_mode &msg_vehicle_control_mode); /** * Sets offboard controll flags in msg_vehicle_control_mode */ void SetOffboardControl(const px4::offboard_control_mode &msg_offboard_control_mode, - px4::vehicle_control_mode &msg_vehicle_control_mode); + px4::vehicle_control_mode &msg_vehicle_control_mode); ros::NodeHandle _n; ros::Subscriber _man_ctrl_sp_sub; diff --git a/src/platforms/ros/nodes/position_estimator/position_estimator.cpp b/src/platforms/ros/nodes/position_estimator/position_estimator.cpp index ed3a4efa5c..e4273687e7 100644 --- a/src/platforms/ros/nodes/position_estimator/position_estimator.cpp +++ b/src/platforms/ros/nodes/position_estimator/position_estimator.cpp @@ -64,14 +64,16 @@ void PositionEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstP /* Fill px4 position topic with contents from modelstates topic */ int index = 0; + //XXX: maybe a more clever approach would be to do this not on every loop, need to check if and when //gazebo rearranges indexes. - for(std::vector::const_iterator it = msg->name.begin(); it != msg->name.end(); ++it) { + for (std::vector::const_iterator it = msg->name.begin(); it != msg->name.end(); ++it) { if (*it == "iris" || *it == "ardrone") { index = it - msg->name.begin(); break; } } + msg_v_l_pos.xy_valid = true; msg_v_l_pos.z_valid = true; msg_v_l_pos.v_xy_valid = true; diff --git a/src/systemcmds/perf/perf.c b/src/systemcmds/perf/perf.c index a788dfc11c..4ab92dde6b 100644 --- a/src/systemcmds/perf/perf.c +++ b/src/systemcmds/perf/perf.c @@ -68,11 +68,13 @@ int perf_main(int argc, char *argv[]) if (strcmp(argv[1], "reset") == 0) { perf_reset_all(); return 0; + } else if (strcmp(argv[1], "latency") == 0) { perf_print_latency(0 /* stdout */); fflush(stdout); return 0; } + printf("Usage: perf [reset | latency]\n"); return -1; } diff --git a/src/systemcmds/reboot/reboot.c b/src/systemcmds/reboot/reboot.c index 91a2c2eb8d..d46f96545e 100644 --- a/src/systemcmds/reboot/reboot.c +++ b/src/systemcmds/reboot/reboot.c @@ -57,9 +57,10 @@ int reboot_main(int argc, char *argv[]) case 'b': to_bootloader = true; break; + default: errx(1, "usage: reboot [-b]\n" - " -b reboot into the bootloader"); + " -b reboot into the bootloader"); } } diff --git a/src/systemcmds/tests/test_adc.c b/src/systemcmds/tests/test_adc.c index 9f69052391..ef12c9ad2e 100644 --- a/src/systemcmds/tests/test_adc.c +++ b/src/systemcmds/tests/test_adc.c @@ -71,8 +71,9 @@ int test_adc(int argc, char *argv[]) /* read all channels available */ ssize_t count = read(fd, data, sizeof(data)); - if (count < 0) + if (count < 0) { goto errout_with_dev; + } unsigned channels = count / sizeof(data[0]); @@ -88,7 +89,7 @@ int test_adc(int argc, char *argv[]) errout_with_dev: - if (fd != 0) close(fd); + if (fd != 0) { close(fd); } return OK; } diff --git a/src/systemcmds/tests/test_hott_telemetry.c b/src/systemcmds/tests/test_hott_telemetry.c index 270dc38570..ef6173c9ee 100644 --- a/src/systemcmds/tests/test_hott_telemetry.c +++ b/src/systemcmds/tests/test_hott_telemetry.c @@ -190,7 +190,8 @@ int test_hott_telemetry(int argc, char *argv[]) warnx("PASS: Received %d out of %d valid byte pairs from the HoTT receiver device.", received_count, max_polls); } else { - warnx("WARN: Received %d out of %d byte pairs of which %d were valid from the HoTT receiver device.", received_count, max_polls, valid_count); + warnx("WARN: Received %d out of %d byte pairs of which %d were valid from the HoTT receiver device.", received_count, + max_polls, valid_count); } } else { diff --git a/src/systemcmds/tests/test_uart_baudchange.c b/src/systemcmds/tests/test_uart_baudchange.c index 609a65c624..cd2e561313 100644 --- a/src/systemcmds/tests/test_uart_baudchange.c +++ b/src/systemcmds/tests/test_uart_baudchange.c @@ -146,8 +146,9 @@ int test_uart_baudchange(int argc, char *argv[]) /* uart2 -> */ r = write(uart2, sample_uart2, sizeof(sample_uart2)); - if (r > 0) + if (r > 0) { uart2_nwrite += r; + } } close(uart2); diff --git a/unittests/sumd_test.cpp b/unittests/sumd_test.cpp index a919de31b7..bf99709475 100644 --- a/unittests/sumd_test.cpp +++ b/unittests/sumd_test.cpp @@ -8,8 +8,9 @@ #include "gtest/gtest.h" -TEST(SUMDTest, SUMD) { - const char* filepath = "testdata/sumd_data.txt"; +TEST(SUMDTest, SUMD) +{ + const char *filepath = "testdata/sumd_data.txt"; warnx("loading data from: %s", filepath); From 32e790110e766c0f6fbd5c439d15f2649df2c542 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Mar 2015 19:51:15 +0100 Subject: [PATCH 103/268] Tests: Fix code style on system tests --- src/systemcmds/tests/test_bson.c | 72 +++++++++++---- src/systemcmds/tests/test_conv.cpp | 8 +- src/systemcmds/tests/test_dataman.c | 38 ++++++-- src/systemcmds/tests/test_eigen.cpp | 101 ++++++++++++---------- src/systemcmds/tests/test_file.c | 52 ++++++----- src/systemcmds/tests/test_file2.c | 74 ++++++++++------ src/systemcmds/tests/test_float.c | 24 ++--- src/systemcmds/tests/test_hrt.c | 15 ++-- src/systemcmds/tests/test_mathlib.cpp | 47 +++++----- src/systemcmds/tests/test_mixer.cpp | 82 +++++++++++------- src/systemcmds/tests/test_mount.c | 28 ++++-- src/systemcmds/tests/test_ppm_loopback.c | 24 ++--- src/systemcmds/tests/test_sensors.c | 23 ++--- src/systemcmds/tests/test_servo.c | 10 ++- src/systemcmds/tests/test_time.c | 9 +- src/systemcmds/tests/test_uart_loopback.c | 21 +++-- src/systemcmds/tests/tests_main.c | 33 ++++--- 17 files changed, 424 insertions(+), 237 deletions(-) diff --git a/src/systemcmds/tests/test_bson.c b/src/systemcmds/tests/test_bson.c index 12d598df48..02384ebfeb 100644 --- a/src/systemcmds/tests/test_bson.c +++ b/src/systemcmds/tests/test_bson.c @@ -58,18 +58,29 @@ static const uint8_t sample_data[256] = {0}; static int encode(bson_encoder_t encoder) { - if (bson_encoder_append_bool(encoder, "bool1", sample_bool) != 0) + if (bson_encoder_append_bool(encoder, "bool1", sample_bool) != 0) { warnx("FAIL: encoder: append bool failed"); - if (bson_encoder_append_int(encoder, "int1", sample_small_int) != 0) + } + + if (bson_encoder_append_int(encoder, "int1", sample_small_int) != 0) { warnx("FAIL: encoder: append int failed"); - if (bson_encoder_append_int(encoder, "int2", sample_big_int) != 0) + } + + if (bson_encoder_append_int(encoder, "int2", sample_big_int) != 0) { warnx("FAIL: encoder: append int failed"); - if (bson_encoder_append_double(encoder, "double1", sample_double) != 0) + } + + if (bson_encoder_append_double(encoder, "double1", sample_double) != 0) { warnx("FAIL: encoder: append double failed"); - if (bson_encoder_append_string(encoder, "string1", sample_string) != 0) + } + + if (bson_encoder_append_string(encoder, "string1", sample_string) != 0) { warnx("FAIL: encoder: append string failed"); - if (bson_encoder_append_binary(encoder, "data1", BSON_BIN_BINARY, sizeof(sample_data), sample_data) != 0) + } + + if (bson_encoder_append_binary(encoder, "data1", BSON_BIN_BINARY, sizeof(sample_data), sample_data) != 0) { warnx("FAIL: encoder: append data failed"); + } bson_encoder_fini(encoder); @@ -86,51 +97,63 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node) warnx("FAIL: decoder: bool1 type %d, expected %d", node->type, BSON_BOOL); return 1; } + if (node->b != sample_bool) { - warnx("FAIL: decoder: bool1 value %s, expected %s", - (node->b ? "true" : "false"), - (sample_bool ? "true" : "false")); + warnx("FAIL: decoder: bool1 value %s, expected %s", + (node->b ? "true" : "false"), + (sample_bool ? "true" : "false")); return 1; } + warnx("PASS: decoder: bool1"); return 1; } + if (!strcmp(node->name, "int1")) { if (node->type != BSON_INT32) { warnx("FAIL: decoder: int1 type %d, expected %d", node->type, BSON_INT32); return 1; } + if (node->i != sample_small_int) { warnx("FAIL: decoder: int1 value %lld, expected %d", node->i, sample_small_int); return 1; } + warnx("PASS: decoder: int1"); return 1; } + if (!strcmp(node->name, "int2")) { if (node->type != BSON_INT64) { warnx("FAIL: decoder: int2 type %d, expected %d", node->type, BSON_INT64); return 1; } + if (node->i != sample_big_int) { warnx("FAIL: decoder: int2 value %lld, expected %lld", node->i, sample_big_int); return 1; } + warnx("PASS: decoder: int2"); return 1; } + if (!strcmp(node->name, "double1")) { if (node->type != BSON_DOUBLE) { warnx("FAIL: decoder: double1 type %d, expected %d", node->type, BSON_DOUBLE); return 1; } + if (fabs(node->d - sample_double) > 1e-12) { warnx("FAIL: decoder: double1 value %f, expected %f", node->d, sample_double); return 1; } + warnx("PASS: decoder: double1"); return 1; } + if (!strcmp(node->name, "string1")) { if (node->type != BSON_STRING) { warnx("FAIL: decoder: string1 type %d, expected %d", node->type, BSON_STRING); @@ -150,21 +173,26 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node) warnx("FAIL: decoder: string1 copy failed"); return 1; } + if (bson_decoder_data_pending(decoder) != 0) { warnx("FAIL: decoder: string1 copy did not exhaust all data"); return 1; } + if (sbuf[len - 1] != '\0') { warnx("FAIL: decoder: string1 not 0-terminated"); return 1; } + if (strcmp(sbuf, sample_string)) { warnx("FAIL: decoder: string1 value '%s', expected '%s'", sbuf, sample_string); return 1; } + warnx("PASS: decoder: string1"); return 1; } + if (!strcmp(node->name, "data1")) { if (node->type != BSON_BINDATA) { warnx("FAIL: decoder: data1 type %d, expected %d", node->type, BSON_BINDATA); @@ -177,7 +205,7 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node) warnx("FAIL: decoder: data1 length %d, expected %d", len, sizeof(sample_data)); return 1; } - + if (node->subtype != BSON_BIN_BINARY) { warnx("FAIL: decoder: data1 subtype %d, expected %d", node->subtype, BSON_BIN_BINARY); return 1; @@ -189,20 +217,25 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node) warnx("FAIL: decoder: data1 copy failed"); return 1; } + if (bson_decoder_data_pending(decoder) != 0) { warnx("FAIL: decoder: data1 copy did not exhaust all data"); return 1; } + if (memcmp(sample_data, dbuf, len)) { warnx("FAIL: decoder: data1 compare fail"); return 1; } + warnx("PASS: decoder: data1"); return 1; } - if (node->type != BSON_EOO) + if (node->type != BSON_EOO) { warnx("FAIL: decoder: unexpected node name '%s'", node->name); + } + return 1; } @@ -225,19 +258,28 @@ test_bson(int argc, char *argv[]) int len; /* encode data to a memory buffer */ - if (bson_encoder_init_buf(&encoder, NULL, 0)) + if (bson_encoder_init_buf(&encoder, NULL, 0)) { errx(1, "FAIL: bson_encoder_init_buf"); + } + encode(&encoder); len = bson_encoder_buf_size(&encoder); - if (len <= 0) + + if (len <= 0) { errx(1, "FAIL: bson_encoder_buf_len"); + } + buf = bson_encoder_buf_data(&encoder); - if (buf == NULL) + + if (buf == NULL) { errx(1, "FAIL: bson_encoder_buf_data"); + } /* now test-decode it */ - if (bson_decoder_init_buf(&decoder, buf, len, decode_callback, NULL)) + if (bson_decoder_init_buf(&decoder, buf, len, decode_callback, NULL)) { errx(1, "FAIL: bson_decoder_init_buf"); + } + decode(&decoder); free(buf); diff --git a/src/systemcmds/tests/test_conv.cpp b/src/systemcmds/tests/test_conv.cpp index fda949c610..9a41b19b95 100644 --- a/src/systemcmds/tests/test_conv.cpp +++ b/src/systemcmds/tests/test_conv.cpp @@ -61,11 +61,13 @@ int test_conv(int argc, char *argv[]) { warnx("Testing system conversions"); - for (int i = -10000; i <= 10000; i+=1) { - float f = i/10000.0f; + for (int i = -10000; i <= 10000; i += 1) { + float f = i / 10000.0f; float fres = REG_TO_FLOAT(FLOAT_TO_REG(f)); + if (fabsf(f - fres) > 0.0001f) { - warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", (double)f, REG_TO_SIGNED(FLOAT_TO_REG(f)), (double)fres); + warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", (double)f, REG_TO_SIGNED(FLOAT_TO_REG(f)), + (double)fres); return 1; } } diff --git a/src/systemcmds/tests/test_dataman.c b/src/systemcmds/tests/test_dataman.c index 1f844a97de..c58e1a51ea 100644 --- a/src/systemcmds/tests/test_dataman.c +++ b/src/systemcmds/tests/test_dataman.c @@ -70,19 +70,23 @@ task_main(int argc, char *argv[]) warnx("Starting dataman test task %s", argv[1]); /* try to read an invalid item */ int my_id = atoi(argv[1]); + /* try to read an invalid item */ if (dm_read(DM_KEY_NUM_KEYS, 0, buffer, sizeof(buffer)) >= 0) { warnx("%d read an invalid item failed", my_id); goto fail; } + /* try to read an invalid index */ if (dm_read(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, buffer, sizeof(buffer)) >= 0) { warnx("%d read an invalid index failed", my_id); goto fail; } + srand(hrt_absolute_time() ^ my_id); unsigned hit = 0, miss = 0; wstart = hrt_absolute_time(); + for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { memset(buffer, my_id, sizeof(buffer)); buffer[1] = i; @@ -91,44 +95,53 @@ task_main(int argc, char *argv[]) int ret = dm_write(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len); warnx("ret: %d", ret); + if (ret != len) { warnx("%d write failed, index %d, length %d", my_id, hash, len); goto fail; } + usleep(rand() & ((64 * 1024) - 1)); } + rstart = hrt_absolute_time(); wend = rstart; for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { unsigned hash = i ^ my_id; unsigned len2, len = (hash & 63) + 2; + if ((len2 = dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, buffer, sizeof(buffer))) < 2) { warnx("%d read failed length test, index %d", my_id, hash); goto fail; } + if (buffer[0] == my_id) { hit++; + if (len2 != len) { warnx("%d read failed length test, index %d, wanted %d, got %d", my_id, hash, len, len2); goto fail; } + if (buffer[1] != i) { warnx("%d data verification failed, index %d, wanted %d, got %d", my_id, hash, my_id, buffer[1]); goto fail; } - } - else + + } else { miss++; + } } + rend = hrt_absolute_time(); warnx("Test %d pass, hit %d, miss %d, io time read %llums. write %llums.", - my_id, hit, miss, (rend - rstart) / NUM_MISSIONS_SUPPORTED / 1000, (wend - wstart) / NUM_MISSIONS_SUPPORTED / 1000); + my_id, hit, miss, (rend - rstart) / NUM_MISSIONS_SUPPORTED / 1000, (wend - wstart) / NUM_MISSIONS_SUPPORTED / 1000); sem_post(sems + my_id); return 0; fail: warnx("Test %d fail, buffer %02x %02x %02x %02x %02x %02x", - my_id, buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5]); + my_id, buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5]); sem_post(sems + my_id); return -1; } @@ -138,11 +151,13 @@ int test_dataman(int argc, char *argv[]) int i, num_tasks = 4; char buffer[DM_MAX_DATA_SIZE]; - if (argc > 1) + if (argc > 1) { num_tasks = atoi(argv[1]); - + } + sems = (sem_t *)malloc(num_tasks * sizeof(sem_t)); warnx("Running %d tasks", num_tasks); + for (i = 0; i < num_tasks; i++) { int task; char a[16]; @@ -151,32 +166,41 @@ int test_dataman(int argc, char *argv[]) av[0] = a; av[1] = 0; sem_init(sems + i, 1, 0); + /* start the task */ if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, av)) <= 0) { warn("task start failed"); } } + for (i = 0; i < num_tasks; i++) { sem_wait(sems + i); sem_destroy(sems + i); } + free(sems); dm_restart(DM_INIT_REASON_IN_FLIGHT); + for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { - if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) + if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) { break; + } } + if (i >= NUM_MISSIONS_SUPPORTED) { warnx("Restart in-flight failed"); return -1; } + dm_restart(DM_INIT_REASON_POWER_ON); + for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) { warnx("Restart power-on failed"); return -1; } } + return 0; } diff --git a/src/systemcmds/tests/test_eigen.cpp b/src/systemcmds/tests/test_eigen.cpp index de5630cc3e..fb50e0d7af 100644 --- a/src/systemcmds/tests/test_eigen.cpp +++ b/src/systemcmds/tests/test_eigen.cpp @@ -52,27 +52,27 @@ namespace Eigen { - typedef Matrix Vector10f; +typedef Matrix Vector10f; } static constexpr size_t OPERATOR_ITERATIONS = 60000; #define TEST_OP(_title, _op) \ -{ \ - const hrt_abstime t0 = hrt_absolute_time(); \ - for (size_t j = 0; j < OPERATOR_ITERATIONS; j++) { \ - _op; \ - } \ - printf(_title ": %.6fus", static_cast(hrt_absolute_time() - t0) / OPERATOR_ITERATIONS); \ -} + { \ + const hrt_abstime t0 = hrt_absolute_time(); \ + for (size_t j = 0; j < OPERATOR_ITERATIONS; j++) { \ + _op; \ + } \ + printf(_title ": %.6fus", static_cast(hrt_absolute_time() - t0) / OPERATOR_ITERATIONS); \ + } #define VERIFY_OP(_title, _op, __OP_TEST__) \ -{ \ - _op; \ - if(!(__OP_TEST__)) { \ - printf(_title " Failed! ("#__OP_TEST__")"); \ - } \ -} + { \ + _op; \ + if(!(__OP_TEST__)) { \ + printf(_title " Failed! ("#__OP_TEST__")"); \ + } \ + } #define TEST_OP_VERIFY(_title, _op, __OP_TEST__) \ VERIFY_OP(_title, _op, __OP_TEST__) \ @@ -83,18 +83,20 @@ static constexpr size_t OPERATOR_ITERATIONS = 60000; * Prints an Eigen::Matrix to stdout **/ template -void printEigen(const Eigen::MatrixBase& b) +void printEigen(const Eigen::MatrixBase &b) { - for(int i = 0; i < b.rows(); ++i) { + for (int i = 0; i < b.rows(); ++i) { printf("("); - for(int j = 0; j < b.cols(); ++i) { - if(j > 0) { + + for (int j = 0; j < b.cols(); ++i) { + if (j > 0) { printf(","); } printf("%.3f", static_cast(b(i, j))); } - printf(")%s\n", i+1 < b.rows() ? "," : ""); + + printf(")%s\n", i + 1 < b.rows() ? "," : ""); } } @@ -108,7 +110,7 @@ Eigen::Quaternion quatFromEuler(const T roll, const T pitch, const T yaw) Eigen::AngleAxis rollAngle(roll, Eigen::Matrix::UnitZ()); Eigen::AngleAxis yawAngle(yaw, Eigen::Matrix::UnitY()); Eigen::AngleAxis pitchAngle(pitch, Eigen::Matrix::UnitX()); - return rollAngle * yawAngle * pitchAngle; + return rollAngle * yawAngle * pitchAngle; } @@ -127,9 +129,9 @@ int test_eigen(int argc, char *argv[]) TEST_OP_VERIFY("Constructor Vector2f(float[])", Eigen::Vector2f v3(data), v3[0] == data[0] && v3[1] == data[1]); TEST_OP_VERIFY("Constructor Vector2f(float, float)", Eigen::Vector2f v3(1.0f, 2.0f), v3(0) == 1.0f && v3(1) == 2.0f); TEST_OP_VERIFY("Vector2f = Vector2f", v = v1, v.isApprox(v1)); - TEST_OP_VERIFY("Vector2f + Vector2f", v + v1, v.isApprox(v1+v1)); + TEST_OP_VERIFY("Vector2f + Vector2f", v + v1, v.isApprox(v1 + v1)); TEST_OP_VERIFY("Vector2f - Vector2f", v - v1, v.isApprox(v1)); - TEST_OP_VERIFY("Vector2f += Vector2f", v += v1, v.isApprox(v1+v1)); + TEST_OP_VERIFY("Vector2f += Vector2f", v += v1, v.isApprox(v1 + v1)); TEST_OP_VERIFY("Vector2f -= Vector2f", v -= v1, v.isApprox(v1)); TEST_OP_VERIFY("Vector2f dot Vector2f", v.dot(v1), fabs(v.dot(v1) - 5.0f) <= FLT_EPSILON); //TEST_OP("Vector2f cross Vector2f", v1.cross(v2)); //cross product for 2d array? @@ -185,7 +187,7 @@ int test_eigen(int argc, char *argv[]) TEST_OP("Vector<4> dot Vector<4>", v.dot(v1)); } - { + { Eigen::Vector10f v1; v1.Zero(); float data[10]; @@ -222,18 +224,18 @@ int test_eigen(int argc, char *argv[]) // test nonsymmetric +, -, +=, -= const Eigen::Matrix m1_orig = - (Eigen::Matrix() << 1, 3, 3, - 4, 6, 6).finished(); + (Eigen::Matrix() << 1, 3, 3, + 4, 6, 6).finished(); Eigen::Matrix m1(m1_orig); Eigen::Matrix m2; m2 << 2, 4, 6, - 8, 10, 12; + 8, 10, 12; Eigen::Matrix m3; m3 << 3, 6, 9, - 12, 15, 18; + 12, 15, 18; if (m1 + m2 != m3) { warnx("Matrix<2, 3> + Matrix<2, 3> failed!"); @@ -349,7 +351,7 @@ int test_eigen(int argc, char *argv[]) { // test quaternion method "rotate" (rotate vector by quaternion) - Eigen::Vector3f vector = {1.0f,1.0f,1.0f}; + Eigen::Vector3f vector = {1.0f, 1.0f, 1.0f}; Eigen::Vector3f vector_q; Eigen::Vector3f vector_r; Eigen::Quaternionf q; @@ -363,11 +365,12 @@ int test_eigen(int argc, char *argv[]) for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) { for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) { R.eulerAngles(roll, pitch, yaw); - q = quatFromEuler(roll,pitch,yaw); - vector_r = R*vector; + q = quatFromEuler(roll, pitch, yaw); + vector_r = R * vector; vector_q = q._transformVector(vector); + for (int i = 0; i < 3; i++) { - if(fabsf(vector_r(i) - vector_q(i)) > tol) { + if (fabsf(vector_r(i) - vector_q(i)) > tol) { warnx("Quaternion method 'rotate' outside tolerance"); rc = 1; } @@ -378,41 +381,45 @@ int test_eigen(int argc, char *argv[]) // test some values calculated with matlab tol = 0.0001f; - q = quatFromEuler(M_PI_2_F,0.0f,0.0f); + q = quatFromEuler(M_PI_2_F, 0.0f, 0.0f); vector_q = q._transformVector(vector); - Eigen::Vector3f vector_true = {1.00f,-1.00f,1.00f}; - for(size_t i = 0;i<3;i++) { - if(fabsf(vector_true(i) - vector_q(i)) > tol) { + Eigen::Vector3f vector_true = {1.00f, -1.00f, 1.00f}; + + for (size_t i = 0; i < 3; i++) { + if (fabsf(vector_true(i) - vector_q(i)) > tol) { warnx("Quaternion method 'rotate' outside tolerance"); rc = 1; } } - q = quatFromEuler(0.3f,0.2f,0.1f); + q = quatFromEuler(0.3f, 0.2f, 0.1f); vector_q = q._transformVector(vector); - vector_true = {1.1566,0.7792,1.0273}; - for(size_t i = 0;i<3;i++) { - if(fabsf(vector_true(i) - vector_q(i)) > tol) { + vector_true = {1.1566, 0.7792, 1.0273}; + + for (size_t i = 0; i < 3; i++) { + if (fabsf(vector_true(i) - vector_q(i)) > tol) { warnx("Quaternion method 'rotate' outside tolerance"); rc = 1; } } - q = quatFromEuler(-1.5f,-0.2f,0.5f); + q = quatFromEuler(-1.5f, -0.2f, 0.5f); vector_q = q._transformVector(vector); - vector_true = {0.5095,1.4956,-0.7096}; - for(size_t i = 0;i<3;i++) { - if(fabsf(vector_true(i) - vector_q(i)) > tol) { + vector_true = {0.5095, 1.4956, -0.7096}; + + for (size_t i = 0; i < 3; i++) { + if (fabsf(vector_true(i) - vector_q(i)) > tol) { warnx("Quaternion method 'rotate' outside tolerance"); rc = 1; } } - q = quatFromEuler(M_PI_2_F,-M_PI_2_F,-M_PI_F/3.0f); + q = quatFromEuler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f); vector_q = q._transformVector(vector); - vector_true = {-1.3660,0.3660,1.0000}; - for(size_t i = 0;i<3;i++) { - if(fabsf(vector_true(i) - vector_q(i)) > tol) { + vector_true = { -1.3660, 0.3660, 1.0000}; + + for (size_t i = 0; i < 3; i++) { + if (fabsf(vector_true(i) - vector_q(i)) > tol) { warnx("Quaternion method 'rotate' outside tolerance"); rc = 1; } diff --git a/src/systemcmds/tests/test_file.c b/src/systemcmds/tests/test_file.c index 570583dee9..a43e01d6fb 100644 --- a/src/systemcmds/tests/test_file.c +++ b/src/systemcmds/tests/test_file.c @@ -56,7 +56,8 @@ static int check_user_abort(int fd); -int check_user_abort(int fd) { +int check_user_abort(int fd) +{ /* check if user wants to abort */ char c; @@ -74,13 +75,12 @@ int check_user_abort(int fd) { case 0x03: // ctrl-c case 0x1b: // esc case 'c': - case 'q': - { - warnx("Test aborted."); - fsync(fd); - close(fd); - return OK; - /* not reached */ + case 'q': { + warnx("Test aborted."); + fsync(fd); + close(fd); + return OK; + /* not reached */ } } } @@ -96,6 +96,7 @@ test_file(int argc, char *argv[]) /* check if microSD card is mounted */ struct stat buffer; + if (stat("/fs/microsd/", &buffer)) { warnx("no microSD card mounted, aborting file test"); return 1; @@ -118,7 +119,7 @@ test_file(int argc, char *argv[]) /* fill write buffer with known values */ for (size_t i = 0; i < sizeof(write_buf); i++) { /* this will wrap, but we just need a known value with spacing */ - write_buf[i] = i+11; + write_buf[i] = i + 11; } uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); @@ -129,24 +130,28 @@ test_file(int argc, char *argv[]) warnx("testing unaligned writes - please wait.."); start = hrt_absolute_time(); + for (unsigned i = 0; i < iterations; i++) { int wret = write(fd, write_buf + a, chunk_sizes[c]); if (wret != chunk_sizes[c]) { warn("WRITE ERROR!"); - if ((0x3 & (uintptr_t)(write_buf + a))) + if ((0x3 & (uintptr_t)(write_buf + a))) { warnx("memory is unaligned, align shift: %d", a); + } return 1; } fsync(fd); - if (!check_user_abort(fd)) + if (!check_user_abort(fd)) { return OK; + } } + end = hrt_absolute_time(); warnx("write took %llu us", (end - start)); @@ -162,7 +167,7 @@ test_file(int argc, char *argv[]) warnx("READ ERROR!"); return 1; } - + /* compare value */ bool compare_ok = true; @@ -179,8 +184,9 @@ test_file(int argc, char *argv[]) return 1; } - if (!check_user_abort(fd)) + if (!check_user_abort(fd)) { return OK; + } } @@ -202,8 +208,9 @@ test_file(int argc, char *argv[]) return 1; } - if (!check_user_abort(fd)) + if (!check_user_abort(fd)) { return OK; + } } @@ -224,7 +231,7 @@ test_file(int argc, char *argv[]) warnx("READ ERROR!"); return 1; } - + for (int j = 0; j < chunk_sizes[c]; j++) { if (read_buf[j] != write_buf[j]) { warnx("COMPARISON ERROR: byte %d: %u != %u", j, (unsigned int)read_buf[j], (unsigned int)write_buf[j]); @@ -232,8 +239,9 @@ test_file(int argc, char *argv[]) break; } - if (!check_user_abort(fd)) + if (!check_user_abort(fd)) { return OK; + } } if (!align_read_ok) { @@ -267,16 +275,19 @@ test_file(int argc, char *argv[]) for (int j = 0; j < chunk_sizes[c]; j++) { if ((read_buf + a)[j] != write_buf[j]) { - warnx("COMPARISON ERROR: byte %d, align shift: %d: %u != %u", j, a, (unsigned int)read_buf[j + a], (unsigned int)write_buf[j]); + warnx("COMPARISON ERROR: byte %d, align shift: %d: %u != %u", j, a, (unsigned int)read_buf[j + a], + (unsigned int)write_buf[j]); unalign_read_ok = false; unalign_read_err_count++; - - if (unalign_read_err_count > 10) + + if (unalign_read_err_count > 10) { break; + } } - if (!check_user_abort(fd)) + if (!check_user_abort(fd)) { return OK; + } } if (!unalign_read_ok) { @@ -300,6 +311,7 @@ test_file(int argc, char *argv[]) DIR *d; struct dirent *dir; d = opendir("/fs/microsd"); + if (d) { while ((dir = readdir(d)) != NULL) { diff --git a/src/systemcmds/tests/test_file2.c b/src/systemcmds/tests/test_file2.c index 8db3ea5ae6..2e7b5c184f 100644 --- a/src/systemcmds/tests/test_file2.c +++ b/src/systemcmds/tests/test_file2.c @@ -69,42 +69,51 @@ static uint8_t get_value(uint32_t ofs) static void test_corruption(const char *filename, uint32_t write_chunk, uint32_t write_size, uint16_t flags) { - printf("Testing on %s with write_chunk=%u write_size=%u\n", + printf("Testing on %s with write_chunk=%u write_size=%u\n", filename, (unsigned)write_chunk, (unsigned)write_size); - + uint32_t ofs = 0; int fd = open(filename, O_CREAT | O_RDWR | O_TRUNC); + if (fd == -1) { perror(filename); exit(1); } - + // create a file of size write_size, in write_chunk blocks uint8_t counter = 0; + while (ofs < write_size) { - uint8_t buffer[write_chunk]; - for (uint16_t j=0; j= 0) + if (fd >= 0) { close(fd); + } return 0; } diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index a66cebd2f4..3a890c30b6 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -284,11 +284,11 @@ int test_mathlib(int argc, char *argv[]) { // test quaternion method "rotate" (rotate vector by quaternion) - Vector<3> vector = {1.0f,1.0f,1.0f}; + Vector<3> vector = {1.0f, 1.0f, 1.0f}; Vector<3> vector_q; Vector<3> vector_r; Quaternion q; - Matrix<3,3> R; + Matrix<3, 3> R; float diff = 0.1f; float tol = 0.00001f; @@ -298,11 +298,12 @@ int test_mathlib(int argc, char *argv[]) for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) { for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) { R.from_euler(roll, pitch, yaw); - q.from_euler(roll,pitch,yaw); - vector_r = R*vector; + q.from_euler(roll, pitch, yaw); + vector_r = R * vector; vector_q = q.rotate(vector); + for (int i = 0; i < 3; i++) { - if(fabsf(vector_r(i) - vector_q(i)) > tol) { + if (fabsf(vector_r(i) - vector_q(i)) > tol) { warnx("Quaternion method 'rotate' outside tolerance"); rc = 1; } @@ -313,41 +314,45 @@ int test_mathlib(int argc, char *argv[]) // test some values calculated with matlab tol = 0.0001f; - q.from_euler(M_PI_2_F,0.0f,0.0f); + q.from_euler(M_PI_2_F, 0.0f, 0.0f); vector_q = q.rotate(vector); - Vector<3> vector_true = {1.00f,-1.00f,1.00f}; - for(unsigned i = 0;i<3;i++) { - if(fabsf(vector_true(i) - vector_q(i)) > tol) { + Vector<3> vector_true = {1.00f, -1.00f, 1.00f}; + + for (unsigned i = 0; i < 3; i++) { + if (fabsf(vector_true(i) - vector_q(i)) > tol) { warnx("Quaternion method 'rotate' outside tolerance"); rc = 1; } } - q.from_euler(0.3f,0.2f,0.1f); + q.from_euler(0.3f, 0.2f, 0.1f); vector_q = q.rotate(vector); - vector_true = {1.1566,0.7792,1.0273}; - for(unsigned i = 0;i<3;i++) { - if(fabsf(vector_true(i) - vector_q(i)) > tol) { + vector_true = {1.1566, 0.7792, 1.0273}; + + for (unsigned i = 0; i < 3; i++) { + if (fabsf(vector_true(i) - vector_q(i)) > tol) { warnx("Quaternion method 'rotate' outside tolerance"); rc = 1; } } - q.from_euler(-1.5f,-0.2f,0.5f); + q.from_euler(-1.5f, -0.2f, 0.5f); vector_q = q.rotate(vector); - vector_true = {0.5095,1.4956,-0.7096}; - for(unsigned i = 0;i<3;i++) { - if(fabsf(vector_true(i) - vector_q(i)) > tol) { + vector_true = {0.5095, 1.4956, -0.7096}; + + for (unsigned i = 0; i < 3; i++) { + if (fabsf(vector_true(i) - vector_q(i)) > tol) { warnx("Quaternion method 'rotate' outside tolerance"); rc = 1; } } - q.from_euler(M_PI_2_F,-M_PI_2_F,-M_PI_F/3.0f); + q.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f); vector_q = q.rotate(vector); - vector_true = {-1.3660,0.3660,1.0000}; - for(unsigned i = 0;i<3;i++) { - if(fabsf(vector_true(i) - vector_q(i)) > tol) { + vector_true = { -1.3660, 0.3660, 1.0000}; + + for (unsigned i = 0; i < 3; i++) { + if (fabsf(vector_true(i) - vector_q(i)) > tol) { warnx("Quaternion method 'rotate' outside tolerance"); rc = 1; } diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index 2896a8e405..64e734b008 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -83,8 +83,9 @@ int test_mixer(int argc, char *argv[]) const char *filename = "/etc/mixers/IO_pass.mix"; - if (argc > 1) + if (argc > 1) { filename = argv[1]; + } warnx("loading: %s", filename); @@ -108,8 +109,10 @@ int test_mixer(int argc, char *argv[]) unsigned xx = loaded; mixer_group.load_from_buf(&buf[0], xx); warnx("complete buffer load: loaded %u mixers", mixer_group.count()); - if (mixer_group.count() != 8) + + if (mixer_group.count() != 8) { return 1; + } unsigned empty_load = 2; char empty_buf[2]; @@ -118,8 +121,10 @@ int test_mixer(int argc, char *argv[]) mixer_group.reset(); mixer_group.load_from_buf(&empty_buf[0], empty_load); warnx("empty buffer load: loaded %u mixers, used: %u", mixer_group.count(), empty_load); - if (empty_load != 0) + + if (empty_load != 0) { return 1; + } /* FIRST mark the mixer as invalid */ /* THEN actually delete it */ @@ -155,8 +160,9 @@ int test_mixer(int argc, char *argv[]) warnx("used %u", mixer_text_length - resid); /* copy any leftover text to the base of the buffer for re-use */ - if (resid > 0) + if (resid > 0) { memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid); + } mixer_text_length = resid; } @@ -166,11 +172,12 @@ int test_mixer(int argc, char *argv[]) warnx("chunked load: loaded %u mixers", mixer_group.count()); - if (mixer_group.count() != 8) + if (mixer_group.count() != 8) { return 1; + } /* execute the mixer */ - + float outputs[output_max]; unsigned mixed; const int jmax = 5; @@ -193,30 +200,31 @@ int test_mixer(int argc, char *argv[]) unsigned sleepcount = 0; while (hrt_elapsed_time(&starttime) < INIT_TIME_US + RAMP_TIME_US) { - + /* mix */ mixed = mixer_group.mix(&outputs[0], output_max); - pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, + r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); - for (unsigned i = 0; i < mixed; i++) - { + for (unsigned i = 0; i < mixed; i++) { /* check mixed outputs to be zero during init phase */ if (hrt_elapsed_time(&starttime) < INIT_TIME_US && - r_page_servos[i] != r_page_servo_disarmed[i]) { + r_page_servos[i] != r_page_servo_disarmed[i]) { warnx("disarmed servo value mismatch"); return 1; } if (hrt_elapsed_time(&starttime) >= INIT_TIME_US && - r_page_servos[i] + 1 <= r_page_servo_disarmed[i]) { + r_page_servos[i] + 1 <= r_page_servo_disarmed[i]) { warnx("ramp servo value mismatch"); return 1; } //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, (double)outputs_unlimited[i], (double)outputs[i], (int)r_page_servos[i]); } + usleep(sleep_quantum_us); sleepcount++; @@ -225,6 +233,7 @@ int test_mixer(int argc, char *argv[]) fflush(stdout); } } + printf("\n"); warnx("ARMING TEST: NORMAL OPERATION"); @@ -232,7 +241,7 @@ int test_mixer(int argc, char *argv[]) for (int j = -jmax; j <= jmax; j++) { for (unsigned i = 0; i < output_max; i++) { - actuator_controls[i] = j/10.0f + 0.1f * i; + actuator_controls[i] = j / 10.0f + 0.1f * i; r_page_servo_disarmed[i] = PWM_LOWEST_MIN; r_page_servo_control_min[i] = PWM_DEFAULT_MIN; r_page_servo_control_max[i] = PWM_DEFAULT_MAX; @@ -241,12 +250,14 @@ int test_mixer(int argc, char *argv[]) /* mix */ mixed = mixer_group.mix(&outputs[0], output_max); - pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, + r_page_servos, &pwm_limit); warnx("mixed %d outputs (max %d)", mixed, output_max); - for (unsigned i = 0; i < mixed; i++) - { + + for (unsigned i = 0; i < mixed; i++) { servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f; + if (fabsf(servo_predicted[i] - r_page_servos[i]) > 2) { printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]); warnx("mixer violated predicted value"); @@ -262,15 +273,15 @@ int test_mixer(int argc, char *argv[]) should_arm = false; while (hrt_elapsed_time(&starttime) < 600000) { - + /* mix */ mixed = mixer_group.mix(&outputs[0], output_max); - pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, + r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); - for (unsigned i = 0; i < mixed; i++) - { + for (unsigned i = 0; i < mixed; i++) { /* check mixed outputs to be zero during init phase */ if (r_page_servos[i] != r_page_servo_disarmed[i]) { warnx("disarmed servo value mismatch"); @@ -279,6 +290,7 @@ int test_mixer(int argc, char *argv[]) //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]); } + usleep(sleep_quantum_us); sleepcount++; @@ -287,6 +299,7 @@ int test_mixer(int argc, char *argv[]) fflush(stdout); } } + printf("\n"); warnx("ARMING TEST: REARMING: STARTING RAMP"); @@ -296,30 +309,30 @@ int test_mixer(int argc, char *argv[]) should_arm = true; while (hrt_elapsed_time(&starttime) < 600000 + RAMP_TIME_US) { - + /* mix */ mixed = mixer_group.mix(&outputs[0], output_max); - pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, + r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); - for (unsigned i = 0; i < mixed; i++) - { + for (unsigned i = 0; i < mixed; i++) { /* predict value */ servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f; /* check ramp */ if (hrt_elapsed_time(&starttime) < RAMP_TIME_US && - (r_page_servos[i] + 1 <= r_page_servo_disarmed[i] || - r_page_servos[i] > servo_predicted[i])) { + (r_page_servos[i] + 1 <= r_page_servo_disarmed[i] || + r_page_servos[i] > servo_predicted[i])) { warnx("ramp servo value mismatch"); return 1; } /* check post ramp phase */ if (hrt_elapsed_time(&starttime) > RAMP_TIME_US && - fabsf(servo_predicted[i] - r_page_servos[i]) > 2) { + fabsf(servo_predicted[i] - r_page_servos[i]) > 2) { printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]); warnx("mixer violated predicted value"); return 1; @@ -327,6 +340,7 @@ int test_mixer(int argc, char *argv[]) //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]); } + usleep(sleep_quantum_us); sleepcount++; @@ -335,15 +349,18 @@ int test_mixer(int argc, char *argv[]) fflush(stdout); } } + printf("\n"); /* load multirotor at once test */ mixer_group.reset(); - if (argc > 2) + if (argc > 2) { filename = argv[2]; - else + + } else { filename = "/etc/mixers/quad_w.main.mix"; + } load_mixer_file(filename, &buf[0], sizeof(buf)); loaded = strlen(buf); @@ -353,6 +370,7 @@ int test_mixer(int argc, char *argv[]) unsigned mc_loaded = loaded; mixer_group.load_from_buf(&buf[0], mc_loaded); warnx("complete buffer load: loaded %u mixers", mixer_group.count()); + if (mixer_group.count() != 5) { warnx("FAIL: Quad W mixer load failed"); return 1; @@ -368,11 +386,13 @@ mixer_callback(uintptr_t handle, uint8_t control_index, float &control) { - if (control_group != 0) + if (control_group != 0) { return -1; + } - if (control_index > (sizeof(actuator_controls) / sizeof(actuator_controls[0]))) + if (control_index > (sizeof(actuator_controls) / sizeof(actuator_controls[0]))) { return -1; + } control = actuator_controls[control_index]; diff --git a/src/systemcmds/tests/test_mount.c b/src/systemcmds/tests/test_mount.c index 4b6303cfb1..68052258ef 100644 --- a/src/systemcmds/tests/test_mount.c +++ b/src/systemcmds/tests/test_mount.c @@ -63,11 +63,12 @@ test_mount(int argc, char *argv[]) const unsigned iterations = 2000; const unsigned alignments = 10; - const char* cmd_filename = "/fs/microsd/mount_test_cmds.txt"; + const char *cmd_filename = "/fs/microsd/mount_test_cmds.txt"; /* check if microSD card is mounted */ struct stat buffer; + if (stat("/fs/microsd/", &buffer)) { warnx("no microSD card mounted, aborting file test"); return 1; @@ -77,6 +78,7 @@ test_mount(int argc, char *argv[]) DIR *d; struct dirent *dir; d = opendir("/fs/microsd"); + if (d) { while ((dir = readdir(d)) != NULL) { @@ -105,6 +107,7 @@ test_mount(int argc, char *argv[]) int it_left_abort = abort_tries; int cmd_fd; + if (stat(cmd_filename, &buffer) == OK) { /* command file exists, read off state */ @@ -115,24 +118,28 @@ test_mount(int argc, char *argv[]) if (ret > 0) { int count = 0; ret = sscanf(buf, "TEST: %u %u %n", &it_left_fsync, &it_left_abort, &count); + } else { buf[0] = '\0'; } - if (it_left_fsync > fsync_tries) + if (it_left_fsync > fsync_tries) { it_left_fsync = fsync_tries; + } - if (it_left_abort > abort_tries) + if (it_left_abort > abort_tries) { it_left_abort = abort_tries; + } warnx("Iterations left: #%d / #%d of %d / %d\n(%s)", it_left_fsync, it_left_abort, - fsync_tries, abort_tries, buf); + fsync_tries, abort_tries, buf); int it_left_fsync_prev = it_left_fsync; /* now write again what to do next */ - if (it_left_fsync > 0) + if (it_left_fsync > 0) { it_left_fsync--; + } if (it_left_fsync == 0 && it_left_abort > 0) { @@ -172,7 +179,8 @@ test_mount(int argc, char *argv[]) for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) { - printf("\n\n====== FILE TEST: %u bytes chunks (%s) ======\n", chunk_sizes[c], (it_left_fsync > 0) ? "FSYNC" : "NO FSYNC"); + printf("\n\n====== FILE TEST: %u bytes chunks (%s) ======\n", chunk_sizes[c], + (it_left_fsync > 0) ? "FSYNC" : "NO FSYNC"); printf("unpower the system immediately (within 0.5s) when the hash (#) sign appears\n"); fsync(fileno(stdout)); fsync(fileno(stderr)); @@ -187,7 +195,7 @@ test_mount(int argc, char *argv[]) /* fill write buffer with known values */ for (unsigned i = 0; i < sizeof(write_buf); i++) { /* this will wrap, but we just need a known value with spacing */ - write_buf[i] = i+11; + write_buf[i] = i + 11; } uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); @@ -201,8 +209,9 @@ test_mount(int argc, char *argv[]) if (wret != (int)chunk_sizes[c]) { warn("WRITE ERROR!"); - if ((0x3 & (uintptr_t)(write_buf + a))) + if ((0x3 & (uintptr_t)(write_buf + a))) { warnx("memory is unaligned, align shift: %d", a); + } return 1; @@ -210,6 +219,7 @@ test_mount(int argc, char *argv[]) if (it_left_fsync > 0) { fsync(fd); + } else { printf("#"); fsync(fileno(stdout)); @@ -237,7 +247,7 @@ test_mount(int argc, char *argv[]) warnx("READ ERROR!"); return 1; } - + /* compare value */ bool compare_ok = true; diff --git a/src/systemcmds/tests/test_ppm_loopback.c b/src/systemcmds/tests/test_ppm_loopback.c index 6866531d78..c802cdf0c6 100644 --- a/src/systemcmds/tests/test_ppm_loopback.c +++ b/src/systemcmds/tests/test_ppm_loopback.c @@ -77,6 +77,7 @@ int test_ppm_loopback(int argc, char *argv[]) unsigned servo_count; result = ioctl(servo_fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); + if (result != OK) { warnx("PWM_SERVO_GET_COUNT"); return ERROR; @@ -97,7 +98,7 @@ int test_ppm_loopback(int argc, char *argv[]) // result = ioctl(servo_fd, PWM_SERVO_SET_ARM_OK, 0); // if (result != OK) // warnx("FAIL: PWM_SERVO_SET_ARM_OK"); - // tell output device that the system is armed (it will output values if safety is off) + // tell output device that the system is armed (it will output values if safety is off) // result = ioctl(servo_fd, PWM_SERVO_ARM, 0); // if (result != OK) // warnx("FAIL: PWM_SERVO_ARM"); @@ -105,15 +106,17 @@ int test_ppm_loopback(int argc, char *argv[]) int pwm_values[] = {1200, 1300, 1900, 1700, 1500, 1250, 1800, 1400}; - for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) { - result = ioctl(servo_fd, PWM_SERVO_SET(i), pwm_values[i]); - if (result) { - (void)close(servo_fd); - return ERROR; - } else { - warnx("channel %d set to %d", i, pwm_values[i]); - } - } + for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) { + result = ioctl(servo_fd, PWM_SERVO_SET(i), pwm_values[i]); + + if (result) { + (void)close(servo_fd); + return ERROR; + + } else { + warnx("channel %d set to %d", i, pwm_values[i]); + } + } warnx("servo count: %d", servo_count); @@ -165,6 +168,7 @@ int test_ppm_loopback(int argc, char *argv[]) return ERROR; } } + } else { warnx("failed reading RC input data"); return ERROR; diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c index f73f8b87a4..b6d660c836 100644 --- a/src/systemcmds/tests/test_sensors.c +++ b/src/systemcmds/tests/test_sensors.c @@ -34,7 +34,7 @@ /** * @file test_sensors.c * Tests the onboard sensors. - * + * * @author Lorenz Meier */ @@ -62,10 +62,10 @@ #include #include -static int accel(int argc, char *argv[], const char* path); -static int gyro(int argc, char *argv[], const char* path); -static int mag(int argc, char *argv[], const char* path); -static int baro(int argc, char *argv[], const char* path); +static int accel(int argc, char *argv[], const char *path); +static int gyro(int argc, char *argv[], const char *path); +static int mag(int argc, char *argv[], const char *path); +static int baro(int argc, char *argv[], const char *path); /**************************************************************************** * Private Data @@ -74,7 +74,7 @@ static int baro(int argc, char *argv[], const char* path); struct { const char *name; const char *path; - int (* test)(int argc, char *argv[], const char* path); + int (* test)(int argc, char *argv[], const char *path); } sensors[] = { {"accel0", ACCEL0_DEVICE_PATH, accel}, {"accel1", ACCEL1_DEVICE_PATH, accel}, @@ -86,7 +86,7 @@ struct { }; static int -accel(int argc, char *argv[], const char* path) +accel(int argc, char *argv[], const char *path) { printf("\tACCEL: test start\n"); fflush(stdout); @@ -136,7 +136,7 @@ accel(int argc, char *argv[], const char* path) } static int -gyro(int argc, char *argv[], const char* path) +gyro(int argc, char *argv[], const char *path) { printf("\tGYRO: test start\n"); fflush(stdout); @@ -181,7 +181,7 @@ gyro(int argc, char *argv[], const char* path) } static int -mag(int argc, char *argv[], const char* path) +mag(int argc, char *argv[], const char *path) { printf("\tMAG: test start\n"); fflush(stdout); @@ -226,7 +226,7 @@ mag(int argc, char *argv[], const char* path) } static int -baro(int argc, char *argv[], const char* path) +baro(int argc, char *argv[], const char *path) { printf("\tBARO: test start\n"); fflush(stdout); @@ -253,7 +253,8 @@ baro(int argc, char *argv[], const char* path) return ERROR; } else { - printf("\tBARO pressure: %8.4f mbar\talt: %8.4f m\ttemp: %8.4f deg C\n", (double)buf.pressure, (double)buf.altitude, (double)buf.temperature); + printf("\tBARO pressure: %8.4f mbar\talt: %8.4f m\ttemp: %8.4f deg C\n", (double)buf.pressure, (double)buf.altitude, + (double)buf.temperature); } /* Let user know everything is ok */ diff --git a/src/systemcmds/tests/test_servo.c b/src/systemcmds/tests/test_servo.c index ba68485224..9ffd83c65f 100644 --- a/src/systemcmds/tests/test_servo.c +++ b/src/systemcmds/tests/test_servo.c @@ -81,6 +81,7 @@ int test_servo(int argc, char *argv[]) unsigned servo_count; result = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); + if (result != OK) { warnx("PWM_SERVO_GET_COUNT"); return ERROR; @@ -100,12 +101,17 @@ int test_servo(int argc, char *argv[]) /* tell safety that its ok to disable it with the switch */ result = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); - if (result != OK) + + if (result != OK) { warnx("FAIL: PWM_SERVO_SET_ARM_OK"); + } + /* tell output device that the system is armed (it will output values if safety is off) */ result = ioctl(fd, PWM_SERVO_ARM, 0); - if (result != OK) + + if (result != OK) { warnx("FAIL: PWM_SERVO_ARM"); + } usleep(5000000); printf("Advancing channel 0 to 1500\n"); diff --git a/src/systemcmds/tests/test_time.c b/src/systemcmds/tests/test_time.c index 8a164f3fc2..78e13a2420 100644 --- a/src/systemcmds/tests/test_time.c +++ b/src/systemcmds/tests/test_time.c @@ -90,8 +90,9 @@ cycletime(void) cycles = *(unsigned long *)0xe0001004; - if (cycles < lasttime) + if (cycles < lasttime) { basetime += 0x100000000ULL; + } lasttime = cycles; @@ -147,11 +148,13 @@ int test_time(int argc, char *argv[]) delta = abs(h - c); deltadelta = abs(delta - lowdelta); - if (deltadelta > maxdelta) + if (deltadelta > maxdelta) { maxdelta = deltadelta; + } - if (deltadelta > 1000) + if (deltadelta > 1000) { fprintf(stderr, "h %llu c %llu d %lld\n", h, c, delta - lowdelta); + } } printf("Maximum jitter %lldus\n", maxdelta); diff --git a/src/systemcmds/tests/test_uart_loopback.c b/src/systemcmds/tests/test_uart_loopback.c index f1d41739be..6076dbdeae 100644 --- a/src/systemcmds/tests/test_uart_loopback.c +++ b/src/systemcmds/tests/test_uart_loopback.c @@ -92,8 +92,9 @@ int test_uart_loopback(int argc, char *argv[]) /* uart2 -> uart5 */ r = write(uart2, sample_uart2, sizeof(sample_uart2)); - if (r > 0) + if (r > 0) { uart2_nwrite += r; + } // printf("TEST #%d\n",i); write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd)); @@ -101,8 +102,9 @@ int test_uart_loopback(int argc, char *argv[]) /* uart2 -> uart5 */ r = write(uart5, sample_uart5, sizeof(sample_uart5)); - if (r > 0) + if (r > 0) { uart5_nwrite += r; + } // printf("TEST #%d\n",i); write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd)); @@ -111,8 +113,9 @@ int test_uart_loopback(int argc, char *argv[]) do { r = read(uart5, sample_uart2, sizeof(sample_uart2)); - if (r > 0) + if (r > 0) { uart5_nread += r; + } } while (r > 0); // printf("TEST #%d\n",i); @@ -121,8 +124,9 @@ int test_uart_loopback(int argc, char *argv[]) do { r = read(uart2, sample_uart5, sizeof(sample_uart5)); - if (r > 0) + if (r > 0) { uart2_nread += r; + } } while (r > 0); // printf("TEST #%d\n",i); @@ -134,16 +138,19 @@ int test_uart_loopback(int argc, char *argv[]) /* try to read back values */ r = read(uart5, sample_uart2, sizeof(sample_uart2)); - if (r > 0) + if (r > 0) { uart5_nread += r; + } r = read(uart2, sample_uart5, sizeof(sample_uart5)); - if (r > 0) + if (r > 0) { uart2_nread += r; + } - if ((uart2_nread == uart2_nwrite) && (uart5_nread == uart5_nwrite)) + if ((uart2_nread == uart2_nwrite) && (uart5_nread == uart5_nwrite)) { break; + } } diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 7ffd478925..ced64b31cd 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -126,8 +126,9 @@ test_help(int argc, char *argv[]) printf("Available tests:\n"); - for (i = 0; tests[i].name; i++) + for (i = 0; tests[i].name; i++) { printf(" %s\n", tests[i].name); + } return 0; } @@ -155,11 +156,13 @@ test_all(int argc, char *argv[]) fflush(stderr); failcount++; passed[i] = false; + } else { printf(" [%s] \t\t\tPASS\n", tests[i].name); fflush(stdout); passed[i] = true; } + testcount++; } } @@ -198,7 +201,7 @@ test_all(int argc, char *argv[]) printf("\n"); /* Print failed tests */ - if (failcount > 0) printf(" Failed tests:\n\n"); + if (failcount > 0) { printf(" Failed tests:\n\n"); } unsigned int k; @@ -254,22 +257,26 @@ int test_jig(int argc, char *argv[]) bool passed[NTESTS]; printf("\nRunning all tests...\n\n"); + for (i = 0; tests[i].name; i++) { /* Only run tests that are not excluded */ if (!(tests[i].options & OPT_NOJIGTEST)) { printf(" [%s] \t\t\tSTARTING TEST\n", tests[i].name); fflush(stdout); + /* Execute test */ if (tests[i].fn(1, args) != 0) { fprintf(stderr, " [%s] \t\t\tFAIL\n", tests[i].name); fflush(stderr); failcount++; passed[i] = false; + } else { printf(" [%s] \t\t\tPASS\n", tests[i].name); fflush(stdout); passed[i] = true; } + testcount++; } } @@ -277,13 +284,15 @@ int test_jig(int argc, char *argv[]) /* Print summary */ printf("\n"); int j; - for (j = 0; j < 40; j++) - { + + for (j = 0; j < 40; j++) { printf("-"); } + printf("\n\n"); printf(" T E S T S U M M A R Y\n\n"); + if (failcount == 0) { printf(" ______ __ __ ______ __ __ \n"); printf(" /\\ __ \\ /\\ \\ /\\ \\ /\\ __ \\ /\\ \\/ / \n"); @@ -292,6 +301,7 @@ int test_jig(int argc, char *argv[]) printf(" \\/_/\\/_/ \\/_____/ \\/_____/ \\/_____/ \\/_/\\/_/ \n"); printf("\n"); printf(" All tests passed (%d of %d)\n", testcount, testcount); + } else { printf(" ______ ______ __ __ \n"); printf(" /\\ ___\\ /\\ __ \\ /\\ \\ /\\ \\ \n"); @@ -301,18 +311,20 @@ int test_jig(int argc, char *argv[]) printf("\n"); printf(" Some tests failed (%d of %d)\n", failcount, testcount); } + printf("\n"); /* Print failed tests */ - if (failcount > 0) printf(" Failed tests:\n\n"); + if (failcount > 0) { printf(" Failed tests:\n\n"); } + unsigned int k; - for (k = 0; k < i; k++) - { - if (!passed[i] && !(tests[k].options & OPT_NOJIGTEST)) - { + + for (k = 0; k < i; k++) { + if (!passed[i] && !(tests[k].options & OPT_NOJIGTEST)) { printf(" [%s] to obtain details, please re-run with\n\t nsh> tests %s\n\n", tests[k].name, tests[k].name); } } + fflush(stdout); return 0; @@ -333,8 +345,9 @@ int tests_main(int argc, char *argv[]) } for (i = 0; tests[i].name; i++) { - if (!strcmp(tests[i].name, argv[1])) + if (!strcmp(tests[i].name, argv[1])) { return tests[i].fn(argc - 1, argv + 1); + } } printf("tests: no test called '%s' - 'tests help' for a list of tests\n", argv[1]); From 23655675d3f71daf74e64859c8ef519e8d49eab0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Mar 2015 19:54:00 +0100 Subject: [PATCH 104/268] PWM command: Fix code style --- src/systemcmds/pwm/pwm.c | 367 +++++++++++++++++++++++++++------------ 1 file changed, 260 insertions(+), 107 deletions(-) diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 620b0a6f67..6bb9f235cb 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -68,41 +68,43 @@ __EXPORT int pwm_main(int argc, char *argv[]); static void usage(const char *reason) { - if (reason != NULL) + if (reason != NULL) { warnx("%s", reason); + } + errx(1, - "usage:\n" - "pwm arm|disarm|rate|failsafe|disarmed|min|max|test|info ...\n" - "\n" - "arm\t\t\t\tArm output\n" - "disarm\t\t\t\tDisarm output\n" - "\n" - "rate ...\t\t\tConfigure PWM rates\n" - "\t[-g ]\t(e.g. 0,1,2)\n" - "\t[-m ]\t(e.g. 0xF)\n" - "\t[-a]\t\t\tConfigure all outputs\n" - "\t-r \t\tPWM rate (50 to 400 Hz)\n" - "\n" - "failsafe ...\t\t\tFailsafe PWM\n" - "disarmed ...\t\t\tDisarmed PWM\n" - "min ...\t\t\t\tMinimum PWM\n" - "max ...\t\t\t\tMaximum PWM\n" - "\t[-c ]\t\t(e.g. 1234)\n" - "\t[-m ]\t(e.g. 0xF)\n" - "\t[-a]\t\t\tConfigure all outputs\n" - "\t-p \t\tPWM value\n" - "\n" - "test ...\t\t\tDirectly set PWM\n" - "\t[-c ]\t\t(e.g. 1234)\n" - "\t[-m ]\t(e.g. 0xF)\n" - "\t[-a]\t\t\tConfigure all outputs\n" - "\t-p \t\tPWM value\n" - "\n" - "info\t\t\t\tPrint information\n" - "\n" - "\t-v\t\t\tVerbose\n" - "\t-d \t\t(default " PWM_OUTPUT0_DEVICE_PATH ")\n" - ); + "usage:\n" + "pwm arm|disarm|rate|failsafe|disarmed|min|max|test|info ...\n" + "\n" + "arm\t\t\t\tArm output\n" + "disarm\t\t\t\tDisarm output\n" + "\n" + "rate ...\t\t\tConfigure PWM rates\n" + "\t[-g ]\t(e.g. 0,1,2)\n" + "\t[-m ]\t(e.g. 0xF)\n" + "\t[-a]\t\t\tConfigure all outputs\n" + "\t-r \t\tPWM rate (50 to 400 Hz)\n" + "\n" + "failsafe ...\t\t\tFailsafe PWM\n" + "disarmed ...\t\t\tDisarmed PWM\n" + "min ...\t\t\t\tMinimum PWM\n" + "max ...\t\t\t\tMaximum PWM\n" + "\t[-c ]\t\t(e.g. 1234)\n" + "\t[-m ]\t(e.g. 0xF)\n" + "\t[-a]\t\t\tConfigure all outputs\n" + "\t-p \t\tPWM value\n" + "\n" + "test ...\t\t\tDirectly set PWM\n" + "\t[-c ]\t\t(e.g. 1234)\n" + "\t[-m ]\t(e.g. 0xF)\n" + "\t[-a]\t\t\tConfigure all outputs\n" + "\t-p \t\tPWM value\n" + "\n" + "info\t\t\t\tPrint information\n" + "\n" + "\t-v\t\t\tVerbose\n" + "\t-d \t\t(default " PWM_OUTPUT0_DEVICE_PATH ")\n" + ); } @@ -123,10 +125,11 @@ pwm_main(int argc, char *argv[]) unsigned single_ch = 0; unsigned pwm_value = 0; - if (argc < 2) + if (argc < 2) { usage(NULL); + } - while ((ch = getopt(argc-1, &argv[1], "d:vc:g:m:ap:r:")) != EOF) { + while ((ch = getopt(argc - 1, &argv[1], "d:vc:g:m:ap:r:")) != EOF) { switch (ch) { case 'd': @@ -134,6 +137,7 @@ pwm_main(int argc, char *argv[]) warnx("device %s not valid", optarg); usage(NULL); } + dev = optarg; break; @@ -147,15 +151,19 @@ pwm_main(int argc, char *argv[]) while ((single_ch = channels % 10)) { - set_mask |= 1<<(single_ch-1); + set_mask |= 1 << (single_ch - 1); channels /= 10; } + break; case 'g': group = strtoul(optarg, &ep, 0); - if ((*ep != '\0') || (group >= 32)) + + if ((*ep != '\0') || (group >= 32)) { usage("bad channel_group value"); + } + alt_channel_groups |= (1 << group); alt_channels_set = true; warnx("alt channels set, group: %d", group); @@ -164,25 +172,38 @@ pwm_main(int argc, char *argv[]) case 'm': /* Read in mask directly */ set_mask = strtoul(optarg, &ep, 0); - if (*ep != '\0') + + if (*ep != '\0') { usage("BAD set_mask VAL"); + } + break; case 'a': - for (unsigned i = 0; i 0) { warnx("Channels: "); printf(" "); - for (unsigned i = 0; i 0) { ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate); - if (ret != OK) + + if (ret != OK) { err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)"); + } } /* directly supplied channel mask */ if (set_mask > 0) { ret = ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, set_mask); - if (ret != OK) + + if (ret != OK) { err(1, "PWM_SERVO_SET_SELECT_UPDATE_RATE"); + } } /* assign alternate rate to channel groups */ @@ -258,17 +301,22 @@ pwm_main(int argc, char *argv[]) uint32_t group_mask; ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask); - if (ret != OK) + + if (ret != OK) { err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group); + } mask |= group_mask; } } ret = ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, mask); - if (ret != OK) + + if (ret != OK) { err(1, "PWM_SERVO_SET_SELECT_UPDATE_RATE"); + } } + exit(0); } else if (!strcmp(argv[1], "min")) { @@ -276,34 +324,46 @@ pwm_main(int argc, char *argv[]) if (set_mask == 0) { usage("no channels set"); } - if (pwm_value == 0) + + if (pwm_value == 0) { usage("no PWM value provided"); + } struct pwm_output_values pwm_values; + memset(&pwm_values, 0, sizeof(pwm_values)); + pwm_values.channel_count = servo_count; + /* first get current state before modifying it */ ret = ioctl(fd, PWM_SERVO_GET_MIN_PWM, (long unsigned int)&pwm_values); + if (ret != OK) { errx(ret, "failed get min values"); } for (unsigned i = 0; i < servo_count; i++) { - if (set_mask & 1< 0) { - read(0, &c, 1); + read(0, &c, 1); + if (c == 0x03 || c == 0x63 || c == 'q') { /* reset output to the last value */ for (unsigned i = 0; i < servo_count; i++) { - if (set_mask & 1< 0) { ret = read(0, &c, 1); @@ -546,23 +660,29 @@ pwm_main(int argc, char *argv[]) if (ret > 0) { /* reset output to the last value */ for (unsigned i = 0; i < servo_count; i++) { - if (set_mask & 1< Date: Sat, 21 Mar 2015 22:01:28 +0100 Subject: [PATCH 105/268] param subsystem: Only send the instantiated parameters via telemetry --- src/modules/mavlink/mavlink_parameters.cpp | 4 +- src/modules/systemlib/param/param.c | 64 ++++++++++++++++++++-- src/modules/systemlib/param/param.h | 14 ++++- src/systemcmds/param/param.c | 2 +- 4 files changed, 75 insertions(+), 9 deletions(-) diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index e9858b73c6..e910f674aa 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -94,7 +94,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) /* enforce null termination */ name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; /* attempt to find parameter, set and send it */ - param_t param = param_find(name); + param_t param = param_find_no_notification(name); if (param == PARAM_INVALID) { char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; @@ -127,7 +127,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) /* enforce null termination */ name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; /* attempt to find parameter and send it */ - send_param(param_find(name)); + send_param(param_find_no_notification(name)); } else { /* when index is >= 0, send this parameter again */ diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index d0d24960ee..ae680eeb07 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -91,6 +91,9 @@ struct param_wbuf_s { bool unsaved; }; +// XXX this should be param_info_count, but need to work out linking +uint8_t param_changed_storage[(600 / sizeof(uint8_t)) + 1] = {}; + /** flexible array holding modified parameter values */ UT_array *param_values; @@ -103,6 +106,12 @@ ORB_DEFINE(parameter_update, struct parameter_update_s); /** parameter update topic handle */ static orb_advert_t param_topic = -1; +static bool param_used_internal(param_t param); + +static void param_set_used_internal(param_t param); + +static param_t param_find_internal(const char *name, bool notification); + /** lock the parameter store */ static void param_lock(void) @@ -205,20 +214,36 @@ param_notify_changes(void) } param_t -param_find(const char *name) +param_find_internal(const char *name, bool notification) { param_t param; /* perform a linear search of the known parameters */ for (param = 0; handle_in_range(param); param++) { - if (!strcmp(param_info_base[param].name, name)) + if (!strcmp(param_info_base[param].name, name)) { + if (notification) { + param_set_used_internal(param); + } return param; + } } /* not found */ return PARAM_INVALID; } +param_t +param_find(const char *name) +{ + return param_find_internal(name, true); +} + +param_t +param_find_no_notification(const char *name) +{ + return param_find_internal(name, false); +} + unsigned param_count(void) { @@ -430,6 +455,8 @@ param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_ } out: + param_set_used_internal(param); + param_unlock(); /* @@ -454,6 +481,28 @@ param_set_no_notification(param_t param, const void *val) return param_set_internal(param, val, false, false); } +bool param_used_internal(param_t param) +{ + int param_index = param_get_index(param); + if (param_index < 0) { + return false; + } + + unsigned bitindex = param_index - (param_index / sizeof(param_changed_storage[0])); + return param_changed_storage[param_index / sizeof(param_changed_storage[0])] & (1 << bitindex); +} + +void param_set_used_internal(param_t param) +{ + int param_index = param_get_index(param); + if (param_index < 0) { + return; + } + + unsigned bitindex = param_index - (param_index / sizeof(param_changed_storage[0])); + param_changed_storage[param_index / sizeof(param_changed_storage[0])] |= (1 << bitindex); +} + int param_reset(param_t param) { @@ -717,7 +766,7 @@ param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node) * Find the parameter this node represents. If we don't know it, * ignore the node. */ - param_t param = param_find(node->name); + param_t param = param_find_no_notification(node->name); if (param == PARAM_INVALID) { debug("ignoring unrecognised parameter '%s'", node->name); @@ -843,15 +892,20 @@ param_load(int fd) } void -param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_changed) +param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_changed, bool only_used) { param_t param; for (param = 0; handle_in_range(param); param++) { /* if requested, skip unchanged values */ - if (only_changed && (param_find_changed(param) == NULL)) + if (only_changed && (param_find_changed(param) == NULL)) { continue; + } + + if (only_used && !param_used_internal(param)) { + continue; + } func(arg, param); } diff --git a/src/modules/systemlib/param/param.h b/src/modules/systemlib/param/param.h index 69e984a8f4..e3e1b50eb6 100644 --- a/src/modules/systemlib/param/param.h +++ b/src/modules/systemlib/param/param.h @@ -87,9 +87,19 @@ typedef uintptr_t param_t; * * @param name The canonical name of the parameter being looked up. * @return A handle to the parameter, or PARAM_INVALID if the parameter does not exist. + * This call will also set the parameter as "used" in the system, which is used + * to e.g. show the parameter via the RC interface */ __EXPORT param_t param_find(const char *name); +/** + * Look up a parameter by name. + * + * @param name The canonical name of the parameter being looked up. + * @return A handle to the parameter, or PARAM_INVALID if the parameter does not exist. + */ +__EXPORT param_t param_find_no_notification(const char *name); + /** * Return the total number of parameters. * @@ -254,8 +264,10 @@ __EXPORT int param_load(int fd); * @param arg Argument passed to the function. * @param only_changed If true, the function is only called for parameters whose values have * been changed from the default. + * @param only_changed If true, the function is only called for parameters which have been + * used in one of the running applications. */ -__EXPORT void param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_changed); +__EXPORT void param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_changed, bool only_used); /** * Set the default parameter file name. diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index c15e042a77..36a00c0f0b 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -236,7 +236,7 @@ static void do_show(const char *search_string) { printf(" + = saved, * = unsaved\n"); - param_foreach(do_show_print, (char *)search_string, false); + param_foreach(do_show_print, (char *)search_string, false, false); exit(0); } From d6f7c9b8b4a9531aa77c65e106093380e3300d07 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Mar 2015 23:15:59 -0700 Subject: [PATCH 106/268] systemlib: Implement active param list fully --- src/modules/systemlib/param/param.c | 42 ++++++++++++++++++++++++++--- src/modules/systemlib/param/param.h | 22 +++++++++++++++ 2 files changed, 60 insertions(+), 4 deletions(-) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index ae680eeb07..8dea6e6cc0 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -106,8 +106,6 @@ ORB_DEFINE(parameter_update, struct parameter_update_s); /** parameter update topic handle */ static orb_advert_t param_topic = -1; -static bool param_used_internal(param_t param); - static void param_set_used_internal(param_t param); static param_t param_find_internal(const char *name, bool notification); @@ -250,6 +248,20 @@ param_count(void) return param_info_count; } +unsigned +param_count_used(void) +{ + unsigned count = 0; + for (unsigned i = 0; i < sizeof(param_changed_storage) / sizeof(param_changed_storage[0]); i++) { + for (unsigned j = 0; j < 8; j++) { + if (param_changed_storage[i] & (1 << j)) { + count++; + } + } + } + return count; +} + param_t param_for_index(unsigned index) { @@ -268,6 +280,27 @@ param_get_index(param_t param) return -1; } +int +param_get_used_index(param_t param) +{ + if (!handle_in_range(param)) { + return -1; + } + + /* walk all params and count */ + int count = 0; + + for (unsigned i = 0; i < (unsigned)param + 1; i++) { + for (unsigned j = 0; j < 8; j++) { + if (param_changed_storage[i] & (1 << j)) { + count++; + } + } + } + + return count; +} + const char * param_name(param_t param) { @@ -481,7 +514,8 @@ param_set_no_notification(param_t param, const void *val) return param_set_internal(param, val, false, false); } -bool param_used_internal(param_t param) +bool +param_used(param_t param) { int param_index = param_get_index(param); if (param_index < 0) { @@ -903,7 +937,7 @@ param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_chang continue; } - if (only_used && !param_used_internal(param)) { + if (only_used && !param_used(param)) { continue; } diff --git a/src/modules/systemlib/param/param.h b/src/modules/systemlib/param/param.h index e3e1b50eb6..b29a7e51d6 100644 --- a/src/modules/systemlib/param/param.h +++ b/src/modules/systemlib/param/param.h @@ -107,6 +107,20 @@ __EXPORT param_t param_find_no_notification(const char *name); */ __EXPORT unsigned param_count(void); +/** + * Return the actually used number of parameters. + * + * @return The number of parameters. + */ +__EXPORT unsigned param_count_used(void); + +/** + * Wether a parameter is in use in the system. + * + * @return True if it has been written or read + */ +__EXPORT bool param_used(param_t param); + /** * Look up a parameter by index. * @@ -123,6 +137,14 @@ __EXPORT param_t param_for_index(unsigned index); */ __EXPORT int param_get_index(param_t param); +/** + * Look up the index of an used parameter. + * + * @param param The parameter to obtain the index for. + * @return The index of the parameter in use, or -1 if the parameter does not exist. + */ +__EXPORT int param_get_used_index(param_t param); + /** * Obtain the name of a parameter. * From 4c6ddf93724e51d8af914671bfd1090f42ce09a6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Mar 2015 23:16:22 -0700 Subject: [PATCH 107/268] Param command: Add functionality to view active params --- src/systemcmds/param/param.c | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 36a00c0f0b..6b855cf582 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -235,8 +235,9 @@ do_import(const char *param_file_name) static void do_show(const char *search_string) { - printf(" + = saved, * = unsaved\n"); + printf("Symbols: x = used, + = saved, * = unsaved\n"); param_foreach(do_show_print, (char *)search_string, false, false); + printf("\n %u parameters total, %u used.\n", param_count(), param_count_used()); exit(0); } @@ -278,12 +279,12 @@ do_show_print(void *arg, param_t param) } /* the search string must have been consumed */ - if (!(*ss == '\0' || *ss == '*')) { + if (!(*ss == '\0' || *ss == '*') || *pp != '\0') { return; } } - printf("%c %s: ", + printf("%c %c %s: ", (param_used(param) ? 'x' : ' '), param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), param_name(param)); From 78741c87e5415c9e894f619e28b145e127576a56 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Mar 2015 23:16:45 -0700 Subject: [PATCH 108/268] MAVLink app: 1) only transmit active params, 2) send params faster, 3) ensure no overflow occurs on buffer when sending at higher rate. --- src/modules/mavlink/mavlink_main.cpp | 2 +- src/modules/mavlink/mavlink_main.h | 30 +++++++++++----------- src/modules/mavlink/mavlink_parameters.cpp | 23 ++++++++++++++--- 3 files changed, 35 insertions(+), 20 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index f8e819ce7a..632c8c709f 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1361,7 +1361,7 @@ Mavlink::task_main(int argc, char *argv[]) /* PARAM_VALUE stream */ _parameters_manager = (MavlinkParametersManager *) MavlinkParametersManager::new_instance(this); - _parameters_manager->set_interval(interval_from_rate(30.0f)); + _parameters_manager->set_interval(interval_from_rate(80.0f)); LL_APPEND(_streams, _parameters_manager); /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index baaa7bc139..6eef594f51 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -143,6 +143,13 @@ public: bool get_forwarding_on() { return _forwarding_on; } + /** + * Get the free space in the transmit buffer + * + * @return free space in the UART TX buffer + */ + unsigned get_free_tx_buf(); + static int start_helper(int argc, char *argv[]); /** @@ -162,12 +169,12 @@ public: */ int set_hil_enabled(bool hil_enabled); - void send_message(const uint8_t msgid, const void *msg); + void send_message(const uint8_t msgid, const void *msg); /** * Resend message as is, don't change sequence number and CRC. */ - void resend_message(mavlink_message_t *msg); + void resend_message(mavlink_message_t *msg); void handle_message(const mavlink_message_t *msg); @@ -309,7 +316,7 @@ private: int _baudrate; int _datarate; ///< data rate for normal streams (attitude, position, etc.) int _datarate_events; ///< data rate for params, waypoints, text messages - float _rate_mult; + float _rate_mult; /** * If the queue index is not at 0, the queue sending @@ -331,9 +338,9 @@ private: unsigned _bytes_txerr; unsigned _bytes_rx; uint64_t _bytes_timestamp; - float _rate_tx; - float _rate_txerr; - float _rate_rx; + float _rate_tx; + float _rate_txerr; + float _rate_rx; struct telemetry_status_s _rstatus; ///< receive status @@ -363,16 +370,9 @@ private: void mavlink_update_system(); - int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); + int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); - /** - * Get the free space in the transmit buffer - * - * @return free space in the UART TX buffer - */ - unsigned get_free_tx_buf(); - - static unsigned int interval_from_rate(float rate); + static unsigned int interval_from_rate(float rate); int configure_stream(const char *stream_name, const float rate); diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index e910f674aa..e7e0c11dfb 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -184,8 +184,23 @@ MavlinkParametersManager::send(const hrt_abstime t) { /* send all parameters if requested */ if (_send_all_index >= 0) { - send_param(param_for_index(_send_all_index)); - _send_all_index++; + + /* skip if no space is available */ + if (_mavlink->get_free_tx_buf() < get_size()) { + return; + } + + /* look for the first parameter which is used */ + param_t p; + do { + p = param_for_index(_send_all_index); + _send_all_index++; + } while (p != PARAM_INVALID && !param_used(p)); + + if (p != PARAM_INVALID) { + send_param(p); + } + if (_send_all_index >= (int) param_count()) { _send_all_index = -1; } @@ -209,8 +224,8 @@ MavlinkParametersManager::send_param(param_t param) return; } - msg.param_count = param_count(); - msg.param_index = param_get_index(param); + msg.param_count = param_count_used(); + msg.param_index = param_get_used_index(param); /* copy parameter name */ strncpy(msg.param_id, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); From f8fd67d3e17e8f32b7ee5a7d9e112bf944e2735f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 23 Mar 2015 10:52:32 -0700 Subject: [PATCH 109/268] Auto-save parameters on change --- src/modules/commander/commander.cpp | 48 +++++++++++++++++++++++------ 1 file changed, 38 insertions(+), 10 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b8638c6a18..dadb5ea6e7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -161,9 +161,10 @@ static int autostart_id; /* flags */ static bool commander_initialized = false; -static volatile bool thread_should_exit = false; /**< daemon exit flag */ +static volatile bool thread_should_exit = false; /**< daemon exit flag */ static volatile bool thread_running = false; /**< daemon status flag */ -static int daemon_task; /**< Handle of daemon task / thread */ +static int daemon_task; /**< Handle of daemon task / thread */ +static bool _param_autosave = false; static unsigned int leds_counter; /* To remember when last notification was sent */ @@ -1070,8 +1071,6 @@ int commander_thread_main(int argc, char *argv[]) /* Subscribe to parameters changed topic */ int param_changed_sub = orb_subscribe(ORB_ID(parameter_update)); - struct parameter_update_s param_changed; - memset(¶m_changed, 0, sizeof(param_changed)); /* Subscribe to battery topic */ int battery_sub = orb_subscribe(ORB_ID(battery_status)); @@ -1158,9 +1157,16 @@ int commander_thread_main(int argc, char *argv[]) /* update parameters */ orb_check(param_changed_sub, &updated); + if (updated) { + /* trigger an autosave */ + _param_autosave = true; + } + if (updated || param_init_forced) { param_init_forced = false; + /* parameters changed */ + struct parameter_update_s param_changed; orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); /* update parameters */ @@ -2532,6 +2538,9 @@ void *commander_low_prio_loop(void *arg) struct vehicle_command_s cmd; memset(&cmd, 0, sizeof(cmd)); + /* timeout for param autosave */ + hrt_abstime _param_autosave_timeout = 0; + /* wakeup source(s) */ struct pollfd fds[1]; @@ -2673,11 +2682,11 @@ void *commander_low_prio_loop(void *arg) int ret = param_load_default(); if (ret == OK) { - mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); + mavlink_log_info(mavlink_fd, "settings loaded"); answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); } else { - mavlink_log_critical(mavlink_fd, "#audio: parameters load ERROR"); + mavlink_log_critical(mavlink_fd, "settings load ERROR"); /* convenience as many parts of NuttX use negative errno */ if (ret < 0) { @@ -2685,7 +2694,7 @@ void *commander_low_prio_loop(void *arg) } if (ret < 1000) { - mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret)); + mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret)); } answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); @@ -2695,11 +2704,11 @@ void *commander_low_prio_loop(void *arg) int ret = param_save_default(); if (ret == OK) { - mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); + mavlink_log_info(mavlink_fd, "settings saved"); answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); } else { - mavlink_log_critical(mavlink_fd, "#audio: parameters save error"); + mavlink_log_critical(mavlink_fd, "settings save error"); /* convenience as many parts of NuttX use negative errno */ if (ret < 0) { @@ -2707,7 +2716,7 @@ void *commander_low_prio_loop(void *arg) } if (ret < 1000) { - mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret)); + mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret)); } answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); @@ -2726,6 +2735,25 @@ void *commander_low_prio_loop(void *arg) break; } + /* trigger a param autosave if required */ + if (_param_autosave) { + if (_param_autosave_timeout > 0 && hrt_elapsed_time(&_param_autosave_timeout) > 200000ULL) { + int ret = param_save_default(); + + if (ret == OK) { + mavlink_and_console_log_info(mavlink_fd, "settings autosaved"); + + } else { + mavlink_and_console_log_critical(mavlink_fd, "settings save error"); + } + + _param_autosave = false; + _param_autosave_timeout = 0; + } else { + _param_autosave_timeout = hrt_absolute_time(); + } + } + /* send any requested ACKs */ if (cmd.confirmation > 0 && cmd.command != VEHICLE_CMD_DO_SET_MODE && cmd.command != VEHICLE_CMD_COMPONENT_ARM_DISARM) { From a098ca4ec68b8737243d8e7aab5bdb2db4d842a1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Mar 2015 13:06:05 -0700 Subject: [PATCH 110/268] Move autosave into the 1-second timeout check. --- src/modules/commander/commander.cpp | 377 ++++++++++++++-------------- 1 file changed, 187 insertions(+), 190 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index dadb5ea6e7..35f538b230 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2549,216 +2549,213 @@ void *commander_low_prio_loop(void *arg) fds[0].events = POLLIN; while (!thread_should_exit) { - /* wait for up to 200ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200); + /* wait for up to 1000ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); /* timed out - periodic check for thread_should_exit, etc. */ if (pret == 0) { - continue; - } - - /* this is undesirable but not much we can do - might want to flag unhappy status */ - if (pret < 0) { - warn("poll error %d, %d", pret, errno); - continue; - } - - /* if we reach here, we have a valid command */ - orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); - - /* ignore commands the high-prio loop handles */ - if (cmd.command == VEHICLE_CMD_DO_SET_MODE || - cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM || - cmd.command == VEHICLE_CMD_NAV_TAKEOFF || - cmd.command == VEHICLE_CMD_DO_SET_SERVO) { - continue; - } - - /* only handle low-priority commands here */ - switch (cmd.command) { - - case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: - if (is_safe(&status, &safety, &armed)) { - - if (((int)(cmd.param1)) == 1) { - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - usleep(100000); - /* reboot */ - systemreset(false); - - } else if (((int)(cmd.param1)) == 3) { - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - usleep(100000); - /* reboot to bootloader */ - systemreset(true); - - } else { - answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); - } - - } else { - answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); - } - - break; - - case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - - int calib_ret = ERROR; - - /* try to go to INIT/PREFLIGHT arming state */ - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_INIT, &armed, - true /* fRunPreArmChecks */, mavlink_fd)) { - answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); - break; - } - - if ((int)(cmd.param1) == 1) { - /* gyro calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - calib_ret = do_gyro_calibration(mavlink_fd); - - } else if ((int)(cmd.param2) == 1) { - /* magnetometer calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - calib_ret = do_mag_calibration(mavlink_fd); - - } else if ((int)(cmd.param3) == 1) { - /* zero-altitude pressure calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); - - } else if ((int)(cmd.param4) == 1) { - /* RC calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - /* disable RC control input completely */ - status.rc_input_blocked = true; - calib_ret = OK; - mavlink_log_info(mavlink_fd, "CAL: Disabling RC IN"); - - } else if ((int)(cmd.param4) == 2) { - /* RC trim calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - calib_ret = do_trim_calibration(mavlink_fd); - - } else if ((int)(cmd.param5) == 1) { - /* accelerometer calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - calib_ret = do_accel_calibration(mavlink_fd); - - } else if ((int)(cmd.param6) == 1) { - /* airspeed calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - calib_ret = do_airspeed_calibration(mavlink_fd); - - } else if ((int)(cmd.param4) == 0) { - /* RC calibration ended - have we been in one worth confirming? */ - if (status.rc_input_blocked) { - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - /* enable RC control input */ - status.rc_input_blocked = false; - mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN"); - } - - /* this always succeeds */ - calib_ret = OK; - - } - - if (calib_ret == OK) { - tune_positive(true); - - } else { - tune_negative(true); - } - - arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); - - break; - } - - case VEHICLE_CMD_PREFLIGHT_STORAGE: { - - if (((int)(cmd.param1)) == 0) { - int ret = param_load_default(); - - if (ret == OK) { - mavlink_log_info(mavlink_fd, "settings loaded"); - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - - } else { - mavlink_log_critical(mavlink_fd, "settings load ERROR"); - - /* convenience as many parts of NuttX use negative errno */ - if (ret < 0) { - ret = -ret; - } - - if (ret < 1000) { - mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret)); - } - - answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); - } - - } else if (((int)(cmd.param1)) == 1) { + /* trigger a param autosave if required */ + if (_param_autosave) { + if (_param_autosave_timeout > 0 && hrt_elapsed_time(&_param_autosave_timeout) > 200000ULL) { int ret = param_save_default(); if (ret == OK) { - mavlink_log_info(mavlink_fd, "settings saved"); - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + mavlink_and_console_log_info(mavlink_fd, "settings autosaved"); } else { - mavlink_log_critical(mavlink_fd, "settings save error"); - - /* convenience as many parts of NuttX use negative errno */ - if (ret < 0) { - ret = -ret; - } - - if (ret < 1000) { - mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret)); - } - - answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); + mavlink_and_console_log_critical(mavlink_fd, "settings save error"); } + + _param_autosave = false; + _param_autosave_timeout = 0; + } else { + _param_autosave_timeout = hrt_absolute_time(); + } + } + } else if (pret < 0) { + /* this is undesirable but not much we can do - might want to flag unhappy status */ + warn("poll error %d, %d", pret, errno); + continue; + } else { + + /* if we reach here, we have a valid command */ + orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); + + /* ignore commands the high-prio loop handles */ + if (cmd.command == VEHICLE_CMD_DO_SET_MODE || + cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM || + cmd.command == VEHICLE_CMD_NAV_TAKEOFF || + cmd.command == VEHICLE_CMD_DO_SET_SERVO) { + continue; + } + + /* only handle low-priority commands here */ + switch (cmd.command) { + + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: + if (is_safe(&status, &safety, &armed)) { + + if (((int)(cmd.param1)) == 1) { + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + usleep(100000); + /* reboot */ + systemreset(false); + + } else if (((int)(cmd.param1)) == 3) { + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + usleep(100000); + /* reboot to bootloader */ + systemreset(true); + + } else { + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + } + + } else { + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); } break; - } - case VEHICLE_CMD_START_RX_PAIR: - /* handled in the IO driver */ - break; + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - default: - /* don't answer on unsupported commands, it will be done in main loop */ - break; - } + int calib_ret = ERROR; - /* trigger a param autosave if required */ - if (_param_autosave) { - if (_param_autosave_timeout > 0 && hrt_elapsed_time(&_param_autosave_timeout) > 200000ULL) { - int ret = param_save_default(); + /* try to go to INIT/PREFLIGHT arming state */ + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_INIT, &armed, + true /* fRunPreArmChecks */, mavlink_fd)) { + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + break; + } - if (ret == OK) { - mavlink_and_console_log_info(mavlink_fd, "settings autosaved"); + if ((int)(cmd.param1) == 1) { + /* gyro calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_gyro_calibration(mavlink_fd); - } else { - mavlink_and_console_log_critical(mavlink_fd, "settings save error"); + } else if ((int)(cmd.param2) == 1) { + /* magnetometer calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_mag_calibration(mavlink_fd); + + } else if ((int)(cmd.param3) == 1) { + /* zero-altitude pressure calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + + } else if ((int)(cmd.param4) == 1) { + /* RC calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + /* disable RC control input completely */ + status.rc_input_blocked = true; + calib_ret = OK; + mavlink_log_info(mavlink_fd, "CAL: Disabling RC IN"); + + } else if ((int)(cmd.param4) == 2) { + /* RC trim calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_trim_calibration(mavlink_fd); + + } else if ((int)(cmd.param5) == 1) { + /* accelerometer calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_accel_calibration(mavlink_fd); + + } else if ((int)(cmd.param6) == 1) { + /* airspeed calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_airspeed_calibration(mavlink_fd); + + } else if ((int)(cmd.param4) == 0) { + /* RC calibration ended - have we been in one worth confirming? */ + if (status.rc_input_blocked) { + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + /* enable RC control input */ + status.rc_input_blocked = false; + mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN"); + } + + /* this always succeeds */ + calib_ret = OK; + + } + + if (calib_ret == OK) { + tune_positive(true); + + } else { + tune_negative(true); + } + + arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); + + break; } - _param_autosave = false; - _param_autosave_timeout = 0; - } else { - _param_autosave_timeout = hrt_absolute_time(); - } - } + case VEHICLE_CMD_PREFLIGHT_STORAGE: { - /* send any requested ACKs */ - if (cmd.confirmation > 0 && cmd.command != VEHICLE_CMD_DO_SET_MODE - && cmd.command != VEHICLE_CMD_COMPONENT_ARM_DISARM) { - /* send acknowledge command */ - // XXX TODO + if (((int)(cmd.param1)) == 0) { + int ret = param_load_default(); + + if (ret == OK) { + mavlink_log_info(mavlink_fd, "settings loaded"); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + + } else { + mavlink_log_critical(mavlink_fd, "settings load ERROR"); + + /* convenience as many parts of NuttX use negative errno */ + if (ret < 0) { + ret = -ret; + } + + if (ret < 1000) { + mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret)); + } + + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); + } + + } else if (((int)(cmd.param1)) == 1) { + int ret = param_save_default(); + + if (ret == OK) { + mavlink_log_info(mavlink_fd, "settings saved"); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + + } else { + mavlink_log_critical(mavlink_fd, "settings save error"); + + /* convenience as many parts of NuttX use negative errno */ + if (ret < 0) { + ret = -ret; + } + + if (ret < 1000) { + mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret)); + } + + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); + } + } + + break; + } + + case VEHICLE_CMD_START_RX_PAIR: + /* handled in the IO driver */ + break; + + default: + /* don't answer on unsupported commands, it will be done in main loop */ + break; + } + + /* send any requested ACKs */ + if (cmd.confirmation > 0 && cmd.command != VEHICLE_CMD_DO_SET_MODE + && cmd.command != VEHICLE_CMD_COMPONENT_ARM_DISARM) { + /* send acknowledge command */ + // XXX TODO + } } } From 07853fbb5807ff54762b662485f1735af399b625 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 23 Mar 2015 19:00:25 -0700 Subject: [PATCH 111/268] Fixed warnx messages to show correct sensor # --- src/modules/sensors/sensors.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 3b93393d26..259361be62 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -145,7 +145,7 @@ #endif static const int ERROR = -1; -#define CAL_FAILED_APPLY_CAL_MSG "FAILED APPLYING SENSOR CAL" +#define CAL_FAILED_APPLY_CAL_MSG "FAILED APPLYING %s CAL #%u" /** * Sensor app start / stop handling function @@ -1382,12 +1382,12 @@ Sensors::parameter_update_poll(bool forced) failed = failed || (OK != param_get(param_find(str), &gscale.z_scale)); if (failed) { - warnx("%s: gyro #%u", CAL_FAILED_APPLY_CAL_MSG, gyro_count); + warnx(CAL_FAILED_APPLY_CAL_MSG, "gyro", i); } else { /* apply new scaling and offsets */ res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale); if (res) { - warnx(CAL_FAILED_APPLY_CAL_MSG); + warnx(CAL_FAILED_APPLY_CAL_MSG, "gyro", i); } else { gyro_count++; config_ok = true; @@ -1449,12 +1449,12 @@ Sensors::parameter_update_poll(bool forced) failed = failed || (OK != param_get(param_find(str), &gscale.z_scale)); if (failed) { - warnx("%s: acc #%u", CAL_FAILED_APPLY_CAL_MSG, accel_count); + warnx(CAL_FAILED_APPLY_CAL_MSG, "accel", i); } else { /* apply new scaling and offsets */ res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&gscale); if (res) { - warnx(CAL_FAILED_APPLY_CAL_MSG); + warnx(CAL_FAILED_APPLY_CAL_MSG, "accel", i); } else { accel_count++; config_ok = true; @@ -1566,12 +1566,12 @@ Sensors::parameter_update_poll(bool forced) } if (failed) { - warnx("%s: mag #%u", CAL_FAILED_APPLY_CAL_MSG, mag_count); + warnx(CAL_FAILED_APPLY_CAL_MSG, "mag", i); } else { /* apply new scaling and offsets */ res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&gscale); if (res) { - warnx(CAL_FAILED_APPLY_CAL_MSG); + warnx(CAL_FAILED_APPLY_CAL_MSG, "mag", i); } else { mag_count++; config_ok = true; From eff3d9d71313245caec22ab7a80a7aff3fa13b66 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 23 Mar 2015 19:00:49 -0700 Subject: [PATCH 112/268] New worker routines for orientation detection --- .../commander/calibration_routines.cpp | 272 ++++++++++++++++++ src/modules/commander/calibration_routines.h | 45 ++- 2 files changed, 316 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 290e83a0ba..192b8c7270 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -38,11 +38,22 @@ * @author Lorenz Meier */ +#include #include #include +#include +#include +#include +#include +#include +#include #include "calibration_routines.h" +#include "calibration_messages.h" +#include "commander_helper.h" +// FIXME: Fix return codes +static const int ERROR = -1; int sphere_fit_least_squares(const float x[], const float y[], const float z[], unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, @@ -219,3 +230,264 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[], return 0; } +enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub) +{ + const unsigned ndim = 3; + + struct accel_report sensor; + /* exponential moving average of accel */ + float accel_ema[ndim] = { 0.0f }; + /* max-hold dispersion of accel */ + float accel_disp[3] = { 0.0f, 0.0f, 0.0f }; + /* EMA time constant in seconds*/ + float ema_len = 0.5f; + /* set "still" threshold to 0.25 m/s^2 */ + float still_thr2 = powf(0.25f, 2); + /* set accel error threshold to 5m/s^2 */ + float accel_err_thr = 5.0f; + /* still time required in us */ + hrt_abstime still_time = 2000000; + struct pollfd fds[1]; + fds[0].fd = accel_sub; + fds[0].events = POLLIN; + + hrt_abstime t_start = hrt_absolute_time(); + /* set timeout to 30s */ + hrt_abstime timeout = 30000000; + hrt_abstime t_timeout = t_start + timeout; + hrt_abstime t = t_start; + hrt_abstime t_prev = t_start; + hrt_abstime t_still = 0; + + unsigned poll_errcount = 0; + + while (true) { + /* wait blocking for new data */ + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(sensor_accel), accel_sub, &sensor); + t = hrt_absolute_time(); + float dt = (t - t_prev) / 1000000.0f; + t_prev = t; + float w = dt / ema_len; + + for (unsigned i = 0; i < ndim; i++) { + + float di = 0.0f; + switch (i) { + case 0: + di = sensor.x; + break; + case 1: + di = sensor.y; + break; + case 2: + di = sensor.z; + break; + } + + float d = di - accel_ema[i]; + accel_ema[i] += d * w; + d = d * d; + accel_disp[i] = accel_disp[i] * (1.0f - w); + + if (d > still_thr2 * 8.0f) { + d = still_thr2 * 8.0f; + } + + if (d > accel_disp[i]) { + accel_disp[i] = d; + } + } + + /* still detector with hysteresis */ + if (accel_disp[0] < still_thr2 && + accel_disp[1] < still_thr2 && + accel_disp[2] < still_thr2) { + /* is still now */ + if (t_still == 0) { + /* first time */ + mavlink_and_console_log_info(mavlink_fd, "detected rest position, hold still..."); + t_still = t; + t_timeout = t + timeout; + + } else { + /* still since t_still */ + if (t > t_still + still_time) { + /* vehicle is still, exit from the loop to detection of its orientation */ + break; + } + } + + } else if (accel_disp[0] > still_thr2 * 4.0f || + accel_disp[1] > still_thr2 * 4.0f || + accel_disp[2] > still_thr2 * 4.0f) { + /* not still, reset still start time */ + if (t_still != 0) { + mavlink_and_console_log_info(mavlink_fd, "detected motion, hold still..."); + sleep(3); + t_still = 0; + } + } + + } else if (poll_ret == 0) { + poll_errcount++; + } + + if (t > t_timeout) { + poll_errcount++; + } + + if (poll_errcount > 1000) { + mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); + return DETECT_ORIENTATION_ERROR; + } + } + + if (fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr) { + return DETECT_ORIENTATION_TAIL_DOWN; // [ g, 0, 0 ] + } + + if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr) { + return DETECT_ORIENTATION_NOSE_DOWN; // [ -g, 0, 0 ] + } + + if (fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr) { + return DETECT_ORIENTATION_LEFT; // [ 0, g, 0 ] + } + + if (fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr) { + return DETECT_ORIENTATION_RIGHT; // [ 0, -g, 0 ] + } + + if (fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) { + return DETECT_ORIENTATION_UPSIDE_DOWN; // [ 0, 0, g ] + } + + if (fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) { + return DETECT_ORIENTATION_RIGHTSIDE_UP; // [ 0, 0, -g ] + } + + mavlink_and_console_log_critical(mavlink_fd, "ERROR: invalid orientation"); + + return DETECT_ORIENTATION_ERROR; // Can't detect orientation +} + +const char* detect_orientation_str(enum detect_orientation_return orientation) +{ + static const char* rgOrientationStrs[] = { + "back", // tail down + "front", // nose down + "left", + "right", + "up", // upside-down + "down", // right-side up + "error" + }; + + return rgOrientationStrs[orientation]; +} + +int calibrate_from_orientation(int mavlink_fd, + bool side_data_collected[detect_orientation_side_count], + calibration_from_orientation_worker_t calibration_worker, + void* worker_data) +{ + int result = OK; + + // Setup subscriptions to onboard accel sensor + + int sub_accel = orb_subscribe_multi(ORB_ID(sensor_accel), 0); + if (sub_accel < 0) { + mavlink_and_console_log_critical(mavlink_fd, "No onboard accel found, abort"); + return ERROR; + } + + unsigned orientation_failures = 0; + + // Rotate through all three main positions + while (true) { + if (orientation_failures > 10) { + result = ERROR; + mavlink_and_console_log_info(mavlink_fd, CAL_FAILED_ORIENTATION_TIMEOUT); + break; + } + + unsigned int side_complete_count = 0; + + // Update the number of completed sides + for (unsigned i = 0; i < detect_orientation_side_count; i++) { + if (side_data_collected[i]) { + side_complete_count++; + } + } + + if (side_complete_count == detect_orientation_side_count) { + // We have completed all sides, move on + break; + } + + /* inform user which orientations are still needed */ + char pendingStr[256]; + pendingStr[0] = 0; + + for (unsigned int cur_orientation=0; cur_orientation= 0) { + close(sub_accel); + } + + // FIXME: Do we need an orientation complete routine? + + return result; +} diff --git a/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h index ba4ca07ccb..8f34f02044 100644 --- a/src/modules/commander/calibration_routines.h +++ b/src/modules/commander/calibration_routines.h @@ -58,4 +58,47 @@ */ int sphere_fit_least_squares(const float x[], const float y[], const float z[], unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, - float *sphere_radius); \ No newline at end of file + float *sphere_radius); + +// FIXME: Change the name +static const unsigned max_accel_sens = 3; + +// The order of these cannot change since the calibration calculations depend on them in this order +enum detect_orientation_return { + DETECT_ORIENTATION_TAIL_DOWN, + DETECT_ORIENTATION_NOSE_DOWN, + DETECT_ORIENTATION_LEFT, + DETECT_ORIENTATION_RIGHT, + DETECT_ORIENTATION_UPSIDE_DOWN, + DETECT_ORIENTATION_RIGHTSIDE_UP, + DETECT_ORIENTATION_ERROR +}; +static const unsigned detect_orientation_side_count = 6; + +/** + * Wait for vehicle to become still and detect it's orientation. + * + * @param mavlink_fd the MAVLink file descriptor to print output to + * @param accel_sub Subscription to onboard accel + * + * @return detect_orientation)_return according to orientation when vehicle is still and ready for measurements, + * DETECT_ORIENTATION_ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2 + */ +enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub); + + +/** + * Returns the human readable string representation of the orientation + * + * @param orientation Orientation to return string for, "error" if buffer is too small + * + * @return str Returned orientation string + */ +const char* detect_orientation_str(enum detect_orientation_return orientation); + +typedef int (*calibration_from_orientation_worker_t)(detect_orientation_return orientation, void* worker_data); + +int calibrate_from_orientation(int mavlink_fd, + bool side_data_collected[detect_orientation_side_count], + calibration_from_orientation_worker_t calibration_worker, + void* worker_data); From fdc053e88355dcd5690f5cd9e28ca2069c1a2776 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 23 Mar 2015 19:01:03 -0700 Subject: [PATCH 113/268] Add sensor number to messages --- src/modules/commander/calibration_messages.h | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/calibration_messages.h b/src/modules/commander/calibration_messages.h index b1e209efc0..29f44dc368 100644 --- a/src/modules/commander/calibration_messages.h +++ b/src/modules/commander/calibration_messages.h @@ -47,10 +47,12 @@ #define CAL_FAILED_MSG "%s calibration: failed" #define CAL_PROGRESS_MSG "%s calibration: progress <%u>" -#define CAL_FAILED_SENSOR_MSG "ERROR: failed reading sensor" -#define CAL_FAILED_RESET_CAL_MSG "ERROR: failed to reset calibration" -#define CAL_FAILED_APPLY_CAL_MSG "ERROR: failed to apply calibration" -#define CAL_FAILED_SET_PARAMS_MSG "ERROR: failed to set parameters" +#define CAL_FAILED_UNKNOWN_ERROR "ERROR: unknown error" +#define CAL_FAILED_SENSOR_MSG "ERROR: failed reading sensor" +#define CAL_FAILED_RESET_CAL_MSG "ERROR: failed to reset calibration, sensor %u" +#define CAL_FAILED_APPLY_CAL_MSG "ERROR: failed to apply calibration, sensor %u" +#define CAL_FAILED_SET_PARAMS_MSG "ERROR: failed to set parameters, sensor %u" #define CAL_FAILED_SAVE_PARAMS_MSG "ERROR: failed to save parameters" +#define CAL_FAILED_ORIENTATION_TIMEOUT "ERROR: timed out waiting for correct orientation" #endif /* CALIBRATION_MESSAGES_H_ */ From 9d61e3a7db733309c02b238703c3773d436864f2 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 23 Mar 2015 19:01:53 -0700 Subject: [PATCH 114/268] Use new calibrate_from_orientation api --- .../commander/accelerometer_calibration.cpp | 375 +++++------------- 1 file changed, 96 insertions(+), 279 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 526b135d8b..37db59c148 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -120,8 +120,11 @@ * @author Anton Babushkin */ +// FIXME: Can some of these headers move out with detect_ move? + #include "accelerometer_calibration.h" #include "calibration_messages.h" +#include "calibration_routines.h" #include "commander_helper.h" #include @@ -149,18 +152,24 @@ static const int ERROR = -1; static const char *sensor_name = "accel"; -static const unsigned max_sens = 3; - -int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_sens][3], float (&accel_T)[max_sens][3][3], unsigned *active_sensors); -int detect_orientation(int mavlink_fd, int (&subs)[max_sens]); -int read_accelerometer_avg(int (&subs)[max_sens], float (&accel_avg)[max_sens][6][3], unsigned orient, unsigned samples_num); +int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors); +int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num); int mat_invert3(float src[3][3], float dst[3][3]); -int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_sens][6][3], float (&accel_T)[max_sens][3][3], float (&accel_offs)[max_sens][3], float g); +int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g); +int accel_calibration_worker(detect_orientation_return orientation, void* worker_data); + +/// Data passed to calibration worker routine +typedef struct { + int mavlink_fd; + unsigned done_count; + int subs[max_accel_sens]; + float accel_ref[max_accel_sens][detect_orientation_side_count][3]; +} accel_worker_data_t; int do_accel_calibration(int mavlink_fd) { int fd; - int32_t device_id[max_sens]; + int32_t device_id[max_accel_sens]; mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); @@ -178,7 +187,7 @@ int do_accel_calibration(int mavlink_fd) char str[30]; /* reset all sensors */ - for (unsigned s = 0; s < max_sens; s++) { + for (unsigned s = 0; s < max_accel_sens; s++) { sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s); /* reset all offsets to zero and all scales to one */ fd = open(str, 0); @@ -193,12 +202,12 @@ int do_accel_calibration(int mavlink_fd) close(fd); if (res != OK) { - mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, s); } } - float accel_offs[max_sens][3]; - float accel_T[max_sens][3][3]; + float accel_offs[max_accel_sens][3]; + float accel_T[max_accel_sens][3][3]; unsigned active_sensors; if (res == OK) { @@ -234,27 +243,27 @@ int do_accel_calibration(int mavlink_fd) accel_scale.y_scale = accel_T_rotated(1, 1); accel_scale.z_offset = accel_offs_rotated(2); accel_scale.z_scale = accel_T_rotated(2, 2); - + bool failed = false; /* set parameters */ (void)sprintf(str, "CAL_ACC%u_XOFF", i); - failed |= (OK != param_set(param_find(str), &(accel_scale.x_offset))); + failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset))); (void)sprintf(str, "CAL_ACC%u_YOFF", i); - failed |= (OK != param_set(param_find(str), &(accel_scale.y_offset))); + failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.y_offset))); (void)sprintf(str, "CAL_ACC%u_ZOFF", i); - failed |= (OK != param_set(param_find(str), &(accel_scale.z_offset))); + failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.z_offset))); (void)sprintf(str, "CAL_ACC%u_XSCALE", i); - failed |= (OK != param_set(param_find(str), &(accel_scale.x_scale))); + failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.x_scale))); (void)sprintf(str, "CAL_ACC%u_YSCALE", i); - failed |= (OK != param_set(param_find(str), &(accel_scale.y_scale))); + failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.y_scale))); (void)sprintf(str, "CAL_ACC%u_ZSCALE", i); - failed |= (OK != param_set(param_find(str), &(accel_scale.z_scale))); + failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.z_scale))); (void)sprintf(str, "CAL_ACC%u_ID", i); - failed |= (OK != param_set(param_find(str), &(device_id[i]))); + failed |= (OK != param_set_no_notification(param_find(str), &(device_id[i]))); if (failed) { - mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG, i); return ERROR; } @@ -270,7 +279,7 @@ int do_accel_calibration(int mavlink_fd) } if (res != OK) { - mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG, i); } } @@ -291,309 +300,117 @@ int do_accel_calibration(int mavlink_fd) return res; } -int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_sens][3], float (&accel_T)[max_sens][3][3], unsigned *active_sensors) +int accel_calibration_worker(detect_orientation_return orientation, void* data) { const unsigned samples_num = 3000; + accel_worker_data_t* worker_data = (accel_worker_data_t*)(data); + + mavlink_and_console_log_info(worker_data->mavlink_fd, "Hold still, starting to measure %s side", detect_orientation_str(orientation)); + sleep(1); + + read_accelerometer_avg(worker_data->subs, worker_data->accel_ref, orientation, samples_num); + + mavlink_and_console_log_info(worker_data->mavlink_fd, "%s side result: [ %8.4f %8.4f %8.4f ]", detect_orientation_str(orientation), + (double)worker_data->accel_ref[0][orientation][0], + (double)worker_data->accel_ref[0][orientation][1], + (double)worker_data->accel_ref[0][orientation][2]); + + worker_data->done_count++; + mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * worker_data->done_count); + + return OK; +} + +int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors) +{ + int result = OK; + *active_sensors = 0; + + accel_worker_data_t worker_data; + + worker_data.mavlink_fd = mavlink_fd; + worker_data.done_count = 0; - float accel_ref[max_sens][6][3]; - bool data_collected[6] = { false, false, false, false, false, false }; - const char *orientation_strs[6] = { "back", "front", "left", "right", "up", "down" }; + bool data_collected[detect_orientation_side_count] = { false, false, false, false, false, false }; - int subs[max_sens]; + // Initialize subs to error condition so we know which ones are open and which are not + for (size_t i=0; i= 0) { + /* figure out which sensors were active */ + struct accel_report arp = {}; + (void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp); + if (arp.timestamp != 0 && timestamps[i] != arp.timestamp) { + (*active_sensors)++; + } + close(worker_data.subs[i]); } - close(subs[i]); } - if (res == OK) { + if (result == OK) { /* calculate offsets and transform matrix */ for (unsigned i = 0; i < (*active_sensors); i++) { - res = calculate_calibration_values(i, accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); + result = calculate_calibration_values(i, worker_data.accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); - if (res != OK) { + if (result != OK) { mavlink_and_console_log_critical(mavlink_fd, "ERROR: calibration values calculation error"); break; } } } - return res; -} - -/** - * Wait for vehicle become still and detect it's orientation. - * - * @param mavlink_fd the MAVLink file descriptor to print output to - * @param subs the accelerometer subscriptions. Only the first one will be used. - * - * @return 0..5 according to orientation when vehicle is still and ready for measurements, - * ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2 - */ -int detect_orientation(int mavlink_fd, int (&subs)[max_sens]) -{ - const unsigned ndim = 3; - - struct accel_report sensor; - /* exponential moving average of accel */ - float accel_ema[ndim] = { 0.0f }; - /* max-hold dispersion of accel */ - float accel_disp[3] = { 0.0f, 0.0f, 0.0f }; - /* EMA time constant in seconds*/ - float ema_len = 0.5f; - /* set "still" threshold to 0.25 m/s^2 */ - float still_thr2 = powf(0.25f, 2); - /* set accel error threshold to 5m/s^2 */ - float accel_err_thr = 5.0f; - /* still time required in us */ - hrt_abstime still_time = 2000000; - struct pollfd fds[1]; - fds[0].fd = subs[0]; - fds[0].events = POLLIN; - - hrt_abstime t_start = hrt_absolute_time(); - /* set timeout to 30s */ - hrt_abstime timeout = 30000000; - hrt_abstime t_timeout = t_start + timeout; - hrt_abstime t = t_start; - hrt_abstime t_prev = t_start; - hrt_abstime t_still = 0; - - unsigned poll_errcount = 0; - - while (true) { - /* wait blocking for new data */ - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(sensor_accel), subs[0], &sensor); - t = hrt_absolute_time(); - float dt = (t - t_prev) / 1000000.0f; - t_prev = t; - float w = dt / ema_len; - - for (unsigned i = 0; i < ndim; i++) { - - float di = 0.0f; - switch (i) { - case 0: - di = sensor.x; - break; - case 1: - di = sensor.y; - break; - case 2: - di = sensor.z; - break; - } - - float d = di - accel_ema[i]; - accel_ema[i] += d * w; - d = d * d; - accel_disp[i] = accel_disp[i] * (1.0f - w); - - if (d > still_thr2 * 8.0f) { - d = still_thr2 * 8.0f; - } - - if (d > accel_disp[i]) { - accel_disp[i] = d; - } - } - - /* still detector with hysteresis */ - if (accel_disp[0] < still_thr2 && - accel_disp[1] < still_thr2 && - accel_disp[2] < still_thr2) { - /* is still now */ - if (t_still == 0) { - /* first time */ - mavlink_and_console_log_info(mavlink_fd, "detected rest position, hold still..."); - t_still = t; - t_timeout = t + timeout; - - } else { - /* still since t_still */ - if (t > t_still + still_time) { - /* vehicle is still, exit from the loop to detection of its orientation */ - break; - } - } - - } else if (accel_disp[0] > still_thr2 * 4.0f || - accel_disp[1] > still_thr2 * 4.0f || - accel_disp[2] > still_thr2 * 4.0f) { - /* not still, reset still start time */ - if (t_still != 0) { - mavlink_and_console_log_info(mavlink_fd, "detected motion, hold still..."); - sleep(3); - t_still = 0; - } - } - - } else if (poll_ret == 0) { - poll_errcount++; - } - - if (t > t_timeout) { - poll_errcount++; - } - - if (poll_errcount > 1000) { - mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); - return ERROR; - } - } - - if (fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr && - fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr) { - return 0; // [ g, 0, 0 ] - } - - if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr && - fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr) { - return 1; // [ -g, 0, 0 ] - } - - if (fabsf(accel_ema[0]) < accel_err_thr && - fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr) { - return 2; // [ 0, g, 0 ] - } - - if (fabsf(accel_ema[0]) < accel_err_thr && - fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr) { - return 3; // [ 0, -g, 0 ] - } - - if (fabsf(accel_ema[0]) < accel_err_thr && - fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) { - return 4; // [ 0, 0, g ] - } - - if (fabsf(accel_ema[0]) < accel_err_thr && - fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) { - return 5; // [ 0, 0, -g ] - } - - mavlink_and_console_log_critical(mavlink_fd, "ERROR: invalid orientation"); - - return ERROR; // Can't detect orientation + return result; } /* * Read specified number of accelerometer samples, calculate average and dispersion. */ -int read_accelerometer_avg(int (&subs)[max_sens], float (&accel_avg)[max_sens][6][3], unsigned orient, unsigned samples_num) +int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num) { - struct pollfd fds[max_sens]; + struct pollfd fds[max_accel_sens]; - for (unsigned i = 0; i < max_sens; i++) { + for (unsigned i = 0; i < max_accel_sens; i++) { fds[i].fd = subs[i]; fds[i].events = POLLIN; } - unsigned counts[max_sens] = { 0 }; - float accel_sum[max_sens][3]; + unsigned counts[max_accel_sens] = { 0 }; + float accel_sum[max_accel_sens][3]; memset(accel_sum, 0, sizeof(accel_sum)); unsigned errcount = 0; /* use the first sensor to pace the readout, but do per-sensor counts */ while (counts[0] < samples_num) { - int poll_ret = poll(&fds[0], max_sens, 1000); + int poll_ret = poll(&fds[0], max_accel_sens, 1000); if (poll_ret > 0) { - for (unsigned s = 0; s < max_sens; s++) { + for (unsigned s = 0; s < max_accel_sens; s++) { bool changed; orb_check(subs[s], &changed); @@ -620,7 +437,7 @@ int read_accelerometer_avg(int (&subs)[max_sens], float (&accel_avg)[max_sens][6 } } - for (unsigned s = 0; s < max_sens; s++) { + for (unsigned s = 0; s < max_accel_sens; s++) { for (unsigned i = 0; i < 3; i++) { accel_avg[s][orient][i] = accel_sum[s][i] / counts[s]; } @@ -652,7 +469,7 @@ int mat_invert3(float src[3][3], float dst[3][3]) return OK; } -int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_sens][6][3], float (&accel_T)[max_sens][3][3], float (&accel_offs)[max_sens][3], float g) +int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g) { /* calculate offsets */ for (unsigned i = 0; i < 3; i++) { From 716fb561aa24aa0e18aad64dd6fa29a0a9b9ea34 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 23 Mar 2015 19:02:05 -0700 Subject: [PATCH 115/268] Include sensor number --- src/modules/commander/gyro_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index d4a6d6801c..47ee9a5e00 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -114,7 +114,7 @@ int do_gyro_calibration(int mavlink_fd) close(fd); if (res != OK) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG); + mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, s); } } From d88916d20e962ddd60915a80172e83535d526193 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 23 Mar 2015 19:03:23 -0700 Subject: [PATCH 116/268] New mag cal changes - Use new calibrate_from_orientation worker routine to detect orientaions - Calibrate all mags at once - Change to 3-side calibration mechanism --- src/modules/commander/mag_calibration.cpp | 549 +++++++++++++--------- 1 file changed, 334 insertions(+), 215 deletions(-) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index a0aadab39b..5f0754f980 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -49,6 +49,7 @@ #include #include #include +#include #include #include #include @@ -62,283 +63,401 @@ static const int ERROR = -1; static const char *sensor_name = "mag"; +static const unsigned max_mags = 3; + +int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]); +int mag_calibration_worker(detect_orientation_return orientation, void* worker_data); + +/// Data passed to calibration worker routine +typedef struct { + int mavlink_fd; + unsigned done_count; + int sub_mag[max_mags]; + unsigned int calibration_points_perside; + unsigned int calibration_interval_perside_seconds; + uint64_t calibration_interval_perside_useconds; + unsigned int calibration_counter_total; + bool side_data_collected[detect_orientation_side_count]; + float* x[max_mags]; + float* y[max_mags]; + float* z[max_mags]; +} mag_worker_data_t; -int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id); int do_mag_calibration(int mavlink_fd) { - const unsigned max_mags = 3; - - int32_t device_id[max_mags]; mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); - sleep(1); - struct mag_scale mscale_null[max_mags] = { - { + struct mag_scale mscale_null = { 0.0f, 1.0f, 0.0f, 1.0f, 0.0f, 1.0f, - } - } ; + }; - int res = ERROR; + int result = OK; + + // Determine which mags are available and reset each + int32_t device_ids[max_mags]; char str[30]; - unsigned calibrated_ok = 0; + for (size_t i=0; imavlink_fd, "Rotate vehicle around the detected orientation"); + mavlink_and_console_log_info(worker_data->mavlink_fd, "Continue rotation for %u seconds", worker_data->calibration_interval_perside_seconds); + sleep(2); + + uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds; + unsigned poll_errcount = 0; + + calibration_counter_side = 0; + + while (hrt_absolute_time() < calibration_deadline && + calibration_counter_side < worker_data->calibration_points_perside) { + + // Wait clocking for new data on all mags + struct pollfd fds[max_mags]; + size_t fd_count = 0; + for (size_t cur_mag=0; cur_magsub_mag[cur_mag] >= 0) { + fds[fd_count].fd = worker_data->sub_mag[cur_mag]; + fds[fd_count].events = POLLIN; + fd_count++; + } } + int poll_ret = poll(fds, fd_count, 1000); + + if (poll_ret > 0) { + for (size_t cur_mag=0; cur_magsub_mag[cur_mag] >= 0) { + struct mag_report mag; - if (y != nullptr) { - delete y; + orb_copy(ORB_ID(sensor_mag), worker_data->sub_mag[cur_mag], &mag); + + worker_data->x[cur_mag][worker_data->calibration_counter_total] = mag.x; + worker_data->y[cur_mag][worker_data->calibration_counter_total] = mag.y; + worker_data->z[cur_mag][worker_data->calibration_counter_total] = mag.z; + + } + } + + worker_data->calibration_counter_total++; + calibration_counter_side++; + + // Progress indicator for side + mavlink_and_console_log_info(worker_data->mavlink_fd, + "%s %s side calibration: progress <%u>", + sensor_name, + detect_orientation_str(orientation), + (unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside))); + } else { + poll_errcount++; } - - if (z != nullptr) { - delete z; + + if (poll_errcount > worker_data->calibration_points_perside * 3) { + result = ERROR; + mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_FAILED_SENSOR_MSG); + break; } + } + + // Mark the opposite side as collected as well. No need to collect opposite side since it + // would generate similar points. + switch (orientation) { + case DETECT_ORIENTATION_TAIL_DOWN: + worker_data->side_data_collected[DETECT_ORIENTATION_NOSE_DOWN] = true; + break; + case DETECT_ORIENTATION_NOSE_DOWN: + worker_data->side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = true; + break; + case DETECT_ORIENTATION_LEFT: + worker_data->side_data_collected[DETECT_ORIENTATION_RIGHT] = true; + break; + case DETECT_ORIENTATION_RIGHT: + worker_data->side_data_collected[DETECT_ORIENTATION_LEFT] = true; + break; + case DETECT_ORIENTATION_UPSIDE_DOWN: + worker_data->side_data_collected[DETECT_ORIENTATION_RIGHTSIDE_UP] = true; + break; + case DETECT_ORIENTATION_RIGHTSIDE_UP: + worker_data->side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = true; + break; + case DETECT_ORIENTATION_ERROR: + warnx("Invalid orientation in mag_calibration_worker"); + break; + } + + worker_data->done_count++; + mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 34 * worker_data->done_count); + + return result; +} - res = ERROR; - return res; +int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]) +{ + int result = OK; + + mag_worker_data_t worker_data; + + worker_data.mavlink_fd = mavlink_fd; + worker_data.done_count = 0; + worker_data.calibration_counter_total = 0; + worker_data.calibration_points_perside = 80; + worker_data.calibration_interval_perside_seconds = 20; + worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000; + + // Initialize to collect all sides + for (size_t cur_side=0; cur_side<6; cur_side++) { + worker_data.side_data_collected[cur_side] = false; + } + + for (size_t cur_mag=0; cur_mag(malloc(sizeof(float) * calibration_points_maxcount)); + worker_data.y[cur_mag] = reinterpret_cast(malloc(sizeof(float) * calibration_points_maxcount)); + worker_data.z[cur_mag] = reinterpret_cast(malloc(sizeof(float) * calibration_points_maxcount)); + if (worker_data.x[cur_mag] == NULL || worker_data.y[cur_mag] == NULL || worker_data.z[cur_mag] == NULL) { + mavlink_and_console_log_critical(mavlink_fd, "ERROR: out of memory"); + result = ERROR; + } + } - if (sub_mag < 0) { - mavlink_and_console_log_critical(mavlink_fd, "No mag found, abort"); - res = ERROR; - } else { - struct mag_report mag; - - /* limit update rate to get equally spaced measurements over time (in ms) */ - orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); - - /* calibrate offsets */ - uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; - unsigned poll_errcount = 0; - - mavlink_and_console_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down"); - - calibration_counter = 0U; - - while (hrt_absolute_time() < calibration_deadline && - calibration_counter < calibration_maxcount) { - - /* wait blocking for new data */ - struct pollfd fds[1]; - fds[0].fd = sub_mag; - fds[0].events = POLLIN; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret > 0) { - orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); - - x[calibration_counter] = mag.x; - y[calibration_counter] = mag.y; - z[calibration_counter] = mag.z; - - calibration_counter++; - - if (calibration_counter % (calibration_maxcount / 20) == 0) { - mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount); - } - - } else { - poll_errcount++; - } - - if (poll_errcount > 1000) { - mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); - res = ERROR; + + // Setup subscriptions to mag sensors + if (result == OK) { + for (unsigned cur_mag=0; cur_mag (calibration_maxcount / 2)) { - - /* sphere fit */ - mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70); - sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); - mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 80); - - if (!isfinite(sphere_x) || !isfinite(sphere_y) || !isfinite(sphere_z)) { - mavlink_and_console_log_critical(mavlink_fd, "ERROR: NaN in sphere fit"); - res = ERROR; - } - } - - if (x != nullptr) { - delete x; - } - - if (y != nullptr) { - delete y; - } - - if (z != nullptr) { - delete z; - } - - if (res == OK) { - /* apply calibration and set parameters */ - struct mag_scale mscale; - (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s); - int fd = open(str, 0); - res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale); - - if (res != OK) { - mavlink_and_console_log_critical(mavlink_fd, "ERROR: failed to get current calibration"); - } - - if (res == OK) { - mscale.x_offset = sphere_x; - mscale.y_offset = sphere_y; - mscale.z_offset = sphere_z; - - res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale); - - if (res != OK) { - mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); + + // Limit update rate to get equally spaced measurements over time (in ms) + if (result == OK) { + for (unsigned cur_mag=0; cur_mag= 0) { + close(worker_data.sub_mag[cur_mag]); + } + } + + // Calculate calibration values for each mag + + + float sphere_x[max_mags]; + float sphere_y[max_mags]; + float sphere_z[max_mags]; + float sphere_radius[max_mags]; + + // Sphere fit the data to get calibration values + if (result == OK) { + for (unsigned cur_mag=0; cur_mag= 0) { + close(fd_mag); + } + + if (result == OK) { + bool failed = false; + + /* set parameters */ + (void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag); + failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_offset))); + (void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag); + failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_offset))); + (void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag); + failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_offset))); + (void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag); + failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_scale))); + (void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag); + failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_scale))); + (void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag); + failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_scale))); + + if (failed) { + mavlink_and_console_log_info(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG, cur_mag); + result = ERROR; + } else { + mavlink_and_console_log_info(mavlink_fd, "mag #%u off: x:%.2f y:%.2f z:%.2f Ga", + cur_mag, + (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset); + mavlink_and_console_log_info(mavlink_fd, "mag #%u scale: x:%.2f y:%.2f z:%.2f", + cur_mag, + (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale); + } + } + } + } + } + + return result; } From 48e88ec5516c0f9de97f4de7e0ee412e5f82eb71 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Thu, 26 Mar 2015 10:45:56 -0700 Subject: [PATCH 117/268] Set CAL_MAG_ID correctly --- src/modules/commander/mag_calibration.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 5f0754f980..bfe73d3386 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -111,7 +111,7 @@ int do_mag_calibration(int mavlink_fd) for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) { // Reset mag id to mag not available (void)sprintf(str, "CAL_MAG%u_ID", cur_mag); - result = param_set_no_notification(param_find(str), &(device_ids[cur_mag]));; + result = param_set_no_notification(param_find(str), &(device_ids[cur_mag])); if (result != OK) { mavlink_and_console_log_info(mavlink_fd, "Unabled to reset CAL_MAG%u_ID", cur_mag); break; @@ -430,6 +430,8 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]) bool failed = false; /* set parameters */ + (void)sprintf(str, "CAL_MAG%u_ID", cur_mag); + failed |= (OK != param_set_no_notification(param_find(str), &(device_ids[cur_mag]))); (void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag); failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_offset))); (void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag); From 8ebf9755e44c45a957d51f1a75eb102bf1868c58 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Mar 2015 13:54:05 -0700 Subject: [PATCH 118/268] commander: Fix battery config --- src/modules/commander/commander.cpp | 10 +++-- src/modules/commander/commander_helper.cpp | 44 +++++++++++----------- src/modules/commander/commander_helper.h | 2 + 3 files changed, 32 insertions(+), 24 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 35f538b230..21f1eacecc 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -865,12 +865,16 @@ int commander_thread_main(int argc, char *argv[]) pthread_t commander_low_prio_thread; /* initialize */ - if (led_init() != 0) { - warnx("ERROR: LED INIT FAIL"); + if (led_init() != OK) { + mavlink_and_console_log_critical(mavlink_fd, "ERROR: LED INIT FAIL"); } if (buzzer_init() != OK) { - warnx("ERROR: BUZZER INIT FAIL"); + mavlink_and_console_log_critical(mavlink_fd, "ERROR: BUZZER INIT FAIL"); + } + + if (battery_init() != OK) { + mavlink_and_console_log_critical(mavlink_fd, "ERROR: BATTERY INIT FAIL"); } mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index a2e827a15e..a5e4d19725 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -90,6 +90,29 @@ static hrt_abstime tune_end = 0; // end time of currently played tune, 0 for re static int tune_current = TONE_STOP_TUNE; // currently playing tune, can be interrupted after tune_end static unsigned int tune_durations[TONE_NUMBER_OF_TUNES]; +static param_t bat_v_empty_h; +static param_t bat_v_full_h; +static param_t bat_n_cells_h; +static param_t bat_capacity_h; +static param_t bat_v_load_drop_h; +static float bat_v_empty = 3.4f; +static float bat_v_full = 4.2f; +static float bat_v_load_drop = 0.06f; +static int bat_n_cells = 3; +static float bat_capacity = -1.0f; +static unsigned int counter = 0; + +int battery_init() +{ + bat_v_empty_h = param_find("BAT_V_EMPTY"); + bat_v_full_h = param_find("BAT_V_CHARGED"); + bat_n_cells_h = param_find("BAT_N_CELLS"); + bat_capacity_h = param_find("BAT_CAPACITY"); + bat_v_load_drop_h = param_find("BAT_V_LOAD_DROP"); + + return OK; +} + int buzzer_init() { tune_end = 0; @@ -303,27 +326,6 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern) float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized) { float ret = 0; - static param_t bat_v_empty_h; - static param_t bat_v_full_h; - static param_t bat_n_cells_h; - static param_t bat_capacity_h; - static param_t bat_v_load_drop_h; - static float bat_v_empty = 3.4f; - static float bat_v_full = 4.2f; - static float bat_v_load_drop = 0.06f; - static int bat_n_cells = 3; - static float bat_capacity = -1.0f; - static bool initialized = false; - static unsigned int counter = 0; - - if (!initialized) { - bat_v_empty_h = param_find("BAT_V_EMPTY"); - bat_v_full_h = param_find("BAT_V_CHARGED"); - bat_n_cells_h = param_find("BAT_N_CELLS"); - bat_capacity_h = param_find("BAT_CAPACITY"); - bat_v_load_drop_h = param_find("BAT_V_LOAD_DROP"); - initialized = true; - } if (counter % 100 == 0) { param_get(bat_v_empty_h, &bat_v_empty); diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index cd3db73247..0cefedba7c 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -73,6 +73,8 @@ void rgbled_set_color(rgbled_color_t color); void rgbled_set_mode(rgbled_mode_t mode); void rgbled_set_pattern(rgbled_pattern_t *pattern); +int battery_init(); + /** * Estimate remaining battery charge. * From a184aebf0f27a77d528921d66965ab1304c4fca0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Mar 2015 13:54:19 -0700 Subject: [PATCH 119/268] mavlink: Crank up param transmission rate --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 632c8c709f..74395ec030 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1361,7 +1361,7 @@ Mavlink::task_main(int argc, char *argv[]) /* PARAM_VALUE stream */ _parameters_manager = (MavlinkParametersManager *) MavlinkParametersManager::new_instance(this); - _parameters_manager->set_interval(interval_from_rate(80.0f)); + _parameters_manager->set_interval(interval_from_rate(120.0f)); LL_APPEND(_streams, _parameters_manager); /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on From cb99467cdeaffc783c9df8fd286a6a6ca2b2df34 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Mar 2015 14:00:17 -0700 Subject: [PATCH 120/268] commander: Remove unnessary sleep --- src/modules/commander/accelerometer_calibration.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 37db59c148..409c2ea003 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -306,7 +306,6 @@ int accel_calibration_worker(detect_orientation_return orientation, void* data) accel_worker_data_t* worker_data = (accel_worker_data_t*)(data); mavlink_and_console_log_info(worker_data->mavlink_fd, "Hold still, starting to measure %s side", detect_orientation_str(orientation)); - sleep(1); read_accelerometer_avg(worker_data->subs, worker_data->accel_ref, orientation, samples_num); From 17ee20a338a05a9d46c4aa8a4c193c0b0810b3b1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Mar 2015 16:44:26 -0700 Subject: [PATCH 121/268] commander: Improve gyro cal --- src/modules/commander/gyro_calibration.cpp | 132 +-------------------- 1 file changed, 3 insertions(+), 129 deletions(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 47ee9a5e00..e2a7ef7432 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -82,7 +82,7 @@ int do_gyro_calibration(int mavlink_fd) 1.0f, }; - struct gyro_scale gyro_scale[max_gyros]; + struct gyro_scale gyro_scale[max_gyros] = {}; int res = OK; @@ -196,7 +196,7 @@ int do_gyro_calibration(int mavlink_fd) float zdiff = gyro_report_0.z - gyro_scale[0].z_offset; /* maximum allowable calibration error in radians */ - const float maxoff = 0.01f; + const float maxoff = 0.002f; if (!isfinite(gyro_scale[0].x_offset) || !isfinite(gyro_scale[0].y_offset) || @@ -204,7 +204,7 @@ int do_gyro_calibration(int mavlink_fd) fabsf(xdiff) > maxoff || fabsf(ydiff) > maxoff || fabsf(zdiff) > maxoff) { - mavlink_log_critical(mavlink_fd, "ERROR: Calibration failed"); + mavlink_log_critical(mavlink_fd, "ERROR: Motion during calibration"); res = ERROR; } } @@ -252,132 +252,6 @@ int do_gyro_calibration(int mavlink_fd) } } -#if 0 - /* beep on offset calibration end */ - mavlink_log_info(mavlink_fd, "gyro offset calibration done"); - tune_neutral(); - - /* scale calibration */ - /* this was only a proof of concept and is currently not working. scaling will be set to 1.0 for now. */ - - mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip."); - warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip."); - - /* apply new offsets */ - fd = open(GYRO_DEVICE_PATH, 0); - - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) { - warn("WARNING: failed to apply new offsets for gyro"); - } - - close(fd); - - - unsigned rotations_count = 30; - float gyro_integral = 0.0f; - float baseline_integral = 0.0f; - - // XXX change to mag topic - orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); - - float mag_last = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]); - - if (mag_last > M_PI_F) { mag_last -= 2 * M_PI_F; } - - if (mag_last < -M_PI_F) { mag_last += 2 * M_PI_F; } - - - uint64_t last_time = hrt_absolute_time(); - uint64_t start_time = hrt_absolute_time(); - - while ((int)fabsf(baseline_integral / (2.0f * M_PI_F)) < rotations_count) { - - /* abort this loop if not rotated more than 180 degrees within 5 seconds */ - if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f) - && (hrt_absolute_time() - start_time > 5 * 1e6)) { - mavlink_log_info(mavlink_fd, "scale skipped, gyro calibration done"); - close(sub_sensor_combined); - return OK; - } - - /* wait blocking for new data */ - struct pollfd fds[1]; - fds[0].fd = sub_sensor_combined; - fds[0].events = POLLIN; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - - float dt_ms = (hrt_absolute_time() - last_time) / 1e3f; - last_time = hrt_absolute_time(); - - orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); - - // XXX this is just a proof of concept and needs world / body - // transformation and more - - //math::Vector2f magNav(raw.magnetometer_ga); - - // calculate error between estimate and measurement - // apply declination correction for true heading as well. - //float mag = -atan2f(magNav(1),magNav(0)); - float mag = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]); - - if (mag > M_PI_F) { mag -= 2 * M_PI_F; } - - if (mag < -M_PI_F) { mag += 2 * M_PI_F; } - - float diff = mag - mag_last; - - if (diff > M_PI_F) { diff -= 2 * M_PI_F; } - - if (diff < -M_PI_F) { diff += 2 * M_PI_F; } - - baseline_integral += diff; - mag_last = mag; - // Jump through some timing scale hoops to avoid - // operating near the 1e6/1e8 max sane resolution of float. - gyro_integral += (raw.gyro_rad_s[2] * dt_ms) / 1e3f; - -// warnx("dbg: b: %6.4f, g: %6.4f", (double)baseline_integral, (double)gyro_integral); - - // } else if (poll_ret == 0) { - // /* any poll failure for 1s is a reason to abort */ - // mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); - // return; - } - } - - float gyro_scale = baseline_integral / gyro_integral; - - warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale); - mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale); - - - if (!isfinite(gyro_scale.x_scale) || !isfinite(gyro_scale.y_scale) || !isfinite(gyro_scale.z_scale)) { - mavlink_log_info(mavlink_fd, "gyro scale calibration FAILED (NaN)"); - close(sub_sensor_gyro); - mavlink_log_critical(mavlink_fd, "gyro calibration failed"); - return ERROR; - } - - /* beep on calibration end */ - mavlink_log_info(mavlink_fd, "gyro scale calibration done"); - tune_neutral(); - - if (res == OK) { - /* set scale parameters to new values */ - if (param_set(param_find("CAL_GYRO0_XSCALE"), &(gyro_scale.x_scale)) - || param_set(param_find("CAL_GYRO0_YSCALE"), &(gyro_scale.y_scale)) - || param_set(param_find("CAL_GYRO0_ZSCALE"), &(gyro_scale.z_scale))) { - mavlink_log_critical(mavlink_fd, "ERROR: failed to set scale params"); - res = ERROR; - } - } - -#endif - if (res == OK) { /* auto-save to EEPROM */ res = param_save_default(); From 2d19726ee781793d18a4dab4e003d5a24abaf2c3 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 29 Mar 2015 20:17:09 +0100 Subject: [PATCH 122/268] rotation: added another missing rotation --- src/lib/conversion/rotation.cpp | 5 +++++ src/lib/conversion/rotation.h | 4 +++- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index 1fe36d3950..7418ba4ef1 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -194,5 +194,10 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z) tmp = z; z = x; x = -tmp; return; } + case ROTATION_ROLL_180_PITCH_270: { + tmp = z; z = x; x = tmp; + y = -y; + return; + } } } diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 13fe701dff..ef0f41c3a8 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -75,6 +75,7 @@ enum Rotation { ROTATION_PITCH_90 = 24, ROTATION_PITCH_270 = 25, ROTATION_ROLL_270_YAW_270 = 26, + ROTATION_ROLL_180_PITCH_270 = 27, ROTATION_MAX }; @@ -111,7 +112,8 @@ const rot_lookup_t rot_lookup[] = { {270, 0, 135 }, { 0, 90, 0 }, { 0, 270, 0 }, - {270, 0, 270 } + {270, 0, 270 }, + {180, 270, 0 } }; /** From ce9f46d4e75678729e014b3f94f01875acac2322 Mon Sep 17 00:00:00 2001 From: Matt Beall Date: Tue, 31 Mar 2015 10:30:57 -0600 Subject: [PATCH 123/268] Added ping message handling to mavlink receiver --- src/modules/mavlink/mavlink_receiver.cpp | 15 +++++++++++++++ src/modules/mavlink/mavlink_receiver.h | 1 + 2 files changed, 16 insertions(+) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e3f274b8bf..1f91654dab 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -157,6 +157,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_optical_flow_rad(msg); break; + case MAVLINK_MSG_ID_PING: + handle_message_ping(msg); + break; + case MAVLINK_MSG_ID_SET_MODE: handle_message_set_mode(msg); break; @@ -946,6 +950,17 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) } } +void +MavlinkReceiver::handle_message_ping(mavlink_message_t *msg) +{ + mavlink_ping_t ping; + mavlink_msg_ping_decode( msg, &ping); + if (mavlink_system.sysid == ping.target_system && + mavlink_system.compid == ping.target_component) { + _mavlink->send_message(MAVLINK_MSG_ID_PING, &msg); + } +} + void MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 1d214b948f..ffacb59a63 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -127,6 +127,7 @@ private: void handle_message_radio_status(mavlink_message_t *msg); void handle_message_manual_control(mavlink_message_t *msg); void handle_message_heartbeat(mavlink_message_t *msg); + void handle_message_ping(mavlink_message_t *msg); void handle_message_request_data_stream(mavlink_message_t *msg); void handle_message_system_time(mavlink_message_t *msg); void handle_message_timesync(mavlink_message_t *msg); From fa9f7075fa42197e2daed6513647d07761dde090 Mon Sep 17 00:00:00 2001 From: Matt Beall Date: Tue, 31 Mar 2015 10:52:26 -0600 Subject: [PATCH 124/268] Changed message staging --- src/modules/mavlink/mavlink_receiver.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 1f91654dab..3f9f7e1392 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -955,9 +955,11 @@ MavlinkReceiver::handle_message_ping(mavlink_message_t *msg) { mavlink_ping_t ping; mavlink_msg_ping_decode( msg, &ping); - if (mavlink_system.sysid == ping.target_system && - mavlink_system.compid == ping.target_component) { - _mavlink->send_message(MAVLINK_MSG_ID_PING, &msg); + if ((mavlink_system.sysid == ping.target_system) && + (mavlink_system.compid == ping.target_component)) { + mavlink_message_t msg_out; + mavlink_msg_ping_encode(_mavlink->get_system_id(), _mavlink->get_component_id(), &msg_out, &ping); + _mavlink->send_message(MAVLINK_MSG_ID_PING, &msg_out); } } From 8380b81705b182ef1a5ce5ee38134e9490e2ced4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marcel=20St=C3=BCttgen?= Date: Tue, 31 Mar 2015 22:52:43 +0200 Subject: [PATCH 125/268] Update rc.axialracing_ax10_defaults back to original PWM default values --- ROMFS/px4fmu_common/init.d/rc.axialracing_ax10_defaults | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.axialracing_ax10_defaults b/ROMFS/px4fmu_common/init.d/rc.axialracing_ax10_defaults index e5d400abe4..23bf47df16 100644 --- a/ROMFS/px4fmu_common/init.d/rc.axialracing_ax10_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.axialracing_ax10_defaults @@ -19,8 +19,8 @@ set PWM_RATE 50 set PWM_DISARMED 1500 # PWM range -set PWM_MIN 1100 -set PWM_MAX 1900 +set PWM_MIN 1200 +set PWM_MAX 1800 # Enable servo output on pins 3 and 4 (steering and thrust) # but also include 1+2 as they form together one output group From 368a21ddab652ede39ffee3bd0a4f8e770fd0c8d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 3 Apr 2015 08:54:49 +0200 Subject: [PATCH 126/268] Fix typo in vector matrix product, fixes #1969, reported by @cat888 --- src/modules/ekf_att_pos_estimator/estimator_utilities.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp index 470eb4e2c2..83a7384635 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp @@ -140,7 +140,7 @@ Vector3f operator*(const Mat3f &matIn, const Vector3f &vecIn) Vector3f vecOut; vecOut.x = matIn.x.x*vecIn.x + matIn.x.y*vecIn.y + matIn.x.z*vecIn.z; vecOut.y = matIn.y.x*vecIn.x + matIn.y.y*vecIn.y + matIn.y.z*vecIn.z; - vecOut.z = matIn.x.x*vecIn.x + matIn.z.y*vecIn.y + matIn.z.z*vecIn.z; + vecOut.z = matIn.z.x*vecIn.x + matIn.z.y*vecIn.y + matIn.z.z*vecIn.z; return vecOut; } From a8d4572cfa23af889c752d95d0512e5ced70a98a Mon Sep 17 00:00:00 2001 From: Bharat Tak Date: Fri, 3 Apr 2015 11:14:40 +0200 Subject: [PATCH 127/268] Fix issue #1972. Tested in gazebo and verified vehicle_status.arming_state on rqt_plot --- .../ros/nodes/commander/commander.cpp | 31 +++++++++---------- src/platforms/ros/nodes/commander/commander.h | 7 +++-- 2 files changed, 19 insertions(+), 19 deletions(-) diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp index 0c32026f39..abaa6fc60d 100644 --- a/src/platforms/ros/nodes/commander/commander.cpp +++ b/src/platforms/ros/nodes/commander/commander.cpp @@ -53,6 +53,7 @@ Commander::Commander() : _msg_parameter_update(), _msg_actuator_armed(), _msg_vehicle_control_mode(), + _msg_vehicle_status(), _msg_offboard_control_mode(), _got_manual_control(false) { @@ -64,10 +65,9 @@ Commander::Commander() : void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg) { _got_manual_control = true; - px4::vehicle_status msg_vehicle_status; /* fill vehicle control mode based on (faked) stick positions*/ - EvalSwitches(msg, msg_vehicle_status, _msg_vehicle_control_mode); + EvalSwitches(msg, _msg_vehicle_status, _msg_vehicle_control_mode); _msg_vehicle_control_mode.timestamp = px4::get_time_micros(); /* fill actuator armed */ @@ -79,7 +79,7 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon if (msg->r < -arm_th && msg->z < (1 - arm_th)) { _msg_actuator_armed.armed = false; _msg_vehicle_control_mode.flag_armed = false; - msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_STANDBY; + _msg_vehicle_status.arming_state = _msg_vehicle_status.ARMING_STATE_STANDBY; } } else { @@ -87,19 +87,19 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon if (msg->r > arm_th && msg->z < (1 - arm_th)) { _msg_actuator_armed.armed = true; _msg_vehicle_control_mode.flag_armed = true; - msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED; + _msg_vehicle_status.arming_state = _msg_vehicle_status.ARMING_STATE_ARMED; } } /* fill vehicle status */ - msg_vehicle_status.timestamp = px4::get_time_micros(); - msg_vehicle_status.hil_state = msg_vehicle_status.HIL_STATE_OFF; - msg_vehicle_status.hil_state = msg_vehicle_status.VEHICLE_TYPE_QUADROTOR; - msg_vehicle_status.is_rotary_wing = true; + _msg_vehicle_status.timestamp = px4::get_time_micros(); + _msg_vehicle_status.hil_state = _msg_vehicle_status.HIL_STATE_OFF; + _msg_vehicle_status.hil_state = _msg_vehicle_status.VEHICLE_TYPE_QUADROTOR; + _msg_vehicle_status.is_rotary_wing = true; _vehicle_control_mode_pub.publish(_msg_vehicle_control_mode); _actuator_armed_pub.publish(_msg_actuator_armed); - _vehicle_status_pub.publish(msg_vehicle_status); + _vehicle_status_pub.publish(_msg_vehicle_status); /* Fill parameter update */ if (px4::get_time_micros() - _msg_parameter_update.timestamp > 1e6) { @@ -206,12 +206,11 @@ void Commander::OffboardControlModeCallback(const px4::offboard_control_modeCons if (!_got_manual_control) { SetOffboardControl(_msg_offboard_control_mode, _msg_vehicle_control_mode); - px4::vehicle_status msg_vehicle_status; - msg_vehicle_status.timestamp = px4::get_time_micros(); - msg_vehicle_status.hil_state = msg_vehicle_status.HIL_STATE_OFF; - msg_vehicle_status.hil_state = msg_vehicle_status.VEHICLE_TYPE_QUADROTOR; - msg_vehicle_status.is_rotary_wing = true; - msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED; + _msg_vehicle_status.timestamp = px4::get_time_micros(); + _msg_vehicle_status.hil_state = _msg_vehicle_status.HIL_STATE_OFF; + _msg_vehicle_status.hil_state = _msg_vehicle_status.VEHICLE_TYPE_QUADROTOR; + _msg_vehicle_status.is_rotary_wing = true; + _msg_vehicle_status.arming_state = _msg_vehicle_status.ARMING_STATE_ARMED; _msg_actuator_armed.armed = true; @@ -223,7 +222,7 @@ void Commander::OffboardControlModeCallback(const px4::offboard_control_modeCons _vehicle_control_mode_pub.publish(_msg_vehicle_control_mode); _actuator_armed_pub.publish(_msg_actuator_armed); - _vehicle_status_pub.publish(msg_vehicle_status); + _vehicle_status_pub.publish(_msg_vehicle_status); } } diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h index fc88260ba8..856144389d 100644 --- a/src/platforms/ros/nodes/commander/commander.h +++ b/src/platforms/ros/nodes/commander/commander.h @@ -68,14 +68,14 @@ protected: * Set control mode flags based on stick positions (equiv to code in px4 commander) */ void EvalSwitches(const px4::manual_control_setpointConstPtr &msg, - px4::vehicle_status &msg_vehicle_status, - px4::vehicle_control_mode &msg_vehicle_control_mode); + px4::vehicle_status &msg_vehicle_status, + px4::vehicle_control_mode &msg_vehicle_control_mode); /** * Sets offboard controll flags in msg_vehicle_control_mode */ void SetOffboardControl(const px4::offboard_control_mode &msg_offboard_control_mode, - px4::vehicle_control_mode &msg_vehicle_control_mode); + px4::vehicle_control_mode &msg_vehicle_control_mode); ros::NodeHandle _n; ros::Subscriber _man_ctrl_sp_sub; @@ -88,6 +88,7 @@ protected: px4::parameter_update _msg_parameter_update; px4::actuator_armed _msg_actuator_armed; px4::vehicle_control_mode _msg_vehicle_control_mode; + px4::vehicle_status _msg_vehicle_status; px4::offboard_control_mode _msg_offboard_control_mode; bool _got_manual_control; From cc11e8a0985f1a9790510fb3f19447a6c6946ba1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 3 Apr 2015 17:01:32 +0200 Subject: [PATCH 128/268] EKF: Sync to upstream repo --- .../estimator_22states.cpp | 8 ++++--- .../estimator_utilities.cpp | 21 +++++++++++++++++++ .../estimator_utilities.h | 7 ++++++- 3 files changed, 32 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 817590babf..967402fff7 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -41,13 +41,17 @@ #include #include #include -#include +#include #include #ifndef M_PI_F #define M_PI_F static_cast(M_PI) #endif +#ifndef isfinite +#define isfinite(__x) std::isfinite(__x) +#endif + constexpr float EKF_COVARIANCE_DIVERGED = 1.0e8f; AttPosEKF::AttPosEKF() : @@ -2626,11 +2630,9 @@ float AttPosEKF::ConstrainFloat(float val, float min_val, float max_val) ret = val; } -#if 0 if (!isfinite(val)) { ekf_debug("constrain: non-finite!"); } -#endif return ret; } diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp index 83a7384635..284a099023 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp @@ -114,6 +114,27 @@ Mat3f Mat3f::transpose() const return ret; } +void calcvelNED(float (&velNEDr)[3], float gpsCourse, float gpsGndSpd, float gpsVelD) +{ + velNEDr[0] = gpsGndSpd*cosf(gpsCourse); + velNEDr[1] = gpsGndSpd*sinf(gpsCourse); + velNEDr[2] = gpsVelD; +} + +void calcposNED(float (&posNEDr)[3], double lat, double lon, float hgt, double latReference, double lonReference, float hgtReference) +{ + posNEDr[0] = earthRadius * (lat - latReference); + posNEDr[1] = earthRadius * cos(latReference) * (lon - lonReference); + posNEDr[2] = -(hgt - hgtReference); +} + +void calcLLH(float posNEDi[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef) +{ + lat = latRef + (double)posNEDi[0] * earthRadiusInv; + lon = lonRef + (double)posNEDi[1] * earthRadiusInv / cos(latRef); + hgt = hgtRef - posNEDi[2]; +} + // overload + operator to provide a vector addition Vector3f operator+(const Vector3f &vecIn1, const Vector3f &vecIn2) { diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h index c137209ffe..788fb85ec9 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -45,7 +45,6 @@ #define GRAVITY_MSS 9.80665f #define deg2rad 0.017453292f #define rad2deg 57.295780f -#define pi 3.141592657f #define earthRate 0.000072921f #define earthRadius 6378145.0 #define earthRadiusInv 1.5678540e-7 @@ -128,3 +127,9 @@ struct ekf_status_report { }; void ekf_debug(const char *fmt, ...); + +void calcvelNED(float (&velNEDr)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); + +void calcposNED(float (&posNEDr)[3], double lat, double lon, float hgt, double latReference, double lonReference, float hgtReference); + +void calcLLH(float posNEDi[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef); From 8075ff5e0bf6d8695a06c069037f067fcbf4fb08 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 3 Apr 2015 17:01:43 +0200 Subject: [PATCH 129/268] Eigen: Re-enable tests --- src/systemcmds/tests/tests_main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index ced64b31cd..484c6d9d61 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -112,7 +112,7 @@ const struct { #ifndef TESTS_MATHLIB_DISABLE {"mathlib", test_mathlib, 0}, #endif - {"eigen", test_eigen, OPT_NOALLTEST}, + {"eigen", test_eigen, OPT_NOJIGTEST}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} }; From 2dec10b8b73b7074cc05d8b8b504853fb8df74c2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 3 Apr 2015 17:07:49 +0200 Subject: [PATCH 130/268] Travis CI: Change settings to make package installation operational again --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index d659bf6508..9796144bbe 100644 --- a/.travis.yml +++ b/.travis.yml @@ -11,7 +11,7 @@ before_script: # Travis specific tools - sudo apt-get install s3cmd grep zip mailutils # General toolchain dependencies - - sudo apt-get install libc6:i386 libgcc1:i386 gcc-4.6-base:i386 libstdc++5:i386 libstdc++6:i386 + - sudo apt-get install libc6:i386 libgcc1:i386 gcc-4.6-base:i386 libstdc++5:i386 - sudo apt-get install python-serial python-argparse python-empy - sudo apt-get install flex bison libncurses5-dev autoconf texinfo build-essential libtool zlib1g-dev genromfs git wget cmake - pushd . From bb6b30f57a02912d63260456b1f8d1792a0e02e7 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Fri, 3 Apr 2015 19:51:58 -0700 Subject: [PATCH 131/268] Report alternate side done --- src/modules/commander/mag_calibration.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index bfe73d3386..198acb0272 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -239,29 +239,32 @@ int mag_calibration_worker(detect_orientation_return orientation, void* data) // Mark the opposite side as collected as well. No need to collect opposite side since it // would generate similar points. + detect_orientation_return alternateOrientation = orientation; switch (orientation) { case DETECT_ORIENTATION_TAIL_DOWN: - worker_data->side_data_collected[DETECT_ORIENTATION_NOSE_DOWN] = true; + alternateOrientation = DETECT_ORIENTATION_NOSE_DOWN; break; case DETECT_ORIENTATION_NOSE_DOWN: - worker_data->side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = true; + alternateOrientation = DETECT_ORIENTATION_TAIL_DOWN; break; case DETECT_ORIENTATION_LEFT: - worker_data->side_data_collected[DETECT_ORIENTATION_RIGHT] = true; + alternateOrientation = DETECT_ORIENTATION_RIGHT; break; case DETECT_ORIENTATION_RIGHT: - worker_data->side_data_collected[DETECT_ORIENTATION_LEFT] = true; + alternateOrientation = DETECT_ORIENTATION_LEFT; break; case DETECT_ORIENTATION_UPSIDE_DOWN: - worker_data->side_data_collected[DETECT_ORIENTATION_RIGHTSIDE_UP] = true; + alternateOrientation = DETECT_ORIENTATION_RIGHTSIDE_UP; break; case DETECT_ORIENTATION_RIGHTSIDE_UP: - worker_data->side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = true; + alternateOrientation = DETECT_ORIENTATION_UPSIDE_DOWN; break; case DETECT_ORIENTATION_ERROR: warnx("Invalid orientation in mag_calibration_worker"); break; } + worker_data->side_data_collected[alternateOrientation] = true; + mavlink_and_console_log_info(worker_data->mavlink_fd, "%s side done, rotate to a different side", detect_orientation_str(alternateOrientation)); worker_data->done_count++; mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 34 * worker_data->done_count); From 64cdcfc0cca78978499248054bde932f6ba27c47 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 27 Feb 2015 19:31:04 +0100 Subject: [PATCH 132/268] Fix detection of USB connected state. Needs bench test without USB --- msg/vehicle_status.msg | 1 + src/modules/commander/commander.cpp | 14 +++++--------- 2 files changed, 6 insertions(+), 9 deletions(-) diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 18df3252fe..2e001cac73 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -112,6 +112,7 @@ bool condition_airspeed_valid # set to true by the commander app if there is a bool condition_landed # true if vehicle is landed, always true if disarmed bool condition_power_input_valid # set if input power is valid float32 avionics_power_rail_voltage # voltage of the avionics power rail +bool usb_connected # status of the USB power supply bool rc_signal_found_once bool rc_signal_lost # true if RC reception lost diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 21f1eacecc..9330ef6827 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -169,8 +169,6 @@ static bool _param_autosave = false; static unsigned int leds_counter; /* To remember when last notification was sent */ static uint64_t last_print_mode_reject_time = 0; -/* if connected via USB */ -static bool on_usb_power = false; static float takeoff_alt = 5.0f; static int parachute_enabled = 0; @@ -380,8 +378,8 @@ void usage(const char *reason) void print_status() { - warnx("type: %s", (status.is_rotary_wing) ? "ROTARY" : "PLANE"); - warnx("usb powered: %s", (on_usb_power) ? "yes" : "no"); + warnx("type: %s", (status.is_rotary_wing) ? "symmetric motion" : "forward motion"); + warnx("usb powered: %s", (status.usb_connected) ? "yes" : "no"); warnx("avionics rail: %6.2f V", (double)status.avionics_power_rail_voltage); /* read all relevant states */ @@ -911,6 +909,7 @@ int commander_thread_main(int argc, char *argv[]) status.condition_power_input_valid = true; status.avionics_power_rail_voltage = -1.0f; + status.usb_connected = false; // CIRCUIT BREAKERS status.circuit_breaker_engaged_power_check = false; @@ -1334,6 +1333,7 @@ int commander_thread_main(int argc, char *argv[]) /* copy avionics voltage */ status.avionics_power_rail_voltage = system_power.voltage5V_v; + status.usb_connected = system_power.usb_connected; } } @@ -1537,10 +1537,6 @@ int commander_thread_main(int argc, char *argv[]) } last_idle_time = system_load.tasks[0].total_runtime; - - /* check if board is connected via USB */ - struct stat statbuf; - on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0); } /* if battery voltage is getting lower, warn using buzzer, etc. */ @@ -1550,7 +1546,7 @@ int commander_thread_main(int argc, char *argv[]) status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW; status_changed = true; - } else if (!on_usb_power && status.condition_battery_voltage_valid && status.battery_remaining < 0.09f + } else if (!status.usb_connected && status.condition_battery_voltage_valid && status.battery_remaining < 0.09f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { /* critical battery voltage, this is rather an emergency, change state machine */ critical_battery_voltage_actions_done = true; From 26ccac347d89874360e46f49b0f590bd33f63a55 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Apr 2015 10:18:40 +0200 Subject: [PATCH 133/268] commander: Go only into armed error state if on low battery and disarmed --- src/modules/commander/commander.cpp | 15 ++++----------- 1 file changed, 4 insertions(+), 11 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 9330ef6827..b0cd0b9e43 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1553,17 +1553,10 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY"); status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL; - if (armed.armed) { - arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, - mavlink_fd); - - if (arming_ret == TRANSITION_CHANGED) { - arming_state_changed = true; - } - - } else { - arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, - mavlink_fd); + if (!armed.armed) { + arming_ret = arming_state_transition(&status, &safety, + vehicle_status_s::ARMING_STATE_STANDBY_ERROR, + &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; From d06761bba8a9b5dea91a1e38a033018eeceb2c27 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Fri, 28 Nov 2014 11:31:01 +0100 Subject: [PATCH 134/268] uavcan-baro: add read()-style interface to baro device --- src/modules/uavcan/sensors/baro.cpp | 44 ++++++++++++++++++++++++----- src/modules/uavcan/sensors/baro.hpp | 2 ++ 2 files changed, 39 insertions(+), 7 deletions(-) diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 5348d44180..26a5fb27e1 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -61,6 +61,30 @@ int UavcanBarometerBridge::init() return 0; } +ssize_t UavcanBarometerBridge::read(struct file *filp, char *buffer, size_t buflen) +{ + static uint64_t last_read = 0; + struct baro_report *baro_buf = reinterpret_cast(buffer); + + /* buffer must be large enough */ + unsigned count = buflen / sizeof(struct baro_report); + if (count < 1) { + return -ENOSPC; + } + + if (last_read < _report.timestamp) { + /* copy report */ + lock(); + *baro_buf = _report; + last_read = _report.timestamp; + unlock(); + return sizeof(struct baro_report); + } else { + /* no new data available, warn caller */ + return -EAGAIN; + } +} + int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { @@ -76,6 +100,14 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) case BAROIOCGMSLPRESSURE: { return _msl_pressure; } + case SENSORIOCSPOLLRATE: { + // not supported yet, pretend that everything is ok + return OK; + } + case SENSORIOCSQUEUEDEPTH: { + // not supported yet, pretend that everything is ok + return OK; + } default: { return CDev::ioctl(filp, cmd, arg); } @@ -84,11 +116,9 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure &msg) { - auto report = ::baro_report(); - - report.timestamp = msg.getMonotonicTimestamp().toUSec(); - report.temperature = msg.static_temperature; - report.pressure = msg.static_pressure / 100.0F; // Convert to millibar + _report.timestamp = msg.getMonotonicTimestamp().toUSec(); + _report.temperature = msg.static_temperature; + _report.pressure = msg.static_pressure / 100.0F; // Convert to millibar /* * Altitude computation @@ -102,7 +132,7 @@ void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure< const double p1 = _msl_pressure / 1000.0; // current pressure at MSL in kPa const double p = double(msg.static_pressure) / 1000.0; // measured pressure in kPa - report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; + _report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; - publish(msg.getSrcNodeID().get(), &report); + publish(msg.getSrcNodeID().get(), &_report); } diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp index 16cd476ea7..99e3270618 100644 --- a/src/modules/uavcan/sensors/baro.hpp +++ b/src/modules/uavcan/sensors/baro.hpp @@ -54,6 +54,7 @@ public: int init() override; private: + ssize_t read(struct file *filp, char *buffer, size_t buflen); int ioctl(struct file *filp, int cmd, unsigned long arg) override; void air_data_sub_cb(const uavcan::ReceivedDataStructure &msg); @@ -65,4 +66,5 @@ private: uavcan::Subscriber _sub_air_data; unsigned _msl_pressure = 101325; + baro_report _report = {}; }; From a4bece7595c13579fda2a822fd867e374d27219e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 16 Mar 2015 20:14:06 +1100 Subject: [PATCH 135/268] uavcan-baro: use RingBuffer for read() support --- src/modules/uavcan/sensors/baro.cpp | 62 +++++++++++++++++++---------- src/modules/uavcan/sensors/baro.hpp | 4 +- 2 files changed, 44 insertions(+), 22 deletions(-) diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 26a5fb27e1..576e758df5 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -36,13 +36,15 @@ */ #include "baro.hpp" +#include #include const char *const UavcanBarometerBridge::NAME = "baro"; UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode& node) : UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_BASE_DEVICE_PATH, ORB_ID(sensor_baro)), -_sub_air_data(node) +_sub_air_data(node), +_reports(nullptr) { } @@ -53,6 +55,11 @@ int UavcanBarometerBridge::init() return res; } + /* allocate basic report buffers */ + _reports = new RingBuffer(2, sizeof(baro_report)); + if (_reports == nullptr) + return -1; + res = _sub_air_data.start(AirDataCbBinder(this, &UavcanBarometerBridge::air_data_sub_cb)); if (res < 0) { log("failed to start uavcan sub: %d", res); @@ -63,26 +70,23 @@ int UavcanBarometerBridge::init() ssize_t UavcanBarometerBridge::read(struct file *filp, char *buffer, size_t buflen) { - static uint64_t last_read = 0; + unsigned count = buflen / sizeof(struct baro_report); struct baro_report *baro_buf = reinterpret_cast(buffer); + int ret = 0; /* buffer must be large enough */ - unsigned count = buflen / sizeof(struct baro_report); - if (count < 1) { + if (count < 1) return -ENOSPC; + + while (count--) { + if (_reports->get(baro_buf)) { + ret += sizeof(*baro_buf); + baro_buf++; + } } - if (last_read < _report.timestamp) { - /* copy report */ - lock(); - *baro_buf = _report; - last_read = _report.timestamp; - unlock(); - return sizeof(struct baro_report); - } else { - /* no new data available, warn caller */ - return -EAGAIN; - } + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; } int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) @@ -105,7 +109,17 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) return OK; } case SENSORIOCSQUEUEDEPTH: { - // not supported yet, pretend that everything is ok + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; + + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } + irqrestore(flags); + return OK; } default: { @@ -116,9 +130,12 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure &msg) { - _report.timestamp = msg.getMonotonicTimestamp().toUSec(); - _report.temperature = msg.static_temperature; - _report.pressure = msg.static_pressure / 100.0F; // Convert to millibar + baro_report report; + + report.timestamp = msg.getMonotonicTimestamp().toUSec(); + report.temperature = msg.static_temperature; + report.pressure = msg.static_pressure / 100.0F; // Convert to millibar + report.error_count = 0; /* * Altitude computation @@ -132,7 +149,10 @@ void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure< const double p1 = _msl_pressure / 1000.0; // current pressure at MSL in kPa const double p = double(msg.static_pressure) / 1000.0; // measured pressure in kPa - _report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; + report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; - publish(msg.getSrcNodeID().get(), &_report); + // add to the ring buffer + _reports->force(&report); + + publish(msg.getSrcNodeID().get(), &report); } diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp index 99e3270618..6a39eebfb6 100644 --- a/src/modules/uavcan/sensors/baro.hpp +++ b/src/modules/uavcan/sensors/baro.hpp @@ -42,6 +42,8 @@ #include +class RingBuffer; + class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase { public: @@ -66,5 +68,5 @@ private: uavcan::Subscriber _sub_air_data; unsigned _msl_pressure = 101325; - baro_report _report = {}; + RingBuffer *_reports; }; From a530f16518333384295920e56cde636e39ba6809 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 2 Apr 2015 17:42:14 +0200 Subject: [PATCH 136/268] log quaternion setpoint --- src/modules/sdlog2/sdlog2.c | 4 ++++ src/modules/sdlog2/sdlog2_messages.h | 6 +++++- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 1c8cdd4be0..0bfd356a1c 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1495,6 +1495,10 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_ATSP.pitch_sp = buf.att_sp.pitch_body; log_msg.body.log_ATSP.yaw_sp = buf.att_sp.yaw_body; log_msg.body.log_ATSP.thrust_sp = buf.att_sp.thrust; + log_msg.body.log_ATSP.q_w = buf.att_sp.q_d[0]; + log_msg.body.log_ATSP.q_x = buf.att_sp.q_d[1]; + log_msg.body.log_ATSP.q_y = buf.att_sp.q_d[2]; + log_msg.body.log_ATSP.q_z = buf.att_sp.q_d[3]; LOGBUFFER_WRITE_AND_COUNT(ATSP); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 49483b15a2..a1fe2c95d0 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -73,6 +73,10 @@ struct log_ATSP_s { float pitch_sp; float yaw_sp; float thrust_sp; + float q_w; + float q_x; + float q_y; + float q_z; }; /* --- IMU - IMU SENSORS --- */ @@ -473,7 +477,7 @@ struct log_PARM_s { static const struct log_format_s log_formats[] = { /* business-level messages, ID < 0x80 */ LOG_FORMAT(ATT, "fffffffffffff", "qw,qx,qy,qz,Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), - LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), + LOG_FORMAT(ATSP, "ffffffff", "RollSP,PitchSP,YawSP,ThrustSP,qw,qx,qy,qz"), LOG_FORMAT_S(IMU, IMU, "ffffffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ,tA,tG,tM"), LOG_FORMAT_S(IMU1, IMU, "ffffffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ,tA,tG,tM"), LOG_FORMAT_S(IMU2, IMU, "ffffffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ,tA,tG,tM"), From fd03e21d8879d4420e4c7884630329d3c2e205d0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Apr 2015 11:21:37 +0200 Subject: [PATCH 137/268] Travis CI: Package cleanup --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 9796144bbe..bbccdeb039 100644 --- a/.travis.yml +++ b/.travis.yml @@ -9,7 +9,7 @@ before_script: - if [ "$CXX" = "g++" ]; then sudo apt-get install -qq g++-4.8 gcc-4.8 libstdc++-4.8-dev; fi - if [ "$CXX" = "g++" ]; then export CXX="g++-4.8" CC="gcc-4.8"; fi # Travis specific tools - - sudo apt-get install s3cmd grep zip mailutils + - sudo apt-get install s3cmd grep zip # General toolchain dependencies - sudo apt-get install libc6:i386 libgcc1:i386 gcc-4.6-base:i386 libstdc++5:i386 - sudo apt-get install python-serial python-argparse python-empy From 11a5afab20e07a029d133a68df6240d99acecf14 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Apr 2015 11:25:29 +0200 Subject: [PATCH 138/268] Travis CI: Update to GCC 4.8 --- .travis.yml | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/.travis.yml b/.travis.yml index bbccdeb039..83fdc85a0c 100644 --- a/.travis.yml +++ b/.travis.yml @@ -9,16 +9,16 @@ before_script: - if [ "$CXX" = "g++" ]; then sudo apt-get install -qq g++-4.8 gcc-4.8 libstdc++-4.8-dev; fi - if [ "$CXX" = "g++" ]; then export CXX="g++-4.8" CC="gcc-4.8"; fi # Travis specific tools - - sudo apt-get install s3cmd grep zip + - sudo apt-get install -qq s3cmd grep zip # General toolchain dependencies - - sudo apt-get install libc6:i386 libgcc1:i386 gcc-4.6-base:i386 libstdc++5:i386 - - sudo apt-get install python-serial python-argparse python-empy - - sudo apt-get install flex bison libncurses5-dev autoconf texinfo build-essential libtool zlib1g-dev genromfs git wget cmake + - sudo apt-get install -qq libc6:i386 libgcc1:i386 gcc-4.6-base:i386 libstdc++5:i386 + - sudo apt-get install -qq python-serial python-argparse python-empy + - sudo apt-get install -qq flex bison libncurses5-dev autoconf texinfo build-essential libtool zlib1g-dev genromfs git wget cmake - pushd . - cd ~ - - wget https://launchpadlibrarian.net/174121628/gcc-arm-none-eabi-4_7-2014q2-20140408-linux.tar.bz2 - - tar -jxf gcc-arm-none-eabi-4_7-2014q2-20140408-linux.tar.bz2 - - exportline="export PATH=$HOME/gcc-arm-none-eabi-4_7-2014q2/bin:\$PATH" + - wget https://launchpadlibrarian.net/186124160/gcc-arm-none-eabi-4_8-2014q3-20140805-linux.tar.bz2 + - tar -jxf gcc-arm-none-eabi-4_8-2014q3-20140805-linux.tar.bz2 + - exportline="export PATH=$HOME/gcc-arm-none-eabi-4_8-2014q3/bin:\$PATH" - if grep -Fxq "$exportline" ~/.profile; then echo nothing to do ; else echo $exportline >> ~/.profile; fi - . ~/.profile - popd From 5633ba88788e3c3240d20f632977a4621e704ade Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Apr 2015 11:32:16 +0200 Subject: [PATCH 139/268] Travis CI: Break packages on multiple lines --- .travis.yml | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 83fdc85a0c..624f6b86e3 100644 --- a/.travis.yml +++ b/.travis.yml @@ -11,7 +11,10 @@ before_script: # Travis specific tools - sudo apt-get install -qq s3cmd grep zip # General toolchain dependencies - - sudo apt-get install -qq libc6:i386 libgcc1:i386 gcc-4.6-base:i386 libstdc++5:i386 + - sudo apt-get install -qq libc6:i386 + - sudo apt-get install -qq libgcc1:i386 + - sudo apt-get install -qq gcc-4.6-base:i386 + - sudo apt-get install -qq libstdc++5:i386 - sudo apt-get install -qq python-serial python-argparse python-empy - sudo apt-get install -qq flex bison libncurses5-dev autoconf texinfo build-essential libtool zlib1g-dev genromfs git wget cmake - pushd . From 80ee9fa57bdf945b9ab71f0257509e08e572ebb2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Apr 2015 11:38:47 +0200 Subject: [PATCH 140/268] Travis CI: Change libc6 package name --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 624f6b86e3..51152b6d52 100644 --- a/.travis.yml +++ b/.travis.yml @@ -11,7 +11,7 @@ before_script: # Travis specific tools - sudo apt-get install -qq s3cmd grep zip # General toolchain dependencies - - sudo apt-get install -qq libc6:i386 + - sudo apt-get install -qq libc6-i386 - sudo apt-get install -qq libgcc1:i386 - sudo apt-get install -qq gcc-4.6-base:i386 - sudo apt-get install -qq libstdc++5:i386 From 3444b2417f6bbfe36b7317e1444546ab40c5bbf3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Apr 2015 11:41:58 +0200 Subject: [PATCH 141/268] Travis CI: Update all package names --- .travis.yml | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/.travis.yml b/.travis.yml index 51152b6d52..290dfb7150 100644 --- a/.travis.yml +++ b/.travis.yml @@ -11,10 +11,7 @@ before_script: # Travis specific tools - sudo apt-get install -qq s3cmd grep zip # General toolchain dependencies - - sudo apt-get install -qq libc6-i386 - - sudo apt-get install -qq libgcc1:i386 - - sudo apt-get install -qq gcc-4.6-base:i386 - - sudo apt-get install -qq libstdc++5:i386 + - sudo apt-get install -qq libc6-i386 libgcc1-i386 gcc-4.6-base-i386 libstdc++5-i386 libstdc++6-i386 - sudo apt-get install -qq python-serial python-argparse python-empy - sudo apt-get install -qq flex bison libncurses5-dev autoconf texinfo build-essential libtool zlib1g-dev genromfs git wget cmake - pushd . From e5ac953bab755f8d61e98d7671cb2265aea00080 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Apr 2015 11:44:15 +0200 Subject: [PATCH 142/268] Eigen: Attempt to fix unit test --- src/systemcmds/tests/test_eigen.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/tests/test_eigen.cpp b/src/systemcmds/tests/test_eigen.cpp index fb50e0d7af..fb05777875 100644 --- a/src/systemcmds/tests/test_eigen.cpp +++ b/src/systemcmds/tests/test_eigen.cpp @@ -171,7 +171,7 @@ int test_eigen(int argc, char *argv[]) } { - Eigen::Vector4f v; + Eigen::Vector4f v(0.0f, 0.0f, 0.0f, 0.0f); Eigen::Vector4f v1(1.0f, 2.0f, 0.0f, -1.0f); Eigen::Vector4f v2(1.0f, -1.0f, 2.0f, 0.0f); float data[4] = {1.0f, 2.0f, 3.0f, 4.0f}; From 34e4c882c512102095c1b62bd6c5fc2e59a81bfa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Apr 2015 11:49:31 +0200 Subject: [PATCH 143/268] Travis CI: Another go at packages --- .travis.yml | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index 290dfb7150..193ac0e7ee 100644 --- a/.travis.yml +++ b/.travis.yml @@ -10,8 +10,10 @@ before_script: - if [ "$CXX" = "g++" ]; then export CXX="g++-4.8" CC="gcc-4.8"; fi # Travis specific tools - sudo apt-get install -qq s3cmd grep zip -# General toolchain dependencies - - sudo apt-get install -qq libc6-i386 libgcc1-i386 gcc-4.6-base-i386 libstdc++5-i386 libstdc++6-i386 +# General toolchain dependencies libgcc1-i386 + - sudo apt-get install -qq libc6-i386 + - sudo apt-get install gcc-4.6-base:i386 + - sudo apt-get install libstdc++5:i386 libstdc++6:i386 - sudo apt-get install -qq python-serial python-argparse python-empy - sudo apt-get install -qq flex bison libncurses5-dev autoconf texinfo build-essential libtool zlib1g-dev genromfs git wget cmake - pushd . From 4786fe400e41c6f17b649171c368b36e9427e786 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Apr 2015 11:57:13 +0200 Subject: [PATCH 144/268] Travis CI: Package nuisance --- .travis.yml | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/.travis.yml b/.travis.yml index 193ac0e7ee..0b13c538a8 100644 --- a/.travis.yml +++ b/.travis.yml @@ -10,10 +10,9 @@ before_script: - if [ "$CXX" = "g++" ]; then export CXX="g++-4.8" CC="gcc-4.8"; fi # Travis specific tools - sudo apt-get install -qq s3cmd grep zip -# General toolchain dependencies libgcc1-i386 +# General toolchain dependencies libgcc1-i386 libstdc++5:i386 libstdc++6:i386 - sudo apt-get install -qq libc6-i386 - - sudo apt-get install gcc-4.6-base:i386 - - sudo apt-get install libstdc++5:i386 libstdc++6:i386 + - sudo apt-get install gcc-4.7-base:i386 - sudo apt-get install -qq python-serial python-argparse python-empy - sudo apt-get install -qq flex bison libncurses5-dev autoconf texinfo build-essential libtool zlib1g-dev genromfs git wget cmake - pushd . From 81a0ac5d71cc6752d3bf7fca905d4a3366650036 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Apr 2015 12:07:05 +0200 Subject: [PATCH 145/268] Travis CI: Cleanup --- .travis.yml | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/.travis.yml b/.travis.yml index 0b13c538a8..3e97545e15 100644 --- a/.travis.yml +++ b/.travis.yml @@ -10,10 +10,8 @@ before_script: - if [ "$CXX" = "g++" ]; then export CXX="g++-4.8" CC="gcc-4.8"; fi # Travis specific tools - sudo apt-get install -qq s3cmd grep zip -# General toolchain dependencies libgcc1-i386 libstdc++5:i386 libstdc++6:i386 - - sudo apt-get install -qq libc6-i386 - - sudo apt-get install gcc-4.7-base:i386 - - sudo apt-get install -qq python-serial python-argparse python-empy +# General toolchain dependencies + - sudo apt-get install -qq libc6-i386 gcc-4.7-base:i386 python-serial python-argparse python-empy - sudo apt-get install -qq flex bison libncurses5-dev autoconf texinfo build-essential libtool zlib1g-dev genromfs git wget cmake - pushd . - cd ~ From bbec741383a730d612f9cd5691a7b88ec471c324 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Apr 2015 12:07:22 +0200 Subject: [PATCH 146/268] Eigen: Simplify tests --- src/systemcmds/tests/test_eigen.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/systemcmds/tests/test_eigen.cpp b/src/systemcmds/tests/test_eigen.cpp index fb05777875..068e07c388 100644 --- a/src/systemcmds/tests/test_eigen.cpp +++ b/src/systemcmds/tests/test_eigen.cpp @@ -63,14 +63,14 @@ static constexpr size_t OPERATOR_ITERATIONS = 60000; for (size_t j = 0; j < OPERATOR_ITERATIONS; j++) { \ _op; \ } \ - printf(_title ": %.6fus", static_cast(hrt_absolute_time() - t0) / OPERATOR_ITERATIONS); \ + printf(_title ": %.6fus\n", static_cast(hrt_absolute_time() - t0) / OPERATOR_ITERATIONS); \ } #define VERIFY_OP(_title, _op, __OP_TEST__) \ { \ _op; \ if(!(__OP_TEST__)) { \ - printf(_title " Failed! ("#__OP_TEST__")"); \ + printf(_title " Failed! ("#__OP_TEST__")\n"); \ } \ } @@ -174,6 +174,7 @@ int test_eigen(int argc, char *argv[]) Eigen::Vector4f v(0.0f, 0.0f, 0.0f, 0.0f); Eigen::Vector4f v1(1.0f, 2.0f, 0.0f, -1.0f); Eigen::Vector4f v2(1.0f, -1.0f, 2.0f, 0.0f); + Eigen::Vector4f vres; float data[4] = {1.0f, 2.0f, 3.0f, 4.0f}; TEST_OP("Constructor Vector<4>()", Eigen::Vector4f v3); TEST_OP("Constructor Vector<4>(Vector<4>)", Eigen::Vector4f v3(v1)); @@ -182,8 +183,8 @@ int test_eigen(int argc, char *argv[]) TEST_OP("Vector<4> = Vector<4>", v = v1); TEST_OP("Vector<4> + Vector<4>", v + v1); TEST_OP("Vector<4> - Vector<4>", v - v1); - TEST_OP("Vector<4> += Vector<4>", v += v1); - TEST_OP("Vector<4> -= Vector<4>", v -= v1); + //TEST_OP("Vector<4> += Vector<4>", v += v1); + //TEST_OP("Vector<4> -= Vector<4>", v -= v1); TEST_OP("Vector<4> dot Vector<4>", v.dot(v1)); } From de29e43ee6e3cb16af18c71844ee5d8ffa91688c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Apr 2015 12:36:27 +0200 Subject: [PATCH 147/268] Eigen: Fix logical compare warning --- src/systemcmds/tests/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 4b7b73ac65..74114719fa 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -35,5 +35,5 @@ SRCS = test_adc.c \ test_mount.c \ test_eigen.cpp -EXTRACXXFLAGS = -Wframe-larger-than=2500 -Wno-float-equal -Wno-double-promotion +EXTRACXXFLAGS = -Wframe-larger-than=2500 -Wno-float-equal -Wno-double-promotion -Wno-error=logical-op From 1219ef8d43b5e05e334b22441b2bd3b222e76bc5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Apr 2015 12:39:36 +0200 Subject: [PATCH 148/268] Eigen: Disable tests --- src/systemcmds/tests/tests_main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 484c6d9d61..55d53503a8 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -112,7 +112,7 @@ const struct { #ifndef TESTS_MATHLIB_DISABLE {"mathlib", test_mathlib, 0}, #endif - {"eigen", test_eigen, OPT_NOJIGTEST}, + {"eigen", test_eigen, OPT_NOALLTEST | OPT_NOJIGTEST}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} }; From e2d27ade2ecc89d5080d1ad6215a77fea2619dd6 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 6 Apr 2015 14:02:14 +0200 Subject: [PATCH 149/268] remove ROS install scripts, they are now in PX4/containers --- Tools/ros/px4_ros_installation_ubuntu.sh | 41 ------------------------ Tools/ros/px4_workspace_create.sh | 9 ------ Tools/ros/px4_workspace_setup.sh | 28 ---------------- 3 files changed, 78 deletions(-) delete mode 100755 Tools/ros/px4_ros_installation_ubuntu.sh delete mode 100755 Tools/ros/px4_workspace_create.sh delete mode 100755 Tools/ros/px4_workspace_setup.sh diff --git a/Tools/ros/px4_ros_installation_ubuntu.sh b/Tools/ros/px4_ros_installation_ubuntu.sh deleted file mode 100755 index b65df8dcea..0000000000 --- a/Tools/ros/px4_ros_installation_ubuntu.sh +++ /dev/null @@ -1,41 +0,0 @@ -#!/bin/sh -# License: according to LICENSE.md in the root directory of the PX4 Firmware repository - -# main ROS Setup -# following http://wiki.ros.org/indigo/Installation/Ubuntu -# also adding drcsim http://gazebosim.org/tutorials?tut=drcsim_install -# run this file with . ./px4_ros_setup_ubuntu.sh - -## add ROS repository -sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list' - -## add key -wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -O - | \ - sudo apt-key add - - -## Install main ROS pacakges -sudo apt-get update -sudo apt-get -y install ros-indigo-desktop-full -sudo rosdep init -rosdep update - -## Setup environment variables -echo "source /opt/ros/indigo/setup.bash" >> ~/.bashrc -. ~/.bashrc - -# get rosinstall -sudo apt-get -y install python-rosinstall - -# additional dependencies -sudo apt-get -y install ros-indigo-octomap-msgs ros-indigo-joy ros-indigo-mavros ros-indigo-mavros-extras - -## drcsim setup (for models) -### add osrf repository -sudo sh -c 'echo "deb http://packages.osrfoundation.org/drc/ubuntu trusty main" > /etc/apt/sources.list.d/drc-latest.list' - -### add key -wget http://packages.osrfoundation.org/drc.key -O - | sudo apt-key add - - -### install drcsim -sudo apt-get update -sudo apt-get -y install drcsim diff --git a/Tools/ros/px4_workspace_create.sh b/Tools/ros/px4_workspace_create.sh deleted file mode 100755 index cf80bcf8d6..0000000000 --- a/Tools/ros/px4_workspace_create.sh +++ /dev/null @@ -1,9 +0,0 @@ -#!/bin/sh -# this script creates a catkin_ws in the current folder -# License: according to LICENSE.md in the root directory of the PX4 Firmware repository - -mkdir -p catkin_ws/src -cd catkin_ws/src -catkin_init_workspace -cd .. -catkin_make diff --git a/Tools/ros/px4_workspace_setup.sh b/Tools/ros/px4_workspace_setup.sh deleted file mode 100755 index 5599ab9998..0000000000 --- a/Tools/ros/px4_workspace_setup.sh +++ /dev/null @@ -1,28 +0,0 @@ -#!/bin/bash -# License: according to LICENSE.md in the root directory of the PX4 Firmware repository - -# run this script from the root of your catkin_ws -source devel/setup.bash -cd src - -# PX4 Firmware -git clone https://github.com/PX4/Firmware.git - -# euroc simulator -git clone https://github.com/PX4/euroc_simulator.git - -# mav comm -git clone https://github.com/PX4/mav_comm.git - -# gflags catkin -git clone https://github.com/ethz-asl/gflags_catkin.git - -# glog catkin -git clone https://github.com/ethz-asl/glog_catkin.git - -# catkin simple -git clone https://github.com/catkin/catkin_simple.git - -cd .. - -catkin_make From 9a9efdaaa5a1a67be9a0939495503f222a1f3987 Mon Sep 17 00:00:00 2001 From: philipoe Date: Tue, 7 Apr 2015 15:20:05 +0200 Subject: [PATCH 150/268] commander: Increase timeout on airspeed sensor for the prearm_check --- src/modules/commander/state_machine_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 0154f235f6..e1d2d72d1d 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -699,7 +699,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) struct airspeed_s airspeed; if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) || - (hrt_elapsed_time(&airspeed.timestamp) > (50 * 1000))) { + (hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) { mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED SENSOR MISSING"); failed = true; goto system_eval; From a121f6101fb763ec61e5f447731ad0a155e2ac72 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 7 Apr 2015 17:15:17 +0200 Subject: [PATCH 151/268] UBlox: Only use fix and velocity if flags are valid --- src/drivers/gps/ubx.cpp | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index f42c968d37..96b26b6991 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -716,7 +716,18 @@ UBX::payload_rx_done(void) case UBX_MSG_NAV_PVT: UBX_TRACE_RXMSG("Rx NAV-PVT\n"); - _gps_position->fix_type = _buf.payload_rx_nav_pvt.fixType; + //Check if position fix flag is good + if ((_buf.payload_rx_nav_pvt.flags & UBX_RX_NAV_PVT_FLAGS_GNSSFIXOK) == 1) + { + _gps_position->fix_type = _buf.payload_rx_nav_pvt.fixType; + _gps_position->vel_ned_valid = true; + } + else + { + _gps_position->fix_type = 0; + _gps_position->vel_ned_valid = false; + } + _gps_position->satellites_used = _buf.payload_rx_nav_pvt.numSV; _gps_position->lat = _buf.payload_rx_nav_pvt.lat; @@ -732,7 +743,6 @@ UBX::payload_rx_done(void) _gps_position->vel_n_m_s = (float)_buf.payload_rx_nav_pvt.velN * 1e-3f; _gps_position->vel_e_m_s = (float)_buf.payload_rx_nav_pvt.velE * 1e-3f; _gps_position->vel_d_m_s = (float)_buf.payload_rx_nav_pvt.velD * 1e-3f; - _gps_position->vel_ned_valid = true; _gps_position->cog_rad = (float)_buf.payload_rx_nav_pvt.headMot * M_DEG_TO_RAD_F * 1e-5f; _gps_position->c_variance_rad = (float)_buf.payload_rx_nav_pvt.headAcc * M_DEG_TO_RAD_F * 1e-5f; From 891a7af5099f380ac7ec737d8357fd9a9abf4c7a Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 7 Apr 2015 17:15:33 +0200 Subject: [PATCH 152/268] UBlox: Only use time and date if flags are valid --- src/drivers/gps/ubx.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 96b26b6991..09cd6b1c91 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -747,6 +747,10 @@ UBX::payload_rx_done(void) _gps_position->cog_rad = (float)_buf.payload_rx_nav_pvt.headMot * M_DEG_TO_RAD_F * 1e-5f; _gps_position->c_variance_rad = (float)_buf.payload_rx_nav_pvt.headAcc * M_DEG_TO_RAD_F * 1e-5f; + //Check if time and date fix flags are good + if( (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDDATE) + && (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDTIME) + && (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_FULLYRESOLVED)) { /* convert to unix timestamp */ struct tm timeinfo; From 9924f5519f907568351aaa66fba01190b682a396 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Tue, 7 Apr 2015 00:13:12 +0200 Subject: [PATCH 153/268] update attitude and position setpoint topics --- .../demo_tests/mavros_offboard_attctl_test.py | 8 ++++---- .../demo_tests/mavros_offboard_posctl_test.py | 4 ++-- launch/mavros_sitl.launch | 5 +++-- .../demo_offboard_attitude_setpoints.cpp | 4 ++-- .../demo_offboard_position_setpoints.cpp | 2 +- 5 files changed, 12 insertions(+), 11 deletions(-) diff --git a/integrationtests/demo_tests/mavros_offboard_attctl_test.py b/integrationtests/demo_tests/mavros_offboard_attctl_test.py index ef875ce61e..63e2922d00 100755 --- a/integrationtests/demo_tests/mavros_offboard_attctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_attctl_test.py @@ -67,8 +67,8 @@ class MavrosOffboardAttctlTest(unittest.TestCase): rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) rospy.Subscriber("iris/mavros/position/local", PoseStamped, self.position_callback) - self.pub_att = rospy.Publisher('iris/mavros/setpoint/attitude', PoseStamped, queue_size=10) - self.pub_thr = rospy.Publisher('iris/mavros/setpoint/att_throttle', Float64, queue_size=10) + self.pub_att = rospy.Publisher('iris/mavros/setpoint_attitude/attitude', PoseStamped, queue_size=10) + self.pub_thr = rospy.Publisher('iris/mavros/setpoint_attitude/att_throttle', Float64, queue_size=10) self.rate = rospy.Rate(10) # 10hz self.has_pos = False self.control_mode = vehicle_control_mode() @@ -110,9 +110,9 @@ class MavrosOffboardAttctlTest(unittest.TestCase): att.header.stamp = rospy.Time.now() self.pub_att.publish(att) - self.helper.bag_write('mavros/setpoint/attitude', att) + self.helper.bag_write('mavros/setpoint_attitude/attitude', att) self.pub_thr.publish(throttle) - self.helper.bag_write('mavros/setpoint/att_throttle', throttle) + self.helper.bag_write('mavros/setpoint_attitude/att_throttle', throttle) self.rate.sleep() if (self.local_position.pose.position.x > 5 diff --git a/integrationtests/demo_tests/mavros_offboard_posctl_test.py b/integrationtests/demo_tests/mavros_offboard_posctl_test.py index 1383cd04cd..58796dc607 100755 --- a/integrationtests/demo_tests/mavros_offboard_posctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_posctl_test.py @@ -69,7 +69,7 @@ class MavrosOffboardPosctlTest(unittest.TestCase): rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) rospy.Subscriber("iris/mavros/position/local", PoseStamped, self.position_callback) - self.pub_spt = rospy.Publisher('iris/mavros/setpoint/local_position', PoseStamped, queue_size=10) + self.pub_spt = rospy.Publisher('iris/mavros/setpoint_position/local', PoseStamped, queue_size=10) self.rate = rospy.Rate(10) # 10hz self.has_pos = False self.local_position = PoseStamped() @@ -128,7 +128,7 @@ class MavrosOffboardPosctlTest(unittest.TestCase): # update timestamp for each published SP pos.header.stamp = rospy.Time.now() self.pub_spt.publish(pos) - self.helper.bag_write('mavros/setpoint/local_position', pos) + self.helper.bag_write('mavros/setpoint_position/local', pos) if self.is_at_position(pos.pose.position.x, pos.pose.position.y, pos.pose.position.z, 0.5): break diff --git a/launch/mavros_sitl.launch b/launch/mavros_sitl.launch index 7362fa4644..60b85b6ff8 100644 --- a/launch/mavros_sitl.launch +++ b/launch/mavros_sitl.launch @@ -9,10 +9,11 @@ - + + - + diff --git a/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp index fb0b09de1e..328f545c6b 100644 --- a/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp +++ b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp @@ -49,8 +49,8 @@ DemoOffboardAttitudeSetpoints::DemoOffboardAttitudeSetpoints() : _n(), - _attitude_sp_pub(_n.advertise("mavros/setpoint/attitude", 1)), - _thrust_sp_pub(_n.advertise("mavros/setpoint/att_throttle", 1)) + _attitude_sp_pub(_n.advertise("mavros/setpoint_attitude/attitude", 1)), + _thrust_sp_pub(_n.advertise("mavros/setpoint_attitude/att_throttle", 1)) { } diff --git a/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp index 125ceaddc7..8a626f255e 100644 --- a/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp +++ b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp @@ -46,7 +46,7 @@ DemoOffboardPositionSetpoints::DemoOffboardPositionSetpoints() : _n(), - _local_position_sp_pub(_n.advertise("mavros/setpoint/local_position", 1)) + _local_position_sp_pub(_n.advertise("mavros/setpoint_position/local", 1)) { } From 47cc1abc1096875d626cca63fcee55e7f12ed8c2 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Tue, 7 Apr 2015 00:21:04 +0200 Subject: [PATCH 154/268] updated mavros local position topic --- integrationtests/demo_tests/mavros_offboard_attctl_test.py | 2 +- integrationtests/demo_tests/mavros_offboard_posctl_test.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/integrationtests/demo_tests/mavros_offboard_attctl_test.py b/integrationtests/demo_tests/mavros_offboard_attctl_test.py index 63e2922d00..a9cbc2de5b 100755 --- a/integrationtests/demo_tests/mavros_offboard_attctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_attctl_test.py @@ -66,7 +66,7 @@ class MavrosOffboardAttctlTest(unittest.TestCase): self.helper.setUp() rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) - rospy.Subscriber("iris/mavros/position/local", PoseStamped, self.position_callback) + rospy.Subscriber("iris/mavros/local_position/local", PoseStamped, self.position_callback) self.pub_att = rospy.Publisher('iris/mavros/setpoint_attitude/attitude', PoseStamped, queue_size=10) self.pub_thr = rospy.Publisher('iris/mavros/setpoint_attitude/att_throttle', Float64, queue_size=10) self.rate = rospy.Rate(10) # 10hz diff --git a/integrationtests/demo_tests/mavros_offboard_posctl_test.py b/integrationtests/demo_tests/mavros_offboard_posctl_test.py index 58796dc607..1c6bade5cc 100755 --- a/integrationtests/demo_tests/mavros_offboard_posctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_posctl_test.py @@ -68,7 +68,7 @@ class MavrosOffboardPosctlTest(unittest.TestCase): self.helper.setUp() rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) - rospy.Subscriber("iris/mavros/position/local", PoseStamped, self.position_callback) + rospy.Subscriber("iris/mavros/local_position/local", PoseStamped, self.position_callback) self.pub_spt = rospy.Publisher('iris/mavros/setpoint_position/local', PoseStamped, queue_size=10) self.rate = rospy.Rate(10) # 10hz self.has_pos = False From a142b52e7bb3edad5bd6171917fb327ce43c6460 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 8 Apr 2015 09:47:54 +0200 Subject: [PATCH 155/268] removed attitude parameter, configuration works now --- launch/mavros_sitl.launch | 3 --- 1 file changed, 3 deletions(-) diff --git a/launch/mavros_sitl.launch b/launch/mavros_sitl.launch index 60b85b6ff8..f452f09453 100644 --- a/launch/mavros_sitl.launch +++ b/launch/mavros_sitl.launch @@ -9,9 +9,6 @@ - - - From 120fca61ad2fde5a8967f7dc554d8bd05499593a Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 8 Apr 2015 09:48:27 +0200 Subject: [PATCH 156/268] use global namespace for tests --- .../demo_tests/direct_manual_input_test.py | 4 ++-- .../demo_tests/direct_offboard_posctl_test.py | 6 +++--- integrationtests/demo_tests/direct_tests.launch | 10 +++++++--- integrationtests/demo_tests/flight_path_assertion.py | 8 ++++---- integrationtests/demo_tests/manual_input.py | 4 ++-- .../demo_tests/mavros_offboard_attctl_test.py | 10 +++++----- .../demo_tests/mavros_offboard_posctl_test.py | 6 +++--- integrationtests/demo_tests/mavros_tests.launch | 10 ++++++---- 8 files changed, 32 insertions(+), 26 deletions(-) diff --git a/integrationtests/demo_tests/direct_manual_input_test.py b/integrationtests/demo_tests/direct_manual_input_test.py index 394500e941..e5d2887671 100755 --- a/integrationtests/demo_tests/direct_manual_input_test.py +++ b/integrationtests/demo_tests/direct_manual_input_test.py @@ -67,8 +67,8 @@ class ManualInputTest(unittest.TestCase): # def test_manual_input(self): rospy.init_node('test_node', anonymous=True) - rospy.Subscriber('iris/actuator_armed', actuator_armed, self.actuator_armed_callback) - rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + rospy.Subscriber('actuator_armed', actuator_armed, self.actuator_armed_callback) + rospy.Subscriber('vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) man_in = ManualInput() diff --git a/integrationtests/demo_tests/direct_offboard_posctl_test.py b/integrationtests/demo_tests/direct_offboard_posctl_test.py index 734bcf5909..9a6c4af783 100755 --- a/integrationtests/demo_tests/direct_offboard_posctl_test.py +++ b/integrationtests/demo_tests/direct_offboard_posctl_test.py @@ -68,9 +68,9 @@ class DirectOffboardPosctlTest(unittest.TestCase): self.helper = PX4TestHelper("direct_offboard_posctl_test") self.helper.setUp() - rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) - self.sub_vlp = rospy.Subscriber("iris/vehicle_local_position", vehicle_local_position, self.position_callback) - self.pub_spt = rospy.Publisher('iris/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) + rospy.Subscriber('vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + self.sub_vlp = rospy.Subscriber("vehicle_local_position", vehicle_local_position, self.position_callback) + self.pub_spt = rospy.Publisher('position_setpoint_triplet', position_setpoint_triplet, queue_size=10) self.rate = rospy.Rate(10) # 10hz self.has_pos = False self.local_position = vehicle_local_position() diff --git a/integrationtests/demo_tests/direct_tests.launch b/integrationtests/demo_tests/direct_tests.launch index f332332cfb..04ba8a54d3 100644 --- a/integrationtests/demo_tests/direct_tests.launch +++ b/integrationtests/demo_tests/direct_tests.launch @@ -3,7 +3,8 @@ - + + @@ -11,8 +12,11 @@ + - - + + + + diff --git a/integrationtests/demo_tests/flight_path_assertion.py b/integrationtests/demo_tests/flight_path_assertion.py index 244324cd09..7de82cc84d 100644 --- a/integrationtests/demo_tests/flight_path_assertion.py +++ b/integrationtests/demo_tests/flight_path_assertion.py @@ -62,10 +62,10 @@ class FlightPathAssertion(threading.Thread): # def __init__(self, positions, tunnelRadius=1, yaw_offset=0.2): threading.Thread.__init__(self) - rospy.Subscriber("iris/vehicle_local_position", vehicle_local_position, self.position_callback) - self.spawn_model = rospy.ServiceProxy('gazebo/spawn_sdf_model', SpawnModel) - self.set_model_state = rospy.ServiceProxy('gazebo/set_model_state', SetModelState) - self.delete_model = rospy.ServiceProxy('gazebo/delete_model', DeleteModel) + rospy.Subscriber("vehicle_local_position", vehicle_local_position, self.position_callback) + self.spawn_model = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel) + self.set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) + self.delete_model = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel) self.positions = positions self.tunnel_radius = tunnelRadius diff --git a/integrationtests/demo_tests/manual_input.py b/integrationtests/demo_tests/manual_input.py index 167221285a..b7adaa31c4 100755 --- a/integrationtests/demo_tests/manual_input.py +++ b/integrationtests/demo_tests/manual_input.py @@ -49,8 +49,8 @@ class ManualInput(object): def __init__(self): rospy.init_node('test_node', anonymous=True) - self.pub_mcsp = rospy.Publisher('iris/manual_control_setpoint', manual_control_setpoint, queue_size=10) - self.pub_off = rospy.Publisher('iris/offboard_control_mode', offboard_control_mode, queue_size=10) + self.pub_mcsp = rospy.Publisher('manual_control_setpoint', manual_control_setpoint, queue_size=10) + self.pub_off = rospy.Publisher('offboard_control_mode', offboard_control_mode, queue_size=10) def arm(self): rate = rospy.Rate(10) # 10hz diff --git a/integrationtests/demo_tests/mavros_offboard_attctl_test.py b/integrationtests/demo_tests/mavros_offboard_attctl_test.py index a9cbc2de5b..7329c1efcb 100755 --- a/integrationtests/demo_tests/mavros_offboard_attctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_attctl_test.py @@ -61,14 +61,14 @@ class MavrosOffboardAttctlTest(unittest.TestCase): def setUp(self): rospy.init_node('test_node', anonymous=True) - rospy.wait_for_service('iris/mavros/cmd/arming', 30) + rospy.wait_for_service('mavros/cmd/arming', 30) self.helper = PX4TestHelper("mavros_offboard_attctl_test") self.helper.setUp() - rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) - rospy.Subscriber("iris/mavros/local_position/local", PoseStamped, self.position_callback) - self.pub_att = rospy.Publisher('iris/mavros/setpoint_attitude/attitude', PoseStamped, queue_size=10) - self.pub_thr = rospy.Publisher('iris/mavros/setpoint_attitude/att_throttle', Float64, queue_size=10) + rospy.Subscriber('vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + rospy.Subscriber("mavros/local_position/local", PoseStamped, self.position_callback) + self.pub_att = rospy.Publisher('mavros/setpoint_attitude/attitude', PoseStamped, queue_size=10) + self.pub_thr = rospy.Publisher('mavros/setpoint_attitude/att_throttle', Float64, queue_size=10) self.rate = rospy.Rate(10) # 10hz self.has_pos = False self.control_mode = vehicle_control_mode() diff --git a/integrationtests/demo_tests/mavros_offboard_posctl_test.py b/integrationtests/demo_tests/mavros_offboard_posctl_test.py index 1c6bade5cc..58e7ae2eda 100755 --- a/integrationtests/demo_tests/mavros_offboard_posctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_posctl_test.py @@ -67,9 +67,9 @@ class MavrosOffboardPosctlTest(unittest.TestCase): self.helper = PX4TestHelper("mavros_offboard_posctl_test") self.helper.setUp() - rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) - rospy.Subscriber("iris/mavros/local_position/local", PoseStamped, self.position_callback) - self.pub_spt = rospy.Publisher('iris/mavros/setpoint_position/local', PoseStamped, queue_size=10) + rospy.Subscriber('vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + rospy.Subscriber("mavros/local_position/local", PoseStamped, self.position_callback) + self.pub_spt = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10) self.rate = rospy.Rate(10) # 10hz self.has_pos = False self.local_position = PoseStamped() diff --git a/integrationtests/demo_tests/mavros_tests.launch b/integrationtests/demo_tests/mavros_tests.launch index cc49183076..cdafc967c5 100644 --- a/integrationtests/demo_tests/mavros_tests.launch +++ b/integrationtests/demo_tests/mavros_tests.launch @@ -12,12 +12,14 @@ - + - + - - + + + + From dea97dd2776490d39f4e451f5a72ffb3764ca763 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Mar 2015 12:18:24 -0700 Subject: [PATCH 157/268] drv_mag: added ioctl to control temperature compensation --- src/drivers/drv_mag.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index ca33966b01..3c12216a62 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -129,4 +129,7 @@ ORB_DECLARE(sensor_mag); /** determine if mag is external or onboard */ #define MAGIOCGEXTERNAL _MAGIOC(11) +/** enable/disable temperature compensation */ +#define MAGIOCSTEMPCOMP _MAGIOC(12) + #endif /* _DRV_MAG_H */ From 156a7915ae28b3d397a329777c982ddcb2edceeb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Mar 2015 16:37:06 -0700 Subject: [PATCH 158/268] hmc5883: added support for temperature compensation added "hmc5883 tempon" and "hmc5883 tempoff" to enable/disable --- src/drivers/hmc5883/hmc5883.cpp | 109 ++++++++++++++++++++++++++++++-- 1 file changed, 105 insertions(+), 4 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index fd4d4cff5e..fa501844ff 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -94,6 +94,10 @@ #define ADDR_DATA_OUT_Y_LSB 0x08 #define ADDR_STATUS 0x09 +/* temperature on hmc5983 only */ +#define ADDR_TEMP_OUT_MSB 0x31 +#define ADDR_TEMP_OUT_LSB 0x32 + /* modes not changeable outside of driver */ #define HMC5883L_MODE_NORMAL (0 << 0) /* default */ #define HMC5883L_MODE_POSITIVE_BIAS (1 << 0) /* positive bias */ @@ -110,6 +114,8 @@ #define STATUS_REG_DATA_OUT_LOCK (1 << 1) /* page 16: set if data is only partially read, read device to reset */ #define STATUS_REG_DATA_READY (1 << 0) /* page 16: set if all axes have valid measurements */ +#define HMC5983_TEMP_SENSOR_ENABLE (1 << 7) + enum HMC5883_BUS { HMC5883_BUS_ALL = 0, HMC5883_BUS_I2C_INTERNAL, @@ -218,6 +224,11 @@ private: */ int set_excitement(unsigned enable); + /** + * enable hmc5983 temperature compensation + */ + int set_temperature_compensation(unsigned enable); + /** * Set the sensor range. * @@ -722,6 +733,9 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) debug("MAGIOCGEXTERNAL in main driver"); return _interface->ioctl(cmd, dummy); + case MAGIOCSTEMPCOMP: + return set_temperature_compensation(arg); + case DEVIOCGDEVICEID: return _interface->ioctl(cmd, dummy); @@ -847,6 +861,7 @@ HMC5883::collect() perf_begin(_sample_perf); struct mag_report new_report; bool sensor_is_onboard = false; + uint8_t raw_temperature[2]; float xraw_f; float yraw_f; @@ -888,6 +903,20 @@ HMC5883::collect() goto out; } + /* get measurements from the device */ + new_report.temperature = 0; + if (_conf_reg & HMC5983_TEMP_SENSOR_ENABLE) { + /* if temperature compensation is enabled read the + * temperature too */ + ret = _interface->read(ADDR_TEMP_OUT_MSB, + raw_temperature, sizeof(raw_temperature)); + if (ret == OK) { + int16_t temp16 = (((int16_t)raw_temperature[0]) << 8) + + raw_temperature[1]; + new_report.temperature = 25 + (temp16 / (16*8.0f)); + } + } + /* * RAW outputs * @@ -927,9 +956,6 @@ HMC5883::collect() /* z remains z */ new_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale; - /* this sensor does not measure temperature, output a constant value */ - new_report.temperature = 0.0f; - if (!(_pub_blocked)) { if (_mag_topic != -1) { @@ -1234,6 +1260,50 @@ int HMC5883::set_excitement(unsigned enable) return !(_conf_reg == conf_reg_ret); } + +/* + enable/disable temperature compensation on the HMC5983 + + Unfortunately we don't yet know of a way to auto-detect the + difference between the HMC5883 and HMC5983. Both of them do + temperature sensing, but only the 5983 does temperature + compensation. We have noy yet found a behaviour that can be reliably + distinguished by reading registers to know which type a particular + sensor is + */ +int HMC5883::set_temperature_compensation(unsigned enable) +{ + int ret; + /* get current config */ + ret = read_reg(ADDR_CONF_A, _conf_reg); + + if (OK != ret) { + perf_count(_comms_errors); + return -EIO; + } + + if (enable != 0) { + _conf_reg |= HMC5983_TEMP_SENSOR_ENABLE; + } else { + _conf_reg &= ~HMC5983_TEMP_SENSOR_ENABLE; + } + + ret = write_reg(ADDR_CONF_A, _conf_reg); + + if (OK != ret) { + perf_count(_comms_errors); + return -EIO; + } + + uint8_t conf_reg_ret = 0; + if (read_reg(ADDR_CONF_A, conf_reg_ret) != OK) { + perf_count(_comms_errors); + return -EIO; + } + + return conf_reg_ret == _conf_reg; +} + int HMC5883::write_reg(uint8_t reg, uint8_t val) { @@ -1276,6 +1346,7 @@ HMC5883::print_info() printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n", (double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale, (double)(1.0f/_range_scale), (double)_range_ga); + printf("temperature %.2f\n", (double)_last_report.temperature); _reports->print_info("report queue"); } @@ -1318,6 +1389,7 @@ void test(enum HMC5883_BUS busid); void reset(enum HMC5883_BUS busid); int info(enum HMC5883_BUS busid); int calibrate(enum HMC5883_BUS busid); +void temp_enable(HMC5883_BUS busid, bool enable); void usage(); /** @@ -1559,6 +1631,27 @@ reset(enum HMC5883_BUS busid) exit(0); } + +/** + * enable/disable temperature compensation + */ +void +temp_enable(enum HMC5883_BUS busid, bool enable) +{ + struct hmc5883_bus_option &bus = find_bus(busid); + const char *path = bus.devpath; + + int fd = open(path, O_RDONLY); + + if (fd < 0) + err(1, "failed "); + + if (ioctl(fd, MAGIOCSTEMPCOMP, (unsigned)enable) < 0) + err(1, "set temperature compensation failed"); + + exit(0); +} + /** * Print a little info about the driver. */ @@ -1651,6 +1744,14 @@ hmc5883_main(int argc, char *argv[]) if (!strcmp(verb, "reset")) hmc5883::reset(busid); + /* + * enable/disable temperature compensation + */ + if (!strcmp(verb, "tempoff")) + hmc5883::temp_enable(busid, false); + if (!strcmp(verb, "tempon")) + hmc5883::temp_enable(busid, true); + /* * Print driver information. */ @@ -1669,5 +1770,5 @@ hmc5883_main(int argc, char *argv[]) } } - errx(1, "unrecognized command, try 'start', 'test', 'reset' 'calibrate' or 'info'"); + errx(1, "unrecognized command, try 'start', 'test', 'reset' 'calibrate', 'tempoff', 'tempon' or 'info'"); } From e17936e237a7e6a19e4b0f20c2c95f0b08b21954 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 2 Apr 2015 11:09:15 -0700 Subject: [PATCH 159/268] hmc5883: try to cope with genuine 5883 parts if we can't read the temperature registers 10 times then disable the feature. --- src/drivers/hmc5883/hmc5883.cpp | 26 +++++++++++++++++++++++++- 1 file changed, 25 insertions(+), 1 deletion(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index fa501844ff..358e7ff622 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -182,6 +182,7 @@ private: uint8_t _range_bits; uint8_t _conf_reg; + uint8_t _temperature_error_count; /** * Initialise the automatic measurement state machine and start it. @@ -369,7 +370,8 @@ HMC5883::HMC5883(device::Device *interface, const char *path, enum Rotation rota _rotation(rotation), _last_report{0}, _range_bits(0), - _conf_reg(0) + _conf_reg(0), + _temperature_error_count(0) { _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; @@ -914,6 +916,18 @@ HMC5883::collect() int16_t temp16 = (((int16_t)raw_temperature[0]) << 8) + raw_temperature[1]; new_report.temperature = 25 + (temp16 / (16*8.0f)); + _temperature_error_count = 0; + } else { + _temperature_error_count++; + if (_temperature_error_count == 10) { + /* + it probably really is a old HMC5883, + and can't do temperature. Disable it + */ + _temperature_error_count = 0; + debug("disabling temperature compensation"); + set_temperature_compensation(0); + } } } @@ -1270,6 +1284,16 @@ int HMC5883::set_excitement(unsigned enable) compensation. We have noy yet found a behaviour that can be reliably distinguished by reading registers to know which type a particular sensor is + + update: Current best guess is that many sensors marked HMC5883L on + the package are actually 5983 but without temperature compensation + tables. Reading the temperature works, but the mag field is not + automatically adjusted for temperature. We suspect that there may be + some early 5883L parts that don't have the temperature sensor at + all, although we haven't found one yet. The code that reads the + temperature looks for 10 failed transfers in a row and disables the + temperature sensor if that happens. It is hoped that this copes with + the genuine 5883L parts. */ int HMC5883::set_temperature_compensation(unsigned enable) { From 02639411baae6d60c164f954ae9bc9b32cba02f7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 2 Apr 2015 11:51:20 -0700 Subject: [PATCH 160/268] hmc5883: added -T option to enable temperature compensation this also fixes the behaviour of the -C option --- src/drivers/hmc5883/hmc5883.cpp | 39 +++++++++++++++------------------ 1 file changed, 18 insertions(+), 21 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 358e7ff622..ab70bf5b06 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1413,7 +1413,7 @@ void test(enum HMC5883_BUS busid); void reset(enum HMC5883_BUS busid); int info(enum HMC5883_BUS busid); int calibrate(enum HMC5883_BUS busid); -void temp_enable(HMC5883_BUS busid, bool enable); +int temp_enable(HMC5883_BUS busid, bool enable); void usage(); /** @@ -1478,8 +1478,6 @@ start(enum HMC5883_BUS busid, enum Rotation rotation) if (!started) errx(1, "driver start failed"); - - exit(0); } /** @@ -1624,12 +1622,7 @@ int calibrate(enum HMC5883_BUS busid) close(fd); - if (ret == OK) { - errx(0, "PASS"); - - } else { - errx(1, "FAIL"); - } + return ret; } /** @@ -1659,7 +1652,7 @@ reset(enum HMC5883_BUS busid) /** * enable/disable temperature compensation */ -void +int temp_enable(enum HMC5883_BUS busid, bool enable) { struct hmc5883_bus_option &bus = find_bus(busid); @@ -1673,7 +1666,8 @@ temp_enable(enum HMC5883_BUS busid, bool enable) if (ioctl(fd, MAGIOCSTEMPCOMP, (unsigned)enable) < 0) err(1, "set temperature compensation failed"); - exit(0); + close(fd); + return 0; } /** @@ -1711,8 +1705,9 @@ hmc5883_main(int argc, char *argv[]) enum HMC5883_BUS busid = HMC5883_BUS_ALL; enum Rotation rotation = ROTATION_NONE; bool calibrate = false; + bool temp_compensation = false; - while ((ch = getopt(argc, argv, "XISR:C")) != EOF) { + while ((ch = getopt(argc, argv, "XISR:CT")) != EOF) { switch (ch) { case 'R': rotation = (enum Rotation)atoi(optarg); @@ -1731,6 +1726,9 @@ hmc5883_main(int argc, char *argv[]) case 'C': calibrate = true; break; + case 'T': + temp_compensation = true; + break; default: hmc5883::usage(); exit(0); @@ -1744,16 +1742,15 @@ hmc5883_main(int argc, char *argv[]) */ if (!strcmp(verb, "start")) { hmc5883::start(busid, rotation); - if (calibrate) { - if (hmc5883::calibrate(busid) == 0) { - errx(0, "calibration successful"); - - } else { - errx(1, "calibration failed"); - } - } else { - exit(0); + if (calibrate && hmc5883::calibrate(busid) != 0) { + errx(1, "calibration failed"); } + if (temp_compensation) { + // we consider failing to setup temperature + // compensation as non-fatal + hmc5883::temp_enable(busid, true); + } + exit(0); } /* From 219d66188817b1683294f27f752d2718ddddba77 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 2 Apr 2015 14:27:47 -0700 Subject: [PATCH 161/268] hmc5883: read the temperature every 10 samples when enabled --- src/drivers/hmc5883/hmc5883.cpp | 52 +++++++++++++++++++++------------ 1 file changed, 33 insertions(+), 19 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index ab70bf5b06..3a3848446d 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -182,6 +182,7 @@ private: uint8_t _range_bits; uint8_t _conf_reg; + uint8_t _temperature_counter; uint8_t _temperature_error_count; /** @@ -371,6 +372,7 @@ HMC5883::HMC5883(device::Device *interface, const char *path, enum Rotation rota _last_report{0}, _range_bits(0), _conf_reg(0), + _temperature_counter(0), _temperature_error_count(0) { _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; @@ -863,7 +865,6 @@ HMC5883::collect() perf_begin(_sample_perf); struct mag_report new_report; bool sensor_is_onboard = false; - uint8_t raw_temperature[2]; float xraw_f; float yraw_f; @@ -908,26 +909,39 @@ HMC5883::collect() /* get measurements from the device */ new_report.temperature = 0; if (_conf_reg & HMC5983_TEMP_SENSOR_ENABLE) { - /* if temperature compensation is enabled read the - * temperature too */ - ret = _interface->read(ADDR_TEMP_OUT_MSB, - raw_temperature, sizeof(raw_temperature)); - if (ret == OK) { - int16_t temp16 = (((int16_t)raw_temperature[0]) << 8) + - raw_temperature[1]; - new_report.temperature = 25 + (temp16 / (16*8.0f)); - _temperature_error_count = 0; - } else { - _temperature_error_count++; - if (_temperature_error_count == 10) { - /* - it probably really is a old HMC5883, - and can't do temperature. Disable it - */ + /* + if temperature compensation is enabled read the + temperature too. + + We read the temperature every 10 samples to avoid + excessive I2C traffic + */ + if (_temperature_counter++ == 10) { + uint8_t raw_temperature[2]; + + _temperature_counter = 0; + + ret = _interface->read(ADDR_TEMP_OUT_MSB, + raw_temperature, sizeof(raw_temperature)); + if (ret == OK) { + int16_t temp16 = (((int16_t)raw_temperature[0]) << 8) + + raw_temperature[1]; + new_report.temperature = 25 + (temp16 / (16*8.0f)); _temperature_error_count = 0; - debug("disabling temperature compensation"); - set_temperature_compensation(0); + } else { + _temperature_error_count++; + if (_temperature_error_count == 10) { + /* + it probably really is an old HMC5883, + and can't do temperature. Disable it + */ + _temperature_error_count = 0; + debug("disabling temperature compensation"); + set_temperature_compensation(0); + } } + } else { + new_report.temperature = _last_report.temperature; } } From 309a767c06d707f5a95b165b19123c8728659510 Mon Sep 17 00:00:00 2001 From: philipoe Date: Thu, 9 Apr 2015 19:12:38 +0200 Subject: [PATCH 162/268] mavlink: Allow mavlink_send to take component_ID into account. Still use a default argument in case the user does not supply a component_ID --- src/modules/mavlink/mavlink_main.cpp | 4 ++-- src/modules/mavlink/mavlink_main.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 74395ec030..209ac5d8f9 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -746,7 +746,7 @@ Mavlink::get_free_tx_buf() } void -Mavlink::send_message(const uint8_t msgid, const void *msg) +Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID) { /* If the wait until transmit flag is on, only transmit after we've received messages. Otherwise, transmit all the time. */ @@ -780,7 +780,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) /* use mavlink's internal counter for the TX seq */ buf[2] = mavlink_get_channel_status(_channel)->current_tx_seq++; buf[3] = mavlink_system.sysid; - buf[4] = mavlink_system.compid; + buf[4] = component_ID==0 ? mavlink_system.compid:component_ID; buf[5] = msgid; /* payload */ diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 6eef594f51..c285bc4052 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -169,7 +169,7 @@ public: */ int set_hil_enabled(bool hil_enabled); - void send_message(const uint8_t msgid, const void *msg); + void send_message(const uint8_t msgid, const void *msg, uint8_t component_ID = 0); /** * Resend message as is, don't change sequence number and CRC. From 79f645974088aaf73b1d76266cac55346295f5b6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Apr 2015 19:35:14 +0200 Subject: [PATCH 163/268] mavlink app: Code style fix --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 209ac5d8f9..ba049bac40 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -780,7 +780,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID /* use mavlink's internal counter for the TX seq */ buf[2] = mavlink_get_channel_status(_channel)->current_tx_seq++; buf[3] = mavlink_system.sysid; - buf[4] = component_ID==0 ? mavlink_system.compid:component_ID; + buf[4] = (component_ID == 0) ? mavlink_system.compid : component_ID; buf[5] = msgid; /* payload */ From 747a00d70de7cfdc90120b51794582e49225440a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 9 Apr 2015 20:06:13 +0200 Subject: [PATCH 164/268] simulator repository was renamed --- .../mc_att_control_multiplatform/mc_att_control_sim.h | 2 +- src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_sim.h b/src/modules/mc_att_control_multiplatform/mc_att_control_sim.h index a1bf44fc96..1d76afb82d 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_sim.h +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_sim.h @@ -82,7 +82,7 @@ public: */ ~MulticopterAttitudeControlSim(); - /* setters and getters for interface with euroc-gazebo simulator */ + /* setters and getters for interface with rotors-gazebo simulator */ void set_attitude(const Eigen::Quaternion attitude); void set_attitude_rates(const Eigen::Vector3d &angular_rate); void set_attitude_reference(const Eigen::Vector4d &control_attitude_thrust_reference); diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index 9b48294b64..002a112b69 100644 --- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -33,7 +33,7 @@ /** * @file mc_mixer.cpp - * Dummy multicopter mixer for euroc simulator (gazebo) + * Dummy multicopter mixer for rotors simulator (gazebo) * * @author Roman Bapst */ @@ -96,7 +96,7 @@ const MultirotorMixer::Rotor _config_quad_plus[] = { { -0.000000, -1.000000, -1.00 }, }; -const MultirotorMixer::Rotor _config_quad_plus_euroc[] = { +const MultirotorMixer::Rotor _config_quad_plus_rotorssim[] = { { 0.000000, 1.000000, 1.00 }, { -0.000000, -1.000000, 1.00 }, { 1.000000, 0.000000, -1.00 }, @@ -118,7 +118,7 @@ const MultirotorMixer::Rotor _config_quad_iris[] = { const MultirotorMixer::Rotor *_config_index[5] = { &_config_x[0], &_config_quad_plus[0], - &_config_quad_plus_euroc[0], + &_config_quad_plus_rotorssim[0], &_config_quad_wide[0], &_config_quad_iris[0] }; From 04863dc2d1975433d95f7922886e335ddca5fabc Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 8 Apr 2015 22:33:07 -0700 Subject: [PATCH 165/268] batt_smbus: add ioctl to return batt capacity Also use full charge capacity instead of design capacity so that an old battery's capacity will appear as lower than its original capacity but it will still report 100% charged after charging --- src/drivers/batt_smbus/batt_smbus.cpp | 53 ++++++++++++++++++++++----- src/drivers/drv_batt_smbus.h | 10 +++++ 2 files changed, 53 insertions(+), 10 deletions(-) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 9f72bd56fc..6d1b3f6ca5 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -85,6 +85,7 @@ #define BATT_SMBUS_TEMP 0x08 ///< temperature register #define BATT_SMBUS_VOLTAGE 0x09 ///< voltage register #define BATT_SMBUS_REMAINING_CAPACITY 0x0f ///< predicted remaining battery capacity as a percentage +#define BATT_SMBUS_FULL_CHARGE_CAPACITY 0x10 ///< capacity when fully charged #define BATT_SMBUS_DESIGN_CAPACITY 0x18 ///< design capacity register #define BATT_SMBUS_DESIGN_VOLTAGE 0x19 ///< design voltage register #define BATT_SMBUS_SERIALNUM 0x1c ///< serial number register @@ -115,6 +116,11 @@ public: */ virtual int init(); + /** + * ioctl for retrieving battery capacity and time to empty + */ + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + /** * Test device * @@ -180,7 +186,7 @@ private: orb_advert_t _batt_topic; ///< uORB battery topic orb_id_t _batt_orb_id; ///< uORB battery topic ID uint64_t _start_time; ///< system time we first attempt to communicate with battery - uint16_t _batt_design_capacity; ///< battery's design capacity in mAh (0 means unknown) + uint16_t _batt_capacity; ///< battery's design capacity in mAh (0 means unknown) }; namespace @@ -200,7 +206,7 @@ BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) : _batt_topic(-1), _batt_orb_id(nullptr), _start_time(0), - _batt_design_capacity(0) + _batt_capacity(0) { // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); @@ -250,6 +256,31 @@ BATT_SMBUS::init() return ret; } +int +BATT_SMBUS::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret = -ENODEV; + + switch (cmd) { + case BATT_SMBUS_GET_CAPACITY: + + /* return battery capacity as uint16 */ + if (_enabled) { + *((uint16_t *)arg) = _batt_capacity; + ret = OK; + } + + break; + + default: + /* see if the parent class can make any use of it */ + ret = CDev::ioctl(filp, cmd, arg); + break; + } + + return ret; +} + int BATT_SMBUS::test() { @@ -266,7 +297,8 @@ BATT_SMBUS::test() if (updated) { if (orb_copy(ORB_ID(battery_status), sub, &status) == OK) { - warnx("V=%4.2f C=%4.2f DismAh=%4.2f", (float)status.voltage_v, (float)status.current_a, (float)status.discharged_mah); + warnx("V=%4.2f C=%4.2f DismAh=%4.2f Cap:%d", (float)status.voltage_v, (float)status.current_a, + (float)status.discharged_mah, (int)_batt_capacity); } } @@ -374,21 +406,22 @@ BATT_SMBUS::cycle() uint8_t buff[4]; if (read_block(BATT_SMBUS_CURRENT, buff, 4, false) == 4) { - new_report.current_a = -(float)((int32_t)((uint32_t)buff[3] << 24 | (uint32_t)buff[2] << 16 | (uint32_t)buff[1] << 8 | (uint32_t)buff[0])) / 1000.0f; + new_report.current_a = -(float)((int32_t)((uint32_t)buff[3] << 24 | (uint32_t)buff[2] << 16 | (uint32_t)buff[1] << 8 | + (uint32_t)buff[0])) / 1000.0f; } // read battery design capacity - if (_batt_design_capacity == 0) { - if (read_reg(BATT_SMBUS_DESIGN_CAPACITY, tmp) == OK) { - _batt_design_capacity = tmp; + if (_batt_capacity == 0) { + if (read_reg(BATT_SMBUS_FULL_CHARGE_CAPACITY, tmp) == OK) { + _batt_capacity = tmp; } } // read remaining capacity - if (_batt_design_capacity > 0) { + if (_batt_capacity > 0) { if (read_reg(BATT_SMBUS_REMAINING_CAPACITY, tmp) == OK) { - if (tmp < _batt_design_capacity) { - new_report.discharged_mah = _batt_design_capacity - tmp; + if (tmp < _batt_capacity) { + new_report.discharged_mah = _batt_capacity - tmp; } } } diff --git a/src/drivers/drv_batt_smbus.h b/src/drivers/drv_batt_smbus.h index f12e2bfb3a..57af0a0b68 100644 --- a/src/drivers/drv_batt_smbus.h +++ b/src/drivers/drv_batt_smbus.h @@ -45,3 +45,13 @@ /* device path */ #define BATT_SMBUS0_DEVICE_PATH "/dev/batt_smbus0" + +/* + * ioctl() definitions + */ + +#define _BATT_SMBUS_IOCBASE (0x2e00) +#define _BATT_SMBUS_IOC(_n) (_IOC(_BATT_SMBUS_IOCBASE, _n)) + +/** retrieve battery capacity */ +#define BATT_SMBUS_GET_CAPACITY _BATT_SMBUS_IOC(1) From 051b7b853e48aecc596dbaadc92c2eec2b8192df Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Fri, 10 Apr 2015 09:56:05 +0200 Subject: [PATCH 166/268] UBlox: Check valid time bit for NAV_TIMEUTC messages (used for Ublox 6 and earlier) --- src/drivers/gps/ubx.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 09cd6b1c91..064ce20e5f 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -827,6 +827,7 @@ UBX::payload_rx_done(void) case UBX_MSG_NAV_TIMEUTC: UBX_TRACE_RXMSG("Rx NAV-TIMEUTC\n"); + if(_buf.payload_rx_nav_timeutc.valid & UBX_RX_NAV_TIMEUTC_VALID_VALIDUTC) { // convert to unix timestamp struct tm timeinfo; From 7a2bb7c03a23438b249039cdd1cf3217b2347cae Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 3 Apr 2015 16:11:38 +0200 Subject: [PATCH 167/268] commander: fix whitespace --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b0cd0b9e43..2506857a9d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1409,14 +1409,14 @@ int commander_thread_main(int argc, char *argv[]) if(status.condition_global_position_valid) { set_tune_override(TONE_GPS_WARNING_TUNE); status_changed = true; - status.condition_global_position_valid = false; + status.condition_global_position_valid = false; } } else if(global_position.timestamp != 0) { //Got good global position estimate if(!status.condition_global_position_valid) { status_changed = true; - status.condition_global_position_valid = true; + status.condition_global_position_valid = true; } } From f6f8d646d1b4f849af5f0eedcf1b17dc5267a47b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 3 Apr 2015 16:14:04 +0200 Subject: [PATCH 168/268] commander: rename variable to avoid confusion with parameter --- src/modules/commander/commander.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2506857a9d..55a44ac2e5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -164,7 +164,7 @@ static bool commander_initialized = false; static volatile bool thread_should_exit = false; /**< daemon exit flag */ static volatile bool thread_running = false; /**< daemon status flag */ static int daemon_task; /**< Handle of daemon task / thread */ -static bool _param_autosave = false; +static bool need_param_autosave = false; /**< Flag set to true if parameters should be autosaved in next iteration (happens on param update and if functionality is enabled) */ static unsigned int leds_counter; /* To remember when last notification was sent */ @@ -1162,7 +1162,7 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { /* trigger an autosave */ - _param_autosave = true; + need_param_autosave = true; } if (updated || param_init_forced) { @@ -2532,7 +2532,7 @@ void *commander_low_prio_loop(void *arg) memset(&cmd, 0, sizeof(cmd)); /* timeout for param autosave */ - hrt_abstime _param_autosave_timeout = 0; + hrt_abstime need_param_autosave_timeout = 0; /* wakeup source(s) */ struct pollfd fds[1]; @@ -2548,8 +2548,8 @@ void *commander_low_prio_loop(void *arg) /* timed out - periodic check for thread_should_exit, etc. */ if (pret == 0) { /* trigger a param autosave if required */ - if (_param_autosave) { - if (_param_autosave_timeout > 0 && hrt_elapsed_time(&_param_autosave_timeout) > 200000ULL) { + if (need_param_autosave) { + if (need_param_autosave_timeout > 0 && hrt_elapsed_time(&need_param_autosave_timeout) > 200000ULL) { int ret = param_save_default(); if (ret == OK) { @@ -2559,10 +2559,10 @@ void *commander_low_prio_loop(void *arg) mavlink_and_console_log_critical(mavlink_fd, "settings save error"); } - _param_autosave = false; - _param_autosave_timeout = 0; + need_param_autosave = false; + need_param_autosave_timeout = 0; } else { - _param_autosave_timeout = hrt_absolute_time(); + need_param_autosave_timeout = hrt_absolute_time(); } } } else if (pret < 0) { From a872b69df1b5cae9b60ec0a9a92261b80ab04dbb Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 3 Apr 2015 18:50:44 +0200 Subject: [PATCH 169/268] add parameter to control autosave of parameters --- src/modules/commander/commander.cpp | 19 +++++++++++++------ src/modules/commander/commander_params.c | 10 ++++++++++ 2 files changed, 23 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 55a44ac2e5..453d5f9212 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -156,7 +156,7 @@ enum MAV_MODE_FLAG { /* Mavlink file descriptors */ static int mavlink_fd = 0; -/* Syste autostart ID */ +/* System autostart ID */ static int autostart_id; /* flags */ @@ -822,6 +822,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_ef_current2throttle_thres = param_find("COM_EF_C2T"); param_t _param_ef_time_thres = param_find("COM_EF_TIME"); param_t _param_autostart_id = param_find("SYS_AUTOSTART"); + param_t _param_autosave_params = param_find("COM_AUTOS_PAR"); const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX]; main_states_str[vehicle_status_s::MAIN_STATE_MANUAL] = "MANUAL"; @@ -1128,6 +1129,8 @@ int commander_thread_main(int argc, char *argv[]) int32_t ef_time_thres = 1000.0f; uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine was healty */ + int autosave_params; /**< Autosave of parameters enabled/disabled, loaded from parameter */ + /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; bool main_state_changed = false; @@ -1160,11 +1163,6 @@ int commander_thread_main(int argc, char *argv[]) /* update parameters */ orb_check(param_changed_sub, &updated); - if (updated) { - /* trigger an autosave */ - need_param_autosave = true; - } - if (updated || param_init_forced) { param_init_forced = false; @@ -1232,6 +1230,15 @@ int commander_thread_main(int argc, char *argv[]) /* Autostart id */ param_get(_param_autostart_id, &autostart_id); + + /* Parameter autosave setting */ + param_get(_param_autosave_params, &autosave_params); + } + + /* Set flag to autosave parameters if necessary */ + if (updated && autosave_params != 0) { + /* trigger an autosave */ + need_param_autosave = true; } orb_check(sp_man_sub, &updated); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 94eeb6f71f..6a177046b1 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -176,3 +176,13 @@ PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f); * @max 35 */ PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5); + +/** Autosaving of params + * + * If not equal to zero the commander will automatically save parameters to persistent storage once changed + * + * @group commander + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(COM_AUTOS_PAR, 0); From 2d985dbed315ad621f5c586adca47a4c6f19d016 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 10 Apr 2015 17:32:09 +0200 Subject: [PATCH 170/268] commander: Set param autosave to on by default --- src/modules/commander/commander_params.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 6a177046b1..a550122431 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -179,10 +179,12 @@ PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5); /** Autosaving of params * - * If not equal to zero the commander will automatically save parameters to persistent storage once changed + * If not equal to zero the commander will automatically save parameters to persistent storage once changed. + * Default is on, as the interoperability with currently deployed GCS solutions depends on parameters + * being sticky. Developers can default it to off. * * @group commander * @min 0 * @max 1 */ -PARAM_DEFINE_INT32(COM_AUTOS_PAR, 0); +PARAM_DEFINE_INT32(COM_AUTOS_PAR, 1); From 555e96a37a79a381dd6aeacf0e99cf6621df1018 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Fri, 10 Apr 2015 17:34:38 +0200 Subject: [PATCH 171/268] fixed publication of mixer limit flags --- src/drivers/hil/hil.cpp | 2 +- src/drivers/mkblctrl/mkblctrl.cpp | 2 +- src/drivers/px4fmu/fmu.cpp | 2 +- src/drivers/px4io/px4io.cpp | 20 ++++++++++ src/modules/px4iofirmware/mixer.cpp | 4 +- src/modules/px4iofirmware/protocol.h | 5 +++ src/modules/px4iofirmware/px4io.h | 5 ++- src/modules/px4iofirmware/registers.c | 1 + src/modules/systemlib/mixer/mixer.cpp | 2 +- src/modules/systemlib/mixer/mixer.h | 10 ++--- src/modules/systemlib/mixer/mixer_group.cpp | 4 +- .../systemlib/mixer/mixer_multirotor.cpp | 39 ++++++++----------- src/modules/systemlib/mixer/mixer_simple.cpp | 2 +- .../uORB/topics/multirotor_motor_limits.h | 9 ++--- src/modules/uavcan/uavcan_main.cpp | 2 +- src/systemcmds/tests/test_mixer.cpp | 8 ++-- 16 files changed, 69 insertions(+), 48 deletions(-) diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index 9d28b48d46..961ec47246 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -402,7 +402,7 @@ HIL::task_main() if (_mixers != nullptr) { /* do mixing */ - outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL); outputs.timestamp = hrt_absolute_time(); /* iterate actuators */ diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index cb463eb59f..3b8c4ee777 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -536,7 +536,7 @@ MK::task_main() if (_mixers != nullptr) { /* do mixing */ - outputs.noutputs = _mixers->mix(&outputs.output[0], _num_outputs); + outputs.noutputs = _mixers->mix(&outputs.output[0], _num_outputs, NULL); outputs.timestamp = hrt_absolute_time(); /* iterate actuators */ diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 7b09a4676a..92afc7cd74 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -664,7 +664,7 @@ PX4FMU::task_main() } /* do mixing */ - outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL); outputs.timestamp = hrt_absolute_time(); /* iterate actuators */ diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 3bd2c06a41..33125699f5 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -88,6 +88,7 @@ #include #include #include +#include #include @@ -288,6 +289,7 @@ private: orb_advert_t _to_battery; ///< battery status / voltage orb_advert_t _to_servorail; ///< servorail status orb_advert_t _to_safety; ///< status of safety + orb_advert_t _to_mixer_status; ///< mixer status flags actuator_outputs_s _outputs; ///< mixed outputs servorail_status_s _servorail_status; ///< servorail status @@ -513,6 +515,7 @@ PX4IO::PX4IO(device::Device *interface) : _to_battery(0), _to_servorail(0), _to_safety(0), + _to_mixer_status(0), _outputs{}, _servorail_status{}, _primary_pwm_device(false), @@ -1687,6 +1690,8 @@ PX4IO::io_publish_pwm_outputs() { /* data we are going to fetch */ actuator_outputs_s outputs; + multirotor_motor_limits_s motor_limits; + outputs.timestamp = hrt_absolute_time(); /* get servo values from IO */ @@ -1718,6 +1723,21 @@ PX4IO::io_publish_pwm_outputs() orb_publish(ORB_ID(actuator_outputs), _to_outputs, &outputs); } + /* get mixer status flags from IO */ + uint16_t mixer_status; + ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_MIXER, &mixer_status,sizeof(mixer_status)/sizeof(uint16_t)); + memcpy(&motor_limits,&mixer_status,sizeof(motor_limits)); + + if (ret != OK) + return ret; + + /* publish mixer status */ + if(_to_mixer_status == 0) { + _to_mixer_status = orb_advertise(ORB_ID(multirotor_motor_limits), &motor_limits); + } else { + orb_publish(ORB_ID(multirotor_motor_limits),_to_mixer_status, &motor_limits); + } + return OK; } diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 835dfc0f5e..6fa26d4fff 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -230,7 +230,7 @@ mixer_tick(void) /* poor mans mutex */ in_mixer = true; - mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); + mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT, &r_mixer_limits); in_mixer = false; /* the pwm limit call takes care of out of band errors */ @@ -453,7 +453,7 @@ mixer_set_failsafe() unsigned mixed; /* mix */ - mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); + mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT, &r_mixer_limits); /* scale to PWM and update the servo outputs as required */ for (unsigned i = 0; i < mixed; i++) { diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 874dc0c398..cbafa36410 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -133,6 +133,11 @@ #define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */ #define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */ +#define PX4IO_P_STATUS_MIXER 9 /* mixer actuator limit flags */ +#define PX4IO_P_STATUS_MIXER_LOWER_LIMIT (1 << 0) /**< at least one actuator output has reached lower limit */ +#define PX4IO_P_STATUS_MIXER_UPPER_LIMIT (1 << 1) /**< at least one actuator output has reached upper limit */ +#define PX4IO_P_STATUS_MIXER_YAW_LIMIT (1 << 2) /**< yaw control is limited because it causes output clipping */ + /* array of post-mix actuator outputs, -10000..10000 */ #define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 17699b7c03..8ddf45a12d 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -98,8 +98,9 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ #define r_raw_rc_count r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT] #define r_raw_rc_values (&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE]) #define r_raw_rc_flags r_page_raw_rc_input[PX4IO_P_RAW_RC_FLAGS] -#define r_rc_valid r_page_rc_input[PX4IO_P_RC_VALID] -#define r_rc_values (&r_page_rc_input[PX4IO_P_RC_BASE]) +#define r_rc_valid r_page_rc_input[PX4IO_P_RC_VALID] +#define r_rc_values (&r_page_rc_input[PX4IO_P_RC_BASE]) +#define r_mixer_limits r_page_status[PX4IO_P_STATUS_MIXER] #define r_setup_features r_page_setup[PX4IO_P_SETUP_FEATURES] #define r_setup_arming r_page_setup[PX4IO_P_SETUP_ARMING] diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index ae7aec34e4..90e0fb10ea 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -90,6 +90,7 @@ uint16_t r_page_status[] = { [PX4IO_P_STATUS_VSERVO] = 0, [PX4IO_P_STATUS_VRSSI] = 0, [PX4IO_P_STATUS_PRSSI] = 0, + [PX4IO_P_STATUS_MIXER] = 0, }; /** diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp index 20b1f18ed6..3ab41c5c58 100644 --- a/src/modules/systemlib/mixer/mixer.cpp +++ b/src/modules/systemlib/mixer/mixer.cpp @@ -151,7 +151,7 @@ NullMixer::NullMixer() : } unsigned -NullMixer::mix(float *outputs, unsigned space) +NullMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) { if (space > 0) { *outputs = 0.0f; diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index 67ef521b4e..1190683015 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -174,7 +174,7 @@ public: * @param space The number of available entries in the output array; * @return The number of entries in the output array that were populated. */ - virtual unsigned mix(float *outputs, unsigned space) = 0; + virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg) = 0; /** * Analyses the mix configuration and updates a bitmask of groups @@ -250,7 +250,7 @@ public: MixerGroup(ControlCallback control_cb, uintptr_t cb_handle); ~MixerGroup(); - virtual unsigned mix(float *outputs, unsigned space); + virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg); virtual void groups_required(uint32_t &groups); /** @@ -346,7 +346,7 @@ public: */ static NullMixer *from_text(const char *buf, unsigned &buflen); - virtual unsigned mix(float *outputs, unsigned space); + virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg); virtual void groups_required(uint32_t &groups); }; @@ -411,7 +411,7 @@ public: uint16_t mid, uint16_t max); - virtual unsigned mix(float *outputs, unsigned space); + virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg); virtual void groups_required(uint32_t &groups); /** @@ -515,7 +515,7 @@ public: const char *buf, unsigned &buflen); - virtual unsigned mix(float *outputs, unsigned space); + virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg); virtual void groups_required(uint32_t &groups); private: diff --git a/src/modules/systemlib/mixer/mixer_group.cpp b/src/modules/systemlib/mixer/mixer_group.cpp index 4220b168d3..ca5101e657 100644 --- a/src/modules/systemlib/mixer/mixer_group.cpp +++ b/src/modules/systemlib/mixer/mixer_group.cpp @@ -99,13 +99,13 @@ MixerGroup::reset() } unsigned -MixerGroup::mix(float *outputs, unsigned space) +MixerGroup::mix(float *outputs, unsigned space, uint16_t *status_reg) { Mixer *mixer = _first; unsigned index = 0; while ((mixer != nullptr) && (index < space)) { - index += mixer->mix(outputs + index, space - index); + index += mixer->mix(outputs + index, space - index, status_reg); mixer = mixer->_next; } diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index b753da5ce7..e9a8a87ca8 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -36,10 +36,7 @@ * * Multi-rotor mixers. */ -#include -#include #include - #include #include #include @@ -53,6 +50,8 @@ #include #include +#include + #include "mixer.h" // This file is generated by the multi_tables script which is invoked during the build process @@ -199,7 +198,7 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl } unsigned -MultirotorMixer::mix(float *outputs, unsigned space) +MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) { float roll = constrain(get_control(0, 0) * _roll_scale, -1.0f, 1.0f); //lowsyslog("roll: %d, get_control0: %d, %d\n", (int)(roll), (int)(get_control(0, 0)), (int)(_roll_scale)); @@ -210,10 +209,9 @@ MultirotorMixer::mix(float *outputs, unsigned space) float min_out = 0.0f; float max_out = 0.0f; - _limits.roll_pitch = false; - _limits.yaw = false; - _limits.throttle_upper = false; - _limits.throttle_lower = false; + if (status_reg != NULL) { + (*status_reg) = 0; + } /* perform initial mix pass yielding unbounded outputs, ignore yaw */ for (unsigned i = 0; i < _rotor_count; i++) { @@ -226,7 +224,9 @@ MultirotorMixer::mix(float *outputs, unsigned space) /* limit yaw if it causes outputs clipping */ if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) { yaw = -out / _rotors[i].yaw_scale; - _limits.yaw = true; + if(status_reg != NULL) { + (*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT; + } } /* calculate min and max output values */ @@ -257,7 +257,10 @@ MultirotorMixer::mix(float *outputs, unsigned space) outputs[i] = out; } - _limits.roll_pitch = true; + + if(status_reg != NULL) { + (*status_reg) |= PX4IO_P_STATUS_MIXER_LOWER_LIMIT; + } } else { /* roll/pitch mixed without lower side limiting, add yaw control */ @@ -270,7 +273,10 @@ MultirotorMixer::mix(float *outputs, unsigned space) float scale_out; if (max_out > 1.0f) { scale_out = 1.0f / max_out; - _limits.throttle_upper = true; + + if(status_reg != NULL) { + (*status_reg) |= PX4IO_P_STATUS_MIXER_UPPER_LIMIT; + } } else { scale_out = 1.0f; @@ -278,20 +284,9 @@ MultirotorMixer::mix(float *outputs, unsigned space) /* scale outputs to range _idle_speed..1, and do final limiting */ for (unsigned i = 0; i < _rotor_count; i++) { - if (outputs[i] < _idle_speed) { - _limits.throttle_lower = true; - } outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed) * scale_out), _idle_speed, 1.0f); } -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) || defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - /* publish/advertise motor limits if running on FMU */ - if (_limits_pub > 0) { - orb_publish(ORB_ID(multirotor_motor_limits), _limits_pub, &_limits); - } else { - _limits_pub = orb_advertise(ORB_ID(multirotor_motor_limits), &_limits); - } -#endif return _rotor_count; } diff --git a/src/modules/systemlib/mixer/mixer_simple.cpp b/src/modules/systemlib/mixer/mixer_simple.cpp index c3985b5de6..e48bda6918 100644 --- a/src/modules/systemlib/mixer/mixer_simple.cpp +++ b/src/modules/systemlib/mixer/mixer_simple.cpp @@ -265,7 +265,7 @@ out: } unsigned -SimpleMixer::mix(float *outputs, unsigned space) +SimpleMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) { float sum = 0.0f; diff --git a/src/modules/uORB/topics/multirotor_motor_limits.h b/src/modules/uORB/topics/multirotor_motor_limits.h index 44c51e4d98..589f8a650c 100644 --- a/src/modules/uORB/topics/multirotor_motor_limits.h +++ b/src/modules/uORB/topics/multirotor_motor_limits.h @@ -52,11 +52,10 @@ * Motor limits */ struct multirotor_motor_limits_s { - uint8_t roll_pitch : 1; // roll/pitch limit reached - uint8_t yaw : 1; // yaw limit reached - uint8_t throttle_lower : 1; // lower throttle limit reached - uint8_t throttle_upper : 1; // upper throttle limit reached - uint8_t reserved : 4; + uint8_t lower_limit : 1; // at least one actuator command has saturated on the lower limit + uint8_t upper_limit : 1; // at least one actuator command has saturated on the upper limit + uint8_t yaw : 1; // yaw limit reached + uint8_t reserved : 5; // reserved }; /** diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index b93a95f965..02cde96564 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -421,7 +421,7 @@ int UavcanNode::run() unsigned num_outputs_max = 8; // Do mixing - _outputs.noutputs = _mixers->mix(&_outputs.output[0], num_outputs_max); + _outputs.noutputs = _mixers->mix(&_outputs.output[0], num_outputs_max, NULL); new_output = true; } diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index 64e734b008..19aa059707 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -202,7 +202,7 @@ int test_mixer(int argc, char *argv[]) while (hrt_elapsed_time(&starttime) < INIT_TIME_US + RAMP_TIME_US) { /* mix */ - mixed = mixer_group.mix(&outputs[0], output_max); + mixed = mixer_group.mix(&outputs[0], output_max, NULL); pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); @@ -248,7 +248,7 @@ int test_mixer(int argc, char *argv[]) } /* mix */ - mixed = mixer_group.mix(&outputs[0], output_max); + mixed = mixer_group.mix(&outputs[0], output_max, NULL); pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); @@ -275,7 +275,7 @@ int test_mixer(int argc, char *argv[]) while (hrt_elapsed_time(&starttime) < 600000) { /* mix */ - mixed = mixer_group.mix(&outputs[0], output_max); + mixed = mixer_group.mix(&outputs[0], output_max, NULL); pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); @@ -311,7 +311,7 @@ int test_mixer(int argc, char *argv[]) while (hrt_elapsed_time(&starttime) < 600000 + RAMP_TIME_US) { /* mix */ - mixed = mixer_group.mix(&outputs[0], output_max); + mixed = mixer_group.mix(&outputs[0], output_max, NULL); pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); From a1c698034d69ad84f33ec8fb431d82dab4cf4294 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 11 Apr 2015 00:59:58 +0200 Subject: [PATCH 172/268] HMC5833 driver: Enable internal temperature calibration when available. --- ROMFS/px4fmu_common/init.d/rc.sensors | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 8dff705349..ffded7d6b9 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -9,12 +9,12 @@ adc start if ver hwcmp PX4FMU_V2 then # External I2C bus - if hmc5883 -C -X start + if hmc5883 -C -T -X start then fi # Internal I2C bus - if hmc5883 -C -I -R 4 start + if hmc5883 -C -T -I -R 4 start then fi @@ -43,7 +43,7 @@ then then fi - if hmc5883 -S -R 8 start + if hmc5883 -C -T -S -R 8 start then fi From d4ae721bc0a96509cfbdb004bbe302694e143ee7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 11 Apr 2015 01:02:21 +0200 Subject: [PATCH 173/268] sd card boot: Focus on card reliability, as wider adoption identifies corrupted card to be still a common problem --- ROMFS/px4fmu_common/init.d/rcS | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 111965bd5f..0a8774b305 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -17,8 +17,8 @@ set MODE autostart set FRC /fs/microsd/etc/rc.txt set FCONFIG /fs/microsd/etc/config.txt - set TUNE_ERR ML< Date: Mon, 16 Mar 2015 10:14:07 -0600 Subject: [PATCH 174/268] apply roll/pitch acro_rate_max in MC attitude controller --- src/modules/mc_att_control/mc_att_control_main.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 85a0dda9e3..987fb03148 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -639,6 +639,11 @@ MulticopterAttitudeControl::control_attitude(float dt) /* calculate angular rates setpoint */ _rates_sp = _params.att_p.emult(e_R); + /* limit roll and pitch rates */ + for (int i = 0; i < 2; i++) { + _rates_sp(i) = math::constrain(_rates_sp(i), -_params.acro_rate_max(i), _params.acro_rate_max(i)); + } + /* limit yaw rate */ _rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, _params.yaw_rate_max); From b356508f6b05504af745dd1ee2ba2ab00816057d Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Tue, 17 Mar 2015 20:52:51 -0600 Subject: [PATCH 175/268] add new parameters for roll and pitch angular rate limits --- .../mc_att_control/mc_att_control_main.cpp | 36 +++++++++++++------ .../mc_att_control/mc_att_control_params.c | 24 +++++++++++++ 2 files changed, 49 insertions(+), 11 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 987fb03148..e140acea60 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -178,6 +178,8 @@ private: param_t yaw_rate_d; param_t yaw_rate_ff; param_t yaw_ff; + param_t roll_rate_max; + param_t pitch_rate_max; param_t yaw_rate_max; param_t man_roll_max; @@ -196,7 +198,11 @@ private: math::Vector<3> rate_d; /**< D gain for angular rate error */ math::Vector<3> rate_ff; /**< Feedforward gain for desired rates */ float yaw_ff; /**< yaw control feed-forward */ - float yaw_rate_max; /**< max yaw rate */ + + float roll_rate_max; + float pitch_rate_max; + float yaw_rate_max; + math::Vector<3> mc_rate_max; /**< attitude rate limits in all modes */ float man_roll_max; float man_pitch_max; @@ -322,10 +328,13 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params.rate_d.zero(); _params.rate_ff.zero(); _params.yaw_ff = 0.0f; + _params.roll_rate_max = 0.0f; + _params.pitch_rate_max = 0.0f; _params.yaw_rate_max = 0.0f; _params.man_roll_max = 0.0f; _params.man_pitch_max = 0.0f; _params.man_yaw_max = 0.0f; + _params.mc_rate_max.zero(); _params.acro_rate_max.zero(); _rates_prev.zero(); @@ -352,6 +361,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); _params_handles.yaw_rate_ff = param_find("MC_YAWRATE_FF"); _params_handles.yaw_ff = param_find("MC_YAW_FF"); + _params_handles.roll_rate_max = param_find("MC_ROLLRATE_MAX"); + _params_handles.pitch_rate_max = param_find("MC_PITCHRATE_MAX"); _params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX"); _params_handles.man_roll_max = param_find("MC_MAN_R_MAX"); _params_handles.man_pitch_max = param_find("MC_MAN_P_MAX"); @@ -430,10 +441,16 @@ MulticopterAttitudeControl::parameters_update() _params.rate_ff(2) = v; param_get(_params_handles.yaw_ff, &_params.yaw_ff); - param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max); - _params.yaw_rate_max = math::radians(_params.yaw_rate_max); - /* manual control scale */ + /* angular rate limits */ + param_get(_params_handles.roll_rate_max, &_params.roll_rate_max); + _params.mc_rate_max(0) = math::radians(_params.roll_rate_max); + param_get(_params_handles.pitch_rate_max, &_params.pitch_rate_max); + _params.mc_rate_max(1) = math::radians(_params.pitch_rate_max); + param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max); + _params.mc_rate_max(2) = math::radians(_params.yaw_rate_max); + + /* manual attitude control scale */ param_get(_params_handles.man_roll_max, &_params.man_roll_max); param_get(_params_handles.man_pitch_max, &_params.man_pitch_max); param_get(_params_handles.man_yaw_max, &_params.man_yaw_max); @@ -441,7 +458,7 @@ MulticopterAttitudeControl::parameters_update() _params.man_pitch_max = math::radians(_params.man_pitch_max); _params.man_yaw_max = math::radians(_params.man_yaw_max); - /* acro control scale */ + /* manual rate control scale and auto mode roll/pitch rate limits */ param_get(_params_handles.acro_roll_max, &v); _params.acro_rate_max(0) = math::radians(v); param_get(_params_handles.acro_pitch_max, &v); @@ -639,14 +656,11 @@ MulticopterAttitudeControl::control_attitude(float dt) /* calculate angular rates setpoint */ _rates_sp = _params.att_p.emult(e_R); - /* limit roll and pitch rates */ - for (int i = 0; i < 2; i++) { - _rates_sp(i) = math::constrain(_rates_sp(i), -_params.acro_rate_max(i), _params.acro_rate_max(i)); + /* limit rates */ + for (int i = 0; i < 3; i++) { + _rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i)); } - /* limit yaw rate */ - _rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, _params.yaw_rate_max); - /* feed forward yaw setpoint rate */ _rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff; } diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 6a21f9046f..3f63f2fc06 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -205,6 +205,30 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_FF, 0.0f); */ PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); +/** + * Max roll rate + * + * Limit for roll rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation. + * + * @unit deg/s + * @min 0.0 + * @max 360.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_ROLLRATE_MAX, 120.0f); + +/** + * Max pitch rate + * + * Limit for pitch rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation. + * + * @unit deg/s + * @min 0.0 + * @max 360.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_PITCHRATE_MAX, 120.0f); + /** * Max yaw rate * From 6ccfc222d38ef0f409e7d9d237aeb4c6e80bfeb3 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Wed, 18 Mar 2015 07:11:20 -0600 Subject: [PATCH 176/268] fix comment on MC att controller rate limits --- src/modules/mc_att_control/mc_att_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index e140acea60..d477316222 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -202,7 +202,7 @@ private: float roll_rate_max; float pitch_rate_max; float yaw_rate_max; - math::Vector<3> mc_rate_max; /**< attitude rate limits in all modes */ + math::Vector<3> mc_rate_max; /**< attitude rate limits in stabilized modes */ float man_roll_max; float man_pitch_max; From f1b2efeeaf1ca41fa20263af37b94485dcb9cee6 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sat, 11 Apr 2015 08:10:58 -0600 Subject: [PATCH 177/268] increase default roll/pitch rate limits to 360dps --- src/modules/mc_att_control/mc_att_control_params.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 3f63f2fc06..3f19a51f06 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -215,7 +215,7 @@ PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); * @max 360.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_MAX, 120.0f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_MAX, 360.0f); /** * Max pitch rate @@ -227,7 +227,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_MAX, 120.0f); * @max 360.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_MAX, 120.0f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_MAX, 360.0f); /** * Max yaw rate From 8fd6b3da212cde1b164431bdb5ba909178dc4199 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Mon, 13 Apr 2015 11:37:49 +0200 Subject: [PATCH 178/268] Store the port so the in use port will be referenced when reopening. --- src/drivers/sf0x/sf0x.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index d6599c0361..66641d6408 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -114,6 +114,7 @@ protected: virtual int probe(); private: + char _port[20]; float _min_distance; float _max_distance; work_s _work; @@ -199,8 +200,13 @@ SF0X::SF0X(const char *port) : _comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "sf0x_buffer_overflows")) { + /* store port name */ + strncpy(_port, port, sizeof(_port)); + /* enforce null termination */ + _port[sizeof(_port) - 1] = '\0'; + /* open fd */ - _fd = ::open(port, O_RDWR | O_NOCTTY | O_NONBLOCK); + _fd = ::open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK); if (_fd < 0) { warnx("FAIL: laser fd"); @@ -633,7 +639,7 @@ SF0X::cycle() /* fds initialized? */ if (_fd < 0) { /* open fd */ - _fd = ::open(SF0X_DEFAULT_PORT, O_RDWR | O_NOCTTY | O_NONBLOCK); + _fd = ::open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK); } /* collection phase? */ From 25dfd84b40177d40a5848f6c549f5d326f670bee Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 22 Feb 2015 23:04:23 -0500 Subject: [PATCH 179/268] Geofence max horizontal and vertical distance -changes GF_ON to GF_MODE -adds GF_MAX_HOR_DIST and GF_MAX_VER_DIST params --- src/modules/navigator/geofence.cpp | 53 +++++++++++++++++++++--- src/modules/navigator/geofence.h | 12 +++++- src/modules/navigator/geofence_params.c | 27 +++++++++--- src/modules/navigator/navigator.h | 2 + src/modules/navigator/navigator_main.cpp | 11 ++++- 5 files changed, 91 insertions(+), 14 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 9bc9be245c..efbc2689bd 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -39,6 +39,7 @@ */ #include "geofence.h" +#include #include #include #include @@ -49,6 +50,12 @@ #include #include #include +#include + +#define GEOFENCE_OFF 0 +#define GEOFENCE_FILE_ONLY 1 +#define GEOFENCE_MAX_DISTANCES_ONLY 2 +#define GEOFENCE_FILE_AND_MAX_DISTANCES 3 /* Oddly, ERROR is not defined for C++ */ @@ -60,13 +67,17 @@ static const int ERROR = -1; Geofence::Geofence() : SuperBlock(NULL, "GF"), _fence_pub(-1), + _home_pos{}, + _home_pos_set(false), _altitude_min(0), _altitude_max(0), _verticesCount(0), - _param_geofence_on(this, "ON"), + _param_geofence_mode(this, "MODE"), _param_altitude_mode(this, "ALTMODE"), _param_source(this, "SOURCE"), _param_counter_threshold(this, "COUNT"), + _param_max_hor_distance(this, "MAX_HOR_DIST"), + _param_max_ver_distance(this, "MAX_VER_DIST"), _outside_counter(0), _mavlinkFd(-1) { @@ -92,10 +103,14 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position, f bool Geofence::inside(const struct vehicle_global_position_s &global_position, - const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl) + const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl, + const struct home_position_s home_pos, bool home_position_set) { updateParams(); + _home_pos = home_pos; + _home_pos_set = home_position_set; + if (getAltitudeMode() == Geofence::GF_ALT_MODE_WGS84) { if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) { return inside(global_position); @@ -118,13 +133,40 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position, bool Geofence::inside(double lat, double lon, float altitude) { + if (_param_geofence_mode.get() >= GEOFENCE_MAX_DISTANCES_ONLY) { + int32_t max_horizontal_distance = _param_max_hor_distance.get(); + int32_t max_vertical_distance = _param_max_ver_distance.get(); + + if (max_horizontal_distance > 0 || max_vertical_distance > 0) { + if (_home_pos_set) { + float dist_xy = -1.0f; + float dist_z = -1.0f; + get_distance_to_point_global_wgs84(lat, lon, altitude, + _home_pos.lat, _home_pos.lon, _home_pos.alt, + &dist_xy, &dist_z); + + if (max_vertical_distance > 0 && (dist_z > max_vertical_distance)) { + mavlink_log_critical(_mavlinkFd, "#audio: Geofence exceeded max vertical distance by %.0f m", + (double)(dist_z - max_vertical_distance)); + return false; + } + + if (max_horizontal_distance > 0 && (dist_xy > max_horizontal_distance)) { + mavlink_log_critical(_mavlinkFd, "#audio: Geofence exceeded max horizontal distance by %.0f m", + (double)(dist_xy - max_horizontal_distance)); + return false; + } + } + } + } + bool inside_fence = inside_polygon(lat, lon, altitude); if (inside_fence) { _outside_counter = 0; return inside_fence; - } { + } else { _outside_counter++; if (_outside_counter > _param_counter_threshold.get()) { @@ -139,8 +181,9 @@ bool Geofence::inside(double lat, double lon, float altitude) bool Geofence::inside_polygon(double lat, double lon, float altitude) { - /* Return true if geofence is disabled */ - if (_param_geofence_on.get() != 1) { + /* Return true if geofence is disabled or only checking max distances */ + if ((_param_geofence_mode.get() == GEOFENCE_OFF) + || (_param_geofence_mode.get() == GEOFENCE_MAX_DISTANCES_ONLY)) { return true; } diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 37a41b68a3..effce9e976 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -45,6 +45,7 @@ #include #include #include +#include #include #include @@ -76,7 +77,9 @@ public: * @return true: system is inside fence, false: system is outside fence */ bool inside(const struct vehicle_global_position_s &global_position, - const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl); + const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl, + const struct home_position_s home_pos, bool home_position_set); + bool inside_polygon(double lat, double lon, float altitude); int clearDm(); @@ -103,16 +106,21 @@ public: private: orb_advert_t _fence_pub; /**< publish fence topic */ + home_position_s _home_pos; + bool _home_pos_set; + float _altitude_min; float _altitude_max; unsigned _verticesCount; /* Params */ - control::BlockParamInt _param_geofence_on; + control::BlockParamInt _param_geofence_mode; control::BlockParamInt _param_altitude_mode; control::BlockParamInt _param_source; control::BlockParamInt _param_counter_threshold; + control::BlockParamInt _param_max_hor_distance; + control::BlockParamInt _param_max_ver_distance; uint8_t _outside_counter; diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index fca3918e1d..f3e7d4c84a 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -48,16 +48,15 @@ */ /** - * Enable geofence. + * Geofence mode. * - * Set to 1 to enable geofence. - * Defaults to 1 because geofence is only enabled when the geofence.txt file is present. + * 0 = disabled, 1 = geofence file only, 2 = max horizontal (GF_MAX_HOR_DIST) and vertical (GF_MAX_VER_DIST) distances, 3 = both * * @min 0 - * @max 1 + * @max 3 * @group Geofence */ -PARAM_DEFINE_INT32(GF_ON, 1); +PARAM_DEFINE_INT32(GF_MODE, 0); /** * Geofence altitude mode @@ -94,3 +93,21 @@ PARAM_DEFINE_INT32(GF_SOURCE, 0); * @group Geofence */ PARAM_DEFINE_INT32(GF_COUNT, -1); + +/** + * Max horizontal distance in meters. + * + * Set to > 0 to activate RTL if horizontal distance to home exceeds this value. + * + * @group Geofence + */ +PARAM_DEFINE_INT32(GF_MAX_HOR_DIST, -1); + +/** + * Max vertical distance in meters. + * + * Set to > 0 to activate RTL if vertical distance to home exceeds this value. + * + * @group Geofence + */ +PARAM_DEFINE_INT32(GF_MAX_VER_DIST, -1); diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index d9d911d9c7..d2acce7899 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -186,6 +186,8 @@ private: geofence_result_s _geofence_result; vehicle_attitude_setpoint_s _att_sp; + bool _home_position_set; + bool _mission_item_valid; /**< flags if the current mission item is valid */ perf_counter_t _loop_perf; /**< loop performance counter */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ad2349c947..05fac7b810 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -123,6 +123,7 @@ Navigator::Navigator() : _pos_sp_triplet{}, _mission_result{}, _att_sp{}, + _home_position_set(false), _mission_item_valid(false), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), _geofence{}, @@ -203,7 +204,13 @@ Navigator::sensor_combined_update() void Navigator::home_position_update() { - orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); + bool updated = false; + orb_check(_home_pos_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); + _home_position_set = true; + } } void @@ -392,7 +399,7 @@ Navigator::task_main() /* Check geofence violation */ static hrt_abstime last_geofence_check = 0; if (have_geofence_position_data && hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL) { - bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter); + bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos, _home_position_set); last_geofence_check = hrt_absolute_time(); have_geofence_position_data = false; if (!inside) { From 66e6938c6d979b1a955af7dbb3abb4d420d7c241 Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Sun, 11 Jan 2015 12:59:30 +0530 Subject: [PATCH 180/268] timesync: Add uORB topic, general fixes --- src/modules/mavlink/mavlink_receiver.cpp | 24 +++++++-- src/modules/mavlink/mavlink_receiver.h | 7 ++- src/modules/uORB/objects_common.cpp | 4 ++ src/modules/uORB/topics/time_offset.h | 67 ++++++++++++++++++++++++ 4 files changed, 96 insertions(+), 6 deletions(-) create mode 100644 src/modules/uORB/topics/time_offset.h diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 3f9f7e1392..faede15cb9 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -121,6 +121,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _rc_pub(-1), _manual_pub(-1), _land_detector_pub(-1), + _time_offset_pub(-1), _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), _hil_frames(0), _old_timestamp(0), @@ -729,8 +730,8 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) // Use the component ID to identify the vision sensor vision_position.id = msg->compid; - vision_position.timestamp_boot = to_hrt(pos.usec); // Synced time - vision_position.timestamp_computer = pos.usec; + vision_position.timestamp_boot = hrt_absolute_time(); // Monotonic time + vision_position.timestamp_computer = sync_stamp(pos.usec); // Synced time vision_position.x = pos.x; vision_position.y = pos.y; vision_position.z = pos.z; @@ -1013,6 +1014,9 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) mavlink_timesync_t tsync; mavlink_msg_timesync_decode(msg, &tsync); + struct time_offset_s tsync_offset; + memset(&tsync_offset, 0, sizeof(tsync_offset)); + uint64_t now_ns = hrt_absolute_time() * 1000LL ; if (tsync.tc1 == 0) { @@ -1039,6 +1043,15 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) } } + tsync_offset.offset_ns = _time_offset ; + + if (_time_offset_pub < 0) { + _time_offset_pub = orb_advertise(ORB_ID(time_offset), &tsync_offset); + + } else { + orb_publish(ORB_ID(time_offset), _time_offset_pub, &tsync_offset); + } + } void @@ -1522,9 +1535,12 @@ void MavlinkReceiver::print_status() } -uint64_t MavlinkReceiver::to_hrt(uint64_t usec) +uint64_t MavlinkReceiver::sync_stamp(uint64_t usec) { - return usec - (_time_offset / 1000) ; + if(_time_offset > 0) + return usec - (_time_offset / 1000) ; + else + return hrt_absolute_time(); } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index ffacb59a63..2b6174f8fe 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -73,6 +73,7 @@ #include #include #include +#include #include "mavlink_ftp.h" @@ -138,9 +139,10 @@ private: void *receive_thread(void *arg); /** - * Convert remote nsec timestamp to local hrt time (usec) + * Convert remote timestamp to local hrt time (usec) + * Use timesync if available, monotonic boot time otherwise */ - uint64_t to_hrt(uint64_t nsec); + uint64_t sync_stamp(uint64_t usec); /** * Exponential moving average filter to smooth time offset */ @@ -177,6 +179,7 @@ private: orb_advert_t _rc_pub; orb_advert_t _manual_pub; orb_advert_t _land_detector_pub; + orb_advert_t _time_offset_pub; int _control_mode_sub; int _hil_frames; uint64_t _old_timestamp; diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index dbed297744..75977ffd1b 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -250,3 +250,7 @@ ORB_DEFINE(wind_estimate, struct wind_estimate_s); #include "topics/rc_parameter_map.h" ORB_DEFINE(rc_parameter_map, struct rc_parameter_map_s); + +#include "topics/time_offset.h" +ORB_DEFINE(time_offset, struct time_offset_s); + diff --git a/src/modules/uORB/topics/time_offset.h b/src/modules/uORB/topics/time_offset.h new file mode 100644 index 0000000000..99e526c765 --- /dev/null +++ b/src/modules/uORB/topics/time_offset.h @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file time_offset.h + * Time synchronisation offset + */ + +#ifndef TOPIC_TIME_OFFSET_H_ +#define TOPIC_TIME_OFFSET_H_ + +#include +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * Timesync offset for synchronisation with companion computer, GCS, etc. + */ +struct time_offset_s { + + uint64_t offset_ns; /**< time offset between companion system and PX4, in nanoseconds */ + +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(time_offset); + +#endif /* TOPIC_TIME_OFFSET_H_ */ From 2f164ca34508aee54dc3f01ca389c84ddf282dee Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Tue, 14 Apr 2015 12:53:05 +0530 Subject: [PATCH 181/268] sdlog2 : field for timesync offset --- src/modules/sdlog2/sdlog2.c | 12 ++++++++++++ src/modules/sdlog2/sdlog2_messages.h | 7 +++++++ 2 files changed, 19 insertions(+) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 0bfd356a1c..2ec7336272 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -99,6 +99,7 @@ #include #include #include +#include #include #include @@ -1027,6 +1028,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct wind_estimate_s wind_estimate; struct encoders_s encoders; struct vtol_vehicle_status_s vtol_status; + struct time_offset_s time_offset; } buf; memset(&buf, 0, sizeof(buf)); @@ -1071,6 +1073,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_TECS_s log_TECS; struct log_WIND_s log_WIND; struct log_ENCD_s log_ENCD; + struct log_TSYN_s log_TSYN; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1111,6 +1114,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int servorail_status_sub; int wind_sub; int encoders_sub; + int tsync_sub; } subs; subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -1142,6 +1146,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.system_power_sub = orb_subscribe(ORB_ID(system_power)); subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status)); subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate)); + subs.tsync_sub = orb_subscribe(ORB_ID(time_offset)); /* we need to rate-limit wind, as we do not need the full update rate */ orb_set_interval(subs.wind_sub, 90); @@ -1821,6 +1826,13 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(ENCD); } + /* --- TIMESYNC OFFSET --- */ + if (copy_if_updated(ORB_ID(time_offset), subs.tsync_sub, &buf.time_offset)) { + log_msg.msg_type = LOG_TSYN_MSG; + log_msg.body.log_TSYN.time_offset = buf.time_offset.offset_ns; + LOGBUFFER_WRITE_AND_COUNT(TSYN); + } + /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { /* only request write if several packets can be written at once */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index a1fe2c95d0..598f12a536 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -449,6 +449,12 @@ struct log_VTOL_s { float airspeed_tot; }; +/* --- TIMESYNC - TIME SYNCHRONISATION OFFSET */ +#define LOG_TSYN_MSG 43 +struct log_TSYN_s { + uint64_t time_offset; +}; + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -517,6 +523,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"), + LOG_FORMAT(TSYN, "Q", "TimeOffset"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ From cc0d78b6069410dcbf875ebe609955ffd7a8166f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 13 Apr 2015 18:45:18 +0200 Subject: [PATCH 182/268] sdlog2: Do not abort startup without microSD --- ROMFS/px4fmu_common/init.d/rc.logging | 10 ++++++---- ROMFS/px4fmu_common/init.d/rcS | 2 ++ 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index a14eb5247a..454af8da7a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -7,10 +7,12 @@ if [ -d /fs/microsd ] then if ver hwcmp PX4FMU_V1 then - echo "Start sdlog2 at 50Hz" - sdlog2 start -r 40 -a -b 3 -t + if sdlog2 start -r 40 -a -b 3 -t + then + fi else - echo "Start sdlog2 at 200Hz" - sdlog2 start -r 200 -a -b 22 -t + if sdlog2 start -r 200 -a -b 22 -t + then + fi fi fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 0a8774b305..2d3722f089 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -42,6 +42,8 @@ else tone_alarm MNBG set LOG_FILE /dev/null fi + else + set LOG_FILE /dev/null fi fi From 4b3422161442dd00522860616527df6c46079c7b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 14 Apr 2015 13:25:49 +0200 Subject: [PATCH 183/268] navigator: Get rid of audio tag in strings and use appropriate priority to get audio out when needed in the GCS --- src/modules/navigator/datalinkloss.cpp | 10 +++++----- src/modules/navigator/enginefailure.cpp | 2 +- src/modules/navigator/geofence.cpp | 6 +++--- src/modules/navigator/gpsfailure.cpp | 4 ++-- src/modules/navigator/mission_block.cpp | 2 +- .../navigator/mission_feasibility_checker.cpp | 12 ++++++------ src/modules/navigator/navigator_main.cpp | 2 +- src/modules/navigator/rcloss.cpp | 6 +++--- src/modules/navigator/rtl.cpp | 16 ++++++++-------- 9 files changed, 30 insertions(+), 30 deletions(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 87a6e023a5..7925809f12 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -186,7 +186,7 @@ DataLinkLoss::advance_dll() if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) { warnx("%d data link losses, limit is %d, fly to airfield home", _navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get()); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "too many DL losses, fly to airfield home"); _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->set_mission_result_updated(); reset_mission_item_reached(); @@ -194,19 +194,19 @@ DataLinkLoss::advance_dll() } else { if (!_param_skipcommshold.get()) { warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "fly to comms hold"); _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; } else { /* comms hold wp not active, fly to airfield home directly */ warnx("Skipping comms hold wp. Flying directly to airfield home"); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home, comms hold skipped"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "fly to airfield home, comms hold skipped"); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; } } break; case DLL_STATE_FLYTOCOMMSHOLDWP: warnx("fly to airfield home"); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "fly to airfield home"); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->set_mission_result_updated(); @@ -215,7 +215,7 @@ DataLinkLoss::advance_dll() case DLL_STATE_FLYTOAIRFIELDHOMEWP: _dll_state = DLL_STATE_TERMINATE; warnx("time is up, state should have been changed manually by now"); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "no manual control, terminating"); _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->set_mission_result_updated(); reset_mission_item_reached(); diff --git a/src/modules/navigator/enginefailure.cpp b/src/modules/navigator/enginefailure.cpp index e007b208b2..b0130a1f5c 100644 --- a/src/modules/navigator/enginefailure.cpp +++ b/src/modules/navigator/enginefailure.cpp @@ -140,7 +140,7 @@ EngineFailure::advance_ef() { switch (_ef_state) { case EF_STATE_NONE: - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: Engine failure. Loitering down"); + mavlink_log_emergency(_navigator->get_mavlink_fd(), "Engine failure. Loitering down"); _ef_state = EF_STATE_LOITERDOWN; break; default: diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index efbc2689bd..31c4db61a5 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -146,13 +146,13 @@ bool Geofence::inside(double lat, double lon, float altitude) &dist_xy, &dist_z); if (max_vertical_distance > 0 && (dist_z > max_vertical_distance)) { - mavlink_log_critical(_mavlinkFd, "#audio: Geofence exceeded max vertical distance by %.0f m", + mavlink_log_critical(_mavlinkFd, "Geofence exceeded max vertical distance by %.0f m", (double)(dist_z - max_vertical_distance)); return false; } if (max_horizontal_distance > 0 && (dist_xy > max_horizontal_distance)) { - mavlink_log_critical(_mavlinkFd, "#audio: Geofence exceeded max horizontal distance by %.0f m", + mavlink_log_critical(_mavlinkFd, "Geofence exceeded max horizontal distance by %.0f m", (double)(dist_xy - max_horizontal_distance)); return false; } @@ -409,7 +409,7 @@ Geofence::loadFromFile(const char *filename) } else { warnx("Geofence: import error"); - mavlink_log_critical(_mavlinkFd, "#audio: Geofence import error"); + mavlink_log_critical(_mavlinkFd, "Geofence import error"); } error: diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp index e370796c07..0ca69f0f75 100644 --- a/src/modules/navigator/gpsfailure.cpp +++ b/src/modules/navigator/gpsfailure.cpp @@ -161,12 +161,12 @@ GpsFailure::advance_gpsf() case GPSF_STATE_NONE: _gpsf_state = GPSF_STATE_LOITER; warnx("gpsf loiter"); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: open loop loiter"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "GPS failed: open loop loiter"); break; case GPSF_STATE_LOITER: _gpsf_state = GPSF_STATE_TERMINATE; warnx("gpsf terminate"); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no gps recovery, termination"); + mavlink_log_emergency(_navigator->get_mavlink_fd(), "no gps recovery, termination"); warnx("mavlink sent"); break; case GPSF_STATE_TERMINATE: diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 52ccebac9f..dce7d45171 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -159,7 +159,7 @@ MissionBlock::is_mission_item_reached() _time_first_inside_orbit = now; // if (_mission_item.time_inside > 0.01f) { - // mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs", + // mavlink_log_critical(_mavlink_fd, "waypoint reached, wait for %.1fs", // (double)_mission_item.time_inside); // } } diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 389cdd0d2d..904f442389 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -121,7 +121,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss } if (!geofence.inside_polygon(missionitem.lat, missionitem.lon, missionitem.altitude)) { - mavlink_log_info(_mavlink_fd, "#audio: Geofence violation waypoint %d", i); + mavlink_log_critical(_mavlink_fd, "Geofence violation waypoint %d", i); return false; } } @@ -203,25 +203,25 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size return true; } else { /* Landing waypoint is above altitude of slope at the given waypoint distance */ - mavlink_log_info(_mavlink_fd, "#audio: Landing: last waypoint too high/too close"); - mavlink_log_info(_mavlink_fd, "Move down to %.1fm or move further away by %.1fm", + mavlink_log_critical(_mavlink_fd, "Landing: last waypoint too high/too close"); + mavlink_log_critical(_mavlink_fd, "Move down to %.1fm or move further away by %.1fm", (double)(slope_alt_req), (double)(wp_distance_req - wp_distance)); return false; } } else { /* Landing waypoint is above last waypoint */ - mavlink_log_info(_mavlink_fd, "#audio: Landing waypoint above last nav waypoint"); + mavlink_log_critical(_mavlink_fd, "Landing waypoint above last nav waypoint"); return false; } } else { /* Last wp is in flare region */ //xxx give recommendations - mavlink_log_info(_mavlink_fd, "#audio: Warning: Landing: last waypoint in flare region"); + mavlink_log_critical(_mavlink_fd, "Warning: Landing: last waypoint in flare region"); return false; } } else { - mavlink_log_info(_mavlink_fd, "#audio: Warning: starting with land waypoint"); + mavlink_log_critical(_mavlink_fd, "Warning: starting with land waypoint"); return false; } } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 05fac7b810..9691e26a8d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -409,7 +409,7 @@ Navigator::task_main() /* Issue a warning about the geofence violation once */ if (!_geofence_violation_warning_sent) { - mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation"); + mavlink_log_critical(_mavlink_fd, "Geofence violation"); _geofence_violation_warning_sent = true; } } else { diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index a7cde6325d..40bf7f0223 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -155,11 +155,11 @@ RCLoss::advance_rcl() case RCL_STATE_NONE: if (_param_loitertime.get() > 0.0f) { warnx("RC loss, OBC mode, loiter"); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, loitering"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "rc loss, loitering"); _rcl_state = RCL_STATE_LOITER; } else { warnx("RC loss, OBC mode, slip loiter, terminate"); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, terminating"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "rc loss, terminating"); _rcl_state = RCL_STATE_TERMINATE; _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->set_mission_result_updated(); @@ -169,7 +169,7 @@ RCLoss::advance_rcl() case RCL_STATE_LOITER: _rcl_state = RCL_STATE_TERMINATE; warnx("time is up, no RC regain, terminating"); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "RC not regained, terminating"); _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->set_mission_result_updated(); reset_mission_item_reached(); diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index b6c4b8fdff..c1ed8cb7c1 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -89,7 +89,7 @@ RTL::on_activation() /* for safety reasons don't go into RTL if landed */ if (_navigator->get_vstatus()->condition_landed) { _rtl_state = RTL_STATE_LANDED; - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "no RTL when landed"); /* if lower than return altitude, climb up first */ } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt @@ -146,7 +146,7 @@ RTL::set_rtl_item() _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: climb to %d meters above home", + mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: climb to %d meters above home", (int)(climb_alt - _navigator->get_home_position()->alt)); break; } @@ -177,7 +177,7 @@ RTL::set_rtl_item() _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: return at %d meters above home", + mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: return at %d meters above home", (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); break; } @@ -197,7 +197,7 @@ RTL::set_rtl_item() _mission_item.autocontinue = false; _mission_item.origin = ORIGIN_ONBOARD; - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home", + mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: descend to %d meters above home", (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); break; } @@ -222,10 +222,10 @@ RTL::set_rtl_item() _navigator->set_can_loiter_at_sp(true); if (autoland) { - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: loiter %.1fs", (double)_mission_item.time_inside); + mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: loiter %.1fs", (double)_mission_item.time_inside); } else { - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, loiter"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: completed, loiter"); } break; } @@ -245,7 +245,7 @@ RTL::set_rtl_item() _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: land at home"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: land at home"); break; } @@ -264,7 +264,7 @@ RTL::set_rtl_item() _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, landed"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: completed, landed"); break; } From 93eff2bb956e920b99b2058550fdbc3f23ec6155 Mon Sep 17 00:00:00 2001 From: philipoe Date: Tue, 7 Apr 2015 17:52:23 +0200 Subject: [PATCH 184/268] px4io firmware: Allow actuator update rates down to 25Hz. This allows to set the same update rate on PX4IO as on many commercial RC systems (e.g. Spektrum, which works at 45Hz servo update rate). --- src/modules/px4iofirmware/registers.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index f0c2cfd26d..49c5d99da0 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -543,8 +543,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; case PX4IO_P_SETUP_PWM_DEFAULTRATE: - if (value < 50) { - value = 50; + if (value < 25) { + value = 25; } if (value > 400) { value = 400; @@ -553,8 +553,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; case PX4IO_P_SETUP_PWM_ALTRATE: - if (value < 50) { - value = 50; + if (value < 25) { + value = 25; } if (value > 400) { value = 400; From c8452bb9d668237a84a22805e45feda07281a468 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 14 Apr 2015 19:17:30 -0700 Subject: [PATCH 185/268] Store parameter xml size This is needed in order to decompress in QGroundControl --- Tools/px_mkfw.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/px_mkfw.py b/Tools/px_mkfw.py index c2da8a2038..152444f84b 100755 --- a/Tools/px_mkfw.py +++ b/Tools/px_mkfw.py @@ -105,6 +105,7 @@ if args.git_identity != None: if args.parameter_xml != None: f = open(args.parameter_xml, "rb") bytes = f.read() + desc['parameter_xml_size'] = len(bytes) desc['parameter_xml'] = base64.b64encode(zlib.compress(bytes,9)).decode('utf-8') if args.image != None: f = open(args.image, "rb") From 3dc8cf87691d8376f380f7739a6fb43c1f529a1b Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 14 Apr 2015 23:26:14 -0700 Subject: [PATCH 186/268] Clean up parameter meta data as needed by QGC --- src/modules/commander/commander_params.c | 12 +-- .../fw_pos_control_l1/mtecs/mTecs_params.c | 88 +++++++++---------- src/modules/navigator/datalinkloss_params.c | 4 +- src/modules/navigator/navigator_params.c | 4 +- .../position_estimator_inav_params.c | 9 +- .../vtol_att_control_params.c | 6 +- 6 files changed, 61 insertions(+), 62 deletions(-) diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index a550122431..2682e5f591 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -139,8 +139,8 @@ PARAM_DEFINE_INT32(COM_DL_REG_T, 0); * Engine failure triggers only above this throttle value * * @group commander - * @min 0.0f - * @max 1.0f + * @min 0.0 + * @max 1.0 */ PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f); @@ -149,8 +149,8 @@ PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f); * Engine failure triggers only below this current/throttle value * * @group commander - * @min 0.0f - * @max 7.0f + * @min 0.0 + * @max 7.0 */ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f); @@ -161,8 +161,8 @@ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f); * * @group commander * @unit second - * @min 0.0f - * @max 7.0f + * @min 0.0 + * @max 7.0 */ PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f); diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index 58a1e9f6b1..fff506865a 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -157,8 +157,8 @@ PARAM_DEFINE_FLOAT(MT_THR_MAX, 1.0f); /** * Minimal Pitch Setpoint in Degrees * - * @min -90.0f - * @max 90.0f + * @min -90.0 + * @max 90.0 * @unit deg * @group mTECS */ @@ -167,8 +167,8 @@ PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f); /** * Maximal Pitch Setpoint in Degrees * - * @min -90.0f - * @max 90.0f + * @min -90.0 + * @max 90.0 * @unit deg * @group mTECS */ @@ -192,8 +192,8 @@ PARAM_DEFINE_FLOAT(MT_FPA_LP, 1.0f); * P gain for the altitude control * Maps the altitude error to the flight path angle setpoint * - * @min 0.0f - * @max 10.0f + * @min 0.0 + * @max 10.0 * @group mTECS */ PARAM_DEFINE_FLOAT(MT_FPA_P, 0.3f); @@ -202,8 +202,8 @@ PARAM_DEFINE_FLOAT(MT_FPA_P, 0.3f); * D gain for the altitude control * Maps the change of altitude error to the flight path angle setpoint * - * @min 0.0f - * @max 10.0f + * @min 0.0 + * @max 10.0 * @group mTECS */ PARAM_DEFINE_FLOAT(MT_FPA_D, 0.0f); @@ -219,8 +219,8 @@ PARAM_DEFINE_FLOAT(MT_FPA_D_LP, 1.0f); /** * Minimal flight path angle setpoint * - * @min -90.0f - * @max 90.0f + * @min -90.0 + * @max 90.0 * @unit deg * @group mTECS */ @@ -229,8 +229,8 @@ PARAM_DEFINE_FLOAT(MT_FPA_MIN, -20.0f); /** * Maximal flight path angle setpoint * - * @min -90.0f - * @max 90.0f + * @min -90.0 + * @max 90.0 * @unit deg * @group mTECS */ @@ -254,8 +254,8 @@ PARAM_DEFINE_FLOAT(MT_AD_LP, 0.5f); * P gain for the airspeed control * Maps the airspeed error to the acceleration setpoint * - * @min 0.0f - * @max 10.0f + * @min 0.0 + * @max 10.0 * @group mTECS */ PARAM_DEFINE_FLOAT(MT_ACC_P, 0.3f); @@ -264,8 +264,8 @@ PARAM_DEFINE_FLOAT(MT_ACC_P, 0.3f); * D gain for the airspeed control * Maps the change of airspeed error to the acceleration setpoint * - * @min 0.0f - * @max 10.0f + * @min 0.0 + * @max 10.0 * @group mTECS */ PARAM_DEFINE_FLOAT(MT_ACC_D, 0.0f); @@ -296,8 +296,8 @@ PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f); /** * Minimal throttle during takeoff * - * @min 0.0f - * @max 1.0f + * @min 0.0 + * @max 1.0 * @group mTECS */ PARAM_DEFINE_FLOAT(MT_TKF_THR_MIN, 1.0f); @@ -305,8 +305,8 @@ PARAM_DEFINE_FLOAT(MT_TKF_THR_MIN, 1.0f); /** * Maximal throttle during takeoff * - * @min 0.0f - * @max 1.0f + * @min 0.0 + * @max 1.0 * @group mTECS */ PARAM_DEFINE_FLOAT(MT_TKF_THR_MAX, 1.0f); @@ -314,8 +314,8 @@ PARAM_DEFINE_FLOAT(MT_TKF_THR_MAX, 1.0f); /** * Minimal pitch during takeoff * - * @min -90.0f - * @max 90.0f + * @min -90.0 + * @max 90.0 * @unit deg * @group mTECS */ @@ -324,8 +324,8 @@ PARAM_DEFINE_FLOAT(MT_TKF_PIT_MIN, 0.0f); /** * Maximal pitch during takeoff * - * @min -90.0f - * @max 90.0f + * @min -90.0 + * @max 90.0 * @unit deg * @group mTECS */ @@ -334,8 +334,8 @@ PARAM_DEFINE_FLOAT(MT_TKF_PIT_MAX, 45.0f); /** * Minimal throttle in underspeed mode * - * @min 0.0f - * @max 1.0f + * @min 0.0 + * @max 1.0 * @group mTECS */ PARAM_DEFINE_FLOAT(MT_USP_THR_MIN, 1.0f); @@ -343,8 +343,8 @@ PARAM_DEFINE_FLOAT(MT_USP_THR_MIN, 1.0f); /** * Maximal throttle in underspeed mode * - * @min 0.0f - * @max 1.0f + * @min 0.0 + * @max 1.0 * @group mTECS */ PARAM_DEFINE_FLOAT(MT_USP_THR_MAX, 1.0f); @@ -352,8 +352,8 @@ PARAM_DEFINE_FLOAT(MT_USP_THR_MAX, 1.0f); /** * Minimal pitch in underspeed mode * - * @min -90.0f - * @max 90.0f + * @min -90.0 + * @max 90.0 * @unit deg * @group mTECS */ @@ -362,8 +362,8 @@ PARAM_DEFINE_FLOAT(MT_USP_PIT_MIN, -45.0f); /** * Maximal pitch in underspeed mode * - * @min -90.0f - * @max 90.0f + * @min -90.0 + * @max 90.0 * @unit deg * @group mTECS */ @@ -372,8 +372,8 @@ PARAM_DEFINE_FLOAT(MT_USP_PIT_MAX, 0.0f); /** * Minimal throttle in landing mode (only last phase of landing) * - * @min 0.0f - * @max 1.0f + * @min 0.0 + * @max 1.0 * @group mTECS */ PARAM_DEFINE_FLOAT(MT_LND_THR_MIN, 0.0f); @@ -381,8 +381,8 @@ PARAM_DEFINE_FLOAT(MT_LND_THR_MIN, 0.0f); /** * Maximal throttle in landing mode (only last phase of landing) * - * @min 0.0f - * @max 1.0f + * @min 0.0 + * @max 1.0 * @group mTECS */ PARAM_DEFINE_FLOAT(MT_LND_THR_MAX, 0.0f); @@ -390,8 +390,8 @@ PARAM_DEFINE_FLOAT(MT_LND_THR_MAX, 0.0f); /** * Minimal pitch in landing mode * - * @min -90.0f - * @max 90.0f + * @min -90.0 + * @max 90.0 * @unit deg * @group mTECS */ @@ -400,8 +400,8 @@ PARAM_DEFINE_FLOAT(MT_LND_PIT_MIN, -5.0f); /** * Maximal pitch in landing mode * - * @min -90.0f - * @max 90.0f + * @min -90.0 + * @max 90.0 * @unit deg * @group mTECS */ @@ -410,8 +410,8 @@ PARAM_DEFINE_FLOAT(MT_LND_PIT_MAX, 15.0f); /** * Integrator Limit for Total Energy Rate Control * - * @min 0.0f - * @max 10.0f + * @min 0.0 + * @max 10.0 * @group mTECS */ PARAM_DEFINE_FLOAT(MT_THR_I_MAX, 10.0f); @@ -419,8 +419,8 @@ PARAM_DEFINE_FLOAT(MT_THR_I_MAX, 10.0f); /** * Integrator Limit for Energy Distribution Rate Control * - * @min 0.0f - * @max 10.0f + * @min 0.0 + * @max 10.0 * @group mTECS */ PARAM_DEFINE_FLOAT(MT_PIT_I_MAX, 10.0f); diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c index 6780c0c88c..4591ec38a7 100644 --- a/src/modules/navigator/datalinkloss_params.c +++ b/src/modules/navigator/datalinkloss_params.c @@ -64,7 +64,7 @@ PARAM_DEFINE_FLOAT(NAV_DLL_CH_T, 120.0f); * Latitude of comms hold waypoint * * @unit degrees * 1e7 - * @min 0.0 + * @min 0 * @group DLL */ PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, -266072120); @@ -75,7 +75,7 @@ PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, -266072120); * Longitude of comms hold waypoint * * @unit degrees * 1e7 - * @min 0.0 + * @min 0 * @group DLL */ PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890); diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 5f8f8d02f1..b15f4e7e74 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -94,7 +94,7 @@ PARAM_DEFINE_INT32(NAV_RCL_OBC, 0); * Latitude of airfield home waypoint * * @unit degrees * 1e7 - * @min 0.0 + * @min 0 * @group DLL */ PARAM_DEFINE_INT32(NAV_AH_LAT, -265847810); @@ -105,7 +105,7 @@ PARAM_DEFINE_INT32(NAV_AH_LAT, -265847810); * Longitude of airfield home waypoint * * @unit degrees * 1e7 - * @min 0.0 + * @min 0 * @group DLL */ PARAM_DEFINE_INT32(NAV_AH_LON, 1518423250); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index ed98e222ec..aedb478faa 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -282,8 +282,8 @@ PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f); * * Set to the appropriate key (328754) to disable vision input. * - * @min 0.0 - * @max 1.0 + * @min 0 + * @max 1 * @group Position Estimator INAV */ PARAM_DEFINE_INT32(CBRK_NO_VISION, 0); @@ -295,9 +295,8 @@ PARAM_DEFINE_INT32(CBRK_NO_VISION, 0); * the system uses the combined attitude / position * filter framework. * - * @min 0.0 - * @max 1.0 - * @unit s + * @min 0 + * @max 1 * @group Position Estimator INAV */ PARAM_DEFINE_INT32(INAV_ENABLED, 1); diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 33752b2c43..6da28b1304 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -43,7 +43,7 @@ /** * VTOL number of engines * - * @min 1.0 + * @min 1 * @group VTOL Attitude Control */ PARAM_DEFINE_INT32(VT_MOT_COUNT,0); @@ -92,8 +92,8 @@ PARAM_DEFINE_FLOAT(VT_MC_ARSPD_TRIM,10.0f); * If set to one this parameter will cause permanent attitude stabilization in fw mode. * This parameter has been introduced for pure convenience sake. * - * @min 0.0 - * @max 1.0 + * @min 0 + * @max 1 * @group VTOL Attitude Control */ PARAM_DEFINE_INT32(VT_FW_PERM_STAB,0); From 70246c30c4a7b8f45b0fa88b70802444bc2a222d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 15 Apr 2015 16:49:03 +0200 Subject: [PATCH 187/268] MAVLink: Updated library version --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 7ae438b86e..9682e3ad72 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 7ae438b86ea983e95af5f092e45a5d0f9d093c74 +Subproject commit 9682e3ad72fb166f63be38dc48c75997b35a951a From d84ac41cd355f6c907e0a6e71a1ee12536bfb95e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 15 Apr 2015 18:55:01 +0200 Subject: [PATCH 188/268] MAVLink: Update submodule to fix compile error --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 9682e3ad72..475e7cc434 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 9682e3ad72fb166f63be38dc48c75997b35a951a +Subproject commit 475e7cc4348b207ac3efed45eb61160d23ac7a26 From 05c351183f0425f1c4feba60bf68b28d73eee51f Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 15 Apr 2015 11:29:37 -0700 Subject: [PATCH 189/268] Parameter meta data fixes --- Tools/px4params/srcparser.py | 35 +++++++++- .../attitude_estimator_ekf_params.c | 22 +++--- src/modules/commander/commander_params.c | 16 ++--- src/modules/mavlink/mavlink.c | 4 ++ src/modules/navigator/datalinkloss_params.c | 14 ++-- src/modules/navigator/gpsfailure_params.c | 8 +-- src/modules/navigator/navigator_params.c | 6 +- src/modules/navigator/rcloss_params.c | 2 +- src/modules/navigator/rtl_params.c | 8 +-- src/modules/sensors/sensor_params.c | 69 +++++++++++-------- 10 files changed, 116 insertions(+), 68 deletions(-) diff --git a/Tools/px4params/srcparser.py b/Tools/px4params/srcparser.py index 8e60921951..891db7ecd2 100644 --- a/Tools/px4params/srcparser.py +++ b/Tools/px4params/srcparser.py @@ -83,6 +83,7 @@ class SourceParser(object): re_comment_tag = re.compile(r'@([a-zA-Z][a-zA-Z0-9_]*)\s*(.*)') re_comment_end = re.compile(r'(.*?)\s*\*\/') re_parameter_definition = re.compile(r'PARAM_DEFINE_([A-Z_][A-Z0-9_]*)\s*\(([A-Z_][A-Z0-9_]*)\s*,\s*([^ ,\)]+)\s*\)\s*;') + re_px4_parameter_definition = re.compile(r'PX4_PARAM_DEFINE_([A-Z_][A-Z0-9_]*)\s*\(([A-Z_][A-Z0-9_]*)\s*\)\s*;') re_cut_type_specifier = re.compile(r'[a-z]+$') re_is_a_number = re.compile(r'^-?[0-9\.]') re_remove_dots = re.compile(r'\.+$') @@ -206,8 +207,38 @@ class SourceParser(object): if group not in self.param_groups: self.param_groups[group] = ParameterGroup(group) self.param_groups[group].AddParameter(param) - # Reset parsed comment. - state = None + else: + # Nasty code dup, but this will all go away soon, so quick and dirty (DonLakeFlyer) + m = self.re_px4_parameter_definition.match(line) + if m: + tp, code = m.group(1, 2) + param = Parameter() + param.SetField("code", code) + param.SetField("short_desc", code) + param.SetField("type", tp) + # If comment was found before the parameter declaration, + # inject its data into the newly created parameter. + group = "Miscellaneous" + if state == "comment-processed": + if short_desc is not None: + param.SetField("short_desc", + self.re_remove_dots.sub('', short_desc)) + if long_desc is not None: + param.SetField("long_desc", long_desc) + for tag in tags: + if tag == "group": + group = tags[tag] + elif tag not in self.valid_tags: + sys.stderr.write("Skipping invalid " + "documentation tag: '%s'\n" % tag) + else: + param.SetField(tag, tags[tag]) + # Store the parameter + if group not in self.param_groups: + self.param_groups[group] = ParameterGroup(group) + self.param_groups[group].AddParameter(param) + # Reset parsed comment. + state = None def GetParamGroups(self): """ diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index e143f37b9b..fe480e12b7 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -48,49 +48,49 @@ /** * Body angular rate process noise * - * @group attitude_ekf + * @group Attitude EKF estimator */ PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f); /** * Body angular acceleration process noise * - * @group attitude_ekf + * @group Attitude EKF estimator */ PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f); /** * Acceleration process noise * - * @group attitude_ekf + * @group Attitude EKF estimator */ PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f); /** * Magnet field vector process noise * - * @group attitude_ekf + * @group Attitude EKF estimator */ PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f); /** * Gyro measurement noise * - * @group attitude_ekf + * @group Attitude EKF estimator */ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f); /** * Accel measurement noise * - * @group attitude_ekf + * @group Attitude EKF estimator */ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f); /** * Mag measurement noise * - * @group attitude_ekf + * @group Attitude EKF estimator */ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f); @@ -102,7 +102,7 @@ PARAM_DEFINE_INT32(ATT_ACC_COMP, 2); /** * Moment of inertia matrix diagonal entry (1, 1) * - * @group attitude_ekf + * @group Attitude EKF estimator * @unit kg*m^2 */ PARAM_DEFINE_FLOAT(ATT_J11, 0.0018); @@ -110,7 +110,7 @@ PARAM_DEFINE_FLOAT(ATT_J11, 0.0018); /** * Moment of inertia matrix diagonal entry (2, 2) * - * @group attitude_ekf + * @group Attitude EKF estimator * @unit kg*m^2 */ PARAM_DEFINE_FLOAT(ATT_J22, 0.0018); @@ -118,7 +118,7 @@ PARAM_DEFINE_FLOAT(ATT_J22, 0.0018); /** * Moment of inertia matrix diagonal entry (3, 3) * - * @group attitude_ekf + * @group Attitude EKF estimator * @unit kg*m^2 */ PARAM_DEFINE_FLOAT(ATT_J33, 0.0037); @@ -128,7 +128,7 @@ PARAM_DEFINE_FLOAT(ATT_J33, 0.0037); * * If set to != 0 the moment of inertia will be used in the estimator * - * @group attitude_ekf + * @group Attitude EKF estimator * @min 0 * @max 1 */ diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 2682e5f591..6663525cc4 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -105,7 +105,7 @@ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); * * Set to 1 to enable actions triggered when the datalink is lost. * - * @group commander + * @group Commander * @min 0 * @max 1 */ @@ -115,7 +115,7 @@ PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0); * * After this amount of seconds without datalink the data link lost mode triggers * - * @group commander + * @group Commander * @unit second * @min 0 * @max 30 @@ -127,7 +127,7 @@ PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10); * After a data link loss: after this this amount of seconds with a healthy datalink the 'datalink loss' * flag is set back to false * - * @group commander + * @group Commander * @unit second * @min 0 * @max 30 @@ -138,7 +138,7 @@ PARAM_DEFINE_INT32(COM_DL_REG_T, 0); * * Engine failure triggers only above this throttle value * - * @group commander + * @group Commander * @min 0.0 * @max 1.0 */ @@ -148,7 +148,7 @@ PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f); * * Engine failure triggers only below this current/throttle value * - * @group commander + * @group Commander * @min 0.0 * @max 7.0 */ @@ -159,7 +159,7 @@ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f); * Engine failure triggers only if the throttle threshold and the * current to throttle threshold are violated for this time * - * @group commander + * @group Commander * @unit second * @min 0.0 * @max 7.0 @@ -170,7 +170,7 @@ PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f); * * After this amount of seconds without RC connection the rc lost flag is set to true * - * @group commander + * @group Commander * @unit second * @min 0 * @max 35 @@ -183,7 +183,7 @@ PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5); * Default is on, as the interoperability with currently deployed GCS solutions depends on parameters * being sticky. Developers can default it to off. * - * @group commander + * @group Commander * @min 0 * @max 1 */ diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 9afe74af1c..796d5cbf28 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -64,14 +64,18 @@ PARAM_DEFINE_INT32(MAV_COMP_ID, 50); PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING); /** * Use/Accept HIL GPS message (even if not in HIL mode) + * * If set to 1 incomming HIL GPS messages are parsed. + * * @group MAVLink */ PARAM_DEFINE_INT32(MAV_USEHILGPS, 0); /** * Forward external setpoint messages + * * If set to 1 incomming external setpoint messages will be directly forwarded to the controllers if in offboard * control mode + * * @group MAVLink */ PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1); diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c index 4591ec38a7..9abc012cf2 100644 --- a/src/modules/navigator/datalinkloss_params.c +++ b/src/modules/navigator/datalinkloss_params.c @@ -54,7 +54,7 @@ * * @unit seconds * @min 0.0 - * @group DLL + * @group Data Link Loss */ PARAM_DEFINE_FLOAT(NAV_DLL_CH_T, 120.0f); @@ -65,7 +65,7 @@ PARAM_DEFINE_FLOAT(NAV_DLL_CH_T, 120.0f); * * @unit degrees * 1e7 * @min 0 - * @group DLL + * @group Data Link Loss */ PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, -266072120); @@ -76,7 +76,7 @@ PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, -266072120); * * @unit degrees * 1e7 * @min 0 - * @group DLL + * @group Data Link Loss */ PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890); @@ -87,7 +87,7 @@ PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890); * * @unit m * @min 0.0 - * @group DLL + * @group Data Link Loss */ PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f); @@ -98,7 +98,7 @@ PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f); * * @unit seconds * @min 0.0 - * @group DLL + * @group Data Link Loss */ PARAM_DEFINE_FLOAT(NAV_DLL_AH_T, 120.0f); @@ -107,7 +107,7 @@ PARAM_DEFINE_FLOAT(NAV_DLL_AH_T, 120.0f); * * After more than this number of data link timeouts the aircraft returns home directly * - * @group DLL + * @group Data Link Loss * @min 0 * @max 1000 */ @@ -119,7 +119,7 @@ PARAM_DEFINE_INT32(NAV_DLL_N, 2); * If set to 1 the system will skip the comms hold wp on data link loss and will directly fly to * airfield home * - * @group DLL + * @group Data Link Loss * @min 0 * @max 1 */ diff --git a/src/modules/navigator/gpsfailure_params.c b/src/modules/navigator/gpsfailure_params.c index 39d179eed7..98104af29e 100644 --- a/src/modules/navigator/gpsfailure_params.c +++ b/src/modules/navigator/gpsfailure_params.c @@ -55,7 +55,7 @@ * * @unit seconds * @min 0.0 - * @group GPSF + * @group GPS Failure Navigation */ PARAM_DEFINE_FLOAT(NAV_GPSF_LT, 30.0f); @@ -67,7 +67,7 @@ PARAM_DEFINE_FLOAT(NAV_GPSF_LT, 30.0f); * @unit deg * @min 0.0 * @max 30.0 - * @group GPSF + * @group GPS Failure Navigation */ PARAM_DEFINE_FLOAT(NAV_GPSF_R, 15.0f); @@ -79,7 +79,7 @@ PARAM_DEFINE_FLOAT(NAV_GPSF_R, 15.0f); * @unit deg * @min -30.0 * @max 30.0 - * @group GPSF + * @group GPS Failure Navigation */ PARAM_DEFINE_FLOAT(NAV_GPSF_P, 0.0f); @@ -90,7 +90,7 @@ PARAM_DEFINE_FLOAT(NAV_GPSF_P, 0.0f); * * @min 0.0 * @max 1.0 - * @group GPSF + * @group GPS Failure Navigation */ PARAM_DEFINE_FLOAT(NAV_GPSF_TR, 0.7f); diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index b15f4e7e74..ef4a8dc0c7 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -95,7 +95,7 @@ PARAM_DEFINE_INT32(NAV_RCL_OBC, 0); * * @unit degrees * 1e7 * @min 0 - * @group DLL + * @group Data Link Loss */ PARAM_DEFINE_INT32(NAV_AH_LAT, -265847810); @@ -106,7 +106,7 @@ PARAM_DEFINE_INT32(NAV_AH_LAT, -265847810); * * @unit degrees * 1e7 * @min 0 - * @group DLL + * @group Data Link Loss */ PARAM_DEFINE_INT32(NAV_AH_LON, 1518423250); @@ -117,6 +117,6 @@ PARAM_DEFINE_INT32(NAV_AH_LON, 1518423250); * * @unit m * @min 0.0 - * @group DLL + * @group Data Link Loss */ PARAM_DEFINE_FLOAT(NAV_AH_ALT, 600.0f); diff --git a/src/modules/navigator/rcloss_params.c b/src/modules/navigator/rcloss_params.c index f1a01c73b5..f48aabc808 100644 --- a/src/modules/navigator/rcloss_params.c +++ b/src/modules/navigator/rcloss_params.c @@ -55,6 +55,6 @@ * * @unit seconds * @min -1.0 - * @group RCL + * @group Radio Signal Loss */ PARAM_DEFINE_FLOAT(NAV_RCL_LT, 120.0f); diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index 10394fed18..7800a6f03f 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -55,7 +55,7 @@ * @unit meters * @min 20 * @max 200 - * @group RTL + * @group Return To Land */ PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f); @@ -67,7 +67,7 @@ PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f); * @unit meters * @min 0 * @max 150 - * @group RTL + * @group Return To Land */ PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 60); @@ -81,7 +81,7 @@ PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 60); * @unit meters * @min 2 * @max 100 - * @group RTL + * @group Return To Land */ PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 30); @@ -94,6 +94,6 @@ PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 30); * @unit seconds * @min -1 * @max 300 - * @group RTL + * @group Return To Land */ PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f); diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 1acc14fc6c..18e47865b1 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -1099,7 +1099,7 @@ PARAM_DEFINE_INT32(RC_MAP_YAW, 4); * * @min 0 * @max 18 - * @group Radio Calibration + * @group Radio Switches */ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0); @@ -1108,7 +1108,7 @@ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0); * * @min 0 * @max 18 - * @group Radio Calibration + * @group Radio Switches */ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); @@ -1117,7 +1117,7 @@ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); * * @min 0 * @max 18 - * @group Radio Calibration + * @group Radio Switches */ PARAM_DEFINE_INT32(RC_MAP_POSCTL_SW, 0); @@ -1126,7 +1126,7 @@ PARAM_DEFINE_INT32(RC_MAP_POSCTL_SW, 0); * * @min 0 * @max 18 - * @group Radio Calibration + * @group Radio Switches */ PARAM_DEFINE_INT32(RC_MAP_LOITER_SW, 0); @@ -1135,7 +1135,7 @@ PARAM_DEFINE_INT32(RC_MAP_LOITER_SW, 0); * * @min 0 * @max 18 - * @group Radio Calibration + * @group Radio Switches */ PARAM_DEFINE_INT32(RC_MAP_ACRO_SW, 0); @@ -1144,7 +1144,7 @@ PARAM_DEFINE_INT32(RC_MAP_ACRO_SW, 0); * * @min 0 * @max 18 - * @group Radio Calibration + * @group Radio Switches */ PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); @@ -1153,7 +1153,7 @@ PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); * * @min 0 * @max 18 - * @group Radio Calibration + * @group Radio Switches */ PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0); @@ -1238,9 +1238,6 @@ PARAM_DEFINE_INT32(RC_FAILS_THR, 0); /** * Threshold for selecting assist mode * - * min:-1 - * max:+1 - * * 0-1 indicate where in the full channel range the threshold sits * 0 : min * 1 : max @@ -1248,15 +1245,17 @@ PARAM_DEFINE_INT32(RC_FAILS_THR, 0); * positive : true when channel>th * negative : true when channelth * negative : true when channelth * negative : true when channelth * negative : true when channelth * negative : true when channelth * negative : true when channelth * negative : true when channel Date: Wed, 15 Apr 2015 21:15:19 +0200 Subject: [PATCH 190/268] Fix #1789 - takeoff sideways on beginning of missions --- src/modules/navigator/mission.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index a94082d6f0..c1763ba93d 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -449,7 +449,7 @@ Mission::set_mission_items() } /* check if we already above takeoff altitude */ - if (_navigator->get_global_position()->alt < takeoff_alt - _navigator->get_acceptance_radius()) { + if (_navigator->get_global_position()->alt < takeoff_alt) { mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); _mission_item.nav_cmd = NAV_CMD_TAKEOFF; From 2fa0c333cb92557b5ba39e91db41327ae381b6a7 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 15 Apr 2015 14:53:06 -0700 Subject: [PATCH 191/268] Add version number to parameter meta data --- Tools/px4params/xmlout.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/Tools/px4params/xmlout.py b/Tools/px4params/xmlout.py index e845cd1b10..ee95f9b673 100644 --- a/Tools/px4params/xmlout.py +++ b/Tools/px4params/xmlout.py @@ -6,6 +6,10 @@ class XMLOutput(): impl = getDOMImplementation() xml_document = impl.createDocument(None, "parameters", None) xml_parameters = xml_document.documentElement + xml_version = xml_document.createElement("version") + xml_parameters.appendChild(xml_version) + xml_version_value = xml_document.createTextNode("1") + xml_version.appendChild(xml_version_value) for group in groups: xml_group = xml_document.createElement("group") xml_group.setAttribute("name", group.GetName()) From 202cfd21d04f9d8ec9fec3b921f6b4d85df5560d Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Thu, 16 Apr 2015 10:21:18 -0700 Subject: [PATCH 192/268] Change to V2 spec of param meta data Had to switch to ElementTree to get attribute support --- Tools/px4params/xmlout.py | 54 +++++++++++++++++++++++++-------------- 1 file changed, 35 insertions(+), 19 deletions(-) diff --git a/Tools/px4params/xmlout.py b/Tools/px4params/xmlout.py index ee95f9b673..89f495dc02 100644 --- a/Tools/px4params/xmlout.py +++ b/Tools/px4params/xmlout.py @@ -1,30 +1,46 @@ -from xml.dom.minidom import getDOMImplementation +import xml.etree.ElementTree as ET import codecs +def indent(elem, level=0): + i = "\n" + level*" " + if len(elem): + if not elem.text or not elem.text.strip(): + elem.text = i + " " + if not elem.tail or not elem.tail.strip(): + elem.tail = i + for elem in elem: + indent(elem, level+1) + if not elem.tail or not elem.tail.strip(): + elem.tail = i + else: + if level and (not elem.tail or not elem.tail.strip()): + elem.tail = i + class XMLOutput(): + def __init__(self, groups): - impl = getDOMImplementation() - xml_document = impl.createDocument(None, "parameters", None) - xml_parameters = xml_document.documentElement - xml_version = xml_document.createElement("version") - xml_parameters.appendChild(xml_version) - xml_version_value = xml_document.createTextNode("1") - xml_version.appendChild(xml_version_value) + xml_parameters = ET.Element("parameters") + xml_version = ET.SubElement(xml_parameters, "version") + xml_version.text = "2" for group in groups: - xml_group = xml_document.createElement("group") - xml_group.setAttribute("name", group.GetName()) - xml_parameters.appendChild(xml_group) + xml_group = ET.SubElement(xml_parameters, "group") + xml_group.attrib["name"] = group.GetName() for param in group.GetParams(): - xml_param = xml_document.createElement("parameter") - xml_group.appendChild(xml_param) + xml_param = ET.SubElement(xml_group, "parameter") for code in param.GetFieldCodes(): value = param.GetFieldValue(code) - xml_field = xml_document.createElement(code) - xml_param.appendChild(xml_field) - xml_value = xml_document.createTextNode(value) - xml_field.appendChild(xml_value) - self.xml_document = xml_document + if code == "code": + xml_param.attrib["name"] = value + elif code == "default": + xml_param.attrib["default"] = value + elif code == "type": + xml_param.attrib["type"] = value + else: + xml_field = ET.SubElement(xml_param, code) + xml_field.text = value + indent(xml_parameters) + self.xml_document = ET.ElementTree(xml_parameters) def Save(self, filename): with codecs.open(filename, 'w', 'utf-8') as f: - self.xml_document.writexml(f, indent=" ", addindent=" ", newl="\n") + self.xml_document.write(f) From 3ee9b441c62dc0ab86816e12d3af2cb9956221d3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 16 Apr 2015 22:50:24 +0200 Subject: [PATCH 193/268] Add STM32F4 discovery config. --- Images/px4-stm32f4discovery.prototype | 12 + makefiles/board_px4-stm32f4discovery.mk | 11 + .../config_px4-stm32f4discovery_default.mk | 92 ++ makefiles/upload.mk | 3 +- .../px4-stm32f4discovery/include/board.h | 273 ++++++ .../include/nsh_romfsimg.h | 42 + .../px4-stm32f4discovery/nsh/Make.defs | 179 ++++ .../px4-stm32f4discovery/nsh/appconfig | 48 + .../px4-stm32f4discovery/nsh/defconfig | 897 ++++++++++++++++++ .../px4-stm32f4discovery/nsh/setenv.sh | 67 ++ .../px4-stm32f4discovery/scripts/ld.script | 149 +++ .../px4-stm32f4discovery/src/Makefile | 84 ++ .../px4-stm32f4discovery/src/empty.c | 4 + .../px4-stm32f4discovery/board_config.h | 136 +++ .../boards/px4-stm32f4discovery/module.mk | 7 + .../px4-stm32f4discovery/px4discovery_init.c | 173 ++++ .../px4-stm32f4discovery/px4discovery_led.c | 97 ++ .../px4-stm32f4discovery/px4discovery_usb.c | 108 +++ src/drivers/drv_gpio.h | 6 +- src/lib/version/version.h | 4 + src/systemcmds/tests/test_gpio.c | 9 +- 21 files changed, 2397 insertions(+), 4 deletions(-) create mode 100644 Images/px4-stm32f4discovery.prototype create mode 100644 makefiles/board_px4-stm32f4discovery.mk create mode 100644 makefiles/config_px4-stm32f4discovery_default.mk create mode 100644 nuttx-configs/px4-stm32f4discovery/include/board.h create mode 100644 nuttx-configs/px4-stm32f4discovery/include/nsh_romfsimg.h create mode 100644 nuttx-configs/px4-stm32f4discovery/nsh/Make.defs create mode 100644 nuttx-configs/px4-stm32f4discovery/nsh/appconfig create mode 100644 nuttx-configs/px4-stm32f4discovery/nsh/defconfig create mode 100755 nuttx-configs/px4-stm32f4discovery/nsh/setenv.sh create mode 100644 nuttx-configs/px4-stm32f4discovery/scripts/ld.script create mode 100644 nuttx-configs/px4-stm32f4discovery/src/Makefile create mode 100644 nuttx-configs/px4-stm32f4discovery/src/empty.c create mode 100644 src/drivers/boards/px4-stm32f4discovery/board_config.h create mode 100644 src/drivers/boards/px4-stm32f4discovery/module.mk create mode 100644 src/drivers/boards/px4-stm32f4discovery/px4discovery_init.c create mode 100644 src/drivers/boards/px4-stm32f4discovery/px4discovery_led.c create mode 100644 src/drivers/boards/px4-stm32f4discovery/px4discovery_usb.c diff --git a/Images/px4-stm32f4discovery.prototype b/Images/px4-stm32f4discovery.prototype new file mode 100644 index 0000000000..9250691f77 --- /dev/null +++ b/Images/px4-stm32f4discovery.prototype @@ -0,0 +1,12 @@ +{ + "board_id": 99, + "magic": "PX4FWv1", + "description": "Firmware for the STM32F4Discovery board", + "image": "", + "build_time": 0, + "summary": "PX4/STM32F4Discovery", + "version": "0.1", + "image_size": 0, + "git_identity": "", + "board_revision": 0 +} diff --git a/makefiles/board_px4-stm32f4discovery.mk b/makefiles/board_px4-stm32f4discovery.mk new file mode 100644 index 0000000000..fe761ba444 --- /dev/null +++ b/makefiles/board_px4-stm32f4discovery.mk @@ -0,0 +1,11 @@ +# +# Board-specific definitions for the PX4_STM32F4DISCOVERY +# + +# +# Configure the toolchain +# +CONFIG_ARCH = CORTEXM4F +CONFIG_BOARD = PX4_STM32F4DISCOVERY + +include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk diff --git a/makefiles/config_px4-stm32f4discovery_default.mk b/makefiles/config_px4-stm32f4discovery_default.mk new file mode 100644 index 0000000000..8f73a7f047 --- /dev/null +++ b/makefiles/config_px4-stm32f4discovery_default.mk @@ -0,0 +1,92 @@ +# +# Makefile for the px4fmu_default configuration +# + +# +# Use the configuration's ROMFS, copy the PX4_STM32F4DISCOVERY firmware into +# the ROMFS if it's available +# +ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common +ROMFS_OPTIONAL_FILES = + +# +# Board support modules +# +MODULES += drivers/device +MODULES += drivers/stm32 +MODULES += drivers/led +MODULES += drivers/boards/px4-stm32f4discovery + +# +# System commands +# +MODULES += systemcmds/bl_update +MODULES += systemcmds/mixer +MODULES += systemcmds/param +MODULES += systemcmds/perf +MODULES += systemcmds/reboot +MODULES += systemcmds/top +MODULES += systemcmds/tests +MODULES += systemcmds/config +MODULES += systemcmds/nshterm +MODULES += systemcmds/ver + +# +# Library modules +# +MODULES += modules/systemlib +MODULES += modules/systemlib/mixer +MODULES += modules/controllib +MODULES += modules/uORB + +# +# Libraries +# +LIBRARIES += lib/mathlib/CMSIS +MODULES += lib/mathlib +MODULES += lib/mathlib/math/filter +MODULES += lib/ecl +MODULES += lib/external_lgpl +MODULES += lib/geo +MODULES += lib/conversion +MODULES += platforms/nuttx + +# +# Demo apps +# +#MODULES += examples/math_demo +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/hello_sky +MODULES += examples/px4_simple_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/daemon +#MODULES += examples/px4_daemon_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/debug_values +#MODULES += examples/px4_mavlink_debug + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control +#MODULES += examples/fixedwing_control + +# Hardware test +#MODULES += examples/hwtest + +# +# Transitional support - add commands from the NuttX export archive. +# +# In general, these should move to modules over time. +# +# Each entry here is ... but we use a helper macro +# to make the table a bit more readable. +# +define _B + $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) +endef + +# command priority stack entrypoint +BUILTIN_COMMANDS := \ + $(call _B, sercon, , 2048, sercon_main ) \ + $(call _B, serdis, , 2048, serdis_main ) diff --git a/makefiles/upload.mk b/makefiles/upload.mk index 29b415688e..dd7710bf72 100644 --- a/makefiles/upload.mk +++ b/makefiles/upload.mk @@ -33,7 +33,8 @@ upload-serial-px4fmu-v2: $(BUNDLE) $(UPLOADER) upload-serial-aerocore: openocd -f $(PX4_BASE)/makefiles/gumstix-aerocore.cfg -c 'init; reset halt; flash write_image erase $(PX4_BASE)/../Bootloader/px4aerocore_bl.bin 0x08000000; flash write_image erase $(PX4_BASE)/Build/aerocore_default.build/firmware.bin 0x08004000; reset run; exit' - +upload-serial-px4-stm32f4discovery: $(BUNDLE) $(UPLOADER) + $(Q) $(PYTHON) -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE) # # JTAG firmware uploading with OpenOCD diff --git a/nuttx-configs/px4-stm32f4discovery/include/board.h b/nuttx-configs/px4-stm32f4discovery/include/board.h new file mode 100644 index 0000000000..0412c3aa6d --- /dev/null +++ b/nuttx-configs/px4-stm32f4discovery/include/board.h @@ -0,0 +1,273 @@ +/************************************************************************************ + * configs/stm32f4discovery/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************************************/ + +#ifndef __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H +#define __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#ifndef __ASSEMBLY__ +# include +#endif + +#include "stm32_rcc.h" +#include "stm32_sdio.h" +#include "stm32.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The STM32F4 Discovery board features a single 8MHz crystal. Space is provided + * for a 32kHz RTC backup crystal, but it is not stuffed. + * + * This is the canonical configuration: + * System Clock source : PLL (HSE) + * SYSCLK(Hz) : 168000000 Determined by PLL configuration + * HCLK(Hz) : 168000000 (STM32_RCC_CFGR_HPRE) + * AHB Prescaler : 1 (STM32_RCC_CFGR_HPRE) + * APB1 Prescaler : 4 (STM32_RCC_CFGR_PPRE1) + * APB2 Prescaler : 2 (STM32_RCC_CFGR_PPRE2) + * HSE Frequency(Hz) : 8000000 (STM32_BOARD_XTAL) + * PLLM : 8 (STM32_PLLCFG_PLLM) + * PLLN : 336 (STM32_PLLCFG_PLLN) + * PLLP : 2 (STM32_PLLCFG_PLLP) + * PLLQ : 7 (STM32_PLLCFG_PLLQ) + * Main regulator output voltage : Scale1 mode Needed for high speed SYSCLK + * Flash Latency(WS) : 5 + * Prefetch Buffer : OFF + * Instruction cache : ON + * Data cache : ON + * Require 48MHz for USB OTG FS, : Enabled + * SDIO and RNG clock + */ + +/* HSI - 16 MHz RC factory-trimmed + * LSI - 32 KHz RC + * HSE - On-board crystal frequency is 8MHz + * LSE - 32.768 kHz + */ + +#define STM32_BOARD_XTAL 8000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL +#define STM32_LSE_FREQUENCY 32768 + +/* Main PLL Configuration. + * + * PLL source is HSE + * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * = (8,000,000 / 8) * 336 + * = 336,000,000 + * SYSCLK = PLL_VCO / PLLP + * = 336,000,000 / 2 = 168,000,000 + * USB OTG FS, SDIO and RNG Clock + * = PLL_VCO / PLLQ + * = 48,000,000 + */ + +#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(8) +#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(336) +#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_2 +#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(7) + +#define STM32_SYSCLK_FREQUENCY 168000000ul + +/* AHB clock (HCLK) is SYSCLK (168MHz) */ + +#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK /* HCLK = SYSCLK / 1 */ +#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/4 (42MHz) */ + +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd4 /* PCLK1 = HCLK / 4 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/4) + +/* Timers driven from APB1 will be twice PCLK1 */ + +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* APB2 clock (PCLK2) is HCLK/2 (84MHz) */ + +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timers driven from APB2 will be twice PCLK2 */ + +#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM9_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY) + +/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx + * otherwise frequency is 2xAPBx. + * Note: TIM1,8 are on APB2, others on APB1 + */ + +#define STM32_TIM18_FREQUENCY (2*STM32_PCLK2_FREQUENCY) +#define STM32_TIM27_FREQUENCY (2*STM32_PCLK1_FREQUENCY) + +/* LED definitions ******************************************************************/ +/* If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any + * way. The following definitions are used to access individual LEDs. + */ + +/* LED index values for use with stm32_setled() */ + +#define BOARD_LED1 0 +#define BOARD_LED2 1 +#define BOARD_NLEDS 2 + +#define BOARD_LED_BLUE BOARD_LED1 +#define BOARD_LED_RED BOARD_LED2 + +/* LED bits for use with stm32_setleds() */ + +#define BOARD_LED1_BIT (1 << BOARD_LED1) +#define BOARD_LED2_BIT (1 << BOARD_LED2) + +/* If CONFIG_ARCH_LEDs is defined, then NuttX will control the 2 LEDs on board the + * px4fmu-v1. The following definitions describe how NuttX controls the LEDs: + */ + +#define LED_STARTED 0 /* LED1 */ +#define LED_HEAPALLOCATE 1 /* LED2 */ +#define LED_IRQSENABLED 2 /* LED1 */ +#define LED_STACKCREATED 3 /* LED1 + LED2 */ +#define LED_INIRQ 4 /* LED1 */ +#define LED_SIGNAL 5 /* LED2 */ +#define LED_ASSERTION 6 /* LED1 + LED2 */ +#define LED_PANIC 7 /* LED1 + LED2 */ + +/* Alternate function pin selections ************************************************/ + +/* UART2: + * + * The STM32F4 Discovery has no on-board serial devices, but the console is + * brought out to PA2 (TX) and PA3 (RX) for connection to an external serial device. + * (See the README.txt file for other options) + */ + +#define GPIO_USART2_RX GPIO_USART2_RX_1 +#define GPIO_USART2_TX GPIO_USART2_TX_1 + +/* SPI - There is a MEMS device on SPI1 using these pins: */ + +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 +#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1 + +/* + * I2C + * + * The optional _GPIO configurations allow the I2C driver to manually + * reset the bus to clear stuck slaves. They match the pin configuration, + * but are normally-high GPIOs. + */ +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_1 +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 +#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN6) +#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9) + +/************************************************************************************ + * Public Data + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/************************************************************************************ + * Public Function Prototypes + ************************************************************************************/ +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +EXTERN void stm32_boardinitialize(void); + +/************************************************************************************ + * Name: stm32_ledinit, stm32_setled, and stm32_setleds + * + * Description: + * If CONFIG_ARCH_LEDS is defined, then NuttX will control the on-board LEDs. If + * CONFIG_ARCH_LEDS is not defined, then the following interfacesare available to + * control the LEDs from user applications. + * + ************************************************************************************/ + +#ifndef CONFIG_ARCH_LEDS +EXTERN void stm32_ledinit(void); +EXTERN void stm32_setled(int led, bool ledon); +EXTERN void stm32_setleds(uint8_t ledset); +#endif + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H */ diff --git a/nuttx-configs/px4-stm32f4discovery/include/nsh_romfsimg.h b/nuttx-configs/px4-stm32f4discovery/include/nsh_romfsimg.h new file mode 100644 index 0000000000..15e4e7a8d5 --- /dev/null +++ b/nuttx-configs/px4-stm32f4discovery/include/nsh_romfsimg.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * nsh_romfsetc.h + * + * This file is a stub for 'make export' purposes; the actual ROMFS + * must be supplied by the library client. + */ + +extern unsigned char romfs_img[]; +extern unsigned int romfs_img_len; diff --git a/nuttx-configs/px4-stm32f4discovery/nsh/Make.defs b/nuttx-configs/px4-stm32f4discovery/nsh/Make.defs new file mode 100644 index 0000000000..e70320aaa4 --- /dev/null +++ b/nuttx-configs/px4-stm32f4discovery/nsh/Make.defs @@ -0,0 +1,179 @@ +############################################################################ +# configs/px4fmu-v2/nsh/Make.defs +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name NuttX nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +include ${TOPDIR}/.config +include ${TOPDIR}/tools/Config.mk + +# +# We only support building with the ARM bare-metal toolchain from +# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. +# +CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI + +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(CROSSDEV)ar rcs +NM = $(CROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +MAXOPTIMIZATION = -O3 +ARCHCPUFLAGS = -mcpu=cortex-m4 \ + -mthumb \ + -march=armv7e-m \ + -mfpu=fpv4-sp-d16 \ + -mfloat-abi=hard + + +# enable precise stack overflow tracking +INSTRUMENTATIONDEFINES = -finstrument-functions \ + -ffixed-r10 + +# pull in *just* libm from the toolchain ... this is grody +LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" +EXTRA_LIBS += $(LIBM) + +# use our linker script +LDSCRIPT = ld.script + +ifeq ($(WINTOOL),y) + # Windows-native toolchains + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}" +else + ifeq ($(PX4_WINTOOL),y) + # Windows-native toolchains (MSYS) + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + else + # Linux/Cygwin-native toolchain + MKDEP = $(TOPDIR)/tools/mkdeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + endif +endif + +# tool versions +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} + +# optimisation flags +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -fno-strict-aliasing \ + -fno-strength-reduce \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections + +ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") +ARCHOPTIMIZATION += -g +endif + +ARCHCFLAGS = -std=gnu99 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHWARNINGS = -Wall \ + -Wextra \ + -Wdouble-promotion \ + -Wshadow \ + -Wfloat-equal \ + -Wframe-larger-than=1024 \ + -Wpointer-arith \ + -Wlogical-op \ + -Wmissing-declarations \ + -Wpacked \ + -Wno-unused-parameter +# -Wcast-qual - generates spurious noreturn attribute warnings, try again later +# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code +# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives + +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wbad-function-cast \ + -Wstrict-prototypes \ + -Wold-style-declaration \ + -Wmissing-parameter-type \ + -Wmissing-prototypes \ + -Wnested-externs \ + -Wunsuffixed-float-constants +ARCHWARNINGSXX = $(ARCHWARNINGS) \ + -Wno-psabi +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +# this seems to be the only way to add linker flags +EXTRA_LIBS += --warn-common \ + --gc-sections + +CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +OBJEXT = .o +LIBEXT = .a +EXEEXT = + + +# produce partially-linked $1 from files in $2 +define PRELINK + @echo "PRELINK: $1" + $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 +endef + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = + diff --git a/nuttx-configs/px4-stm32f4discovery/nsh/appconfig b/nuttx-configs/px4-stm32f4discovery/nsh/appconfig new file mode 100644 index 0000000000..f14ab4044b --- /dev/null +++ b/nuttx-configs/px4-stm32f4discovery/nsh/appconfig @@ -0,0 +1,48 @@ +############################################################################ +# configs/px4fmu/nsh/appconfig +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name NuttX nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# Path to example in apps/examples containing the user_start entry point + +CONFIGURED_APPS += examples/nsh + +# The NSH application library +CONFIGURED_APPS += nshlib +CONFIGURED_APPS += system/readline + +#ifeq ($(CONFIG_USBDEV),y) +#ifeq ($(CONFIG_CDCACM),y) +CONFIGURED_APPS += examples/cdcacm +#endif +#endif diff --git a/nuttx-configs/px4-stm32f4discovery/nsh/defconfig b/nuttx-configs/px4-stm32f4discovery/nsh/defconfig new file mode 100644 index 0000000000..8398c53809 --- /dev/null +++ b/nuttx-configs/px4-stm32f4discovery/nsh/defconfig @@ -0,0 +1,897 @@ +# +# Automatically generated file; DO NOT EDIT. +# Nuttx/ Configuration +# +CONFIG_NUTTX_NEWCONFIG=y + +# +# Build Setup +# +# CONFIG_EXPERIMENTAL is not set +# CONFIG_HOST_LINUX is not set +CONFIG_HOST_OSX=y +# CONFIG_HOST_WINDOWS is not set +# CONFIG_HOST_OTHER is not set + +# +# Build Configuration +# +CONFIG_APPS_DIR="../apps" +# CONFIG_BUILD_2PASS is not set + +# +# Binary Output Formats +# +# CONFIG_RRLOAD_BINARY is not set +# CONFIG_INTELHEX_BINARY is not set +# CONFIG_MOTOROLA_SREC is not set +CONFIG_RAW_BINARY=y + +# +# Customize Header Files +# +# CONFIG_ARCH_STDBOOL_H is not set +CONFIG_ARCH_MATH_H=y +# CONFIG_ARCH_FLOAT_H is not set +# CONFIG_ARCH_STDARG_H is not set + +# +# Debug Options +# +# CONFIG_DEBUG is not set +CONFIG_DEBUG_SYMBOLS=y + +# +# System Type +# +# CONFIG_ARCH_8051 is not set +CONFIG_ARCH_ARM=y +# CONFIG_ARCH_AVR is not set +# CONFIG_ARCH_HC is not set +# CONFIG_ARCH_MIPS is not set +# CONFIG_ARCH_RGMP is not set +# CONFIG_ARCH_SH is not set +# CONFIG_ARCH_SIM is not set +# CONFIG_ARCH_X86 is not set +# CONFIG_ARCH_Z16 is not set +# CONFIG_ARCH_Z80 is not set +CONFIG_ARCH="arm" + +# +# ARM Options +# +# CONFIG_ARCH_CHIP_C5471 is not set +# CONFIG_ARCH_CHIP_CALYPSO is not set +# CONFIG_ARCH_CHIP_DM320 is not set +# CONFIG_ARCH_CHIP_IMX is not set +# CONFIG_ARCH_CHIP_KINETIS is not set +# CONFIG_ARCH_CHIP_KL is not set +# CONFIG_ARCH_CHIP_LM is not set +# CONFIG_ARCH_CHIP_LPC17XX is not set +# CONFIG_ARCH_CHIP_LPC214X is not set +# CONFIG_ARCH_CHIP_LPC2378 is not set +# CONFIG_ARCH_CHIP_LPC31XX is not set +# CONFIG_ARCH_CHIP_LPC43XX is not set +# CONFIG_ARCH_CHIP_NUC1XX is not set +# CONFIG_ARCH_CHIP_SAM34 is not set +CONFIG_ARCH_CHIP_STM32=y +# CONFIG_ARCH_CHIP_STR71X is not set +CONFIG_ARCH_CORTEXM4=y +CONFIG_ARCH_FAMILY="armv7-m" +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARCH_HAVE_CMNVECTOR=y +CONFIG_ARMV7M_CMNVECTOR=y +CONFIG_ARCH_HAVE_FPU=y +CONFIG_ARCH_FPU=y +CONFIG_ARCH_HAVE_MPU=y +# CONFIG_ARMV7M_MPU is not set + +# +# ARMV7M Configuration Options +# +# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set +CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y +CONFIG_ARMV7M_STACKCHECK=y +CONFIG_SERIAL_TERMIOS=y + +# +# STM32 Configuration Options +# +# CONFIG_ARCH_CHIP_STM32L151C6 is not set +# CONFIG_ARCH_CHIP_STM32L151C8 is not set +# CONFIG_ARCH_CHIP_STM32L151CB is not set +# CONFIG_ARCH_CHIP_STM32L151R6 is not set +# CONFIG_ARCH_CHIP_STM32L151R8 is not set +# CONFIG_ARCH_CHIP_STM32L151RB is not set +# CONFIG_ARCH_CHIP_STM32L151V6 is not set +# CONFIG_ARCH_CHIP_STM32L151V8 is not set +# CONFIG_ARCH_CHIP_STM32L151VB is not set +# CONFIG_ARCH_CHIP_STM32L152C6 is not set +# CONFIG_ARCH_CHIP_STM32L152C8 is not set +# CONFIG_ARCH_CHIP_STM32L152CB is not set +# CONFIG_ARCH_CHIP_STM32L152R6 is not set +# CONFIG_ARCH_CHIP_STM32L152R8 is not set +# CONFIG_ARCH_CHIP_STM32L152RB is not set +# CONFIG_ARCH_CHIP_STM32L152V6 is not set +# CONFIG_ARCH_CHIP_STM32L152V8 is not set +# CONFIG_ARCH_CHIP_STM32L152VB is not set +# CONFIG_ARCH_CHIP_STM32F100C8 is not set +# CONFIG_ARCH_CHIP_STM32F100CB is not set +# CONFIG_ARCH_CHIP_STM32F100R8 is not set +# CONFIG_ARCH_CHIP_STM32F100RB is not set +# CONFIG_ARCH_CHIP_STM32F100RC is not set +# CONFIG_ARCH_CHIP_STM32F100RD is not set +# CONFIG_ARCH_CHIP_STM32F100RE is not set +# CONFIG_ARCH_CHIP_STM32F100V8 is not set +# CONFIG_ARCH_CHIP_STM32F100VB is not set +# CONFIG_ARCH_CHIP_STM32F100VC is not set +# CONFIG_ARCH_CHIP_STM32F100VD is not set +# CONFIG_ARCH_CHIP_STM32F100VE is not set +# CONFIG_ARCH_CHIP_STM32F103C4 is not set +# CONFIG_ARCH_CHIP_STM32F103C8 is not set +# CONFIG_ARCH_CHIP_STM32F103RET6 is not set +# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set +# CONFIG_ARCH_CHIP_STM32F103VET6 is not set +# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set +# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set +# CONFIG_ARCH_CHIP_STM32F107VC is not set +# CONFIG_ARCH_CHIP_STM32F207IG is not set +# CONFIG_ARCH_CHIP_STM32F302CB is not set +# CONFIG_ARCH_CHIP_STM32F302CC is not set +# CONFIG_ARCH_CHIP_STM32F302RB is not set +# CONFIG_ARCH_CHIP_STM32F302RC is not set +# CONFIG_ARCH_CHIP_STM32F302VB is not set +# CONFIG_ARCH_CHIP_STM32F302VC is not set +# CONFIG_ARCH_CHIP_STM32F303CB is not set +# CONFIG_ARCH_CHIP_STM32F303CC is not set +# CONFIG_ARCH_CHIP_STM32F303RB is not set +# CONFIG_ARCH_CHIP_STM32F303RC is not set +# CONFIG_ARCH_CHIP_STM32F303VB is not set +# CONFIG_ARCH_CHIP_STM32F303VC is not set +# CONFIG_ARCH_CHIP_STM32F405RG is not set +# CONFIG_ARCH_CHIP_STM32F405VG is not set +# CONFIG_ARCH_CHIP_STM32F405ZG is not set +# CONFIG_ARCH_CHIP_STM32F407VE is not set +CONFIG_ARCH_CHIP_STM32F407VG=y +# CONFIG_ARCH_CHIP_STM32F407ZE is not set +# CONFIG_ARCH_CHIP_STM32F407ZG is not set +# CONFIG_ARCH_CHIP_STM32F407IE is not set +# CONFIG_ARCH_CHIP_STM32F407IG is not set +# CONFIG_ARCH_CHIP_STM32F427V is not set +# CONFIG_ARCH_CHIP_STM32F427Z is not set +# CONFIG_ARCH_CHIP_STM32F427I is not set +# CONFIG_STM32_STM32L15XX is not set +# CONFIG_STM32_ENERGYLITE is not set +# CONFIG_STM32_STM32F10XX is not set +# CONFIG_STM32_VALUELINE is not set +# CONFIG_STM32_CONNECTIVITYLINE is not set +# CONFIG_STM32_PERFORMANCELINE is not set +# CONFIG_STM32_HIGHDENSITY is not set +# CONFIG_STM32_MEDIUMDENSITY is not set +# CONFIG_STM32_LOWDENSITY is not set +# CONFIG_STM32_STM32F20XX is not set +# CONFIG_STM32_STM32F30XX is not set +CONFIG_STM32_STM32F40XX=y +# CONFIG_STM32_STM32F427 is not set +# CONFIG_STM32_DFU is not set + +# +# STM32 Peripheral Support +# +CONFIG_STM32_ADC1=y +# CONFIG_STM32_ADC2 is not set +# CONFIG_STM32_ADC3 is not set +CONFIG_STM32_BKPSRAM=y +# CONFIG_STM32_CAN1 is not set +# CONFIG_STM32_CAN2 is not set +CONFIG_STM32_CCMDATARAM=y +# CONFIG_STM32_CRC is not set +# CONFIG_STM32_CRYP is not set +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=y +# CONFIG_STM32_DAC1 is not set +# CONFIG_STM32_DAC2 is not set +# CONFIG_STM32_DCMI is not set +# CONFIG_STM32_ETHMAC is not set +# CONFIG_STM32_FSMC is not set +# CONFIG_STM32_HASH is not set +CONFIG_STM32_I2C1=y +# CONFIG_STM32_I2C2 is not set +# CONFIG_STM32_I2C3 is not set +CONFIG_STM32_OTGFS=y +# CONFIG_STM32_OTGHS is not set +CONFIG_STM32_PWR=y +# CONFIG_STM32_RNG is not set +# CONFIG_STM32_SDIO is not set +CONFIG_STM32_SPI1=y +# CONFIG_STM32_SPI2 is not set +# CONFIG_STM32_SPI3 is not set +# CONFIG_STM32_SPI4 is not set +# CONFIG_STM32_SPI5 is not set +# CONFIG_STM32_SPI6 is not set +CONFIG_STM32_SYSCFG=y +CONFIG_STM32_TIM1=y +# CONFIG_STM32_TIM2 is not set +CONFIG_STM32_TIM3=y +CONFIG_STM32_TIM4=y +# CONFIG_STM32_TIM5 is not set +# CONFIG_STM32_TIM6 is not set +# CONFIG_STM32_TIM7 is not set +# CONFIG_STM32_TIM8 is not set +CONFIG_STM32_TIM9=y +CONFIG_STM32_TIM10=y +CONFIG_STM32_TIM11=y +# CONFIG_STM32_TIM12 is not set +# CONFIG_STM32_TIM13 is not set +# CONFIG_STM32_TIM14 is not set +# CONFIG_STM32_USART1 is not set +CONFIG_STM32_USART2=y +# CONFIG_STM32_USART3 is not set +# CONFIG_STM32_UART4 is not set +# CONFIG_STM32_UART5 is not set +# CONFIG_STM32_USART6 is not set +# CONFIG_STM32_UART7 is not set +# CONFIG_STM32_UART8 is not set +# CONFIG_STM32_IWDG is not set +# CONFIG_STM32_WWDG is not set +CONFIG_STM32_ADC=y +CONFIG_STM32_SPI=y +CONFIG_STM32_I2C=y + +# +# Alternate Pin Mapping +# +CONFIG_STM32_FLASH_PREFETCH=y +# CONFIG_STM32_JTAG_DISABLE is not set +# CONFIG_STM32_JTAG_FULL_ENABLE is not set +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set +CONFIG_STM32_JTAG_SW_ENABLE=y +CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y +# CONFIG_STM32_FORCEPOWER is not set +# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set +# CONFIG_STM32_CCMEXCLUDE is not set +CONFIG_STM32_DMACAPABLE=y +# CONFIG_STM32_TIM1_PWM is not set +# CONFIG_STM32_TIM3_PWM is not set +# CONFIG_STM32_TIM4_PWM is not set +# CONFIG_STM32_TIM9_PWM is not set +# CONFIG_STM32_TIM10_PWM is not set +# CONFIG_STM32_TIM11_PWM is not set +# CONFIG_STM32_TIM1_ADC is not set +# CONFIG_STM32_TIM3_ADC is not set +# CONFIG_STM32_TIM4_ADC is not set +CONFIG_STM32_USART=y + +# +# U[S]ART Configuration +# +# CONFIG_USART2_RS485 is not set +CONFIG_USART2_RXDMA=y +CONFIG_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32_USART_SINGLEWIRE=y + +# +# SPI Configuration +# +# CONFIG_STM32_SPI_INTERRUPTS is not set +# CONFIG_STM32_SPI_DMA is not set + +# +# I2C Configuration +# +# CONFIG_STM32_I2C_DYNTIMEO is not set +CONFIG_STM32_I2CTIMEOSEC=0 +CONFIG_STM32_I2CTIMEOMS=500 +CONFIG_STM32_I2CTIMEOTICKS=500 +# CONFIG_STM32_I2C_DUTY16_9 is not set + +# +# USB Host Configuration +# + +# +# USB Device Configuration +# + +# +# External Memory Configuration +# + +# +# Architecture Options +# +# CONFIG_ARCH_NOINTC is not set +# CONFIG_ARCH_VECNOTIRQ is not set +CONFIG_ARCH_DMA=y +CONFIG_ARCH_IRQPRIO=y +# CONFIG_CUSTOM_STACK is not set +# CONFIG_ADDRENV is not set +CONFIG_ARCH_HAVE_VFORK=y +CONFIG_ARCH_STACKDUMP=y +# CONFIG_ENDIAN_BIG is not set +# CONFIG_ARCH_HAVE_RAMFUNCS is not set +CONFIG_ARCH_HAVE_RAMVECTORS=y +# CONFIG_ARCH_RAMVECTORS is not set + +# +# Board Settings +# +CONFIG_BOARD_LOOPSPERMSEC=16717 +# CONFIG_ARCH_CALIBRATION is not set +CONFIG_DRAM_START=0x20000000 +CONFIG_DRAM_SIZE=196608 +CONFIG_ARCH_HAVE_INTERRUPTSTACK=y +CONFIG_ARCH_INTERRUPTSTACK=4096 + +# +# Boot options +# +# CONFIG_BOOT_RUNFROMEXTSRAM is not set +CONFIG_BOOT_RUNFROMFLASH=y +# CONFIG_BOOT_RUNFROMISRAM is not set +# CONFIG_BOOT_RUNFROMSDRAM is not set +# CONFIG_BOOT_COPYTORAM is not set + +# +# Board Selection +# +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD="" + +# +# Common Board Options +# +CONFIG_NSH_MMCSDMINOR=0 + +# +# Board-Specific Options +# + +# +# RTOS Features +# +# CONFIG_BOARD_INITIALIZE is not set +CONFIG_MSEC_PER_TICK=1 +CONFIG_RR_INTERVAL=0 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_TASK_NAME_SIZE=24 +# CONFIG_SCHED_HAVE_PARENT is not set +# CONFIG_JULIAN_TIME is not set +CONFIG_START_YEAR=1970 +CONFIG_START_MONTH=1 +CONFIG_START_DAY=1 +CONFIG_DEV_CONSOLE=y +# CONFIG_MUTEX_TYPES is not set +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_NNESTPRIO=8 +# CONFIG_FDCLONE_DISABLE is not set +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SCHED_WAITPID=y +# CONFIG_SCHED_STARTHOOK is not set +CONFIG_SCHED_ATEXIT=y +CONFIG_SCHED_ATEXIT_MAX=1 +# CONFIG_SCHED_ONEXIT is not set +CONFIG_USER_ENTRYPOINT="nsh_main" +# CONFIG_DISABLE_OS_API is not set + +# +# Signal Numbers +# +CONFIG_SIG_SIGUSR1=1 +CONFIG_SIG_SIGUSR2=2 +CONFIG_SIG_SIGALARM=3 +CONFIG_SIG_SIGCONDTIMEDOUT=16 +CONFIG_SIG_SIGWORK=4 + +# +# Sizes of configurable things (0 disables) +# +CONFIG_MAX_TASKS=32 +CONFIG_MAX_TASK_ARGS=10 +CONFIG_NPTHREAD_KEYS=4 +CONFIG_NFILE_DESCRIPTORS=32 +CONFIG_NFILE_STREAMS=8 +CONFIG_NAME_MAX=32 +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=50 +CONFIG_PREALLOC_TIMERS=50 + +# +# Stack and heap information +# +CONFIG_IDLETHREAD_STACKSIZE=6000 +CONFIG_USERMAIN_STACKSIZE=4096 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_PTHREAD_STACK_DEFAULT=2048 + +# +# Device Drivers +# +# CONFIG_DISABLE_POLL is not set +CONFIG_DEV_NULL=y +# CONFIG_DEV_ZERO is not set +# CONFIG_LOOP is not set +# CONFIG_RAMDISK is not set +# CONFIG_CAN is not set +# CONFIG_PWM is not set +CONFIG_I2C=y +# CONFIG_I2C_SLAVE is not set +CONFIG_I2C_TRANSFER=y +# CONFIG_I2C_WRITEREAD is not set +# CONFIG_I2C_POLLED is not set +# CONFIG_I2C_TRACE is not set +CONFIG_ARCH_HAVE_I2CRESET=y +CONFIG_I2C_RESET=y +CONFIG_SPI=y +# CONFIG_SPI_OWNBUS is not set +CONFIG_SPI_EXCHANGE=y +# CONFIG_SPI_CMDDATA is not set +# CONFIG_RTC is not set +CONFIG_WATCHDOG=y +# CONFIG_ANALOG is not set +# CONFIG_AUDIO_DEVICES is not set +# CONFIG_BCH is not set +# CONFIG_INPUT is not set +# CONFIG_LCD is not set +# CONFIG_MMCSD is not set +# CONFIG_MTD is not set +CONFIG_PIPES=y +# CONFIG_PM is not set +# CONFIG_POWER is not set +# CONFIG_SENSORS is not set +CONFIG_SERIAL=y +# CONFIG_DEV_LOWCONSOLE is not set +CONFIG_SERIAL_REMOVABLE=y +# CONFIG_16550_UART is not set +CONFIG_ARCH_HAVE_USART2=y +CONFIG_MCU_SERIAL=y +CONFIG_STANDARD_SERIAL=y +CONFIG_SERIAL_NPOLLWAITERS=2 +CONFIG_USART2_SERIAL_CONSOLE=y +# CONFIG_NO_SERIAL_CONSOLE is not set + +# +# USART2 Configuration +# +CONFIG_USART2_RXBUFSIZE=512 +CONFIG_USART2_TXBUFSIZE=512 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_BITS=8 +CONFIG_USART2_PARITY=0 +CONFIG_USART2_2STOP=0 +# CONFIG_USART2_IFLOWCONTROL is not set +# CONFIG_USART2_OFLOWCONTROL is not set +# CONFIG_SERIAL_IFLOWCONTROL is not set +# CONFIG_SERIAL_OFLOWCONTROL is not set +CONFIG_USBDEV=y + +# +# USB Device Controller Driver Options +# +# CONFIG_USBDEV_ISOCHRONOUS is not set +# CONFIG_USBDEV_DUALSPEED is not set +# CONFIG_USBDEV_SELFPOWERED is not set +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +# CONFIG_USBDEV_DMA is not set +# CONFIG_USBDEV_TRACE is not set + +# +# USB Device Class Driver Options +# +# CONFIG_USBDEV_COMPOSITE is not set +# CONFIG_PL2303 is not set +CONFIG_CDCACM=y +# CONFIG_CDCACM_CONSOLE is not set +CONFIG_CDCACM_EP0MAXPACKET=64 +CONFIG_CDCACM_EPINTIN=1 +CONFIG_CDCACM_EPINTIN_FSSIZE=64 +CONFIG_CDCACM_EPINTIN_HSSIZE=64 +CONFIG_CDCACM_EPBULKOUT=3 +CONFIG_CDCACM_EPBULKOUT_FSSIZE=64 +CONFIG_CDCACM_EPBULKOUT_HSSIZE=512 +CONFIG_CDCACM_EPBULKIN=2 +CONFIG_CDCACM_EPBULKIN_FSSIZE=64 +CONFIG_CDCACM_EPBULKIN_HSSIZE=512 +CONFIG_CDCACM_NWRREQS=4 +CONFIG_CDCACM_NRDREQS=4 +CONFIG_CDCACM_BULKIN_REQLEN=96 +CONFIG_CDCACM_RXBUFSIZE=512 +CONFIG_CDCACM_TXBUFSIZE=2048 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_PRODUCTID=0x0011 +CONFIG_CDCACM_VENDORSTR="3D Robotics" +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v2.x" +# CONFIG_USBMSC is not set +# CONFIG_USBHOST is not set +# CONFIG_WIRELESS is not set + +# +# System Logging Device Options +# + +# +# System Logging +# +# CONFIG_RAMLOG is not set + +# +# Networking Support +# +# CONFIG_NET is not set + +# +# File Systems +# + +# +# File system configuration +# +# CONFIG_DISABLE_MOUNTPOINT is not set +# CONFIG_FS_RAMMAP is not set +CONFIG_FS_FAT=y +# CONFIG_FAT_LCNAMES is not set +# CONFIG_FAT_LFN is not set +# CONFIG_FS_FATTIME is not set +# CONFIG_FAT_DMAMEMORY is not set +CONFIG_FS_NXFFS=y +CONFIG_NXFFS_PREALLOCATED=y +CONFIG_NXFFS_ERASEDSTATE=0xff +CONFIG_NXFFS_PACKTHRESHOLD=32 +CONFIG_NXFFS_MAXNAMLEN=32 +CONFIG_NXFFS_TAILTHRESHOLD=2048 +CONFIG_FS_ROMFS=y +# CONFIG_FS_SMARTFS is not set +CONFIG_FS_BINFS=y + +# +# System Logging +# +# CONFIG_SYSLOG_ENABLE is not set +# CONFIG_SYSLOG is not set + +# +# Graphics Support +# +# CONFIG_NX is not set + +# +# Memory Management +# +# CONFIG_MM_MULTIHEAP is not set +# CONFIG_MM_SMALL is not set +CONFIG_MM_REGIONS=2 +CONFIG_GRAN=y +# CONFIG_GRAN_SINGLE is not set +# CONFIG_GRAN_INTR is not set + +# +# Audio Support +# +# CONFIG_AUDIO is not set + +# +# Binary Formats +# +# CONFIG_BINFMT_DISABLE is not set +# CONFIG_BINFMT_EXEPATH is not set +# CONFIG_NXFLAT is not set +# CONFIG_ELF is not set +CONFIG_BUILTIN=y +# CONFIG_PIC is not set +# CONFIG_SYMTAB_ORDEREDBYNAME is not set + +# +# Library Routines +# + +# +# Standard C Library Options +# +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STDIO_LINEBUFFER=y +CONFIG_NUNGET_CHARS=2 +CONFIG_LIB_HOMEDIR="/" +# CONFIG_NOPRINTF_FIELDWIDTH is not set +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIB_RAND_ORDER=1 +# CONFIG_EOL_IS_CR is not set +# CONFIG_EOL_IS_LF is not set +# CONFIG_EOL_IS_BOTH_CRLF is not set +CONFIG_EOL_IS_EITHER_CRLF=y +# CONFIG_LIBC_EXECFUNCS is not set +CONFIG_POSIX_SPAWN_PROXY_STACKSIZE=1024 +CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=2048 +CONFIG_LIBC_STRERROR=y +# CONFIG_LIBC_STRERROR_SHORT is not set +# CONFIG_LIBC_PERROR_STDOUT is not set +CONFIG_ARCH_LOWPUTC=y +CONFIG_LIB_SENDFILE_BUFSIZE=512 +# CONFIG_ARCH_ROMGETC is not set +CONFIG_ARCH_OPTIMIZED_FUNCTIONS=y +CONFIG_ARCH_MEMCPY=y +# CONFIG_ARCH_MEMCMP is not set +# CONFIG_ARCH_MEMMOVE is not set +# CONFIG_ARCH_MEMSET is not set +# CONFIG_MEMSET_OPTSPEED is not set +# CONFIG_ARCH_STRCHR is not set +# CONFIG_ARCH_STRCMP is not set +# CONFIG_ARCH_STRCPY is not set +# CONFIG_ARCH_STRNCPY is not set +# CONFIG_ARCH_STRLEN is not set +# CONFIG_ARCH_STRNLEN is not set +# CONFIG_ARCH_BZERO is not set + +# +# Non-standard Library Support +# +CONFIG_SCHED_WORKQUEUE=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_WORKPRIORITY=192 +CONFIG_SCHED_WORKPERIOD=5000 +CONFIG_SCHED_WORKSTACKSIZE=4000 +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKPERIOD=50000 +CONFIG_SCHED_LPWORKSTACKSIZE=4000 +# CONFIG_LIB_KBDCODEC is not set +# CONFIG_LIB_SLCDCODEC is not set + +# +# Basic CXX Support +# +CONFIG_C99_BOOL8=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +# CONFIG_CXX_NEWLONG is not set + +# +# uClibc++ Standard C++ Library +# +# CONFIG_UCLIBCXX is not set + +# +# Application Configuration +# + +# +# Built-In Applications +# +CONFIG_BUILTIN_PROXY_STACKSIZE=1024 + +# +# Examples +# +# CONFIG_EXAMPLES_BUTTONS is not set +# CONFIG_EXAMPLES_CAN is not set +CONFIG_EXAMPLES_CDCACM=y +# CONFIG_EXAMPLES_COMPOSITE is not set +# CONFIG_EXAMPLES_CXXTEST is not set +# CONFIG_EXAMPLES_DHCPD is not set +# CONFIG_EXAMPLES_ELF is not set +# CONFIG_EXAMPLES_FTPC is not set +# CONFIG_EXAMPLES_FTPD is not set +# CONFIG_EXAMPLES_HELLO is not set +# CONFIG_EXAMPLES_HELLOXX is not set +# CONFIG_EXAMPLES_JSON is not set +# CONFIG_EXAMPLES_HIDKBD is not set +# CONFIG_EXAMPLES_KEYPADTEST is not set +# CONFIG_EXAMPLES_IGMP is not set +# CONFIG_EXAMPLES_LCDRW is not set +# CONFIG_EXAMPLES_MM is not set +# CONFIG_EXAMPLES_MODBUS is not set +CONFIG_EXAMPLES_MOUNT=y +# CONFIG_EXAMPLES_NRF24L01TERM is not set +CONFIG_EXAMPLES_NSH=y +# CONFIG_EXAMPLES_NULL is not set +# CONFIG_EXAMPLES_NX is not set +# CONFIG_EXAMPLES_NXCONSOLE is not set +# CONFIG_EXAMPLES_NXFFS is not set +# CONFIG_EXAMPLES_NXFLAT is not set +# CONFIG_EXAMPLES_NXHELLO is not set +# CONFIG_EXAMPLES_NXIMAGE is not set +# CONFIG_EXAMPLES_NXLINES is not set +# CONFIG_EXAMPLES_NXTEXT is not set +# CONFIG_EXAMPLES_OSTEST is not set +# CONFIG_EXAMPLES_PASHELLO is not set +# CONFIG_EXAMPLES_PIPE is not set +# CONFIG_EXAMPLES_POSIXSPAWN is not set +# CONFIG_EXAMPLES_QENCODER is not set +# CONFIG_EXAMPLES_RGMP is not set +# CONFIG_EXAMPLES_ROMFS is not set +# CONFIG_EXAMPLES_SENDMAIL is not set +# CONFIG_EXAMPLES_SERLOOP is not set +# CONFIG_EXAMPLES_SLCD is not set +# CONFIG_EXAMPLES_SMART_TEST is not set +# CONFIG_EXAMPLES_SMART is not set +# CONFIG_EXAMPLES_TCPECHO is not set +# CONFIG_EXAMPLES_TELNETD is not set +# CONFIG_EXAMPLES_THTTPD is not set +# CONFIG_EXAMPLES_TIFF is not set +# CONFIG_EXAMPLES_TOUCHSCREEN is not set +# CONFIG_EXAMPLES_UDP is not set +# CONFIG_EXAMPLES_UIP is not set +# CONFIG_EXAMPLES_USBSERIAL is not set +# CONFIG_EXAMPLES_USBMSC is not set +# CONFIG_EXAMPLES_USBTERM is not set +# CONFIG_EXAMPLES_WATCHDOG is not set + +# +# Graphics Support +# +# CONFIG_TIFF is not set + +# +# Interpreters +# +# CONFIG_INTERPRETERS_FICL is not set +# CONFIG_INTERPRETERS_PCODE is not set + +# +# Network Utilities +# + +# +# Networking Utilities +# +# CONFIG_NETUTILS_CODECS is not set +# CONFIG_NETUTILS_DHCPC is not set +# CONFIG_NETUTILS_DHCPD is not set +# CONFIG_NETUTILS_FTPC is not set +# CONFIG_NETUTILS_FTPD is not set +# CONFIG_NETUTILS_JSON is not set +# CONFIG_NETUTILS_RESOLV is not set +# CONFIG_NETUTILS_SMTP is not set +# CONFIG_NETUTILS_TELNETD is not set +# CONFIG_NETUTILS_TFTPC is not set +# CONFIG_NETUTILS_THTTPD is not set +# CONFIG_NETUTILS_UIPLIB is not set +# CONFIG_NETUTILS_WEBCLIENT is not set + +# +# FreeModBus +# +# CONFIG_MODBUS is not set + +# +# NSH Library +# +CONFIG_NSH_LIBRARY=y +CONFIG_NSH_BUILTIN_APPS=y + +# +# Disable Individual commands +# +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_DD is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_HEXDUMP is not set +# CONFIG_NSH_DISABLE_IFCONFIG is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LOSETUP is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MB is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_MKFIFO is not set +# CONFIG_NSH_DISABLE_MKRD is not set +# CONFIG_NSH_DISABLE_MH is not set +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MW is not set +# CONFIG_NSH_DISABLE_NSFMOUNT is not set +# CONFIG_NSH_DISABLE_PS is not set +# CONFIG_NSH_DISABLE_PING is not set +# CONFIG_NSH_DISABLE_PUT is not set +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_RM is not set +# CONFIG_NSH_DISABLE_RMDIR is not set +# CONFIG_NSH_DISABLE_SET is not set +# CONFIG_NSH_DISABLE_SH is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +# CONFIG_NSH_DISABLE_WGET is not set +# CONFIG_NSH_DISABLE_XD is not set + +# +# Configure Command Options +# +# CONFIG_NSH_CMDOPT_DF_H is not set +CONFIG_NSH_CODECS_BUFSIZE=128 +CONFIG_NSH_FILEIOSIZE=512 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=12 +CONFIG_NSH_NESTDEPTH=8 +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLEBG is not set +CONFIG_NSH_ROMFSETC=y +# CONFIG_NSH_ROMFSRC is not set +CONFIG_NSH_ROMFSMOUNTPT="/etc" +CONFIG_NSH_INITSCRIPT="init.d/rcS" +CONFIG_NSH_ROMFSDEVNO=0 +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_ARCHROMFS=y +CONFIG_NSH_FATDEVNO=0 +CONFIG_NSH_FATSECTSIZE=512 +CONFIG_NSH_FATNSECTORS=1024 +CONFIG_NSH_FATMOUNTPT="/tmp" +CONFIG_NSH_CONSOLE=y +# CONFIG_NSH_USBCONSOLE is not set + +# +# USB Trace Support +# +# CONFIG_NSH_CONDEV is not set +CONFIG_NSH_ARCHINIT=y + +# +# NxWidgets/NxWM +# + +# +# System NSH Add-Ons +# + +# +# Custom Free Memory Command +# +# CONFIG_SYSTEM_FREE is not set + +# +# I2C tool +# +# CONFIG_SYSTEM_I2CTOOL is not set + +# +# FLASH Program Installation +# +# CONFIG_SYSTEM_INSTALL is not set + +# +# FLASH Erase-all Command +# + +# +# readline() +# +CONFIG_SYSTEM_READLINE=y +CONFIG_READLINE_ECHO=y + +# +# Power Off +# +# CONFIG_SYSTEM_POWEROFF is not set + +# +# RAMTRON +# +# CONFIG_SYSTEM_RAMTRON is not set + +# +# SD Card +# +# CONFIG_SYSTEM_SDCARD is not set + +# +# Sysinfo +# +CONFIG_SYSTEM_SYSINFO=y + +# +# USB Monitor +# diff --git a/nuttx-configs/px4-stm32f4discovery/nsh/setenv.sh b/nuttx-configs/px4-stm32f4discovery/nsh/setenv.sh new file mode 100755 index 0000000000..265520997d --- /dev/null +++ b/nuttx-configs/px4-stm32f4discovery/nsh/setenv.sh @@ -0,0 +1,67 @@ +#!/bin/bash +# configs/stm3240g-eval/nsh/setenv.sh +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name NuttX nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# + +if [ "$_" = "$0" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +WD=`pwd` +if [ ! -x "setenv.sh" ]; then + echo "This script must be executed from the top-level NuttX build directory" + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then + export PATH_ORIG="${PATH}" +fi + +# This the Cygwin path to the location where I installed the RIDE +# toolchain under windows. You will also have to edit this if you install +# the RIDE toolchain in any other location +#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin" + +# This the Cygwin path to the location where I installed the CodeSourcery +# toolchain under windows. You will also have to edit this if you install +# the CodeSourcery toolchain in any other location +export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin" + +# This the Cygwin path to the location where I build the buildroot +# toolchain. +#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" + +# Add the path to the toolchain to the PATH varialble +export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx-configs/px4-stm32f4discovery/scripts/ld.script b/nuttx-configs/px4-stm32f4discovery/scripts/ld.script new file mode 100644 index 0000000000..f39a95c450 --- /dev/null +++ b/nuttx-configs/px4-stm32f4discovery/scripts/ld.script @@ -0,0 +1,149 @@ +/**************************************************************************** + * configs/px4fmu-v1/scripts/ld.script + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* The STM32F40VGG has 1024Kb of FLASH beginning at address 0x0800:0000 and + * 192Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of CCM SRAM beginning at address 0x1000:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + * + * The first 0x4000 of flash is reserved for the bootloader. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08004000, LENGTH = 1008K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 128K + ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K +} + +OUTPUT_ARCH(arm) + +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + /* + * This is a hack to make the newlib libm __errno() call + * use the NuttX get_errno_ptr() function. + */ + __errno = get_errno_ptr; + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + /* + * Construction data for parameters. + */ + __param ALIGN(4): { + __param_start = ABSOLUTE(.); + KEEP(*(__param*)) + __param_end = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/nuttx-configs/px4-stm32f4discovery/src/Makefile b/nuttx-configs/px4-stm32f4discovery/src/Makefile new file mode 100644 index 0000000000..d4276f7fc5 --- /dev/null +++ b/nuttx-configs/px4-stm32f4discovery/src/Makefile @@ -0,0 +1,84 @@ +############################################################################ +# configs/px4fmu/src/Makefile +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name NuttX nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +-include $(TOPDIR)/Make.defs + +CFLAGS += -I$(TOPDIR)/sched + +ASRCS = +AOBJS = $(ASRCS:.S=$(OBJEXT)) + +CSRCS = empty.c +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ARCH_SRCDIR = $(TOPDIR)/arch/$(CONFIG_ARCH)/src +ifeq ($(WINTOOL),y) + CFLAGS += -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}" +else + CFLAGS += -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m +endif + +all: libboard$(LIBEXT) + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +libboard$(LIBEXT): $(OBJS) + $(call ARCHIVE, $@, $(OBJS)) + +.depend: Makefile $(SRCS) + $(Q) $(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + $(Q) touch $@ + +depend: .depend + +clean: + $(call DELFILE, libboard$(LIBEXT)) + $(call CLEAN) + +distclean: clean + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) + +-include Make.dep + diff --git a/nuttx-configs/px4-stm32f4discovery/src/empty.c b/nuttx-configs/px4-stm32f4discovery/src/empty.c new file mode 100644 index 0000000000..ace900866c --- /dev/null +++ b/nuttx-configs/px4-stm32f4discovery/src/empty.c @@ -0,0 +1,4 @@ +/* + * There are no source files here, but libboard.a can't be empty, so + * we have this empty source file to keep it company. + */ diff --git a/src/drivers/boards/px4-stm32f4discovery/board_config.h b/src/drivers/boards/px4-stm32f4discovery/board_config.h new file mode 100644 index 0000000000..dd66abc19f --- /dev/null +++ b/src/drivers/boards/px4-stm32f4discovery/board_config.h @@ -0,0 +1,136 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file board_config.h + * + * PX4-STM32F4Discovery internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +__BEGIN_DECLS + +/* these headers are not C++ safe */ +#include +#include + +#define UDID_START 0x1FFF7A10 + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ +/* Configuration ************************************************************************************/ + +/* PX4FMU GPIOs ***********************************************************************************/ +/* LEDs */ +/* LEDs */ + +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\ + GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN12) +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\ + GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) +#define GPIO_LED3 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\ + GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) +#define GPIO_LED4 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\ + GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15) + +/* SPI chip selects */ +#define GPIO_SPI_CS_MEMS (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) + +/* USB OTG FS + * + * PA9 OTG_FS_VBUS VBUS sensing + */ +#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) + +/* High-resolution timer */ +#define HRT_TIMER 8 /* use timer8 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************************************** + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ****************************************************************************************************/ + +extern void stm32_spiinitialize(void); + +extern void stm32_usbinitialize(void); + +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization for NSH. + * + * CONFIG_NSH_ARCHINIT=y : + * Called from the NSH library + * + * CONFIG_BOARD_INITIALIZE=y, CONFIG_NSH_LIBRARY=y, && + * CONFIG_NSH_ARCHINIT=n : + * Called from board_initialize(). + * + ****************************************************************************/ + +#ifdef CONFIG_NSH_LIBRARY +int nsh_archinitialize(void); +#endif + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/src/drivers/boards/px4-stm32f4discovery/module.mk b/src/drivers/boards/px4-stm32f4discovery/module.mk new file mode 100644 index 0000000000..6b75c08e92 --- /dev/null +++ b/src/drivers/boards/px4-stm32f4discovery/module.mk @@ -0,0 +1,7 @@ +# +# Board-specific startup code for the PX4-STM32F4Discovery +# + +SRCS = px4discovery_init.c \ + px4discovery_usb.c \ + px4discovery_led.c diff --git a/src/drivers/boards/px4-stm32f4discovery/px4discovery_init.c b/src/drivers/boards/px4-stm32f4discovery/px4discovery_init.c new file mode 100644 index 0000000000..fb1b282364 --- /dev/null +++ b/src/drivers/boards/px4-stm32f4discovery/px4discovery_init.c @@ -0,0 +1,173 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4discovery_init.c + * + * PX4-stm32f4discovery specific early startup code. This file implements the + * nsh_archinitialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include "board_config.h" +#include + +#include + +#include +#include + +#include +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lowsyslog(__VA_ARGS__) +# else +# define message(...) printf(__VA_ARGS__) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lowsyslog +# else +# define message printf +# endif +#endif + +/**************************************************************************** + * Protected Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void +stm32_boardinitialize(void) +{ + /* configure LEDs */ + up_ledinit(); +} + +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization + * + ****************************************************************************/ + +#include + +#ifdef __cplusplus +__EXPORT int matherr(struct __exception *e) +{ + return 1; +} +#else +__EXPORT int matherr(struct exception *e) +{ + return 1; +} +#endif + +__EXPORT int nsh_archinitialize(void) +{ + + /* configure the high-resolution time/callout interface */ + hrt_init(); + + /* configure CPU load estimation */ +#ifdef CONFIG_SCHED_INSTRUMENTATION + cpuload_initialize_once(); +#endif + + /* set up the serial DMA polling */ + static struct hrt_call serial_dma_call; + struct timespec ts; + + /* + * Poll at 1ms intervals for received bytes that have not triggered + * a DMA event. + */ + ts.tv_sec = 0; + ts.tv_nsec = 1000000; + + hrt_call_every(&serial_dma_call, + ts_to_abstime(&ts), + ts_to_abstime(&ts), + (hrt_callout)stm32_serial_dma_poll, + NULL); + + return OK; +} diff --git a/src/drivers/boards/px4-stm32f4discovery/px4discovery_led.c b/src/drivers/boards/px4-stm32f4discovery/px4discovery_led.c new file mode 100644 index 0000000000..1f42843edf --- /dev/null +++ b/src/drivers/boards/px4-stm32f4discovery/px4discovery_led.c @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4discovery_led.c + * + * PX4-stm32f4discovery LED backend. + */ + +#include + +#include + +#include "stm32.h" +#include "board_config.h" + +#include + +/* + * Ideally we'd be able to get these from up_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(int led); +__END_DECLS + +__EXPORT void led_init() +{ + /* Configure LED1 GPIO for output */ + + stm32_configgpio(GPIO_LED1); +} + +__EXPORT void led_on(int led) +{ + if (led == 1) + { + /* Pull down to switch on */ + stm32_gpiowrite(GPIO_LED1, false); + } +} + +__EXPORT void led_off(int led) +{ + if (led == 1) + { + /* Pull up to switch off */ + stm32_gpiowrite(GPIO_LED1, true); + } +} + +__EXPORT void led_toggle(int led) +{ + if (led == 1) + { + if (stm32_gpioread(GPIO_LED1)) + stm32_gpiowrite(GPIO_LED1, false); + else + stm32_gpiowrite(GPIO_LED1, true); + } +} diff --git a/src/drivers/boards/px4-stm32f4discovery/px4discovery_usb.c b/src/drivers/boards/px4-stm32f4discovery/px4discovery_usb.c new file mode 100644 index 0000000000..887ca67eff --- /dev/null +++ b/src/drivers/boards/px4-stm32f4discovery/px4discovery_usb.c @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4discovery_usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include "board_config.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_usbinitialize + * + * Description: + * Called to setup USB-related GPIO pins for the PX4-STM32F4Discovery board. + * + ************************************************************************************/ + +__EXPORT void stm32_usbinitialize(void) +{ + /* The OTG FS has an internal soft pull-up */ + + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + +#ifdef CONFIG_STM32_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); + /* XXX We only support device mode + stm32_configgpio(GPIO_OTGFS_PWRON); + stm32_configgpio(GPIO_OTGFS_OVER); + */ +#endif +} + +/************************************************************************************ + * Name: stm32_usbsuspend + * + * Description: + * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ + +__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + ulldbg("resume: %d\n", resume); +} + diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index be9604b6ee..f18c8162d7 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -110,9 +110,13 @@ /* no GPIO driver on the PX4IOv2 board */ #endif +#ifdef CONFIG_ARCH_BOARD_PX4_STM32F4DISCOVERY +/* no GPIO driver on the PX4_STM32F4DISCOVERY board */ +#endif + #if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2) && \ !defined(CONFIG_ARCH_BOARD_PX4FMU_V1) && !defined(CONFIG_ARCH_BOARD_PX4FMU_V2) && \ - !defined(CONFIG_ARCH_BOARD_AEROCORE) + !defined(CONFIG_ARCH_BOARD_AEROCORE) && !defined(CONFIG_ARCH_BOARD_PX4_STM32F4DISCOVERY) # error No CONFIG_ARCH_BOARD_xxxx set #endif /* diff --git a/src/lib/version/version.h b/src/lib/version/version.h index d8ccb67748..8732406306 100644 --- a/src/lib/version/version.h +++ b/src/lib/version/version.h @@ -63,4 +63,8 @@ #define HW_ARCH "AEROCORE" #endif +#ifdef CONFIG_ARCH_BOARD_PX4_STM32F4DISCOVERY +#define HW_ARCH "PX4_STM32F4DISCOVERY" +#endif + #endif /* VERSION_H_ */ diff --git a/src/systemcmds/tests/test_gpio.c b/src/systemcmds/tests/test_gpio.c index 62a8732702..82c1225500 100644 --- a/src/systemcmds/tests/test_gpio.c +++ b/src/systemcmds/tests/test_gpio.c @@ -89,10 +89,11 @@ int test_gpio(int argc, char *argv[]) { - int fd; int ret = 0; - fd = open(PX4IO_DEVICE_PATH, 0); +#ifdef PX4IO_DEVICE_PATH + + int fd = open(PX4IO_DEVICE_PATH, 0); if (fd < 0) { printf("GPIO: open fail\n"); @@ -109,7 +110,11 @@ int test_gpio(int argc, char *argv[]) /* Go back to default */ ioctl(fd, GPIO_RESET, ~0); + close(fd); printf("\t GPIO test successful.\n"); +#endif + + return ret; } From ea521844fe0ad25f9105ca99dbf79db72c6bdc9c Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Fri, 17 Apr 2015 14:34:53 +0200 Subject: [PATCH 194/268] Correct formatting of parameter definition to unbreak documentation generation. --- src/modules/fw_att_control/fw_att_control_params.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index cfc5ae9654..6ce6cef4eb 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -136,8 +136,8 @@ PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.2f); * @max 2.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_P_ROLLFF, - 0.0f); //xxx: set to 0 as default, see comment in ECL_PitchController::control_attitude (float turn_offset = ...) +//xxx: set to 0 as default, see comment on turn_offset in ECL_PitchController::control_attitude(...) +PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f); /** * Roll rate proportional Gain From 230c0b95e32b84f72cd8a9cc6dbff15026eefa14 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 16 Apr 2015 22:57:21 -0400 Subject: [PATCH 195/268] GF range warning limit mavlink critical messages -only send a mavlink critical message every 3 seconds --- src/modules/navigator/geofence.cpp | 21 +++++++++++++++++---- src/modules/navigator/geofence.h | 4 ++++ 2 files changed, 21 insertions(+), 4 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 31c4db61a5..1b66107347 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -51,12 +51,15 @@ #include #include #include +#include #define GEOFENCE_OFF 0 #define GEOFENCE_FILE_ONLY 1 #define GEOFENCE_MAX_DISTANCES_ONLY 2 #define GEOFENCE_FILE_AND_MAX_DISTANCES 3 +#define GEOFENCE_RANGE_WARNING_LIMIT 3000000 + /* Oddly, ERROR is not defined for C++ */ #ifdef ERROR @@ -69,6 +72,8 @@ Geofence::Geofence() : _fence_pub(-1), _home_pos{}, _home_pos_set(false), + _last_horizontal_range_warning(0), + _last_vertical_range_warning(0), _altitude_min(0), _altitude_max(0), _verticesCount(0), @@ -146,14 +151,22 @@ bool Geofence::inside(double lat, double lon, float altitude) &dist_xy, &dist_z); if (max_vertical_distance > 0 && (dist_z > max_vertical_distance)) { - mavlink_log_critical(_mavlinkFd, "Geofence exceeded max vertical distance by %.0f m", - (double)(dist_z - max_vertical_distance)); + if (hrt_elapsed_time(&_last_vertical_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) { + mavlink_log_critical(_mavlinkFd, "Geofence exceeded max vertical distance by %.1f m", + (double)(dist_z - max_vertical_distance)); + _last_vertical_range_warning = hrt_absolute_time(); + } + return false; } if (max_horizontal_distance > 0 && (dist_xy > max_horizontal_distance)) { - mavlink_log_critical(_mavlinkFd, "Geofence exceeded max horizontal distance by %.0f m", - (double)(dist_xy - max_horizontal_distance)); + if (hrt_elapsed_time(&_last_horizontal_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) { + mavlink_log_critical(_mavlinkFd, "Geofence exceeded max horizontal distance by %.1f m", + (double)(dist_xy - max_horizontal_distance)); + _last_horizontal_range_warning = hrt_absolute_time(); + } + return false; } } diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index effce9e976..96764cc976 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -48,6 +48,7 @@ #include #include #include +#include #define GEOFENCE_FILENAME "/fs/microsd/etc/geofence.txt" @@ -109,6 +110,9 @@ private: home_position_s _home_pos; bool _home_pos_set; + hrt_abstime _last_horizontal_range_warning; + hrt_abstime _last_vertical_range_warning; + float _altitude_min; float _altitude_max; From a4c3208703e38d210a3cccb073674ea098095757 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 17 Apr 2015 22:12:44 -0400 Subject: [PATCH 196/268] travis-ci switch to docker infrastructure --- .travis.yml | 34 +++++++++++++++++++++++++++------- 1 file changed, 27 insertions(+), 7 deletions(-) diff --git a/.travis.yml b/.travis.yml index 3e97545e15..125ce4c09c 100644 --- a/.travis.yml +++ b/.travis.yml @@ -3,16 +3,36 @@ language: cpp +# use travis-ci docker based infrastructure +sudo: false + +cache: + directories: + - $HOME/.ccache + +addons: + apt: + sources: + - ubuntu-toolchain-r-test + packages: + - build-essential + - ccache + - cmake + - g++-4.8 + - gcc-4.8 + - genromfs + - libc6-i386 + - libncurses5-dev + - python-argparse + - python-empy + - python-serial + - s3cmd + - texinfo + - zlib1g-dev + before_script: - - sudo add-apt-repository --yes ppa:ubuntu-toolchain-r/test - - sudo apt-get update -qq - - if [ "$CXX" = "g++" ]; then sudo apt-get install -qq g++-4.8 gcc-4.8 libstdc++-4.8-dev; fi - if [ "$CXX" = "g++" ]; then export CXX="g++-4.8" CC="gcc-4.8"; fi -# Travis specific tools - - sudo apt-get install -qq s3cmd grep zip # General toolchain dependencies - - sudo apt-get install -qq libc6-i386 gcc-4.7-base:i386 python-serial python-argparse python-empy - - sudo apt-get install -qq flex bison libncurses5-dev autoconf texinfo build-essential libtool zlib1g-dev genromfs git wget cmake - pushd . - cd ~ - wget https://launchpadlibrarian.net/186124160/gcc-arm-none-eabi-4_8-2014q3-20140805-linux.tar.bz2 From 6af3f13a1db71a84fdc9ef8d0eb2af3cb4530a29 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 17 Apr 2015 22:27:36 -0400 Subject: [PATCH 197/268] travis-ci enable ccache --- .travis.yml | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/.travis.yml b/.travis.yml index 125ce4c09c..d0d7254ce8 100644 --- a/.travis.yml +++ b/.travis.yml @@ -41,6 +41,13 @@ before_script: - if grep -Fxq "$exportline" ~/.profile; then echo nothing to do ; else echo $exportline >> ~/.profile; fi - . ~/.profile - popd +# setup ccache + - mkdir -p ~/bin + - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-g++ + - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-gcc + - ln -s /usr/bin/ccache ~/bin/g++-4.8 + - ln -s /usr/bin/ccache ~/bin/gcc-4.8 + - export PATH=~/bin:$PATH git: depth: 500 @@ -59,6 +66,7 @@ env: - BUILD_URI=https://px4-travis.s3.amazonaws.com/archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/Firmware.zip script: + - ccache -z - arm-none-eabi-gcc --version - echo 'Running Tests..' && echo -en 'travis_fold:start:script.1\\r' - make tests @@ -66,9 +74,11 @@ script: - echo -en 'travis_fold:end:script.1\\r' - echo 'Building NuttX..' && echo -en 'travis_fold:start:script.2\\r' - make archives + - ccache -s - echo -en 'travis_fold:end:script.2\\r' - echo 'Building Firmware..' && echo -en 'travis_fold:start:script.3\\r' - make -j6 + - ccache -s - echo -en 'travis_fold:end:script.3\\r' - zip Firmware.zip Images/*.px4 From 0629d0dd60839497b6c198b44b93d46200b42583 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 18 Apr 2015 00:19:27 -0400 Subject: [PATCH 198/268] travis-ci reduce clone depth --- .travis.yml | 3 --- 1 file changed, 3 deletions(-) diff --git a/.travis.yml b/.travis.yml index d0d7254ce8..1136f95f1c 100644 --- a/.travis.yml +++ b/.travis.yml @@ -49,9 +49,6 @@ before_script: - ln -s /usr/bin/ccache ~/bin/gcc-4.8 - export PATH=~/bin:$PATH -git: - depth: 500 - env: global: # AWS KEY: $PX4_AWS_KEY From 075227007aa5fd624200dea64efd1809ce5516d8 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 18 Apr 2015 00:19:56 -0400 Subject: [PATCH 199/268] travis-ci lint fix see http://lint.travis-ci.org/px4/firmware --- .travis.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 1136f95f1c..194c912c73 100644 --- a/.travis.yml +++ b/.travis.yml @@ -111,4 +111,3 @@ notifications: - https://webhooks.gitter.im/e/2b9c4a4cb2211f8befba on_success: always # options: [always|never|change] default: always on_failure: always # options: [always|never|change] default: always - on_start: false # default: false From b53a33bc513a33c784b37a700058bfbc1e15c985 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 18 Apr 2015 00:22:18 -0400 Subject: [PATCH 200/268] travis-ci cleanup unused --- .travis.yml | 5 ----- 1 file changed, 5 deletions(-) diff --git a/.travis.yml b/.travis.yml index 194c912c73..7d86522ae4 100644 --- a/.travis.yml +++ b/.travis.yml @@ -56,11 +56,6 @@ env: # AWS SECRET: $PX4_AWS_SECRET - secure: "h6oajlW68dWIr+wZhO58Dv6e68dZHrBLVA6lPXZmheFQBW6Xam1HuLGA0LOW6cL9TnrAsOZ8g4goB58eMQnMEijFZKi3mhRwZhd/Xjq/ZGJOWBUrLoQHZUw2dQk5ja5vmUlKEoQnFZjDuMjx8KfX5ZMNy8A3yssWZtJYHD8c+bk=" - PX4_AWS_BUCKET=px4-travis - - PX4_EMAIL_SUBJECT="Travis CI result" -# Email address: $PX4_EMAIL - - secure: "ei3hKAw6Pk+vEkQBI5Y2Ak74BRAaXcK2UHVnVadviBHI4EVPwn1YGP6A4Y0wnLe4U7ETTl0UiijRoVxyDW0Mq896Pv0siw02amNpjSZZYu+RfN1+//MChB48OxsLDirUdHVrULhl/bOARM02h2Bg28jDE2g7IqmJwg3em3oMbjU=" - - PX4_REPORT=report.txt - - BUILD_URI=https://px4-travis.s3.amazonaws.com/archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/Firmware.zip script: - ccache -z From 207b57869d7ed44950a51bbd9632a8549d7a9c49 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 18 Apr 2015 00:57:26 -0400 Subject: [PATCH 201/268] only define GIT_VERSION where it's used -when the git revision is passed to every file as a define it causes unnecessary ccache cache misses --- makefiles/firmware.mk | 5 ----- src/lib/version/version.h | 8 -------- src/modules/sdlog2/module.mk | 1 + src/modules/sdlog2/sdlog2.c | 2 +- src/modules/uavcan/module.mk | 3 ++- src/modules/uavcan/uavcan_main.cpp | 4 ++-- src/systemcmds/ver/module.mk | 2 ++ src/systemcmds/ver/ver.c | 2 +- 8 files changed, 9 insertions(+), 18 deletions(-) diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index 21e8739aa1..4c10de931c 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -180,11 +180,6 @@ GLOBAL_DEPS += $(MAKEFILE_LIST) EXTRA_CLEANS = -# -# Extra defines for compilation -# -export EXTRADEFINES := -DGIT_VERSION=$(GIT_DESC) - # # Append the per-board driver directory to the header search path. # diff --git a/src/lib/version/version.h b/src/lib/version/version.h index 8732406306..b79fee182e 100644 --- a/src/lib/version/version.h +++ b/src/lib/version/version.h @@ -43,14 +43,6 @@ #ifndef VERSION_H_ #define VERSION_H_ -/* - GIT_VERSION is defined at build time via a Makefile call to the - git command line. - */ -#define FREEZE_STR(s) #s -#define STRINGIFY(s) FREEZE_STR(s) -#define FW_GIT STRINGIFY(GIT_VERSION) - #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 #define HW_ARCH "PX4FMU_V1" #endif diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk index 8fded0bdb2..6964acf339 100644 --- a/src/modules/sdlog2/module.mk +++ b/src/modules/sdlog2/module.mk @@ -48,3 +48,4 @@ MAXOPTIMIZATION = -Os EXTRACFLAGS = -Wframe-larger-than=1400 +EXTRADEFINES = -DGIT_VERSION='"$(GIT_DESC)"' diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 2ec7336272..ed93a06b12 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -764,7 +764,7 @@ int write_version(int fd) }; /* fill version message and write it */ - strncpy(log_msg_VER.body.fw_git, FW_GIT, sizeof(log_msg_VER.body.fw_git)); + strncpy(log_msg_VER.body.fw_git, GIT_VERSION, sizeof(log_msg_VER.body.fw_git)); strncpy(log_msg_VER.body.arch, HW_ARCH, sizeof(log_msg_VER.body.arch)); return write(fd, &log_msg_VER, sizeof(log_msg_VER)); } diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index f7feeadabc..437feb3014 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -63,7 +63,8 @@ SRCS += $(subst $(PX4_MODULE_SRC),../../,$(LIBUAVCAN_SRC)) INCLUDE_DIRS += $(LIBUAVCAN_INC) # Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile # because this platform lacks most of the standard library and STL. Hence we need to force C++03 mode. -override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_NO_ASSERTIONS +override EXTRADEFINES := $(EXTRADEFINES) -DGIT_VERSION='"$(GIT_DESC)"' -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_NO_ASSERTIONS + # # libuavcan drivers for STM32 diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 02cde96564..2d5abf9591 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -210,9 +210,9 @@ void UavcanNode::fill_node_info() /* software version */ uavcan::protocol::SoftwareVersion swver; - // Extracting the first 8 hex digits of FW_GIT and converting them to int + // Extracting the first 8 hex digits of GIT_VERSION and converting them to int char fw_git_short[9] = {}; - std::memmove(fw_git_short, FW_GIT, 8); + std::memmove(fw_git_short, GIT_VERSION, 8); assert(fw_git_short[8] == '\0'); char *end = nullptr; swver.vcs_commit = std::strtol(fw_git_short, &end, 16); diff --git a/src/systemcmds/ver/module.mk b/src/systemcmds/ver/module.mk index 2eeb80b616..4597b5f110 100644 --- a/src/systemcmds/ver/module.mk +++ b/src/systemcmds/ver/module.mk @@ -42,3 +42,5 @@ SRCS = ver.c MODULE_STACKSIZE = 1024 MAXOPTIMIZATION = -Os + +EXTRADEFINES = -DGIT_VERSION='"$(GIT_DESC)"' diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index 087eb52e3d..b794e8b2fa 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -101,7 +101,7 @@ int ver_main(int argc, char *argv[]) } if (show_all || !strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) { - printf("FW git-hash: %s\n", FW_GIT); + printf("FW git-hash: %s\n", GIT_VERSION); ret = 0; } From 870c9532a97a506b674001ff297b730b94757553 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 18 Apr 2015 01:13:25 -0400 Subject: [PATCH 202/268] travis-ci reduce parallel make to 4 threads -travis only has 2 cores --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 7d86522ae4..0405ba560f 100644 --- a/.travis.yml +++ b/.travis.yml @@ -69,7 +69,7 @@ script: - ccache -s - echo -en 'travis_fold:end:script.2\\r' - echo 'Building Firmware..' && echo -en 'travis_fold:start:script.3\\r' - - make -j6 + - make -j4 - ccache -s - echo -en 'travis_fold:end:script.3\\r' - zip Firmware.zip Images/*.px4 From 5fe7f76691b80a1ea488d7ad740be5e6b4520643 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Thu, 16 Apr 2015 12:53:56 -0700 Subject: [PATCH 203/268] Removed usage of PX4_PARAM_DEFINE_* macros This way the meta data parser can pick up default values. There was no usage of the default value defines in any of the code. --- src/modules/systemlib/circuit_breaker_params.c | 13 ++++++------- src/modules/systemlib/circuit_breaker_params.h | 7 ------- 2 files changed, 6 insertions(+), 14 deletions(-) delete mode 100644 src/modules/systemlib/circuit_breaker_params.h diff --git a/src/modules/systemlib/circuit_breaker_params.c b/src/modules/systemlib/circuit_breaker_params.c index e499ae27ac..e5cc034bc9 100644 --- a/src/modules/systemlib/circuit_breaker_params.c +++ b/src/modules/systemlib/circuit_breaker_params.c @@ -43,7 +43,6 @@ */ #include -#include /** * Circuit breaker for power supply check @@ -56,7 +55,7 @@ * @max 894281 * @group Circuit Breaker */ -PX4_PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK); +PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK, 0); /** * Circuit breaker for rate controller output @@ -69,7 +68,7 @@ PX4_PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK); * @max 140253 * @group Circuit Breaker */ -PX4_PARAM_DEFINE_INT32(CBRK_RATE_CTRL); +PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0); /** * Circuit breaker for IO safety @@ -81,7 +80,7 @@ PX4_PARAM_DEFINE_INT32(CBRK_RATE_CTRL); * @max 22027 * @group Circuit Breaker */ -PX4_PARAM_DEFINE_INT32(CBRK_IO_SAFETY); +PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0); /** * Circuit breaker for airspeed sensor @@ -93,7 +92,7 @@ PX4_PARAM_DEFINE_INT32(CBRK_IO_SAFETY); * @max 162128 * @group Circuit Breaker */ -PX4_PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK); +PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0); /** * Circuit breaker for flight termination @@ -106,7 +105,7 @@ PX4_PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK); * @max 121212 * @group Circuit Breaker */ -PX4_PARAM_DEFINE_INT32(CBRK_FLIGHTTERM); +PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212); /** * Circuit breaker for engine failure detection @@ -120,4 +119,4 @@ PX4_PARAM_DEFINE_INT32(CBRK_FLIGHTTERM); * @max 284953 * @group Circuit Breaker */ -PX4_PARAM_DEFINE_INT32(CBRK_ENGINEFAIL); +PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953); diff --git a/src/modules/systemlib/circuit_breaker_params.h b/src/modules/systemlib/circuit_breaker_params.h deleted file mode 100644 index 768bf7f533..0000000000 --- a/src/modules/systemlib/circuit_breaker_params.h +++ /dev/null @@ -1,7 +0,0 @@ -#define PARAM_CBRK_SUPPLY_CHK_DEFAULT 0 -#define PARAM_CBRK_RATE_CTRL_DEFAULT 0 -#define PARAM_CBRK_IO_SAFETY_DEFAULT 0 -#define PARAM_CBRK_AIRSPD_CHK_DEFAULT 0 -#define PARAM_CBRK_FLIGHTTERM_DEFAULT 121212 -#define PARAM_CBRK_ENGINEFAIL_DEFAULT 284953 -#define PARAM_CBRK_GPSFAIL_DEFAULT 240024 From 750b02b4e5aa166e590c5b801310975c2f220635 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Thu, 16 Apr 2015 12:54:28 -0700 Subject: [PATCH 204/268] Use new @board attribute for ifdef support --- src/modules/sensors/sensor_params.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 18e47865b1..11d911ba92 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -960,6 +960,7 @@ PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000); /** * Scaling factor for battery voltage sensor on FMU v2. * + * @board CONFIG_ARCH_BOARD_PX4FMU_V2 * @group Battery Calibration */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); @@ -969,6 +970,7 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); * * For R70 = 133K, R71 = 10K --> scale = 1.8 * 143 / (4096*10) = 0.0063 * + * @board CONFIG_ARCH_BOARD_AEROCORE * @group Battery Calibration */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0063f); From 3a70e7bf1bef904c63f3bbe0a92e7c9aeda978aa Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Thu, 16 Apr 2015 12:54:41 -0700 Subject: [PATCH 205/268] Remove newline so meta data parser can parse --- src/modules/fw_att_control/fw_att_control_params.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 6ce6cef4eb..6b248cbe2e 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -136,7 +136,6 @@ PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.2f); * @max 2.0 * @group FW Attitude Control */ -//xxx: set to 0 as default, see comment on turn_offset in ECL_PitchController::control_attitude(...) PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f); /** From 9ac350a7d1c5c07a4e4ba7824744930f9110dedc Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Thu, 16 Apr 2015 12:55:20 -0700 Subject: [PATCH 206/268] Added ability for board specific meta data generation Use new @board meta data attribute for board specific ifdef support --- Tools/px4params/srcparser.py | 37 ++++++++++++++++++++--------------- Tools/px4params/xmlout.py | 38 +++++++++++++++++++++++------------- Tools/px_process_params.py | 7 ++++++- makefiles/firmware.mk | 2 +- 4 files changed, 52 insertions(+), 32 deletions(-) diff --git a/Tools/px4params/srcparser.py b/Tools/px4params/srcparser.py index 891db7ecd2..0d2413a75f 100644 --- a/Tools/px4params/srcparser.py +++ b/Tools/px4params/srcparser.py @@ -37,20 +37,30 @@ class Parameter(object): # Define sorting order of the fields priority = { - "code": 10, - "type": 9, + "board": 9, "short_desc": 8, "long_desc": 7, - "default": 6, "min": 5, "max": 4, "unit": 3, # all others == 0 (sorted alphabetically) } - def __init__(self): + def __init__(self, name, type, default = ""): self.fields = {} + self.name = name + self.type = type + self.default = default + def GetName(self): + return self.name + + def GetType(self): + return self.type + + def GetDefault(self): + return self.default + def SetField(self, code, value): """ Set named field value @@ -88,7 +98,7 @@ class SourceParser(object): re_is_a_number = re.compile(r'^-?[0-9\.]') re_remove_dots = re.compile(r'\.+$') - valid_tags = set(["group", "min", "max", "unit"]) + valid_tags = set(["group", "board", "min", "max", "unit"]) # Order of parameter groups priority = { @@ -177,15 +187,12 @@ class SourceParser(object): # Non-empty line outside the comment m = self.re_parameter_definition.match(line) if m: - tp, code, defval = m.group(1, 2, 3) + tp, name, defval = m.group(1, 2, 3) # Remove trailing type specifier from numbers: 0.1f => 0.1 if self.re_is_a_number.match(defval): defval = self.re_cut_type_specifier.sub('', defval) - param = Parameter() - param.SetField("code", code) - param.SetField("short_desc", code) - param.SetField("type", tp) - param.SetField("default", defval) + param = Parameter(name, tp, defval) + param.SetField("short_desc", name) # If comment was found before the parameter declaration, # inject its data into the newly created parameter. group = "Miscellaneous" @@ -211,11 +218,9 @@ class SourceParser(object): # Nasty code dup, but this will all go away soon, so quick and dirty (DonLakeFlyer) m = self.re_px4_parameter_definition.match(line) if m: - tp, code = m.group(1, 2) - param = Parameter() - param.SetField("code", code) - param.SetField("short_desc", code) - param.SetField("type", tp) + tp, name = m.group(1, 2) + param = Parameter(name, tp) + param.SetField("short_desc", name) # If comment was found before the parameter declaration, # inject its data into the newly created parameter. group = "Miscellaneous" diff --git a/Tools/px4params/xmlout.py b/Tools/px4params/xmlout.py index 89f495dc02..07cced4786 100644 --- a/Tools/px4params/xmlout.py +++ b/Tools/px4params/xmlout.py @@ -18,26 +18,36 @@ def indent(elem, level=0): class XMLOutput(): - def __init__(self, groups): + def __init__(self, groups, board): xml_parameters = ET.Element("parameters") xml_version = ET.SubElement(xml_parameters, "version") - xml_version.text = "2" + xml_version.text = "3" + last_param_name = "" + board_specific_param_set = False for group in groups: xml_group = ET.SubElement(xml_parameters, "group") xml_group.attrib["name"] = group.GetName() for param in group.GetParams(): - xml_param = ET.SubElement(xml_group, "parameter") - for code in param.GetFieldCodes(): - value = param.GetFieldValue(code) - if code == "code": - xml_param.attrib["name"] = value - elif code == "default": - xml_param.attrib["default"] = value - elif code == "type": - xml_param.attrib["type"] = value - else: - xml_field = ET.SubElement(xml_param, code) - xml_field.text = value + if (last_param_name == param.GetName() and not board_specific_param_set) or last_param_name != param.GetName(): + xml_param = ET.SubElement(xml_group, "parameter") + xml_param.attrib["name"] = param.GetName() + xml_param.attrib["default"] = param.GetDefault() + xml_param.attrib["type"] = param.GetType() + last_param_name = param.GetName() + for code in param.GetFieldCodes(): + value = param.GetFieldValue(code) + if code == "board": + if value == board: + board_specific_param_set = True + xml_field = ET.SubElement(xml_param, code) + xml_field.text = value + else: + xml_group.remove(xml_param) + else: + xml_field = ET.SubElement(xml_param, code) + xml_field.text = value + if last_param_name != param.GetName(): + board_specific_param_set = False indent(xml_parameters) self.xml_document = ET.ElementTree(xml_parameters) diff --git a/Tools/px_process_params.py b/Tools/px_process_params.py index 12128a997e..cb2202d529 100644 --- a/Tools/px_process_params.py +++ b/Tools/px_process_params.py @@ -65,6 +65,11 @@ def main(): metavar="FILENAME", help="Create XML file" " (default FILENAME: parameters.xml)") + parser.add_argument("-b", "--board", + nargs='?', + const="", + metavar="BOARD", + help="Board to create xml parameter xml for") parser.add_argument("-w", "--wiki", nargs='?', const="parameters.wiki", @@ -116,7 +121,7 @@ def main(): # Output to XML file if args.xml: print("Creating XML file " + args.xml) - out = xmlout.XMLOutput(param_groups) + out = xmlout.XMLOutput(param_groups, args.board) out.Save(args.xml) # Output to DokuWiki tables diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index 4c10de931c..af3ca249e5 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -494,7 +494,7 @@ $(filter %.S.o,$(OBJS)): $(WORK_DIR)%.S.o: %.S $(GLOBAL_DEPS) $(PRODUCT_BUNDLE): $(PRODUCT_BIN) @$(ECHO) %% Generating $@ ifdef GEN_PARAM_XML - python $(PX4_BASE)/Tools/px_process_params.py --src-path $(PX4_BASE)/src --xml + python $(PX4_BASE)/Tools/px_process_params.py --src-path $(PX4_BASE)/src --board CONFIG_ARCH_BOARD_$(CONFIG_BOARD) --xml $(Q) $(MKFW) --prototype $(IMAGE_DIR)/$(BOARD).prototype \ --git_identity $(PX4_BASE) \ --parameter_xml $(PRODUCT_PARAMXML) \ From a07712bb1e41538de72686562c8295f2688f6880 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Mon, 16 Mar 2015 10:14:07 -0600 Subject: [PATCH 207/268] apply roll/pitch acro_rate_max in MC attitude controller --- src/modules/mc_att_control/mc_att_control_main.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index d477316222..6c9048bdb4 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -656,11 +656,14 @@ MulticopterAttitudeControl::control_attitude(float dt) /* calculate angular rates setpoint */ _rates_sp = _params.att_p.emult(e_R); - /* limit rates */ - for (int i = 0; i < 3; i++) { - _rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i)); + /* limit roll and pitch rates */ + for (int i = 0; i < 2; i++) { + _rates_sp(i) = math::constrain(_rates_sp(i), -_params.acro_rate_max(i), _params.acro_rate_max(i)); } + /* limit yaw rate */ + _rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, _params.yaw_rate_max); + /* feed forward yaw setpoint rate */ _rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff; } From 7f418445f2a2108b1026a31b3763da5e6d960fe1 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Tue, 17 Mar 2015 20:52:51 -0600 Subject: [PATCH 208/268] add new parameters for roll and pitch angular rate limits --- src/modules/mc_att_control/mc_att_control_main.cpp | 11 ++++------- src/modules/mc_att_control/mc_att_control_params.c | 4 ++-- 2 files changed, 6 insertions(+), 9 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 6c9048bdb4..e140acea60 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -202,7 +202,7 @@ private: float roll_rate_max; float pitch_rate_max; float yaw_rate_max; - math::Vector<3> mc_rate_max; /**< attitude rate limits in stabilized modes */ + math::Vector<3> mc_rate_max; /**< attitude rate limits in all modes */ float man_roll_max; float man_pitch_max; @@ -656,14 +656,11 @@ MulticopterAttitudeControl::control_attitude(float dt) /* calculate angular rates setpoint */ _rates_sp = _params.att_p.emult(e_R); - /* limit roll and pitch rates */ - for (int i = 0; i < 2; i++) { - _rates_sp(i) = math::constrain(_rates_sp(i), -_params.acro_rate_max(i), _params.acro_rate_max(i)); + /* limit rates */ + for (int i = 0; i < 3; i++) { + _rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i)); } - /* limit yaw rate */ - _rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, _params.yaw_rate_max); - /* feed forward yaw setpoint rate */ _rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff; } diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 3f19a51f06..3f63f2fc06 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -215,7 +215,7 @@ PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); * @max 360.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_MAX, 360.0f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_MAX, 120.0f); /** * Max pitch rate @@ -227,7 +227,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_MAX, 360.0f); * @max 360.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_MAX, 360.0f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_MAX, 120.0f); /** * Max yaw rate From 77771b2bbf2ed784fee48d535bbac2e8ea717c49 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Wed, 18 Mar 2015 07:11:20 -0600 Subject: [PATCH 209/268] fix comment on MC att controller rate limits --- src/modules/mc_att_control/mc_att_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index e140acea60..d477316222 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -202,7 +202,7 @@ private: float roll_rate_max; float pitch_rate_max; float yaw_rate_max; - math::Vector<3> mc_rate_max; /**< attitude rate limits in all modes */ + math::Vector<3> mc_rate_max; /**< attitude rate limits in stabilized modes */ float man_roll_max; float man_pitch_max; From f23bc38d3ec45c2b3d2d72b06e2426d40cefd10c Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sat, 11 Apr 2015 08:10:58 -0600 Subject: [PATCH 210/268] increase default roll/pitch rate limits to 360dps --- src/modules/mc_att_control/mc_att_control_params.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 3f63f2fc06..3f19a51f06 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -215,7 +215,7 @@ PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); * @max 360.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_MAX, 120.0f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_MAX, 360.0f); /** * Max pitch rate @@ -227,7 +227,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_MAX, 120.0f); * @max 360.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_MAX, 120.0f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_MAX, 360.0f); /** * Max yaw rate From b52d0ed8a245cc25ffdaf3214f9cf793fe2f1ba6 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 13 Apr 2015 09:52:03 +0200 Subject: [PATCH 211/268] mc_att_control: implemented anti windup --- msg/mc_att_ctrl_status.msg | 5 +++ .../mc_att_control/mc_att_control_main.cpp | 44 ++++++++++++++++++- src/modules/uORB/objects_common.cpp | 2 + 3 files changed, 49 insertions(+), 2 deletions(-) create mode 100644 msg/mc_att_ctrl_status.msg diff --git a/msg/mc_att_ctrl_status.msg b/msg/mc_att_ctrl_status.msg new file mode 100644 index 0000000000..114e843b0e --- /dev/null +++ b/msg/mc_att_ctrl_status.msg @@ -0,0 +1,5 @@ +uint64 timestamp # in microseconds since system start + +float32 roll_rate_integ # roll rate inegrator +float32 pitch_rate_integ # pitch rate integrator +float32 yaw_rate_integ # yaw rate integrator diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index d477316222..b8c1437821 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -77,6 +77,8 @@ #include #include #include +#include +#include #include #include #include @@ -129,10 +131,12 @@ private: int _manual_control_sp_sub; /**< manual control setpoint subscription */ int _armed_sub; /**< arming status subscription */ int _vehicle_status_sub; /**< vehicle status subscription */ + int _motor_limits_sub; /**< motor limits subscription */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */ orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ + orb_advert_t _controller_status_pub; /**< controller status publication */ orb_id_t _rates_sp_id; /**< pointer to correct rates setpoint uORB metadata structure */ orb_id_t _actuators_id; /**< pointer to correct actuator controls0 uORB metadata structure */ @@ -147,6 +151,8 @@ private: struct actuator_controls_s _actuators; /**< actuator controls */ struct actuator_armed_s _armed; /**< actuator arming status */ struct vehicle_status_s _vehicle_status; /**< vehicle status */ + struct multirotor_motor_limits_s _motor_limits; /**< motor limits */ + struct mc_att_ctrl_status_s _controller_status; /**< controller status */ perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _controller_latency_perf; @@ -261,6 +267,11 @@ private: */ void vehicle_status_poll(); + /** + * Check for vehicle motor limits status. + */ + void vehicle_motor_limits_poll(); + /** * Shim for calling task_main from task_create. */ @@ -302,6 +313,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _att_sp_pub(-1), _v_rates_sp_pub(-1), _actuators_0_pub(-1), + _controller_status_pub(-1), _rates_sp_id(0), _actuators_id(0), @@ -320,6 +332,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : memset(&_actuators, 0, sizeof(_actuators)); memset(&_armed, 0, sizeof(_armed)); memset(&_vehicle_status, 0, sizeof(_vehicle_status)); + memset(&_motor_limits, 0, sizeof(_motor_limits)); + memset(&_controller_status,0,sizeof(_controller_status)); _vehicle_status.is_rotary_wing = true; _params.att_p.zero(); @@ -570,6 +584,18 @@ MulticopterAttitudeControl::vehicle_status_poll() } } +void +MulticopterAttitudeControl::vehicle_motor_limits_poll() +{ + /* check if there is a new message */ + bool updated; + orb_check(_motor_limits_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(multirotor_motor_limits), _motor_limits_sub, &_motor_limits); + } +} + /* * Attitude controller. * Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode) @@ -689,8 +715,8 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int + _params.rate_ff.emult(_rates_sp); _rates_prev = rates; - /* update integral only if not saturated on low limit */ - if (_thrust_sp > MIN_TAKEOFF_THRUST) { + /* update integral only if not saturated on low limit and if motor commands are not saturated */ + if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit ) { for (int i = 0; i < 3; i++) { if (fabsf(_att_control(i)) < _thrust_sp) { float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; @@ -725,6 +751,7 @@ MulticopterAttitudeControl::task_main() _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + _motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits)); /* initialize parameters cache */ parameters_update(); @@ -777,6 +804,7 @@ MulticopterAttitudeControl::task_main() arming_status_poll(); vehicle_manual_poll(); vehicle_status_poll(); + vehicle_motor_limits_poll(); if (_v_control_mode.flag_control_attitude_enabled) { control_attitude(dt); @@ -837,6 +865,11 @@ MulticopterAttitudeControl::task_main() _actuators.timestamp = hrt_absolute_time(); _actuators.timestamp_sample = _v_att.timestamp; + _controller_status.roll_rate_integ = _rates_int(0); + _controller_status.pitch_rate_integ = _rates_int(1); + _controller_status.yaw_rate_integ = _rates_int(2); + _controller_status.timestamp = hrt_absolute_time(); + if (!_actuators_0_circuit_breaker_enabled) { if (_actuators_0_pub > 0) { orb_publish(_actuators_id, _actuators_0_pub, &_actuators); @@ -847,6 +880,13 @@ MulticopterAttitudeControl::task_main() } } + + /* publish controller status */ + if(_controller_status_pub > 0) { + orb_publish(ORB_ID(mc_att_ctrl_status),_controller_status_pub, &_controller_status); + } else { + _controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status); + } } } diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 75977ffd1b..1a38b981e8 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -254,3 +254,5 @@ ORB_DEFINE(rc_parameter_map, struct rc_parameter_map_s); #include "topics/time_offset.h" ORB_DEFINE(time_offset, struct time_offset_s); +#include "topics/mc_att_ctrl_status.h" +ORB_DEFINE(mc_att_ctrl_status, struct mc_att_ctrl_status_s); From e097affd7aea14ffed53d31bdbee0576ae619dd5 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 13 Apr 2015 11:23:27 +0200 Subject: [PATCH 212/268] log multirotor attitude controller status --- nuttx-configs/px4-stm32f4discovery/nsh/defconfig | 2 +- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- src/modules/sdlog2/sdlog2.c | 13 +++++++++++++ src/modules/sdlog2/sdlog2_messages.h | 8 ++++++++ 5 files changed, 24 insertions(+), 3 deletions(-) diff --git a/nuttx-configs/px4-stm32f4discovery/nsh/defconfig b/nuttx-configs/px4-stm32f4discovery/nsh/defconfig index 8398c53809..6a2470bea9 100644 --- a/nuttx-configs/px4-stm32f4discovery/nsh/defconfig +++ b/nuttx-configs/px4-stm32f4discovery/nsh/defconfig @@ -392,7 +392,7 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=32 +CONFIG_NFILE_DESCRIPTORS=44 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index cd410051c7..389cb5ab5b 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -405,7 +405,7 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=42 +CONFIG_NFILE_DESCRIPTORS=44 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 4ccc5dacb6..d331aa26a7 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -439,7 +439,7 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=42 +CONFIG_NFILE_DESCRIPTORS=44 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index ed93a06b12..1b6d34d70b 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -100,6 +100,7 @@ #include #include #include +#include #include #include @@ -1029,6 +1030,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct encoders_s encoders; struct vtol_vehicle_status_s vtol_status; struct time_offset_s time_offset; + struct mc_att_ctrl_status_s mc_att_ctrl_status; } buf; memset(&buf, 0, sizeof(buf)); @@ -1074,6 +1076,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_WIND_s log_WIND; struct log_ENCD_s log_ENCD; struct log_TSYN_s log_TSYN; + struct log_MACS_s log_MACS; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1115,6 +1118,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int wind_sub; int encoders_sub; int tsync_sub; + int mc_att_ctrl_status_sub; } subs; subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -1147,6 +1151,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status)); subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate)); subs.tsync_sub = orb_subscribe(ORB_ID(time_offset)); + subs.mc_att_ctrl_status_sub = orb_subscribe(ORB_ID(mc_att_ctrl_status)); /* we need to rate-limit wind, as we do not need the full update rate */ orb_set_interval(subs.wind_sub, 90); @@ -1831,6 +1836,14 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_TSYN_MSG; log_msg.body.log_TSYN.time_offset = buf.time_offset.offset_ns; LOGBUFFER_WRITE_AND_COUNT(TSYN); + + /* --- MULTIROTOR ATTITUDE CONTROLLER STATUS --- */ + if (copy_if_updated(ORB_ID(mc_att_ctrl_status), subs.mc_att_ctrl_status_sub, &buf.mc_att_ctrl_status)) { + log_msg.msg_type = LOG_MACS_MSG; + log_msg.body.log_MACS.roll_rate_integ = buf.mc_att_ctrl_status.roll_rate_integ; + log_msg.body.log_MACS.pitch_rate_integ = buf.mc_att_ctrl_status.pitch_rate_integ; + log_msg.body.log_MACS.yaw_rate_integ = buf.mc_att_ctrl_status.yaw_rate_integ; + LOGBUFFER_WRITE_AND_COUNT(MACS); } /* signal the other thread new data, but not yet unlock */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 598f12a536..1f97cf7222 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -453,6 +453,13 @@ struct log_VTOL_s { #define LOG_TSYN_MSG 43 struct log_TSYN_s { uint64_t time_offset; + +/* --- MACS - MULTIROTOR ATTITUDE CONTROLLER STATUS */ +#define LOG_MACS_MSG 42 +struct log_MACS_s { + float roll_rate_integ; + float pitch_rate_integ; + float yaw_rate_integ; }; /********** SYSTEM MESSAGES, ID > 0x80 **********/ @@ -524,6 +531,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"), LOG_FORMAT(TSYN, "Q", "TimeOffset"), + LOG_FORMAT(MACS, "fff", "RRint,PRint,YRint"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ From f8cf4954942cfb10900d191618aafc1c3da3c751 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 18 Apr 2015 11:38:23 +0200 Subject: [PATCH 213/268] Revert "Added ability for board specific meta data generation" This reverts commit 9ac350a7d1c5c07a4e4ba7824744930f9110dedc. --- Tools/px4params/srcparser.py | 37 +++++++++++++++-------------------- Tools/px4params/xmlout.py | 38 +++++++++++++----------------------- Tools/px_process_params.py | 7 +------ makefiles/firmware.mk | 2 +- 4 files changed, 32 insertions(+), 52 deletions(-) diff --git a/Tools/px4params/srcparser.py b/Tools/px4params/srcparser.py index 0d2413a75f..891db7ecd2 100644 --- a/Tools/px4params/srcparser.py +++ b/Tools/px4params/srcparser.py @@ -37,30 +37,20 @@ class Parameter(object): # Define sorting order of the fields priority = { - "board": 9, + "code": 10, + "type": 9, "short_desc": 8, "long_desc": 7, + "default": 6, "min": 5, "max": 4, "unit": 3, # all others == 0 (sorted alphabetically) } - def __init__(self, name, type, default = ""): + def __init__(self): self.fields = {} - self.name = name - self.type = type - self.default = default - def GetName(self): - return self.name - - def GetType(self): - return self.type - - def GetDefault(self): - return self.default - def SetField(self, code, value): """ Set named field value @@ -98,7 +88,7 @@ class SourceParser(object): re_is_a_number = re.compile(r'^-?[0-9\.]') re_remove_dots = re.compile(r'\.+$') - valid_tags = set(["group", "board", "min", "max", "unit"]) + valid_tags = set(["group", "min", "max", "unit"]) # Order of parameter groups priority = { @@ -187,12 +177,15 @@ class SourceParser(object): # Non-empty line outside the comment m = self.re_parameter_definition.match(line) if m: - tp, name, defval = m.group(1, 2, 3) + tp, code, defval = m.group(1, 2, 3) # Remove trailing type specifier from numbers: 0.1f => 0.1 if self.re_is_a_number.match(defval): defval = self.re_cut_type_specifier.sub('', defval) - param = Parameter(name, tp, defval) - param.SetField("short_desc", name) + param = Parameter() + param.SetField("code", code) + param.SetField("short_desc", code) + param.SetField("type", tp) + param.SetField("default", defval) # If comment was found before the parameter declaration, # inject its data into the newly created parameter. group = "Miscellaneous" @@ -218,9 +211,11 @@ class SourceParser(object): # Nasty code dup, but this will all go away soon, so quick and dirty (DonLakeFlyer) m = self.re_px4_parameter_definition.match(line) if m: - tp, name = m.group(1, 2) - param = Parameter(name, tp) - param.SetField("short_desc", name) + tp, code = m.group(1, 2) + param = Parameter() + param.SetField("code", code) + param.SetField("short_desc", code) + param.SetField("type", tp) # If comment was found before the parameter declaration, # inject its data into the newly created parameter. group = "Miscellaneous" diff --git a/Tools/px4params/xmlout.py b/Tools/px4params/xmlout.py index 07cced4786..89f495dc02 100644 --- a/Tools/px4params/xmlout.py +++ b/Tools/px4params/xmlout.py @@ -18,36 +18,26 @@ def indent(elem, level=0): class XMLOutput(): - def __init__(self, groups, board): + def __init__(self, groups): xml_parameters = ET.Element("parameters") xml_version = ET.SubElement(xml_parameters, "version") - xml_version.text = "3" - last_param_name = "" - board_specific_param_set = False + xml_version.text = "2" for group in groups: xml_group = ET.SubElement(xml_parameters, "group") xml_group.attrib["name"] = group.GetName() for param in group.GetParams(): - if (last_param_name == param.GetName() and not board_specific_param_set) or last_param_name != param.GetName(): - xml_param = ET.SubElement(xml_group, "parameter") - xml_param.attrib["name"] = param.GetName() - xml_param.attrib["default"] = param.GetDefault() - xml_param.attrib["type"] = param.GetType() - last_param_name = param.GetName() - for code in param.GetFieldCodes(): - value = param.GetFieldValue(code) - if code == "board": - if value == board: - board_specific_param_set = True - xml_field = ET.SubElement(xml_param, code) - xml_field.text = value - else: - xml_group.remove(xml_param) - else: - xml_field = ET.SubElement(xml_param, code) - xml_field.text = value - if last_param_name != param.GetName(): - board_specific_param_set = False + xml_param = ET.SubElement(xml_group, "parameter") + for code in param.GetFieldCodes(): + value = param.GetFieldValue(code) + if code == "code": + xml_param.attrib["name"] = value + elif code == "default": + xml_param.attrib["default"] = value + elif code == "type": + xml_param.attrib["type"] = value + else: + xml_field = ET.SubElement(xml_param, code) + xml_field.text = value indent(xml_parameters) self.xml_document = ET.ElementTree(xml_parameters) diff --git a/Tools/px_process_params.py b/Tools/px_process_params.py index cb2202d529..12128a997e 100644 --- a/Tools/px_process_params.py +++ b/Tools/px_process_params.py @@ -65,11 +65,6 @@ def main(): metavar="FILENAME", help="Create XML file" " (default FILENAME: parameters.xml)") - parser.add_argument("-b", "--board", - nargs='?', - const="", - metavar="BOARD", - help="Board to create xml parameter xml for") parser.add_argument("-w", "--wiki", nargs='?', const="parameters.wiki", @@ -121,7 +116,7 @@ def main(): # Output to XML file if args.xml: print("Creating XML file " + args.xml) - out = xmlout.XMLOutput(param_groups, args.board) + out = xmlout.XMLOutput(param_groups) out.Save(args.xml) # Output to DokuWiki tables diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index af3ca249e5..4c10de931c 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -494,7 +494,7 @@ $(filter %.S.o,$(OBJS)): $(WORK_DIR)%.S.o: %.S $(GLOBAL_DEPS) $(PRODUCT_BUNDLE): $(PRODUCT_BIN) @$(ECHO) %% Generating $@ ifdef GEN_PARAM_XML - python $(PX4_BASE)/Tools/px_process_params.py --src-path $(PX4_BASE)/src --board CONFIG_ARCH_BOARD_$(CONFIG_BOARD) --xml + python $(PX4_BASE)/Tools/px_process_params.py --src-path $(PX4_BASE)/src --xml $(Q) $(MKFW) --prototype $(IMAGE_DIR)/$(BOARD).prototype \ --git_identity $(PX4_BASE) \ --parameter_xml $(PRODUCT_PARAMXML) \ From 644fb1c0bb6591154f7163ae80bc95fe2fb1eb6f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 18 Apr 2015 11:38:34 +0200 Subject: [PATCH 214/268] Revert "Remove newline so meta data parser can parse" This reverts commit 3a70e7bf1bef904c63f3bbe0a92e7c9aeda978aa. --- src/modules/fw_att_control/fw_att_control_params.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 6b248cbe2e..6ce6cef4eb 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -136,6 +136,7 @@ PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.2f); * @max 2.0 * @group FW Attitude Control */ +//xxx: set to 0 as default, see comment on turn_offset in ECL_PitchController::control_attitude(...) PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f); /** From c4b98fb47de02b38ae62da1f4e1e523c92ef2785 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 18 Apr 2015 11:39:03 +0200 Subject: [PATCH 215/268] Revert "Use new @board attribute for ifdef support" This reverts commit 750b02b4e5aa166e590c5b801310975c2f220635. --- src/modules/sensors/sensor_params.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 11d911ba92..18e47865b1 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -960,7 +960,6 @@ PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000); /** * Scaling factor for battery voltage sensor on FMU v2. * - * @board CONFIG_ARCH_BOARD_PX4FMU_V2 * @group Battery Calibration */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); @@ -970,7 +969,6 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); * * For R70 = 133K, R71 = 10K --> scale = 1.8 * 143 / (4096*10) = 0.0063 * - * @board CONFIG_ARCH_BOARD_AEROCORE * @group Battery Calibration */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0063f); From 5264eb23b421632bcea83c270a8af57370eba527 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 18 Apr 2015 11:39:13 +0200 Subject: [PATCH 216/268] Revert "Removed usage of PX4_PARAM_DEFINE_* macros" This reverts commit 5fe7f76691b80a1ea488d7ad740be5e6b4520643. --- src/modules/systemlib/circuit_breaker_params.c | 13 +++++++------ src/modules/systemlib/circuit_breaker_params.h | 7 +++++++ 2 files changed, 14 insertions(+), 6 deletions(-) create mode 100644 src/modules/systemlib/circuit_breaker_params.h diff --git a/src/modules/systemlib/circuit_breaker_params.c b/src/modules/systemlib/circuit_breaker_params.c index e5cc034bc9..e499ae27ac 100644 --- a/src/modules/systemlib/circuit_breaker_params.c +++ b/src/modules/systemlib/circuit_breaker_params.c @@ -43,6 +43,7 @@ */ #include +#include /** * Circuit breaker for power supply check @@ -55,7 +56,7 @@ * @max 894281 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK, 0); +PX4_PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK); /** * Circuit breaker for rate controller output @@ -68,7 +69,7 @@ PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK, 0); * @max 140253 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0); +PX4_PARAM_DEFINE_INT32(CBRK_RATE_CTRL); /** * Circuit breaker for IO safety @@ -80,7 +81,7 @@ PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0); * @max 22027 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0); +PX4_PARAM_DEFINE_INT32(CBRK_IO_SAFETY); /** * Circuit breaker for airspeed sensor @@ -92,7 +93,7 @@ PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0); * @max 162128 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0); +PX4_PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK); /** * Circuit breaker for flight termination @@ -105,7 +106,7 @@ PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0); * @max 121212 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212); +PX4_PARAM_DEFINE_INT32(CBRK_FLIGHTTERM); /** * Circuit breaker for engine failure detection @@ -119,4 +120,4 @@ PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212); * @max 284953 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953); +PX4_PARAM_DEFINE_INT32(CBRK_ENGINEFAIL); diff --git a/src/modules/systemlib/circuit_breaker_params.h b/src/modules/systemlib/circuit_breaker_params.h new file mode 100644 index 0000000000..768bf7f533 --- /dev/null +++ b/src/modules/systemlib/circuit_breaker_params.h @@ -0,0 +1,7 @@ +#define PARAM_CBRK_SUPPLY_CHK_DEFAULT 0 +#define PARAM_CBRK_RATE_CTRL_DEFAULT 0 +#define PARAM_CBRK_IO_SAFETY_DEFAULT 0 +#define PARAM_CBRK_AIRSPD_CHK_DEFAULT 0 +#define PARAM_CBRK_FLIGHTTERM_DEFAULT 121212 +#define PARAM_CBRK_ENGINEFAIL_DEFAULT 284953 +#define PARAM_CBRK_GPSFAIL_DEFAULT 240024 From 80b5abeb0d7210ffa7d5a78ec8c59047a4dd7d82 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 18 Apr 2015 11:40:25 +0200 Subject: [PATCH 217/268] Fix comment in FW params --- src/modules/fw_att_control/fw_att_control_params.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 6ce6cef4eb..6b248cbe2e 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -136,7 +136,6 @@ PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.2f); * @max 2.0 * @group FW Attitude Control */ -//xxx: set to 0 as default, see comment on turn_offset in ECL_PitchController::control_attitude(...) PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f); /** From e284691b07463fab0ae0e9de70b5967189d74129 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 18 Apr 2015 11:41:53 +0200 Subject: [PATCH 218/268] sdlog2: Fix typo --- src/modules/sdlog2/sdlog2_messages.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 1f97cf7222..c6f967af43 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -453,6 +453,7 @@ struct log_VTOL_s { #define LOG_TSYN_MSG 43 struct log_TSYN_s { uint64_t time_offset; +}; /* --- MACS - MULTIROTOR ATTITUDE CONTROLLER STATUS */ #define LOG_MACS_MSG 42 From 02ce3d070d2105a1f3d63051cc6b4e8efa414935 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 18 Apr 2015 11:48:44 +0200 Subject: [PATCH 219/268] sdlog2: Fix another typo --- src/modules/sdlog2/sdlog2.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 1b6d34d70b..8193cf1814 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1836,6 +1836,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_TSYN_MSG; log_msg.body.log_TSYN.time_offset = buf.time_offset.offset_ns; LOGBUFFER_WRITE_AND_COUNT(TSYN); + } /* --- MULTIROTOR ATTITUDE CONTROLLER STATUS --- */ if (copy_if_updated(ORB_ID(mc_att_ctrl_status), subs.mc_att_ctrl_status_sub, &buf.mc_att_ctrl_status)) { From 2e824bbeea8787a3166ce47cad90e622a3678b25 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 17 Mar 2015 13:37:01 -0400 Subject: [PATCH 220/268] fix incorrect argc < 1 check for no arguments -requiring arguments should be argc < 2 --- src/drivers/ardrone_interface/ardrone_interface.c | 3 ++- src/drivers/hott/hott_sensors/hott_sensors.cpp | 2 +- src/drivers/hott/hott_telemetry/hott_telemetry.cpp | 2 +- src/drivers/md25/md25_main.cpp | 3 ++- src/drivers/roboclaw/roboclaw_main.cpp | 3 ++- src/examples/fixedwing_control/main.c | 2 +- .../flow_position_estimator/flow_position_estimator_main.c | 2 +- src/examples/matlab_csv_serial/matlab_csv_serial.c | 2 +- src/examples/publisher/publisher_start_nuttx.cpp | 2 +- src/examples/px4_daemon_app/px4_daemon_app.c | 2 +- src/examples/rover_steering_control/main.cpp | 2 +- src/examples/subscriber/subscriber_start_nuttx.cpp | 2 +- .../attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 3 ++- .../attitude_estimator_so3/attitude_estimator_so3_main.cpp | 3 ++- src/modules/commander/commander.cpp | 2 +- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- src/modules/fixedwing_backside/fixedwing_backside_main.cpp | 3 ++- src/modules/fw_att_control/fw_att_control_main.cpp | 3 ++- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 3 ++- src/modules/land_detector/land_detector_main.cpp | 2 +- src/modules/mc_att_control/mc_att_control_main.cpp | 3 ++- .../mc_att_control_start_nuttx.cpp | 2 +- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 +- .../mc_pos_control_start_nuttx.cpp | 2 +- .../position_estimator_inav/position_estimator_inav_main.c | 2 +- src/modules/segway/segway_main.cpp | 3 ++- src/modules/sensors/sensors.cpp | 2 +- src/modules/vtol_att_control/vtol_att_control_main.cpp | 2 +- 28 files changed, 38 insertions(+), 28 deletions(-) diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index 7d4b7d880b..53d98ba9a1 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -108,8 +108,9 @@ usage(const char *reason) */ int ardrone_interface_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 2) { usage("missing command"); + } if (!strcmp(argv[1], "start")) { diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp index 1aade450b1..4ac7e4bdfb 100644 --- a/src/drivers/hott/hott_sensors/hott_sensors.cpp +++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp @@ -197,7 +197,7 @@ hott_sensors_thread_main(int argc, char *argv[]) int hott_sensors_main(int argc, char *argv[]) { - if (argc < 1) { + if (argc < 2) { errx(1, "missing command\n%s", commandline_usage); } diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp index 17a24d1041..43df0cb8cc 100644 --- a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp +++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp @@ -223,7 +223,7 @@ hott_telemetry_thread_main(int argc, char *argv[]) int hott_telemetry_main(int argc, char *argv[]) { - if (argc < 1) { + if (argc < 2) { errx(1, "missing command\n%s", commandline_usage); } diff --git a/src/drivers/md25/md25_main.cpp b/src/drivers/md25/md25_main.cpp index 7e5904d050..d25d16fa15 100644 --- a/src/drivers/md25/md25_main.cpp +++ b/src/drivers/md25/md25_main.cpp @@ -97,8 +97,9 @@ usage(const char *reason) int md25_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 2) { usage("missing command"); + } if (!strcmp(argv[1], "start")) { diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp index 83086fd7c8..ea7178076b 100644 --- a/src/drivers/roboclaw/roboclaw_main.cpp +++ b/src/drivers/roboclaw/roboclaw_main.cpp @@ -92,8 +92,9 @@ static void usage() int roboclaw_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 2) { usage(); + } if (!strcmp(argv[1], "start")) { diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index 17b27290c2..1d590ae61c 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -418,7 +418,7 @@ usage(const char *reason) */ int ex_fixedwing_control_main(int argc, char *argv[]) { - if (argc < 1) { + if (argc < 2) { usage("missing command"); } diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index f8e3f2dc5f..ab67bd2532 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -99,7 +99,7 @@ static void usage(const char *reason) */ int flow_position_estimator_main(int argc, char *argv[]) { - if (argc < 1) { + if (argc < 2) { usage("missing command"); } diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c index 8e439bdac5..085ef1fed7 100644 --- a/src/examples/matlab_csv_serial/matlab_csv_serial.c +++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c @@ -91,7 +91,7 @@ static void usage(const char *reason) */ int matlab_csv_serial_main(int argc, char *argv[]) { - if (argc < 1) { + if (argc < 2) { usage("missing command"); } diff --git a/src/examples/publisher/publisher_start_nuttx.cpp b/src/examples/publisher/publisher_start_nuttx.cpp index 405b3c987f..4ff2cebfbc 100644 --- a/src/examples/publisher/publisher_start_nuttx.cpp +++ b/src/examples/publisher/publisher_start_nuttx.cpp @@ -54,7 +54,7 @@ extern int main(int argc, char **argv); extern "C" __EXPORT int publisher_main(int argc, char *argv[]); int publisher_main(int argc, char *argv[]) { - if (argc < 1) { + if (argc < 2) { errx(1, "usage: publisher {start|stop|status}"); } diff --git a/src/examples/px4_daemon_app/px4_daemon_app.c b/src/examples/px4_daemon_app/px4_daemon_app.c index 860b1af787..6e99939d14 100644 --- a/src/examples/px4_daemon_app/px4_daemon_app.c +++ b/src/examples/px4_daemon_app/px4_daemon_app.c @@ -88,7 +88,7 @@ usage(const char *reason) */ int px4_daemon_app_main(int argc, char *argv[]) { - if (argc < 1) { + if (argc < 2) { usage("missing command"); } diff --git a/src/examples/rover_steering_control/main.cpp b/src/examples/rover_steering_control/main.cpp index 96b40b23f0..41df96775c 100644 --- a/src/examples/rover_steering_control/main.cpp +++ b/src/examples/rover_steering_control/main.cpp @@ -412,7 +412,7 @@ usage(const char *reason) */ int rover_steering_control_main(int argc, char *argv[]) { - if (argc < 1) { + if (argc < 2) { usage("missing command"); } diff --git a/src/examples/subscriber/subscriber_start_nuttx.cpp b/src/examples/subscriber/subscriber_start_nuttx.cpp index b450230c1a..757e8ec521 100644 --- a/src/examples/subscriber/subscriber_start_nuttx.cpp +++ b/src/examples/subscriber/subscriber_start_nuttx.cpp @@ -54,7 +54,7 @@ extern int main(int argc, char **argv); extern "C" __EXPORT int subscriber_main(int argc, char *argv[]); int subscriber_main(int argc, char *argv[]) { - if (argc < 1) { + if (argc < 2) { errx(1, "usage: subscriber {start|stop|status}"); } diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index c60d70b9f8..9bb9393c5d 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -118,8 +118,9 @@ usage(const char *reason) */ int attitude_estimator_ekf_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 2) { usage("missing command"); + } if (!strcmp(argv[1], "start")) { diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp index 9414225ca0..82bcbc66ff 100755 --- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp @@ -136,8 +136,9 @@ usage(const char *reason) */ int attitude_estimator_so3_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 2) { usage("missing command"); + } if (!strcmp(argv[1], "start")) { diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 453d5f9212..2722766b01 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -261,7 +261,7 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul int commander_main(int argc, char *argv[]) { - if (argc < 1) { + if (argc < 2) { usage("missing command"); } diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index c16e72ea0b..33289dacb0 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1516,7 +1516,7 @@ int AttitudePositionEstimatorEKF::trip_nan() int ekf_att_pos_estimator_main(int argc, char *argv[]) { - if (argc < 1) { + if (argc < 2) { errx(1, "usage: ekf_att_pos_estimator {start|stop|status|logging}"); } diff --git a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp index 71b387b1e7..bd14863247 100644 --- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp +++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp @@ -97,8 +97,9 @@ usage(const char *reason) int fixedwing_backside_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 2) { usage("missing command"); + } if (!strcmp(argv[1], "start")) { diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 8b41144f6f..0a8d07fed9 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -1134,8 +1134,9 @@ FixedwingAttitudeControl::start() int fw_att_control_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 2) { errx(1, "usage: fw_att_control {start|stop|status}"); + } if (!strcmp(argv[1], "start")) { diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 427df9739d..34265d6a4e 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1638,8 +1638,9 @@ FixedwingPositionControl::start() int fw_pos_control_l1_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 2) { errx(1, "usage: fw_pos_control_l1 {start|stop|status}"); + } if (!strcmp(argv[1], "start")) { diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index 011567e577..b4b7c33a20 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -178,7 +178,7 @@ static int land_detector_start(const char *mode) int land_detector_main(int argc, char *argv[]) { - if (argc < 1) { + if (argc < 2) { goto exiterr; } diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index b8c1437821..3a0dfe4c3b 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -922,8 +922,9 @@ MulticopterAttitudeControl::start() int mc_att_control_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 2) { errx(1, "usage: mc_att_control {start|stop|status}"); + } if (!strcmp(argv[1], "start")) { diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp index 40eb498b49..dec1e1745d 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp @@ -54,7 +54,7 @@ extern int main(int argc, char **argv); extern "C" __EXPORT int mc_att_control_m_main(int argc, char *argv[]); int mc_att_control_m_main(int argc, char *argv[]) { - if (argc < 1) { + if (argc < 2) { errx(1, "usage: mc_att_control_m {start|stop|status}"); } diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 4910454bd9..5191a4de3b 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1440,7 +1440,7 @@ MulticopterPositionControl::start() int mc_pos_control_main(int argc, char *argv[]) { - if (argc < 1) { + if (argc < 2) { errx(1, "usage: mc_pos_control {start|stop|status}"); } diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp index 1082061f64..0db67d83ff 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp @@ -54,7 +54,7 @@ extern int main(int argc, char **argv); extern "C" __EXPORT int mc_pos_control_m_main(int argc, char *argv[]); int mc_pos_control_m_main(int argc, char *argv[]) { - if (argc < 1) { + if (argc < 2) { errx(1, "usage: mc_pos_control_m {start|stop|status}"); } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index ec297e7f0c..437395b1f1 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -134,7 +134,7 @@ static void usage(const char *reason) */ int position_estimator_inav_main(int argc, char *argv[]) { - if (argc < 1) { + if (argc < 2) { usage("missing command"); } diff --git a/src/modules/segway/segway_main.cpp b/src/modules/segway/segway_main.cpp index ee492f85a2..b36d56d6d7 100644 --- a/src/modules/segway/segway_main.cpp +++ b/src/modules/segway/segway_main.cpp @@ -92,8 +92,9 @@ usage(const char *reason) int segway_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 2) { usage("missing command"); + } if (!strcmp(argv[1], "start")) { diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 259361be62..d4692ea7d2 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -2252,7 +2252,7 @@ Sensors::start() int sensors_main(int argc, char *argv[]) { - if (argc < 1) { + if (argc < 2) { errx(1, "usage: sensors {start|stop|status}"); } diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 74e1efd6cc..defcff8e4a 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -877,7 +877,7 @@ VtolAttitudeControl::start() int vtol_att_control_main(int argc, char *argv[]) { - if (argc < 1) { + if (argc < 2) { errx(1, "usage: vtol_att_control {start|stop|status}"); } From b7a6f18ca6b72529021e6fe3d5bf741673756f70 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Fri, 17 Apr 2015 11:18:02 +0200 Subject: [PATCH 221/268] AttPosEKF: Only fuse GPS velocity if they are valid --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 33289dacb0..4e1573d99a 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -937,7 +937,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const // Convert GPS measurements to Pos NE, hgt and Vel NED // set fusion flags - _ekf->fuseVelData = true; + _ekf->fuseVelData = _gps.vel_ned_valid; _ekf->fusePosData = true; // recall states stored at time of measurement after adjusting for delays From 1c8e79cbf18cd2c41024ec1d91fb8a16b1900c9f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 18 Apr 2015 20:34:18 +0200 Subject: [PATCH 222/268] sensors app: Always set a valid rotation, even if sensor is unconfigured --- src/modules/sensors/sensors.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index d4692ea7d2..09d717c16a 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1480,9 +1480,16 @@ Sensors::parameter_update_poll(bool forced) int fd = open(str, 0); if (fd < 0) { + /* the driver is not running, abort */ continue; } + /* set a valid default rotation (same as board). + * if the mag is configured, this might be replaced + * in the section below. + */ + _mag_rotation[s] = _board_rotation; + bool config_ok = false; /* run through all stored calibrations */ From 3658bf83ed92522f26bce5a4496ca3ac68c577fc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 18 Apr 2015 20:34:44 +0200 Subject: [PATCH 223/268] EKF att-only estimator: Do not fuse zero-length mag vector. --- .../attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 9bb9393c5d..b94a7a079a 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -447,7 +447,11 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds z_k[5] = raw.accelerometer_m_s2[2] - acc(2); /* update magnetometer measurements */ - if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) { + if (sensor_last_timestamp[2] != raw.magnetometer_timestamp && + /* check that the mag vector is > 0 */ + fabsf(sqrtf(raw.magnetometer_ga[0] * raw.magnetometer_ga[0] + + raw.magnetometer_ga[1] * raw.magnetometer_ga[1] + + raw.magnetometer_ga[2] * raw.magnetometer_ga[2])) > 0.1f) { update_vect[2] = 1; // sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); sensor_last_timestamp[2] = raw.magnetometer_timestamp; From 897827832029c3d2c9775ec0e7de919a8c1b62c2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 18 Apr 2015 20:49:18 +0200 Subject: [PATCH 224/268] EKF combined att + pos estimator: Robustify against mag 0 vectors and timeouts --- .../ekf_att_pos_estimator_main.cpp | 29 +++++++++++++------ 1 file changed, 20 insertions(+), 9 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 4e1573d99a..e0b4cb2a0d 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1422,30 +1422,41 @@ void AttitudePositionEstimatorEKF::pollData() int last_mag_main = _mag_main; - // XXX we compensate the offsets upfront - should be close to zero. + Vector3f mag0(_sensor_combined.magnetometer_ga[0], _sensor_combined.magnetometer_ga[1], + _sensor_combined.magnetometer_ga[2]); + + Vector3f mag1(_sensor_combined.magnetometer1_ga[0], _sensor_combined.magnetometer1_ga[1], + _sensor_combined.magnetometer1_ga[2]); + + const unsigned mag_timeout_us = 200000; /* fail over to the 2nd mag if we know the first is down */ - if (_sensor_combined.magnetometer_errcount <= _sensor_combined.magnetometer1_errcount) { - _ekf->magData.x = _sensor_combined.magnetometer_ga[0]; + if (hrt_elapsed_time(&_sensor_combined.magnetometer_timestamp) < mag_timeout_us && + _sensor_combined.magnetometer_errcount <= _sensor_combined.magnetometer1_errcount && + mag0.length() > 0.1f) { + _ekf->magData.x = mag0.x; _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset - _ekf->magData.y = _sensor_combined.magnetometer_ga[1]; + _ekf->magData.y = mag0.y; _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset - _ekf->magData.z = _sensor_combined.magnetometer_ga[2]; + _ekf->magData.z = mag0.z; _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset _mag_main = 0; - } else { - _ekf->magData.x = _sensor_combined.magnetometer1_ga[0]; + } else if (hrt_elapsed_time(&_sensor_combined.magnetometer1_timestamp) < mag_timeout_us && + mag1.length() > 0.1f) { + _ekf->magData.x = mag1.x; _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset - _ekf->magData.y = _sensor_combined.magnetometer1_ga[1]; + _ekf->magData.y = mag1.y; _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset - _ekf->magData.z = _sensor_combined.magnetometer1_ga[2]; + _ekf->magData.z = mag1.z; _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset _mag_main = 1; + } else { + _mag_valid = false; } if (last_mag_main != _mag_main) { From ea2c6549e42f5f797604c235e936ffab3aa653b7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 19 Apr 2015 11:59:31 +0200 Subject: [PATCH 225/268] Attitude only EKF: Minor style cleanup, remove unused code --- .../attitude_estimator_ekf_main.cpp | 34 +++++-------------- 1 file changed, 8 insertions(+), 26 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index b94a7a079a..b2a3572a7d 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -125,7 +125,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (thread_running) { - printf("attitude_estimator_ekf already running\n"); + warnx("already running\n"); /* this is not an error */ exit(0); } @@ -177,8 +177,6 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) int attitude_estimator_ekf_thread_main(int argc, char *argv[]) { -const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds - float dt = 0.005f; /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */ @@ -209,14 +207,10 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds }; /**< init: identity matrix */ float debugOutput[4] = { 0.0f }; - int overloadcounter = 19; /* Initialize filter */ AttitudeEKF_initialize(); - /* store start time to guard against too slow update rates */ - uint64_t last_run = hrt_absolute_time(); - struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); struct vehicle_gps_position_s gps; @@ -318,7 +312,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode); if (!control_mode.flag_system_hil_enabled) { - warnx("WARNING: Not getting sensors - sensor app running?"); + warnx("WARNING: Not getting sensor data - sensor app running?"); } } else { @@ -482,20 +476,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds z_k[8] = raw.magnetometer_ga[2]; } - uint64_t now = hrt_absolute_time(); - unsigned int time_elapsed = now - last_run; - last_run = now; - - if (time_elapsed > loop_interval_alarm) { - //TODO: add warning, cpu overload here - // if (overloadcounter == 20) { - // printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm); - // overloadcounter = 0; - // } - - overloadcounter++; - } - static bool const_initialized = false; /* initialize with good values once we have a reasonable dt estimate */ @@ -565,8 +545,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds continue; } - if (last_data > 0 && raw.timestamp - last_data > 30000) - printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); + if (last_data > 0 && raw.timestamp - last_data > 30000) { + warnx("sensor data missed! (%llu)\n", raw.timestamp - last_data); + } last_data = raw.timestamp; @@ -602,12 +583,13 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds memcpy(&att.q[0],&q.data[0],sizeof(att.q)); att.R_valid = true; - if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { + if (isfinite(att.q[0]) && isfinite(att.q[1]) + && isfinite(att.q[2]) && isfinite(att.q[3])) { // Broadcast orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); } else { - warnx("NaN in roll/pitch/yaw estimate!"); + warnx("ERR: NaN estimate!"); } perf_end(ekf_loop_perf); From 1685f77031935186db771aec47678fe96c705da2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 19 Apr 2015 15:36:02 +0200 Subject: [PATCH 226/268] Loosen the thresholds on gyro calibration based on in-field calibration feedback --- src/modules/commander/gyro_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index e2a7ef7432..1a7e0a34ad 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -196,7 +196,7 @@ int do_gyro_calibration(int mavlink_fd) float zdiff = gyro_report_0.z - gyro_scale[0].z_offset; /* maximum allowable calibration error in radians */ - const float maxoff = 0.002f; + const float maxoff = 0.004f; if (!isfinite(gyro_scale[0].x_offset) || !isfinite(gyro_scale[0].y_offset) || From 1da048549aee5b24861ea37d094b0d770fa9868d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 19 Apr 2015 16:06:52 +0200 Subject: [PATCH 227/268] commander gyro cal: Optimize parameter set calls and allow up to 0.0055 rad/s deviation - tuned to allow in-field calibration, but fail anyone really rotating during the step --- src/modules/commander/gyro_calibration.cpp | 26 +++++++++++----------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 1a7e0a34ad..291ccfe804 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -86,13 +86,6 @@ int do_gyro_calibration(int mavlink_fd) int res = OK; - /* store board ID */ - uint32_t mcu_id[3]; - mcu_unique_id(&mcu_id[0]); - - /* store last 32bit number - not unique, but unique in a given set */ - (void)param_set(param_find("CAL_BOARD_ID"), &mcu_id[2]); - char str[30]; for (unsigned s = 0; s < max_gyros; s++) { @@ -196,7 +189,7 @@ int do_gyro_calibration(int mavlink_fd) float zdiff = gyro_report_0.z - gyro_scale[0].z_offset; /* maximum allowable calibration error in radians */ - const float maxoff = 0.004f; + const float maxoff = 0.0055f; if (!isfinite(gyro_scale[0].x_offset) || !isfinite(gyro_scale[0].y_offset) || @@ -204,7 +197,7 @@ int do_gyro_calibration(int mavlink_fd) fabsf(xdiff) > maxoff || fabsf(ydiff) > maxoff || fabsf(zdiff) > maxoff) { - mavlink_log_critical(mavlink_fd, "ERROR: Motion during calibration"); + mavlink_and_console_log_critical(mavlink_fd, "ERROR: Motion during calibration"); res = ERROR; } } @@ -221,13 +214,13 @@ int do_gyro_calibration(int mavlink_fd) } (void)sprintf(str, "CAL_GYRO%u_XOFF", s); - failed |= (OK != param_set(param_find(str), &(gyro_scale[s].x_offset))); + failed |= (OK != param_set_no_notification(param_find(str), &(gyro_scale[s].x_offset))); (void)sprintf(str, "CAL_GYRO%u_YOFF", s); - failed |= (OK != param_set(param_find(str), &(gyro_scale[s].y_offset))); + failed |= (OK != param_set_no_notification(param_find(str), &(gyro_scale[s].y_offset))); (void)sprintf(str, "CAL_GYRO%u_ZOFF", s); - failed |= (OK != param_set(param_find(str), &(gyro_scale[s].z_offset))); + failed |= (OK != param_set_no_notification(param_find(str), &(gyro_scale[s].z_offset))); (void)sprintf(str, "CAL_GYRO%u_ID", s); - failed |= (OK != param_set(param_find(str), &(device_id[s]))); + failed |= (OK != param_set_no_notification(param_find(str), &(device_id[s]))); /* apply new scaling and offsets */ (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); @@ -252,6 +245,13 @@ int do_gyro_calibration(int mavlink_fd) } } + /* store board ID */ + uint32_t mcu_id[3]; + mcu_unique_id(&mcu_id[0]); + + /* store last 32bit number - not unique, but unique in a given set */ + (void)param_set(param_find("CAL_BOARD_ID"), &mcu_id[2]); + if (res == OK) { /* auto-save to EEPROM */ res = param_save_default(); From fde244f9032d5ce5bdac289699f06b4b3d800d48 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Sun, 8 Mar 2015 14:48:30 +0100 Subject: [PATCH 228/268] Commander: Add PreflightCheck to the commander --- src/modules/commander/PreflightCheck.cpp | 222 +++++++++++++++++++++++ src/modules/commander/PreflightCheck.h | 63 +++++++ src/modules/commander/commander.cpp | 1 + src/modules/commander/module.mk | 3 +- 4 files changed, 288 insertions(+), 1 deletion(-) create mode 100644 src/modules/commander/PreflightCheck.cpp create mode 100644 src/modules/commander/PreflightCheck.h diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp new file mode 100644 index 0000000000..873f2c2890 --- /dev/null +++ b/src/modules/commander/PreflightCheck.cpp @@ -0,0 +1,222 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file PreflightCheck.cpp + * + * Preflight check for main system components + * + * @author Lorenz Meier + * @author Johan Jansen + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include "PreflightCheck.h" + +namespace Commander +{ + static bool magnometerCheck(int mavlink_fd) + { + int fd = open(MAG0_DEVICE_PATH, 0); + if (fd < 0) { + warn("failed to open magnetometer - start with 'hmc5883 start' or 'lsm303d start'"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: NO MAG"); + return false; + } + + int calibration_devid; + int devid = ioctl(fd, DEVIOCGDEVICEID,0); + param_get(param_find("CAL_MAG0_ID"), &(calibration_devid)); + if (devid != calibration_devid){ + warnx("magnetometer calibration is for a different device - calibrate magnetometer first (dev: %d vs cal: %d)", devid, calibration_devid); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CAL ID"); + return false; + } + + int ret = ioctl(fd, MAGIOCSELFTEST, 0); + if (ret != OK) { + warnx("magnetometer calibration missing or bad - calibrate magnetometer first"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CHECK/CAL"); + return false; + } + close(fd); + return true; + } + + static bool accelerometerCheck(int mavlink_fd) + { + int fd = open(ACCEL0_DEVICE_PATH, O_RDONLY); + int ret = ioctl(fd, ACCELIOCSELFTEST, 0); + + int calibration_devid; + int devid = ioctl(fd, DEVIOCGDEVICEID,0); + param_get(param_find("CAL_ACC0_ID"), &(calibration_devid)); + if (devid != calibration_devid){ + warnx("accelerometer calibration is for a different device - calibrate accelerometer first"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACC CAL ID"); + return false; + } + + if (ret != OK) { + warnx("accel self test failed"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK/CAL"); + return false; + } + + // check measurement result range + struct accel_report acc; + ret = read(fd, &acc, sizeof(acc)); + + if (ret == sizeof(acc)) { + // evaluate values + float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); + + // evaluate values + if (accel_magnitude > 30.0f) { //m/s^2 + warnx("accel with spurious values"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: |ACCEL| > 30 m/s^2"); + //this is frickin' fatal + return false; + } + } else { + warnx("accel read failed"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL READ"); + //this is frickin' fatal + return false; + } + + close(fd); + return true; + } + + static bool gyroCheck(int mavlink_fd) + { + int fd = open(GYRO0_DEVICE_PATH, 0); + + int calibration_devid; + int devid = ioctl(fd, DEVIOCGDEVICEID,0); + param_get(param_find("CAL_GYRO0_ID"), &(calibration_devid)); + if (devid != calibration_devid){ + warnx("gyro calibration is for a different device - calibrate gyro first"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CAL ID"); + return false; + } + + int ret = ioctl(fd, GYROIOCSELFTEST, 0); + + if (ret != OK) { + warnx("gyro self test failed"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK/CAL"); + return false; + } + + close(fd); + return true; + } + + static bool baroCheck(int mavlink_fd) + { + int fd = open(BARO0_DEVICE_PATH, 0); + if(fd < 0) { + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: Barometer"); + return false; + } + + close(fd); + return true; + } + + bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro, bool checkBaro, bool checkRC) + { + //give the system some time to sample the sensors in the background + usleep(150000); + + //Magnetometer + if(checkMag) { + if(!magnometerCheck(mavlink_fd)) { + return false; + } + } + + //Accelerometer + if(checkAcc) { + if(!accelerometerCheck(mavlink_fd)) { + return false; + } + } + + // ---- GYRO ---- + if(checkGyro) { + if(!gyroCheck(mavlink_fd)) { + return false; + } + } + + // ---- BARO ---- + if(checkBaro) { + if(!baroCheck(mavlink_fd)) { + return false; + } + } + + // ---- RC CALIBRATION ---- + if(checkRC) { + if(rc_calibration_check(mavlink_fd) != OK) { + return false; + } + } + + //All is good! + return true; + } +} diff --git a/src/modules/commander/PreflightCheck.h b/src/modules/commander/PreflightCheck.h new file mode 100644 index 0000000000..e29357ec91 --- /dev/null +++ b/src/modules/commander/PreflightCheck.h @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file PreflightCheck.h + * + * Preflight check for main system components + * + * @author Johan Jansen + */ + +#pragma once + +namespace Commander +{ + /** + * @brief + * Runs a preflight check on all sensors to see if they are properly calibrated and healthy + * @param mavlink_fd + * Mavlink output file descriptor for feedback when a sensor fails + * @param checkMag + * true if the magneteometer should be checked + * @param checkAcc + * true if the accelerometers should be checked + * @param checkGyro + * true if the gyroscopes should be checked + * @param checkBaro + * true if the barometer should be checked + * @param checkRC + * true if the Remote Controller should be checked + **/ + bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro, bool checkBaro, bool checkRC); +} diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2722766b01..10d49aa941 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -114,6 +114,7 @@ #include "baro_calibration.h" #include "rc_calibration.h" #include "airspeed_calibration.h" +#include "PreflightCheck.h" /* oddly, ERROR is not defined for c++ */ #ifdef ERROR diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index 4ee8732fcf..4437041e26 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -46,7 +46,8 @@ SRCS = commander.cpp \ mag_calibration.cpp \ baro_calibration.cpp \ rc_calibration.cpp \ - airspeed_calibration.cpp + airspeed_calibration.cpp \ + PreflightCheck.cpp MODULE_STACKSIZE = 5000 From 6f338eb1b205a124dab28b61a99b65e9950eb6a8 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Sun, 8 Mar 2015 14:49:55 +0100 Subject: [PATCH 229/268] Commander: Run preflight check on boot --- src/modules/commander/commander.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 10d49aa941..de2355e043 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -536,9 +536,16 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s break; case VEHICLE_CMD_COMPONENT_ARM_DISARM: { + + //Refuse to arm if preflight checks have failed + if(!status.condition_system_sensors_initialized) { + mavlink_log_critical(mavlink_fd, "Arming DENIED. Preflight checks have failed."); + cmd_result = VEHICLE_CMD_RESULT_DENIED; + break; + } + // Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints // for logic state parameters - if (static_cast(cmd->param1 + 0.5f) != 0 && static_cast(cmd->param1 + 0.5f) != 1) { mavlink_log_critical(mavlink_fd, "Unsupported ARM_DISARM param: %.3f", (double)cmd->param1); @@ -1115,6 +1122,8 @@ int commander_thread_main(int argc, char *argv[]) commander_initialized = true; thread_running = true; + //Run preflight check + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, true); const hrt_abstime commander_boot_timestamp = hrt_absolute_time(); transition_result_t arming_ret; @@ -2104,7 +2113,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu rgbled_set_mode(RGBLED_MODE_ON); set_normal_color = true; - } else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) { + } else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR || !status.condition_system_sensors_initialized) { rgbled_set_mode(RGBLED_MODE_BLINK_FAST); rgbled_set_color(RGBLED_COLOR_RED); From c5a178a777c29f72e8d6ec6e9fc0623515aa4773 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Sun, 8 Mar 2015 14:50:35 +0100 Subject: [PATCH 230/268] Commander: Play startup tune if preflight checks are good, play alarm otherwise --- src/drivers/stm32/tone_alarm/tone_alarm.cpp | 5 +++-- src/modules/commander/commander.cpp | 7 +++++++ 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index 8b68473489..a18b54981f 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -890,8 +890,9 @@ tone_alarm_main(int argc, char *argv[]) if (argc > 1) { const char *argv1 = argv[1]; - if (!strcmp(argv1, "start")) - play_tune(TONE_STARTUP_TUNE); + if (!strcmp(argv1, "start")) { + play_tune(TONE_STOP_TUNE); + } if (!strcmp(argv1, "stop")) play_tune(TONE_STOP_TUNE); diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index de2355e043..bf17a7b316 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1124,6 +1124,13 @@ int commander_thread_main(int argc, char *argv[]) //Run preflight check status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, true); + if(!status.condition_system_sensors_initialized) { + set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune + } + else { + set_tune_override(TONE_STARTUP_TUNE); //normal boot tune + } + const hrt_abstime commander_boot_timestamp = hrt_absolute_time(); transition_result_t arming_ret; From b70138c631985fe09a3f373a3084346811199878 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Sun, 8 Mar 2015 14:51:23 +0100 Subject: [PATCH 231/268] Commander: Re-run preflight check after calibration --- src/modules/commander/commander.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bf17a7b316..01f0840073 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2703,12 +2703,22 @@ void *commander_low_prio_loop(void *arg) tune_negative(true); } + // Update preflight check status + // we do not set the calibration return value based on it because the calibration + // might have worked just fine, but the preflight check fails for a different reason, + // so this would be prone to false negatives. + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, true); + arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); break; } case VEHICLE_CMD_PREFLIGHT_STORAGE: { + // Update preflight check status + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, true); + + arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (((int)(cmd.param1)) == 0) { int ret = param_load_default(); From 4654d0f4fcf5d76bdd0f186c013ed1e349d08053 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Sun, 8 Mar 2015 14:51:59 +0100 Subject: [PATCH 232/268] Commander: Enter ARMING_STATE_STANDBY_ERROR by default if preflight has failed --- src/modules/commander/state_machine_helper.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index e1d2d72d1d..58a123adfd 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -78,11 +78,11 @@ static const int ERROR = -1; // code for those checks. static const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX] = { // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE - { /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, false, false, false, false }, + { /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, false, true, false, false }, { /* vehicle_status_s::ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, { /* vehicle_status_s::ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, { /* vehicle_status_s::ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, - { /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false }, + { /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ true, true, true, true, true, false, false }, { /* vehicle_status_s::ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, { /* vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI }; @@ -215,6 +215,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational."); feedback_provided = true; valid_transition = false; + new_arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; } // Finish up the state transition From 3365e577e76263527fd47f17792823ca3fe3b578 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Sun, 8 Mar 2015 14:55:51 +0100 Subject: [PATCH 233/268] ROMFS: Update for preflight check in Commander --- ROMFS/px4fmu_common/init.d/rc.sensors | 8 +++----- ROMFS/px4fmu_common/init.d/rcS | 8 ++++++-- 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index ffded7d6b9..b8a704b90f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -115,9 +115,7 @@ then fi # -# Start sensors -> preflight_check +# Start sensors # -if sensors start -then - preflight_check & -fi +sensors start + diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 2d3722f089..25a46c103f 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -301,6 +301,11 @@ then then fi + # + # Sensors System (start before Commander so Preflight checks are properly run) + # + sh /etc/init.d/rc.sensors + # Needs to be this early for in-air-restarts commander start @@ -467,9 +472,8 @@ then sh /etc/init.d/rc.uavcan # - # Sensors, Logging, GPS + # Logging, GPS # - sh /etc/init.d/rc.sensors sh /etc/init.d/rc.logging if [ $GPS == yes ] From ba25f7a290a57eafca7f866406e57d09adda9177 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Sun, 8 Mar 2015 14:56:55 +0100 Subject: [PATCH 234/268] Make: Disable build of preflight check (pending removal) --- makefiles/config_px4fmu-v2_default.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 324e12696a..ebef243a12 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -53,7 +53,7 @@ MODULES += systemcmds/bl_update MODULES += systemcmds/mixer MODULES += systemcmds/param MODULES += systemcmds/perf -MODULES += systemcmds/preflight_check +#MODULES += systemcmds/preflight_check MODULES += systemcmds/pwm MODULES += systemcmds/esc_calib MODULES += systemcmds/reboot From a6c57afabd22a7f683bad07d396c178c59e3dc7c Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 17 Mar 2015 09:28:24 +0100 Subject: [PATCH 235/268] Make: Remove deprecated preflight check --- makefiles/config_aerocore_default.mk | 1 - makefiles/config_px4fmu-v1_default.mk | 1 - makefiles/config_px4fmu-v2_default.mk | 1 - makefiles/config_px4fmu-v2_multiplatform.mk | 1 - makefiles/config_px4fmu-v2_test.mk | 1 - src/systemcmds/preflight_check/module.mk | 44 --- .../preflight_check/preflight_check.c | 286 ------------------ 7 files changed, 335 deletions(-) delete mode 100644 src/systemcmds/preflight_check/module.mk delete mode 100644 src/systemcmds/preflight_check/preflight_check.c diff --git a/makefiles/config_aerocore_default.mk b/makefiles/config_aerocore_default.mk index 9ab63a9734..c906d54189 100644 --- a/makefiles/config_aerocore_default.mk +++ b/makefiles/config_aerocore_default.mk @@ -31,7 +31,6 @@ MODULES += systemcmds/ver MODULES += systemcmds/mixer MODULES += systemcmds/param MODULES += systemcmds/perf -MODULES += systemcmds/preflight_check MODULES += systemcmds/pwm MODULES += systemcmds/esc_calib MODULES += systemcmds/reboot diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index e3515a927e..55b73ffde3 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -45,7 +45,6 @@ MODULES += systemcmds/mtd MODULES += systemcmds/mixer MODULES += systemcmds/param MODULES += systemcmds/perf -MODULES += systemcmds/preflight_check MODULES += systemcmds/pwm MODULES += systemcmds/esc_calib MODULES += systemcmds/reboot diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index ebef243a12..cbfda9735f 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -53,7 +53,6 @@ MODULES += systemcmds/bl_update MODULES += systemcmds/mixer MODULES += systemcmds/param MODULES += systemcmds/perf -#MODULES += systemcmds/preflight_check MODULES += systemcmds/pwm MODULES += systemcmds/esc_calib MODULES += systemcmds/reboot diff --git a/makefiles/config_px4fmu-v2_multiplatform.mk b/makefiles/config_px4fmu-v2_multiplatform.mk index 568f940eea..b98532f715 100644 --- a/makefiles/config_px4fmu-v2_multiplatform.mk +++ b/makefiles/config_px4fmu-v2_multiplatform.mk @@ -51,7 +51,6 @@ MODULES += systemcmds/bl_update MODULES += systemcmds/mixer MODULES += systemcmds/param MODULES += systemcmds/perf -MODULES += systemcmds/preflight_check MODULES += systemcmds/pwm MODULES += systemcmds/esc_calib MODULES += systemcmds/reboot diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 0d04a1f19d..154f7e5bad 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -45,7 +45,6 @@ MODULES += systemcmds/bl_update MODULES += systemcmds/mixer MODULES += systemcmds/param MODULES += systemcmds/perf -MODULES += systemcmds/preflight_check MODULES += systemcmds/pwm MODULES += systemcmds/esc_calib MODULES += systemcmds/reboot diff --git a/src/systemcmds/preflight_check/module.mk b/src/systemcmds/preflight_check/module.mk deleted file mode 100644 index 0cb2a4cd09..0000000000 --- a/src/systemcmds/preflight_check/module.mk +++ /dev/null @@ -1,44 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Pre-flight check. Locks down system for a few systems with blinking leds -# and buzzer if the sensors do not report an OK status. -# - -MODULE_COMMAND = preflight_check -SRCS = preflight_check.c - -MAXOPTIMIZATION = -Os - -MODULE_STACKSIZE = 1800 diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c deleted file mode 100644 index 361ca67545..0000000000 --- a/src/systemcmds/preflight_check/preflight_check.c +++ /dev/null @@ -1,286 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file preflight_check.c - * - * Preflight check for main system components - * - * @author Lorenz Meier - */ - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -__EXPORT int preflight_check_main(int argc, char *argv[]); -static int led_toggle(int leds, int led); -static int led_on(int leds, int led); -static int led_off(int leds, int led); - -int preflight_check_main(int argc, char *argv[]) -{ - bool fail_on_error = false; - - if (argc > 1 && !strcmp(argv[1], "--help")) { - warnx("usage: preflight_check [--fail-on-error]\n\tif fail on error is enabled, will return 1 on error"); - exit(1); - } - - if (argc > 1 && !strcmp(argv[1], "--fail-on-error")) { - fail_on_error = true; - } - - bool system_ok = true; - - int fd; - /* open text message output path */ - int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - int ret; - int32_t devid, calibration_devid; - - /* give the system some time to sample the sensors in the background */ - usleep(150000); - - /* ---- MAG ---- */ - fd = open(MAG0_DEVICE_PATH, 0); - if (fd < 0) { - warn("failed to open magnetometer - start with 'hmc5883 start' or 'lsm303d start'"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: NO MAG"); - system_ok = false; - goto system_eval; - } - - devid = ioctl(fd, DEVIOCGDEVICEID,0); - param_get(param_find("CAL_MAG0_ID"), &(calibration_devid)); - if (devid != calibration_devid){ - warnx("magnetometer calibration is for a different device - calibrate magnetometer first"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CAL ID"); - system_ok = false; - goto system_eval; - } - - ret = ioctl(fd, MAGIOCSELFTEST, 0); - - if (ret != OK) { - warnx("magnetometer calibration missing or bad - calibrate magnetometer first"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CHECK/CAL"); - system_ok = false; - goto system_eval; - } - - /* ---- ACCEL ---- */ - - close(fd); - fd = open(ACCEL0_DEVICE_PATH, O_RDONLY); - - devid = ioctl(fd, DEVIOCGDEVICEID,0); - param_get(param_find("CAL_ACC0_ID"), &(calibration_devid)); - if (devid != calibration_devid){ - warnx("accelerometer calibration is for a different device - calibrate accelerometer first"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACC CAL ID"); - system_ok = false; - goto system_eval; - } - - ret = ioctl(fd, ACCELIOCSELFTEST, 0); - - if (ret != OK) { - warnx("accel self test failed"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK/CAL"); - system_ok = false; - goto system_eval; - } - - /* check measurement result range */ - struct accel_report acc; - ret = read(fd, &acc, sizeof(acc)); - - if (ret == sizeof(acc)) { - /* evaluate values */ - if (sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z) > 30.0f /* m/s^2 */) { - warnx("accel with spurious values"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: |ACCEL| > 30 m/s^2"); - /* this is frickin' fatal */ - fail_on_error = true; - system_ok = false; - goto system_eval; - } - } else { - warnx("accel read failed"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL READ"); - /* this is frickin' fatal */ - fail_on_error = true; - system_ok = false; - goto system_eval; - } - - /* ---- GYRO ---- */ - - close(fd); - fd = open(GYRO0_DEVICE_PATH, 0); - - devid = ioctl(fd, DEVIOCGDEVICEID,0); - param_get(param_find("CAL_GYRO0_ID"), &(calibration_devid)); - if (devid != calibration_devid){ - warnx("gyro calibration is for a different device - calibrate gyro first"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CAL ID"); - system_ok = false; - goto system_eval; - } - - ret = ioctl(fd, GYROIOCSELFTEST, 0); - - if (ret != OK) { - warnx("gyro self test failed"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK/CAL"); - system_ok = false; - goto system_eval; - } - - /* ---- BARO ---- */ - - close(fd); - fd = open(BARO0_DEVICE_PATH, 0); - close(fd); - - /* ---- RC CALIBRATION ---- */ - - bool rc_ok = (OK == rc_calibration_check(mavlink_fd)); - - /* warn */ - if (!rc_ok) - warnx("rc calibration test failed"); - - /* require RC ok to keep system_ok */ - system_ok &= rc_ok; - - - - -system_eval: - - if (system_ok) { - /* all good, exit silently */ - exit(0); - } else { - fflush(stdout); - - warnx("PREFLIGHT CHECK ERROR! TRIGGERING ALARM"); - fflush(stderr); - - int buzzer = open(TONEALARM0_DEVICE_PATH, O_WRONLY); - int leds = open(LED0_DEVICE_PATH, 0); - - if (leds < 0) { - close(buzzer); - errx(1, "failed to open leds, aborting"); - } - - /* flip blue led into alternating amber */ - led_off(leds, LED_BLUE); - led_off(leds, LED_AMBER); - led_toggle(leds, LED_BLUE); - - /* display and sound error */ - for (int i = 0; i < 14; i++) - { - led_toggle(leds, LED_BLUE); - led_toggle(leds, LED_AMBER); - - if (i % 10 == 0) { - ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE); - } else if (i % 5 == 0) { - ioctl(buzzer, TONE_SET_ALARM, TONE_ERROR_TUNE); - } - usleep(100000); - } - - /* stop alarm */ - ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); - - /* switch off leds */ - led_on(leds, LED_BLUE); - led_on(leds, LED_AMBER); - close(leds); - - if (fail_on_error) { - /* exit with error message */ - exit(1); - } else { - /* do not emit an error code to make sure system still boots */ - exit(0); - } - } -} - -static int led_toggle(int leds, int led) -{ - static int last_blue = LED_ON; - static int last_amber = LED_ON; - - if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON; - - if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON; - - return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led); -} - -static int led_off(int leds, int led) -{ - return ioctl(leds, LED_OFF, led); -} - -static int led_on(int leds, int led) -{ - return ioctl(leds, LED_ON, led); -} From 7a4ac0ad7fa5a8faad927067c5267b052d5b9cd1 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 17 Mar 2015 10:06:02 +0100 Subject: [PATCH 236/268] Commander: Ignore sensor status on in-air restore --- src/modules/commander/commander.cpp | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 01f0840073..e2a143f7f2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -537,13 +537,6 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case VEHICLE_CMD_COMPONENT_ARM_DISARM: { - //Refuse to arm if preflight checks have failed - if(!status.condition_system_sensors_initialized) { - mavlink_log_critical(mavlink_fd, "Arming DENIED. Preflight checks have failed."); - cmd_result = VEHICLE_CMD_RESULT_DENIED; - break; - } - // Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints // for logic state parameters if (static_cast(cmd->param1 + 0.5f) != 0 && static_cast(cmd->param1 + 0.5f) != 1) { @@ -557,6 +550,16 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s if (cmd->source_system == status_local->system_id && cmd->source_component == status_local->component_id) { status_local->arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE; } + else { + + //Refuse to arm if preflight checks have failed + if(!status.condition_system_sensors_initialized) { + mavlink_log_critical(mavlink_fd, "Arming DENIED. Preflight checks have failed."); + cmd_result = VEHICLE_CMD_RESULT_DENIED; + break; + } + + } transition_result_t arming_res = arm_disarm(cmd_arms, mavlink_fd, "arm/disarm component command"); From d8c91b9fbde6a55de9f89e4f385ca61039e37c0e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 19 Apr 2015 13:55:43 +0200 Subject: [PATCH 237/268] MAVLink app: Be less verbose --- src/modules/mavlink/mavlink_parameters.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index e7e0c11dfb..20d7cfdbbe 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -74,7 +74,6 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) (req_list.target_component == mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) { _send_all_index = 0; - _mavlink->send_statustext_info("[pm] sending list"); } break; } From 5c44146c1bf516424aecb187a9c73f5d1b7518d7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 19 Apr 2015 13:56:28 +0200 Subject: [PATCH 238/268] sensors app: Be less verbose --- src/modules/sensors/sensors.cpp | 29 +++++++++++++---------------- 1 file changed, 13 insertions(+), 16 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 09d717c16a..3fc8627c15 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1389,7 +1389,6 @@ Sensors::parameter_update_poll(bool forced) if (res) { warnx(CAL_FAILED_APPLY_CAL_MSG, "gyro", i); } else { - gyro_count++; config_ok = true; } } @@ -1397,11 +1396,11 @@ Sensors::parameter_update_poll(bool forced) } } - close(fd); - - if (!config_ok) { - warnx("NO CONFIG FOR GYRO #%u", s); + if (config_ok) { + gyro_count++; } + + close(fd); } /* run through all accel sensors */ @@ -1456,7 +1455,6 @@ Sensors::parameter_update_poll(bool forced) if (res) { warnx(CAL_FAILED_APPLY_CAL_MSG, "accel", i); } else { - accel_count++; config_ok = true; } } @@ -1464,11 +1462,11 @@ Sensors::parameter_update_poll(bool forced) } } - close(fd); - - if (!config_ok) { - warnx("NO CONFIG FOR ACCEL #%u", s); + if (config_ok) { + accel_count++; } + + close(fd); } /* run through all mag sensors */ @@ -1580,7 +1578,6 @@ Sensors::parameter_update_poll(bool forced) if (res) { warnx(CAL_FAILED_APPLY_CAL_MSG, "mag", i); } else { - mag_count++; config_ok = true; } } @@ -1588,11 +1585,11 @@ Sensors::parameter_update_poll(bool forced) } } - close(fd); - - if (!config_ok) { - warnx("NO CONFIG FOR MAG #%u", s); + if (config_ok) { + mag_count++; } + + close(fd); } int fd = open(AIRSPEED0_DEVICE_PATH, 0); @@ -1612,7 +1609,7 @@ Sensors::parameter_update_poll(bool forced) close(fd); } - warnx("config: %u gyros, %u mags, %u accels", gyro_count, mag_count, accel_count); + warnx("valid configs: %u gyros, %u mags, %u accels", gyro_count, mag_count, accel_count); } } From 7dbb6c4fa80039bd9c42a83161f18da9033ac40a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 19 Apr 2015 13:57:07 +0200 Subject: [PATCH 239/268] Commander: Improved preflight check routines. Running checks on all connected sensors. Re-run checks once GCS is connected. --- src/modules/commander/PreflightCheck.cpp | 429 ++++++++++-------- src/modules/commander/PreflightCheck.h | 50 +- src/modules/commander/commander.cpp | 3 +- .../commander/state_machine_helper.cpp | 8 +- 4 files changed, 284 insertions(+), 206 deletions(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 873f2c2890..70015cddd6 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -1,44 +1,44 @@ /**************************************************************************** - * - * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ +* +* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in +* the documentation and/or other materials provided with the +* distribution. +* 3. Neither the name PX4 nor the names of its contributors may be +* used to endorse or promote products derived from this software +* without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +****************************************************************************/ /** - * @file PreflightCheck.cpp - * - * Preflight check for main system components - * - * @author Lorenz Meier - * @author Johan Jansen - */ +* @file PreflightCheck.cpp +* +* Preflight check for main system components +* +* @author Lorenz Meier +* @author Johan Jansen +*/ #include #include @@ -65,158 +65,219 @@ namespace Commander { - static bool magnometerCheck(int mavlink_fd) - { - int fd = open(MAG0_DEVICE_PATH, 0); - if (fd < 0) { - warn("failed to open magnetometer - start with 'hmc5883 start' or 'lsm303d start'"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: NO MAG"); - return false; - } +static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional) +{ + bool success = true; - int calibration_devid; - int devid = ioctl(fd, DEVIOCGDEVICEID,0); - param_get(param_find("CAL_MAG0_ID"), &(calibration_devid)); - if (devid != calibration_devid){ - warnx("magnetometer calibration is for a different device - calibrate magnetometer first (dev: %d vs cal: %d)", devid, calibration_devid); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CAL ID"); - return false; - } + char s[30]; + sprintf(s, "%s%u", MAG_BASE_DEVICE_PATH, instance); + int fd = open(s, 0); - int ret = ioctl(fd, MAGIOCSELFTEST, 0); - if (ret != OK) { - warnx("magnetometer calibration missing or bad - calibrate magnetometer first"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CHECK/CAL"); - return false; - } - close(fd); - return true; - } + if (fd < 0) { + if (!optional) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance); + } - static bool accelerometerCheck(int mavlink_fd) - { - int fd = open(ACCEL0_DEVICE_PATH, O_RDONLY); - int ret = ioctl(fd, ACCELIOCSELFTEST, 0); + return false; + } - int calibration_devid; - int devid = ioctl(fd, DEVIOCGDEVICEID,0); - param_get(param_find("CAL_ACC0_ID"), &(calibration_devid)); - if (devid != calibration_devid){ - warnx("accelerometer calibration is for a different device - calibrate accelerometer first"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACC CAL ID"); - return false; - } - - if (ret != OK) { - warnx("accel self test failed"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK/CAL"); - return false; - } + int calibration_devid; + int ret; + int devid = ioctl(fd, DEVIOCGDEVICEID, 0); + sprintf(s, "CAL_MAG%u_ID", instance); + param_get(param_find(s), &(calibration_devid)); - // check measurement result range - struct accel_report acc; - ret = read(fd, &acc, sizeof(acc)); + if (devid != calibration_devid) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: MAG #%u UNCALIBRATED (NO ID)", instance); + success = false; + goto out; + } - if (ret == sizeof(acc)) { - // evaluate values - float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); + ret = ioctl(fd, MAGIOCSELFTEST, 0); - // evaluate values - if (accel_magnitude > 30.0f) { //m/s^2 - warnx("accel with spurious values"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: |ACCEL| > 30 m/s^2"); - //this is frickin' fatal - return false; - } - } else { - warnx("accel read failed"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL READ"); - //this is frickin' fatal - return false; - } + if (ret != OK) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance); + success = false; + goto out; + } - close(fd); - return true; - } - - static bool gyroCheck(int mavlink_fd) - { - int fd = open(GYRO0_DEVICE_PATH, 0); - - int calibration_devid; - int devid = ioctl(fd, DEVIOCGDEVICEID,0); - param_get(param_find("CAL_GYRO0_ID"), &(calibration_devid)); - if (devid != calibration_devid){ - warnx("gyro calibration is for a different device - calibrate gyro first"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CAL ID"); - return false; - } - - int ret = ioctl(fd, GYROIOCSELFTEST, 0); - - if (ret != OK) { - warnx("gyro self test failed"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK/CAL"); - return false; - } - - close(fd); - return true; - } - - static bool baroCheck(int mavlink_fd) - { - int fd = open(BARO0_DEVICE_PATH, 0); - if(fd < 0) { - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: Barometer"); - return false; - } - - close(fd); - return true; - } - - bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro, bool checkBaro, bool checkRC) - { - //give the system some time to sample the sensors in the background - usleep(150000); - - //Magnetometer - if(checkMag) { - if(!magnometerCheck(mavlink_fd)) { - return false; - } - } - - //Accelerometer - if(checkAcc) { - if(!accelerometerCheck(mavlink_fd)) { - return false; - } - } - - // ---- GYRO ---- - if(checkGyro) { - if(!gyroCheck(mavlink_fd)) { - return false; - } - } - - // ---- BARO ---- - if(checkBaro) { - if(!baroCheck(mavlink_fd)) { - return false; - } - } - - // ---- RC CALIBRATION ---- - if(checkRC) { - if(rc_calibration_check(mavlink_fd) != OK) { - return false; - } - } - - //All is good! - return true; - } +out: + close(fd); + return success; +} + +static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional) +{ + bool success = true; + + char s[30]; + sprintf(s, "%s%u", ACCEL_BASE_DEVICE_PATH, instance); + int fd = open(s, O_RDONLY); + + if (fd < 0) { + if (!optional) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance); + } + + return false; + } + + int calibration_devid; + int ret; + int devid = ioctl(fd, DEVIOCGDEVICEID, 0); + sprintf(s, "CAL_ACC%u_ID", instance); + param_get(param_find(s), &(calibration_devid)); + + if (devid != calibration_devid) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED (NO ID)", instance); + success = false; + goto out; + } + + ret = ioctl(fd, ACCELIOCSELFTEST, 0); + + if (ret != OK) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: ACCEL #%u SELFTEST FAILED", instance); + success = false; + goto out; + } + +out: + close(fd); + return success; +} + +static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional) +{ + bool success = true; + + char s[30]; + sprintf(s, "%s%u", GYRO_BASE_DEVICE_PATH, instance); + int fd = open(s, 0); + + if (fd < 0) { + if (!optional) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance); + } + + return false; + } + + int calibration_devid; + int ret; + int devid = ioctl(fd, DEVIOCGDEVICEID, 0); + sprintf(s, "CAL_GYRO%u_ID", instance); + param_get(param_find(s), &(calibration_devid)); + + if (devid != calibration_devid) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED (NO ID)", instance); + success = false; + goto out; + } + + ret = ioctl(fd, GYROIOCSELFTEST, 0); + + if (ret != OK) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance); + success = false; + goto out; + } + +out: + close(fd); + return success; +} + +static bool baroCheck(int mavlink_fd, unsigned instance, bool optional) +{ + bool success = true; + + char s[30]; + sprintf(s, "%s%u", BARO_BASE_DEVICE_PATH, instance); + int fd = open(s, 0); + + if (fd < 0) { + if (!optional) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance); + } + + return false; + } + + close(fd); + return success; +} + +bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro, bool checkBaro, bool checkRC) +{ + bool failed = false; + +//Magnetometer + if (checkMag) { + /* check all sensors, but fail only for mandatory ones */ + for (unsigned i = 0; i < max_optional_mag_count; i++) { + bool required = (i < max_mandatory_mag_count); + + if (!magnometerCheck(mavlink_fd, i, !required) && required) { + failed = true; + } + } + } + +//Accelerometer + if (checkAcc) { + /* check all sensors, but fail only for mandatory ones */ + for (unsigned i = 0; i < max_optional_accel_count; i++) { + bool required = (i < max_mandatory_accel_count); + + if (!accelerometerCheck(mavlink_fd, i, !required) && required) { + failed = true; + } + } + } + +// ---- GYRO ---- + if (checkGyro) { + /* check all sensors, but fail only for mandatory ones */ + for (unsigned i = 0; i < max_optional_gyro_count; i++) { + bool required = (i < max_mandatory_gyro_count); + + if (!gyroCheck(mavlink_fd, i, !required) && required) { + failed = true; + } + } + } + +// ---- BARO ---- + if (checkBaro) { + /* check all sensors, but fail only for mandatory ones */ + for (unsigned i = 0; i < max_optional_baro_count; i++) { + bool required = (i < max_mandatory_baro_count); + + if (!baroCheck(mavlink_fd, i, !required) && required) { + failed = true; + } + } + } + +// ---- RC CALIBRATION ---- + if (checkRC) { + if (rc_calibration_check(mavlink_fd) != OK) { + failed = true; + } + } + +// Report status + return !failed; +} } diff --git a/src/modules/commander/PreflightCheck.h b/src/modules/commander/PreflightCheck.h index e29357ec91..935f699690 100644 --- a/src/modules/commander/PreflightCheck.h +++ b/src/modules/commander/PreflightCheck.h @@ -43,21 +43,37 @@ namespace Commander { - /** - * @brief - * Runs a preflight check on all sensors to see if they are properly calibrated and healthy - * @param mavlink_fd - * Mavlink output file descriptor for feedback when a sensor fails - * @param checkMag - * true if the magneteometer should be checked - * @param checkAcc - * true if the accelerometers should be checked - * @param checkGyro - * true if the gyroscopes should be checked - * @param checkBaro - * true if the barometer should be checked - * @param checkRC - * true if the Remote Controller should be checked - **/ - bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro, bool checkBaro, bool checkRC); +/** +* Runs a preflight check on all sensors to see if they are properly calibrated and healthy +* +* The function won't fail the test if optional sensors are not found, however, +* it will fail the test if optional sensors are found but not in working condition. +* +* @param mavlink_fd +* Mavlink output file descriptor for feedback when a sensor fails +* @param checkMag +* true if the magneteometer should be checked +* @param checkAcc +* true if the accelerometers should be checked +* @param checkGyro +* true if the gyroscopes should be checked +* @param checkBaro +* true if the barometer should be checked +* @param checkRC +* true if the Remote Controller should be checked +**/ +bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro, bool checkBaro, bool checkRC); + +const unsigned max_mandatory_gyro_count = 1; +const unsigned max_optional_gyro_count = 3; + +const unsigned max_mandatory_accel_count = 1; +const unsigned max_optional_accel_count = 3; + +const unsigned max_mandatory_mag_count = 1; +const unsigned max_optional_mag_count = 3; + +const unsigned max_mandatory_baro_count = 1; +const unsigned max_optional_baro_count = 1; + } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e2a143f7f2..5c512c15c9 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1302,7 +1302,8 @@ int commander_thread_main(int argc, char *argv[]) telemetry.heartbeat_time > 0 && hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout * 1e6) { - (void)rc_calibration_check(mavlink_fd); + /* provide RC and sensor status feedback to the user */ + (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, true); } telemetry_last_heartbeat[i] = telemetry.heartbeat_time; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 58a123adfd..3be3295002 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -77,8 +77,8 @@ static const int ERROR = -1; // though the transition is marked as true additional checks must be made. See arming_state_transition // code for those checks. static const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX] = { - // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE - { /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, false, true, false, false }, + // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE + { /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, false, true, false, false }, { /* vehicle_status_s::ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, { /* vehicle_status_s::ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, { /* vehicle_status_s::ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, @@ -212,10 +212,10 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s /* Sensors need to be initialized for STANDBY state */ if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { - mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational."); + mavlink_log_critical(mavlink_fd, "Not ready to fly: Sensors not operational."); feedback_provided = true; valid_transition = false; - new_arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; + status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; } // Finish up the state transition From 4f0896b10592e235658620ef8f4e93be4e1a7582 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 19 Apr 2015 14:08:49 +0200 Subject: [PATCH 240/268] commander tests: Update test routine to match expected / designed error handling behaviour --- .../commander/commander_tests/state_machine_helper_test.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 4ddb4e0fbf..967304cb45 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -255,10 +255,10 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) // Sensor tests - { "no transition: init to standby - sensors not initialized", + { "transition to standby error: init to standby - sensors not initialized", { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_STANDBY, - { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, // Safety switch arming tests From 554719c78fe4e0e07e56bc7a57877340924d0d92 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 19 Apr 2015 23:07:32 +0200 Subject: [PATCH 241/268] Harmonize preflight and prearm checks, run same code except for dynamic range check only on arming --- src/modules/commander/PreflightCheck.cpp | 78 ++++++++++++++++--- src/modules/commander/PreflightCheck.h | 3 +- src/modules/commander/commander.cpp | 38 ++++++++- .../commander/state_machine_helper.cpp | 69 ++-------------- 4 files changed, 112 insertions(+), 76 deletions(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 70015cddd6..6bb7e5c245 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -58,6 +58,9 @@ #include #include #include +#include + +#include #include @@ -109,7 +112,7 @@ out: return success; } -static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional) +static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic) { bool success = true; @@ -148,6 +151,29 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional) goto out; } + if (dynamic) { + /* check measurement result range */ + struct accel_report acc; + ret = read(fd, &acc, sizeof(acc)); + + if (ret == sizeof(acc)) { + /* evaluate values */ + float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); + + if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still"); + /* this is frickin' fatal */ + success = false; + goto out; + } + } else { + mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ"); + /* this is frickin' fatal */ + success = false; + goto out; + } + } + out: close(fd); return success; @@ -218,11 +244,37 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional) return success; } -bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro, bool checkBaro, bool checkRC) +static bool airspeedCheck(int mavlink_fd, bool optional) +{ + bool success = true; + int ret; + int fd = orb_subscribe(ORB_ID(airspeed)); + + struct airspeed_s airspeed; + + if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) || + (hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) { + mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING"); + success = false; + goto out; + } + + if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) { + mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE"); + // XXX do not make this fatal yet + } + +out: + close(fd); + return success; +} + +bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro, + bool checkBaro, bool checkAirspeed, bool checkRC, bool checkDynamic) { bool failed = false; -//Magnetometer + /* ---- MAG ---- */ if (checkMag) { /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_mag_count; i++) { @@ -234,19 +286,19 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro } } -//Accelerometer + /* ---- ACCEL ---- */ if (checkAcc) { /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_accel_count; i++) { bool required = (i < max_mandatory_accel_count); - if (!accelerometerCheck(mavlink_fd, i, !required) && required) { + if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic) && required) { failed = true; } } } -// ---- GYRO ---- + /* ---- GYRO ---- */ if (checkGyro) { /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_gyro_count; i++) { @@ -258,7 +310,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro } } -// ---- BARO ---- + /* ---- BARO ---- */ if (checkBaro) { /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_baro_count; i++) { @@ -270,14 +322,22 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro } } -// ---- RC CALIBRATION ---- + /* ---- AIRSPEED ---- */ + if (checkAirspeed) { + if (!airspeedCheck(mavlink_fd, true)) { + failed = true; + } + } + + /* ---- RC CALIBRATION ---- */ if (checkRC) { if (rc_calibration_check(mavlink_fd) != OK) { failed = true; } } -// Report status + /* Report status */ return !failed; } + } diff --git a/src/modules/commander/PreflightCheck.h b/src/modules/commander/PreflightCheck.h index 935f699690..66947a3470 100644 --- a/src/modules/commander/PreflightCheck.h +++ b/src/modules/commander/PreflightCheck.h @@ -62,7 +62,8 @@ namespace Commander * @param checkRC * true if the Remote Controller should be checked **/ -bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro, bool checkBaro, bool checkRC); +bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, + bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkDynamic = false); const unsigned max_mandatory_gyro_count = 1; const unsigned max_optional_gyro_count = 3; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5c512c15c9..bb1ed7f5df 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1125,8 +1125,15 @@ int commander_thread_main(int argc, char *argv[]) commander_initialized = true; thread_running = true; + bool checkAirspeed = false; + /* Perform airspeed check only if circuit breaker is not + * engaged and it's not a rotary wing */ + if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) { + checkAirspeed = true; + } + //Run preflight check - status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, true); + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true); if(!status.condition_system_sensors_initialized) { set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune } @@ -1302,8 +1309,15 @@ int commander_thread_main(int argc, char *argv[]) telemetry.heartbeat_time > 0 && hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout * 1e6) { + bool chAirspeed = false; + /* Perform airspeed check only if circuit breaker is not + * engaged and it's not a rotary wing */ + if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) { + chAirspeed = true; + } + /* provide RC and sensor status feedback to the user */ - (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, true); + (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, true); } telemetry_last_heartbeat[i] = telemetry.heartbeat_time; @@ -2711,7 +2725,15 @@ void *commander_low_prio_loop(void *arg) // we do not set the calibration return value based on it because the calibration // might have worked just fine, but the preflight check fails for a different reason, // so this would be prone to false negatives. - status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, true); + + bool checkAirspeed = false; + /* Perform airspeed check only if circuit breaker is not + * engaged and it's not a rotary wing */ + if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) { + checkAirspeed = true; + } + + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true); arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); @@ -2719,8 +2741,16 @@ void *commander_low_prio_loop(void *arg) } case VEHICLE_CMD_PREFLIGHT_STORAGE: { + + bool checkAirspeed = false; + /* Perform airspeed check only if circuit breaker is not + * engaged and it's not a rotary wing */ + if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) { + checkAirspeed = true; + } + // Update preflight check status - status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, true); + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true); arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 3be3295002..ccfc7a9869 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -52,18 +52,17 @@ #include #include #include -#include #include #include #include #include #include -#include #include #include #include "state_machine_helper.h" #include "commander_helper.h" +#include "PreflightCheck.h" /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -650,69 +649,15 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) { - int ret; - bool failed = false; - - int fd = open(ACCEL0_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { - mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL SENSOR MISSING"); - failed = true; - goto system_eval; - } - - ret = ioctl(fd, ACCELIOCSELFTEST, 0); - - if (ret != OK) { - mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL CALIBRATION"); - failed = true; - goto system_eval; - } - - /* check measurement result range */ - struct accel_report acc; - ret = read(fd, &acc, sizeof(acc)); - - if (ret == sizeof(acc)) { - /* evaluate values */ - float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); - - if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { - mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE, hold still"); - /* this is frickin' fatal */ - failed = true; - goto system_eval; - } - } else { - mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL READ"); - /* this is frickin' fatal */ - failed = true; - goto system_eval; - } - + /* at this point this equals the preflight check, but might add additional + * quantities later. + */ + bool checkAirspeed = false; /* Perform airspeed check only if circuit breaker is not * engaged and it's not a rotary wing */ if (!status->circuit_breaker_engaged_airspd_check && !status->is_rotary_wing) { - /* accel done, close it */ - close(fd); - fd = orb_subscribe(ORB_ID(airspeed)); - - struct airspeed_s airspeed; - - if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) || - (hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) { - mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED SENSOR MISSING"); - failed = true; - goto system_eval; - } - - if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) { - mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE"); - // XXX do not make this fatal yet - } + checkAirspeed = true; } -system_eval: - close(fd); - return (failed); + return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, true); } From c92afb99b66eacad5e2069dd8ea735ff86dabc61 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 19 Apr 2015 23:15:51 +0200 Subject: [PATCH 242/268] Prearm check: provide user instruction to power cycle if things look good --- src/modules/commander/state_machine_helper.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index ccfc7a9869..c7dabee7ea 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -217,6 +217,14 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; } + /* Check if we are trying to arm, checks look good but we are in STANDBY_ERROR */ + if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR && + new_arming_state == vehicle_status_s::ARMING_STATE_ARMED && + status->condition_system_sensors_initialized) { + mavlink_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming"); + feedback_provided = true; + } + // Finish up the state transition if (valid_transition) { armed->armed = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR; From 17e487cad4f618b9ee819367fc9f1157169622ae Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 19 Apr 2015 23:25:17 +0200 Subject: [PATCH 243/268] Update commander test suite --- src/modules/commander/commander_tests/module.mk | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander_tests/module.mk b/src/modules/commander/commander_tests/module.mk index 4d10275d1e..64afa86c44 100644 --- a/src/modules/commander/commander_tests/module.mk +++ b/src/modules/commander/commander_tests/module.mk @@ -38,4 +38,5 @@ MODULE_COMMAND = commander_tests SRCS = commander_tests.cpp \ state_machine_helper_test.cpp \ - ../state_machine_helper.cpp + ../state_machine_helper.cpp \ + ../PreflightCheck.cpp From 0a526e2a5f1b62c3258f6c629da50a595422b447 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 20 Apr 2015 09:13:31 +0200 Subject: [PATCH 244/268] commander: Provide feedback that preflight check failed. --- src/modules/commander/state_machine_helper.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index c7dabee7ea..7410baca3e 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -219,9 +219,12 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s /* Check if we are trying to arm, checks look good but we are in STANDBY_ERROR */ if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR && - new_arming_state == vehicle_status_s::ARMING_STATE_ARMED && - status->condition_system_sensors_initialized) { - mavlink_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming"); + new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + if (status->condition_system_sensors_initialized) { + mavlink_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming"); + } else { + mavlink_log_critical(mavlink_fd, "Preflight check failed, refusing to arm"); + } feedback_provided = true; } From de3a9145213934c64bc99c5c937e010b09ffc088 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 20 Apr 2015 17:03:11 +0200 Subject: [PATCH 245/268] fixed firefly6 configuration file --- ROMFS/px4fmu_common/init.d/13002_firefly6 | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/13002_firefly6 b/ROMFS/px4fmu_common/init.d/13002_firefly6 index 551a19928e..29167f1ede 100644 --- a/ROMFS/px4fmu_common/init.d/13002_firefly6 +++ b/ROMFS/px4fmu_common/init.d/13002_firefly6 @@ -8,7 +8,10 @@ sh /etc/init.d/rc.vtol_defaults set MIXER firefly6 +set MIXER_AUX firefly6 set PWM_OUT 12345678 +set PWM_AUX_OUT 1234 +set PWM_AUX_RATE 50 param set VT_MOT_COUNT 6 param set VT_IDLE_PWM_MC 1080 From 851a66135f7148024b1bc845694a0811be66de75 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 20 Apr 2015 17:05:59 +0200 Subject: [PATCH 246/268] fixed syntax of auxiliary mixer file --- ROMFS/px4fmu_common/init.d/rc.interface | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 5101acc07f..efdba9506e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -107,12 +107,12 @@ then if [ -f $SDCARD_MIXERS_PATH/$MIXER_AUX.mix ] then - set MIXER_AUX_FILE $SDCARD_MIXERS_PATH/$MIXER_AUX.mix + set MIXER_AUX_FILE $SDCARD_MIXERS_PATH/$MIXER_AUX.aux.mix else - if [ -f /etc/mixers/$MIXER_AUX.mix ] + if [ -f /etc/mixers/$MIXER_AUX.aux.mix ] then - set MIXER_AUX_FILE /etc/mixers/$MIXER_AUX.mix + set MIXER_AUX_FILE /etc/mixers/$MIXER_AUX.aux.mix fi fi From 2f5239c17a1604acada0d3ba2748fc1e5cb5676f Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 20 Apr 2015 17:09:09 +0200 Subject: [PATCH 247/268] fixed name of generic firefly mixer --- ROMFS/px4fmu_common/init.d/rcS | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 25a46c103f..67ec7821c9 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -609,7 +609,7 @@ then then set MAV_TYPE 19 fi - if [ $MIXER == firefly6_rotors ] + if [ $MIXER == firefly6 ] then set MAV_TYPE 21 fi From b9d17241a38166aa7e346787cf5f382d277a0e60 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 19 Apr 2015 16:20:07 +0200 Subject: [PATCH 248/268] RTL mode switching: Allow to flick to RTL in any mode. --- src/modules/commander/commander.cpp | 41 ++++++++++++++++------------- 1 file changed, 23 insertions(+), 18 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bb1ed7f5df..3fecd9f58f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2220,13 +2220,34 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s if (res == TRANSITION_DENIED) { print_reject_mode(status_local, "OFFBOARD"); + /* mode rejected, continue to evaluate the main system mode */ } else { + /* changed successfully or already in this state */ return res; } } - /* offboard switched off or denied, check main mode switch */ + /* RTL switch overrides main switch */ + if (sp_man->return_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_RTL); + + if (res == TRANSITION_DENIED) { + print_reject_mode(status_local, "AUTO_RTL"); + + /* fallback to LOITER if home position not set */ + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER); + + if (res != TRANSITION_DENIED) { + /* changed successfully or already in this state */ + return res; + } + + /* mode rejected, continue to evaluate the main system mode */ + } + } + + /* offboard and RTL switches off or denied, check main mode switch */ switch (sp_man->mode_switch) { case manual_control_setpoint_s::SWITCH_POS_NONE: res = TRANSITION_NOT_CHANGED; @@ -2271,23 +2292,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s break; case manual_control_setpoint_s::SWITCH_POS_ON: // AUTO - if (sp_man->return_switch == manual_control_setpoint_s::SWITCH_POS_ON) { - res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_RTL); - - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state - } - - print_reject_mode(status_local, "AUTO_RTL"); - - // fallback to LOITER if home position not set - res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER); - - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state - } - - } else if (sp_man->loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + if (sp_man->loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) { res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { From 7e1a21a39e107713d3985f832eb7d38f26851075 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 20 Apr 2015 19:29:13 +0200 Subject: [PATCH 249/268] update return switch documentation --- Documentation/rc_mode_switch.odg | Bin 33043 -> 35502 bytes Documentation/rc_mode_switch.pdf | Bin 26949 -> 31998 bytes 2 files changed, 0 insertions(+), 0 deletions(-) diff --git a/Documentation/rc_mode_switch.odg b/Documentation/rc_mode_switch.odg index 682c63e471d492c1be9f6286211fa5d778f4b53f..0a4f532eb07f0dc3fee410bb66a65fa7caa68062 100644 GIT binary patch literal 35502 zcmV)1K+V5UO9KQH000O805*%1MxO&N#47**04o3h00;m80Bvb)WpsIPWnpk|Y-wX* zbZKvHFLrKZE^lFTX>%@baAj^}Z)0_BWo~pXXL4b1XlY|}P)h>@6aWAS001_Nlt#>n z62Dkl003B8000;O002~Ib!}p9VQFl0FLY>iZDMX=X>2ZVZfA*5PDc$28VUda01Zh< zL{b0%0RR910JR7L0000#V75a509qeOL_t(|ob9~_d>q$#AU?A*TV{Lj=)D63K!UxB zltf9?F_IN4maW8=>z(5y|Ns3R$I0)1cXpiEr#RJ4^*Js%pRHagi6SM6y#WLP5WNEn zEOr;z-goYsdDy{V0fJowg#9EgX5YSf^WK}UfA4*dq-h$L(0J5;IAxi*RSl5sxfGn(Q$Pz#n)-_}aAPegnvIKBHfeFIkIF4bdR0_Td1Oi^! zZ8iw^qSy|)Mqe`;gvW6uT!p38mTi^~{CoLd7e<#ZL>Fk7>5*o(ymBAz5o8JAeggNh zIfTG&ANl8Ro=2%vioIZ(rCC51=t1Zis4_xIRGF)}w1*st@{tu?qo4TrjmiZAsFVH` z!&7t$5hTA$7>`bFf7o{bSpvA9z`gs4v9S~;$jPzYAvSOfH5`cq0s+0=fGtLMRAFjr zN~6)l<8io_%jM{yp-@mNmGUd|KLPcV$z(VjCI|vPp-?Oqi$o&yQ#2a&dc8)Ykz?WT z84Ly~isJt|Gc%J&B!ohtR;%SpC$0iFgu64pHvw4!xJ}?NgTt!ZYXpEK$#>s+BZ^aB z{^GND<@M8v=&ye9?`<93pL^~rjWy+MSK9mgd*uq{oHwv_%jQHhJTyE!G&EFSSC^!* z8K+Yz6$vQS)hlfeJ+OOZbX2M`IHw&NsVI`dt12r1%hhW2sgF*~c!8=s&|9E^;^N|C z$BsXI@L{=3mWYPVUAU~*Ylr)VQ*)8W9^QZ9%*iAnh6WEEIy5xUABf@OBYliRR{)n)E8b#iphvijstaL4`tD->^ZhP>4uENRp{kvbea2z>>aT5GTZ2wl-Y2 z*j8FwQ&pGp_(Jf+(uzv8aaKYKqVafB^G1Rc1p_{%)~MI26LIL3WN>hZl87FBaKFRp z$+4QQc6N(ML3dA2RYh4m5f=yu0nb!GTGl->k+TGFn}A+MS65dg5&?ahr2(dlwfyKb zDg%Wu#wAA!eA|(`1|6fbqcfRE+HAHvlmiQjJBC5CNTbkc)lR2Vti!4z zm6lh9{Bt8?p)G8K9Iy$?S9)Dw|VC=?C}#L}U`LA_Qp>2TE5H;nZ6Vx-t^H5*N) z(b3VNp+N~HiziZQh16`eL?Yn{$Fz(R&G~~hmE~=%tvPuGg@pxO?X42Esr}*^z0n9d z{mGLbQ8Fn(h;r?=sp)AjLMFzB&GtOI$qQWPxg*||A;PX74F zpw4W|wVI$`@4olm_AMg@lTKBF=&-< zzV>=vao*JAU|x=WU}RK2>%$0fb5qms$Vdtocsy>MTKm@PuLeT_A*I~1Wz$=)zVi6t z$EPR8X6JmDFI}u{V1x`%)X>OyexCiEca9WQRvvxl$j;rneeNl>MoS6==g*y!DKuFE zxc`7&0Pre;AONA&YBdOM^hzk1+}ZoyPe;e9t4dFPaC~@pxM##^(5cV9`kH^Pe&?o| zIj;}L81>-%*)zcKuJ(?`<1sA#M(>g912wMEs6o|5Neeb@*-C2iT_YWDyxU$-V0-!H7fT9qe(Ujr|MPGE`LTUl z176R|ue}oV2abL?ke6q9`IT41c=DrWA2Q+J2KU^E!gBf!#zd42(KU-+)p zTes;fT=W@w0+D^T23A~_!zo?cDC%;&6iF{XUogWOM$MB<6F0Eg+i4|Ra;xbHC0ehz?lSheR8q{ za0`GGmD$q z3WixzSHEKLZrs?+H}2;J4}VQWYMz5Hu7;)E25fYhEmbNRJtvt= zag&i&IMLIxD$xA{ZuW&j;kIqtKvrWcNq|6ri_+3E^y|*O`%(S+4f9Tt4K2IiQ>WDw zmR54D)ip4`?%B1Sw+iU)1K%FN7Ms_6L?VcV)jK$Y+JaOn$)V=5w`05sL3|oR0bUd#z(Hxo z(hL=05;7iWv`8iiTydU$0HDpr#zyS^Er~(ETbo2=lw7Yed?FUGa^;(R^gYG(Wg$xd zw+vlfUA48f7|Q}dc?DA#F#Aqk7?6QNp+M%u{Uv~P2w4KSDc~%&@$qrMRM6Vz**Q&v z#daq_O8%Ondlc-@w`Z~czCe}$ZVa3c z&g1c5Oc-Cwp)7`B+{JCf$FKNifrBgo+!P?*+}+(>Qc{9tG4&G!Spv8L@Y#PX7PH&! zcfMvb3#$jR1aM>E*ne?x@%`>g7H$o0XjWwjU?t$#zt`&xhr_`B`P<2}u=?SKFDpv` zHvq&!1_uWL`?>qf)+wDf)&n)V%EjWMv{H|TQR;Z9A zfR%vL{&RA2fD-uL&n$d0AWHzt1IPX&k%-IX+PimeHcIu0ge(DE7ZAn|4-f10`YikZ zq(YVemH~dghVk+7ty{NZS?&K53RwbJ9#ArZ-rioFPDfExmi>QHAxi+6z-j-0^kZXV zS@!?QhAaUr1Dx+)tyX8*|0f%=1h5GBOPWFZM;l+VsQ(FuECDP7p#K2&i$tP~mCsrD zn4&larHTu{g{A*8^k;U=eWp9xEy;uq^vu2aqLzdEjq+ z0lYUFjal};9w18q3&1UXD=aL;vh06-K$ZX)Si0{qyYXe6K$ZaJA!Fa;3W@%*@JWI! z0W7fpP$(1(2DxqTSy&H{B>)Bx`|s)L$<58>zGm6~dVuv#0NH(y>m1fQ0bpr<-`m@p z-S@Z-VOTTOTV6hfdP}rL^3DfX4(Hd z+^vND_~Jv*R@>aJUjpFod(3WpSqg|Xcs!oAwl)rZQ7UYLAP}cWCX<}Q0L>>SRB?Ra zfElPvQB-4NqevvmN`O@a*?o^I07CSxuCAJz8iT>GaE?LxfUx;P#V~CCU@-P#>D-|M za)63J%vMxXWC>tZKz85bQs687DHez{aCM>~|HZbBZCkf=c6JyH#v@0LM59rw)uNP1 z21gwdi6j*CLj&dI<#xM0H#e6%P#O5k{TbDbV4V^G2Y~%DnJmlxvBgS1AU)1I%oIxt zgn_OrfAPH^{-2j$I`YzzYeEIb^F+t+h zH;zy&*Jx^L?(ZMe>DAgI1CpXwJ37%_qobqzN37cEU8@9;akSR0-iY1VzQ?=bBFq@V zvhCdVFt7KxjHmFAIt3s}^5;MQ`8U7$%|IaV{PWKPDyNu}g_&WJBr!WZCRQ5%^jnW! zzH)6Y5cwbf{lEXG!-r*Z`M&*o|Kj_9b^7?xFMaVj1A8R5QmH(4>=;};`|PuvPIZrt zg1tjndj!Du-oW3Cvutm8FQfOft_+eOZhPP3U2zd|jV#*^f0$5wOT%0WXyDv&X4qoX zM;;qM`o6xt!oosOb-wq#?|t>FU&VO35P(ps-nwn8gp$-%mq%j>zrQ3Fixn2+wO<>2 z=;4E|*|~{Hheo5Got^c1y-z>=v{)=gRu*>84nn?jSbGG3rO^WBG#-rv!vTE$;B)jF zb1pf&PYiKQfq*@U=v7x&7jzPIArJ^K zwCU+-p->3FLgliOVq^@!HT(*7XqKt~!K;|Baju8(E7Wnj-Ea-c($v&cK|#SCzxlfl zYm5NWer1NA_4M|=-!bEjF~^V#NFhrGNREpVi71XUFDVg^!Yd(#LikK36UbF3!J<1V zw3Q|K%G#S<`NOFq$z&2SGqif5sHg~Vh3^9AJH`1I{0e-<23iAc0U7~PfnSkl9r@AG zMSp+a`1mEUI0fVizsBPUiW0}7acD?L646*3a2m#qv$w?I=R^`?IWCx_3AiDN{4fkp z2#0CCzM`_Sj&p)#pgs~2&=Z8^P^Yr8vbD7pNP6j@I09Q%N#idlgih#mI<(MmwHX6z zh5+~?8{QnewwJD9Q9MVNry;SJcV;FWjtMCRB^6H$4OvUdBEGqBAY?DC0TJLB8U)6q zvKZh^0iHx-!q*4guGx9}u3^*-_$rY| zHgDd1+jb6iSIwAv0c(Q*xPT&DFq|nFJJLF7ZrIy?-9J4u*(XsLP4@D_+LANp9SzeCgUP~q`Y@he z&Ub!eX%zQ(JZI0I-L-2MI&=g$rI3*=^ECPXo3DM?KJhRA@cqC1{tq_qdt{{d+Vg+= zqc48xE8%eH)fZnF92_cd*tn09SS;3G|N7V8_{KLlA33N%Kv(FwCr+GzXKmTCg^jji zV`FVQc7Wy}^mtsCE?yIgL}0q;jGE4lGfttZdp2&|2!&w&CK&aCfvfFTnz!yNvZ_1UTBo&g=BH%x?8P>)$YoMWu27!8-0t=H zrIZAkQnTtlCwV||hnCSvK2JRH#KR9i{P^RK18)ZLLXt|qe@>y$3-C}&OHFBMmC4Zi z`fEpRdG@g!%@3?LgVn~UF)ml~=+O&%_cnVx{%|BwWi`^tSWc15;|~c1 zLYCs(063w1>eQ)& z2M<2;%rg?P7)E$tN0aBh)~hZuiNrTSDRZi@6qtM|YsF@;eT7P+NM-VP80*bY)R0WX zVl?9{5=xB|L;ZP**b}>}F&6vzkHgb(P|(qrcBZ1D;>SP!F~-sZe1f)(R3K=he$$SI z%}8^=VzJ2XUAq)=S-|i66bWP_t|~8zghLPP-w%Z_oUf%ljHn&tE<}U<{O3Q<4V-=A z*0#2H8XA&vxuT>bCzbNpaxC^-Gu#c92!mGw63W?bGoCJ!NdtyD;B8%PVQozTlp76I z^gLpOz-YO0Wz=G6rYI4RXj@wwXZvETsk&^`7RPVR*Sz!%@+Aztc*5^4K>q^a2yhtz z%f)Uc2))_~0866+=-AlN0 zgIe9ccgKJJ%9mby{q;lt>F|H~$`|k(>2Ir-065PNypO9_ufp3h7z~`@i1-ko)U9F$ ziW~qDD0};;lWr=>kDVi80P~}! zrUtYl;4i07p9UIu9kA!nA>_6PDlIE3gDJvTMh^503I^O3Pb7pw5NjCR2&hjo0*#>u zXc7&FWP#IQfkOO#|DDX$O^lJDfukqRec>B_vbm}L)TxuPXbitm1bX!n0G3uCr>CcZ zOErKZoYv|Pu>A5iwNi8eEF2VoEnBwSIu$L_l&}>Q&6OLc(=n-~)i_;?|Mm7D-4v10 z6t_MClr)$P!1e$5kN^1k*T4S#?|=WNKmF<6y?YTOM}7P2zy9Ii(C{qe++AZ(4ZP? zD^H(2``W}PXhB%o1G~aqtCRpZ!~&!T9BpiD#P0UCI+z+ztNQ!_dDOwwY22etu#iw@bX(< z|F3^sY_~wQa5%he+cwa7K-t66Z|!4P3xr&Szxj`!$0h2`jdh7+l31fgFkks;Z*K>7 z0{Rc9x!!sEd}ZUdg}^5UxKSgG1$a$UQ`1s60LFx0qK6KDx~kCj{NMff-FM!KCGq0g zrWv>67eD)HZF%wg@4o9O%H6(mH@5)p;>C-Fg@qs!KF;Cjw3@*g(6$0Znr_#s1mG(_ z9QNN`9K91E@CgIH$-xCT_!d9Flb`w&=ty8QJpJUMkIuG!=__9)Q{h=(_;Lkx?5Wd{XhKfi7`;~!0en2lPX?g&OG-+PA3vTI0!sm=0vV`s}m6`-`9dB9au`eJ>D){aAW|D+)8*UUz9O zPXW7n8-4xQl%EXX48D&(`eceh_eKu0$fy>-BwoePv~3*+cTy z7I3_BYI??2QaW{|BOXr*#W&u|wh{qwhABc&jK!5;B+`29nB4DIQIvon;_*0>RUG&? zjuQewf>|&a6EaKUh0J<)IwioFLYz(6A!c{Ja*NBMu;=n6s<^n8-C)(*+Y8u_eO$^< z2B624mzSSDeH!e6tPogRz?mr~i)nUxT1d%AJcVI*MgaWKAfhn%cj4W48|89EQPJf1 z_{@weFE?*&VoWL)`y&yr&u20k%FD`}ZuiL8nAK#VBw~LwZqX_e@q`ekhepT6Vv*5e zjU{42M*Am=mGXh^?#sz!^Oh~OwY8x9EQp$q5pdqxV69f06#{Dx7_EUIIy*am^0Qxk z=69ZwL}*NKQ`*f+m5?d|jJd0;YwzB@3WZ{5cvuyQC<+R&XtaN5sBdIw#5EU>_^Jwv zD6x1Zk(hS5^DNe@y?q_sU1qbb*lz0?=qoO%Fw3b}BzSFfqQIyhb2@t`$Ma0Ks{BHg zL|9Z@Jl@$E*suX5*tsw49|0!>PMtahLSWgg;#pXofY7F@s_OHf|6Q;oH??dLkvGLg zmmvW3%0V3mWk93Ryzs&cfAJT8VKSNi_>cei;K73^=^E4`2-2q4#hFBGjFeC*6xv7x zOD0ApCVbvGokpvZQIRCpP+oGTqf@(~L2t0?JabN8AQ<#1DcQt~`+-fH645Xzp-`Ho zyFV+p0(>E$)9H{9$YTFm0Zwb!v2#~xVa`u~{vQwQe^4s9tw=C>o%;q7I4KqhZsx_^+FBqyy~*+8eyJ7{h%xh514NF7A7O?iX<5z4NiptGoA7lVsN8VAxx4 zyirtHed@i}Km6#-KmF5>Na6;I%2ovdAmD_+eK&>q{V6Fa85kH?4gI-aKwm#~*Uh(( zkstS7%p_V4y=TYP)0f&d?KogInby*dFfNjZ7Uf5yQSp5ZigEh{tX6ApZ|{1}n*!sV zo10rJ+k!P3)wE;EVzE8^5R*_fnYzK_Rto_jKx&*uqZu6?1=h<&-|la()@|}EG~VIi z;h~`+sZ<(|$Cs{e=W?j9Il9@qQp}=xfq>wWnR0qmOG^uvHsY>#6=BP3Z@m4$V}~0n zikW=9Y(k1PqXJ<(E$GUXD?kAEyE_bIlV>>`j=H)!H1^vYIHHp?XU=%NUX*9|t^?Px zx($tIJ~-x`8vpduzr(tM){X?=o1PeZAi(`aI6m`OKSF?0)6oh5bW{||756uP^EX>| zJuunZDp45~3b~pRzWUZX`}Q9o6Oq^7`{0YuePMKTOd*q;JAJBeWa^LppFeu>rI$8s z+3pf=ib!5u{kHR0+^w&c2#@otQ7XctZFTh|hfb9jk$9*@2 zIe+LUHE#+x!-(TdO-()b+;eZg{q}di``yEb4|BP4rBeB!r+;T;cxYl`64*g+PxsNI z?=`oyoICwdTW9~l$3K;fhYvsX+hfYnFLvg zippU8Km$PJfb>s4{q(oK^(~B@0ro(o#OBQ{Bg4ZaDJ(23MECyQ?|p$;FPu!2SCrFi z!ceGv@IO8AC@zSE19deuzEJ$hXTFe($FRlX^sWPnvcbiHL%;RfYp)SBrj*^38-3Lg zz-lvv`Mu&YD5B+h*c~U0S~)_*M@<^1Z*qC#%8z3fAL5I0F`*Uj7#r%EoSXzxr>Lmt zt+(DnC;tKJ!~A!4c3LbJbWkkDW4H5@AQCYxv*t2aD3dGTQpz?yJ2&TcyJ6mxO6466 zVFf^u;JN9FB*N7}2mZnBR;!iU56wN4PH)CQOAqgS z259y_rcLHV%>~~)5^nK${PN|?P_k{?Hc;ZxbCHi9$R0W$jYhE*W!iP0%g!4#SXWnv z&fvI5uM$Oq4<9~+z4EG9Wt0K-uSE%ftv*wj>lO5&tgP((`SV~6gBmb0G6EgTIR81r zVnO-A;l(r6*smxn4Ehy0@1Vi$+qWaB!q=S;>I8kk_TKhTEW}9wW}_2}(4aakoPs)^ zwFzTSg9Ub&_5`wbVCi)@R-Lu9xD*pCaB+A93s2%a6rk zz+81Y9nR)WO0vh%aTtKw1knNf6%hb;ZadnCG(9!_LH`HxBDqK`!dYJ=ouWx0$$XMT zG#V$+VlND55;QT#qA-UjC)qmjI4Cd#o4b$U`k>GNlkAx8=s(lHw_z_OW0Vkm5c8FXU5 z-w%2VKX(u?U6M%s(eMA^AO7ig=ca&cM{Db9fBMs(g8nl;F?s00-F<_j)2_LvKKuEt zO*Md>2zN1-#d1pqki$+n9Uo_=V#S?|K-X z==1qthG9h9`Ph7Oi~GEbD0}Z%7;1U{{@t$G*@q83A(skoCbQEjC4g0E3ZpIocE0%H zi_bmxTwh-wUr|malO#d(T)8kXICTEPr6QX)6b_+7cB`wayL)=10=i+_zC9bu|NIC4 z2m}B)&mAU5vpeiSaDx!|;DZm|eDlr6AAh{1rNuSl66r;hfC|QgQlS(kK`B>F437z< z@`iHzPyXsxpa0h9Ecq6-PThH-(^hD6IbF_Cr_pX#C=8KsSSl9IdEAZ>2PgwYHN_T; zj?=MNB4*Z^yRfd1KO|L1LBL`kSNigM4Tzh(edN7OJNDX4+EgmZu18vfaexe1C8jWE zmBPh8{KG#$2`CSohJdE|rI%i^7Zkkv&YNHT-Di4xd#0x+3(Gcu@&R~PUTlBm{WHPo zL9NM}(K6sY7zRL5N~QA5nKRJ%i4!M&_`@I8R@cUzaYm(Q4OnOLjNsx+1cR&0@%ckaz zwu`p>qTSmz4^Frr-oImJdRio=%F0S7ovukw@C(2D+sItwv<|Q(fD?lj4uznI@7=q1 z$BrGWEsLwW)s9(*RbvILEi5h+iiKLW)@U*N!_g;eYQU(dtf^r9OOym^mz0%Kq$C&% zi!qTk$BNvgx?={_%W_2 z{-$b>0H_fl@{*Dgl$I3CT&T0VY4@?#V^&W_gh4b9tb=C0LF0ISm!Mi)gzMW6)%>HsEz1}~S( zft!MH42)T>Q2PB|uyjGI@p`>#wHg=TZkJmomrE%MO3*6HSS$v2Q4|FgkYGW_^m5)+ zu~hu+zKnV~DR5P2*s18)-_K`q|NVE{J z%*evT>i%?YelE>AIPWAF%(3$%Sckk|aA9E~D48H&&z?PNw}S~&z`1CV+6fw()9FMa zi<7_^Zy`f;U_7pmrDI`vd3pD4M;NDzzWm}#yAC{3nrmKrXN0W`t7HDD+L;u>{xAW?WIeXKp;S2C=|MS^(y>? zjseDa*v}9-P&@0l#dx6-Q~1P*6Py`(e|yE}a~UNoS6*6&&r>sgy%%nHe#;2B6MX=otE#FhD=SZ)Jo)(J zkLTv*o<4mVcs2+quuM3HN~aS9#uG`9Y|s+7N&;E{r4FirRTB(_DM}*Xjq8ICKEUpo z{e~1s5LPwyji*kYywZ+0HZ|SMjjNpi5TNxl3WZ{Pd^{&72fN?S3Vxi3U$R2wmFwK_ z*LwuqLpjb3NDQ{t^Upv3-h1!;$)Ehmcfb2xg!iDx!7F&-g%|Sj3!0l6|L(88-#svT z;ISv9BJAS%vweO2U`2o>125>=XP+O3VdFIoHkx6qioZ`*_2mr{nu&4;d zH*N;j1_9vdy~5Y7T>}DG4O-AA3Ao8bE>Wk#ae}uqJUqZQGVn zz5=WP0^n3tqtV#e**P~ihkPb{x7}?+hPz`u(-q(Ei8jH2hJ#3gt0NHLr%#{R+)(9n zPrv=<>wx`Ar2^=Laoe)51KyEZtx6=5`F49E8e)#=63koM+~++qg1Imy7vH#<8`l5< z@Bx_9(9jUKCFL%*yq-~?15ULA)L+)n+PZby8*jW3NfDp@^l!a;{Pbr&`%KX52A02Z zqZmGAGTFq$#K_3#rj5<}_w61U7?_>&>(q)An^WkX&M=-{xQZ=~vD^^W3<01K78Mm8 zKYrZda9AuBkH^#1)rIs+euA0|i<6soo*6fvE)r)q0p{oDXS09uA!8npG(Zd$tl>ZS zgFis!C6#r|AH5o5*9fA|uYdjP+>Kftqlok48y`#eZw=N40pRKFs%Ouheel5tpLyn) zpZ)A-7cX7}Q2;m%??x)6f`LGa#?U#F++DF)EE1SWQI%$WNXd5Z8fOAZ7A$S_@Ple{(v z082}^9XodH*|R4U3ZW9cUJtVgmRW0Cdv#5%L`ZaWb^^My+f$iWWigvARx4D1Z*5nu zR@c@Ncxu)k(J17RNCa+&b^!H(67KWxRv&PYvW*)z`u+YpQ8Q|9ywD;Q8y5?QmHr4;ZfBp9V_s#!0Hqiaq&pnIX<772!7dWO4GYp*KwmCOH z;&ykbc#oy}(3y?b{@N5|7oKm9j< z`?o2a7T`1?V6uH|lqp1%v?gEko0neQv8e&IyxMruDgl0M?#{P^;pvSNs}0r+0dVhw zA96=OC6cM%{?rrxx#=mli4fq(*kVKXbfH!M=YRfPTTZc3rGE19$NWB5|IpZu2M%65 z^#RZWx;-lat_OE36n&Es!}|hjh5#_0tBdp5Bg^cuM-ECUYR=;k%avdJ(w7iEgOm`9 zCBOCQX9`-|O3ErpoED0N-q|@qBoT{*dv@=RCsO=ov#>s4EfGKl0H;SX0jw*K33_&k zWn*I#KEHWa2Y*u-+0?udV-q$(Wxdgu-u6n#*d2*zPkNRH)-9}I0w^jfI(F5|aFVA2wXf&FvBD6kXO%eb?;sO@#Zkt(HZ?Gl_fLr)6A8@%X z4_(Gu{M&9P3u_wIBmwXjeqguDyqCH-ve>^4;RXc2XOq~iph%XjW?@ahbqRpOcxH(y z#-#OQOKu98QIsl$f`6ZgWE)xdq{2!PU|!D#O(U5Ic)c0FF6{J6(d-^;94^?~*nOZ( zMgzG}Btb5kWkmsByr48`oY1)5ElZ!i>`z?(`GV_)&E9bDB2O6NY+~BXUEvw7B>pNf zCPA$rD-Ty4EKdMwE0e*??%w_nu1?IxMDq$WvjRboOeRQ?I1vkjD$ZQQqp4()5Q=an z1e;)S4VMaH+sibKbyf6rqOD7V?HC)uMJH@>?#qnr*bEvm9b>u&XvJ*tW|HVEB(C8Q zk)Q06GZ+@P#;l9xwJBhCRABRTol4^_lT&}8tDKLOsmeZLex*Q)P27;qpT(uQMN9<0 zi4zO6@bSa)1i&pf=6_j76fcq+B8Awsi}Lh4=tm zhk`*$D&<%^JeFR3fCa{q_vq-T(PWj&C;`h3FfMJ50OuYT32;r6LWD3FK-RGNL> z$%*#U?-W*V3d}_ObCU_0RIAMe8ye1BJh7?Ttx)T(7ikKILqGf3PoI14^FROPFAx9L zVXr^L9te?~ayskk8#OBVnKNft*(1oYS;xmGDypjox;qC(#v5xZXG4h%b=7X?)Zoa7 z)nvg4JcW}(gM*HV@tr%i1p|Spnc2zlkuV zL;dv|8i)IP28PGCwKUCmLmlmHdv~FjTS@axetr#n)=($$0HF+u7Fo6R_Jp| z>*w4vN~O%}ngvd&GUN-ve2~beobv>*?7IiqP+pXC;-eG!rB&BDuC}(f7vvYI4F*ag z>hI~SuBr9-0tA-O>I^Ub>VG%w*#6p!FXa~Hk@49VUwK1sGy(xkIUJpxT?0MczDShD z2$foAHk&;&GqZuPN-2++EWMrWKmdUl9g2m?RLbX@(`&SEzV=Em6cACWmMxp!e)&af zVF4uwo;`P=v!lJTw$VE~>-4y<^$*Ha+R6g!yYIYPQPW_BVTwcsopyAh2YTK)GHKSx zkG^wMC?%)bE{6iXP%NcYE9^PBUXN#Zz$21Ut?ivPRVCx&qe5nv0!|b4QT>=1w zf>NmRl;pH0qBGfyIr+dhquv~!XR6;hUe>sy_5C+YMRk-y@17W-Q%O3N@OzwcjYX|j zB?Kuv71tPy;V34L`wH_buQFEM`a@M& z2|);?QW+Kt`Te0S+qaL645$ow4^}ot*#&JDdyduVQOLzIrP?3x>9pqXoKqr|io%g( z3ahKCoSpSRH5En9`RSUP3a{5ElTveTXPh?cwA%dKJQ0?haeKs4or%! z66F;X1Lf!x!ZT;i0WFVDI4ox4w9{QzTfKv^dbZmay4cP=DN4Pxd%tCUc# zQ`kz`!+Ee|0%wg?Ol2(wfkBga@umaJeK=;-ER+`)6|gqvyt4&u_{z=8L#KZ5e^Qmf~LU~nf`I1S}o8nI*NPV|hu zd-s5-f$Lp+A3!y0>zUSy(k+x!&X2Rasvgm5R{vRTu#y)DKGj;o7Y+zcUA{6t97SpF z(lt;nQpO=jE;wEmw0tl~tukWwz7mt!no66J*K>o;-$%24l_hBMrk0|X3;`bB2h{R%% zIO_#kjc{EFz+tCKqkUxG7Mk%hV%SY5XJu~(q-Pc_3)r)A=Z|*nT-Ria=7tYAes#a0KVMP%S-1S!MOZH zX;es|@WRK53Vf`^Zbc87zj3kJ&fR`9d;EAreru1;!p8$QUJC?TuiHI0GsA2kMB+aq zSj*yhI@JyfD2T>|ez;GGL~66yL?R-ioVRBb#hq~ei3~T4-DZS%mwx5cB)(>arO`2C zRcVH}SoT5aE_%5`-j{7;+5U6S<<6;I+SQC9EITfYWm336fM>p!8${DHdYkdkl@a3; z1J{oOuz%sbT)K2B8XeN>DWuYfM9le{?2E@@u>>h31xWS9(Q6Yh2Ujs?mI#xHBx0US zSTqUOnBN~eb4FNEvE67iEU3sRxSYlVlo6j}oOC*WclwQ@6K-kF9HG+(EwYC`%v%v$ z9%DKk5?;{0*!VGDEkUY1dn^Pq3XbC=6rb-TbJ8edrU!nQ4$EUQ#{m1zz zK7S!#2E{+kA!9=i|8{OfY^0Bij`JpPzViGiGOU#Ja2Cup)>6Xp_3E2iDH05`a`ECR zlgX8rR|hZU+}TUC0MqLXz#25)2bM#)bzR z4z)t6)*JnyfLtP%P*icTE*_sfb?RM_=#W}HpTP-8Fq#x#EgAts2$pwO>WDnufY0B4T-RjE{B zu^3u#PEASVN|i!Jrz6v$P&6D4qikmj7|*mE<654aob2fvXliUy$SF*aIeW9SvqGT| zEFb2ynE6~?sLM3RU#F&~kQ2!{Ip*>D%PT4op+f)Rc8x}Zx`&zsZ05$rE%AWL{y+e( zh6eldi%R8k8Am6vSPZVAg$(s?p_A+Bgp0)Pf1 zd3lwIL>O?$?U_x+W9QFxfBnCF=EGwjMq>$;MtSOF+ol~G?Ivw!N4J0==H>#)WQrhS z;cy@nicn&*`|7oU(MgNNvSn)xh6RN}c}a<;ukT88^Hy~Jm*4N7^|;&~cW~Afjwcea zs7$FOaJ>D>m8$A$y;j}ka7dJT$Ha(Crr5J<$BbjVyMH)8$J#qEq|%ujV?(4^28BA4 z$)r`sqR}`mqC^5fl#A!i%=vs)TTUbv#!~pu$XF~A+Of68;dD*UxV>|;+jj2BvzdE( zyDiq7wo7AyC{5x?lf`!7?8%0P2KV&D#Ee%$NykTrwr<}Y@w?7k>?q97ftF!{q)M$^ zJlxUIwYjD7#g|?xC@hhQ@X47uy+#2O1%0`E>5^0?Z>+Cry);g#nEuP<%I({>kpvzM z1wQ=fBag>p(rYn^+A%(2vzQ%I(=nRd(p)zYSeb{1?73GBhBRNCW=B%*=E& zNre2K&6_uefvB;ob?Iy$b^R2LCa zMJhQpI~M}>Z?l;^zJOk%iN}Ee#IabCBsty&a-48`=C<$HHPG9Qk%FQAA*DvE(`xby z3Y9ALKyM$VQucIp7M7F{$p~by;FuUEgv6EB)?K@|cVE3?Fqz~kwNjxR@r}7X z&Rq5k0pN~Kr)y|%KqQs=Lh<7K{JdQI*yy-ir`IKug$4Q3}@;m)myj zXr3G&FDFwXSZR2q7RgcHpe(6J_?opJak&FihhsPd#^pMZz3**9B z*VN#w&tTS%43GN)zGyrJG%u0K`}_KZGAZi62v3ZS4i^*@Fj(f22r-+{>9%!@V=1?!0m2}i32Vu(cphCDZ{cC1y?*~kTKbj*Vq`ZvNA zKgJ_GJUE+e<;X%-hXyT|!yZtbOjOBexjCU%iO~dSa(?!uGj}nS&1Y>c?=~;4He2@7 zg$Jz|Oz1QGvpTo8)CA23WY`Pg+8@OU6}s%vQ86?tUq!CD=<*?g&nj9h&MZpoxPWpLe>e9 z;Dd)V;e^%6QX(*!Q-^L9E*}Fr+MB5_Bk=Idd7KqWk);PR5tn>344!)}LO)LL>k&g?2P-;XsWFe#eg(g(zq#Yk$s1 z#>bd7s!#*hnDO-X*)^^%uN-5dpR+o}ARTMSf(8dO1=|W6KlaUKsEnNPFz~P>v!*qLqUFVi9Y7Ln{Yg^xilxSrmn0J!jVbT5bCuy1oCZ~Oe!bBZ zxI5L>rn$(P|NPN#uvhJq3S@ZG*56&)z~S}nP%Q@~#gYdkfd~mm4xttg7No(1ipaaC zOH5S<5#ZpItqVIzB%R$72Ivoy?9a^+d6y8ADwXfCilY|bs;l27j{|Z}fuB@@`+;nh zZDyG_oeS6O8vEtFaxq-X?1T?VS|KFaSeZ`~*vgX;Cry~T`y*nyM^0pO`VCr7H*6lQ z>t)4IRaWZCZE{Y0a9i)Xx7qN3&uw$j|S&B!jt(Whp%z;_s8fK}FoRU*Hg@-ImdRlCD zrs!%|H=PvZr!a7*ML1n8T!;*@K1GOBD37pO;+_a?js>tLp_O7|wYP67Gc&0ufp~!@ zf(6ML0!kSeny?OO8}{%*k}Oi569Dz^R+a&{=rER^?@V?IM^Fa{U3(OMgPz&D^-atn z+su7AVcGQM%HwDcb%1r1r>)qpa%VE^Q`aDbuniG&tOmW@_vOv6EUFg`x09yiC(p<^ z-5_2P1UnrIY_A#V`I+4_P%^Ob7~N8(AkLnKs>BmEnmWBK0IAUX`8@-goTeA)&lb{K ziUeS525g2j<*y8JFB&&mbyHJ_eeoaEw!rat1*t04bC(P5oGca?a%K=i9uXZpRoV1k z+lT>D9>W}R2GTUYIzK3jrN7uPx6;q6i=zWvSLiv!G8LF+2FKog8P*Q+9Cf9XKZERF z!@!6PwsXeBWelV#h#%8p-j6yaz-iLRuxfco@#=+k`6L}k^VL6C0qf03AS--Di*Y77 zk7f1w(F3JjS+Jop)iFYBLMF!@|2*gY$Ik=CIB&#MpdPhl(b^)K#aK~NLH^4 zOLwV8-%a3@jzVOd@!sTXD6XJjkah+TUXOzx9JLIYrj}lWc`52}X?VPw@XW1~2@BX0 z8ZEx#;7MfQ$RdSYaqRp#!l6$(e=fo=TBE)vAB%v&x8ahA!dOcm+l1N5_r!)L(z;Om zJR6RrW&TxU>Ro~@1IDeUMNW?|57vx6jLEyBl4%nJ2+$q_1zMDMP~gmz2F)4;|CQ6n z&;hn$Dx}yd^aTwLxR=sZ?$dk7C#%?mBp|s!U^?Ql9U5EA^0F-Vt3Ho?_meO1ppX>? z0M`lGv`|7uv}e5(TCQHQ;I9Sk1)w+h41orI=!{u36mA(<+PDVqxb=X0kiWpS6h40o zlu8FQqdopcf(+Gx<9$5bW39hFuG)xjhH?cP?n{8Zg#zjS?mH)byyB#jdv}g22W=H- zjr5QLqpO>pNFex)bi9o;#h=?P_lvzqvzsO_BDn+!eH+8B8dk%T3T(yqLZxwOhuet3sNHAZprUeJ?dT@}u1_Lvavi|5ex!gtSJ;B;-fw7~aVwnHZsjLfq~)jera`??BIU<-14^CTv)kLhrDyc zZQX!IA|#kAc%GA0Fsjs;R2ZUo++?Y9yOBJ4n32z_NdtIPIB{)k%)!@}cdYX7etp%o zt*NQ%a%yR?Z?{tDh{+rr90G1^m;(n|QCMUi!RSwqG?RL9$`tE6NPENz=$&8GA zh3bR^ZH3ynZk}ed5CN2^2~&l$msFwB>+4HGNI_kZ+@B=FOX>#KXa`+fAic`fTlo+n zrBD##cmRpR8*W%S8Hr@{xC#~=^^lQ2kxr&Ox+>^Pun#32b;QS3pUdF> zT{xzew396;Z-oZ6iBu>4-ZnE+b{i)qcb`7-wV#Yrw%e9gNo&gRLSFFke?&?%qcp^cU~LZwb*bF zi3uSyh%Y*wOFOP_k>xNC)yBrgMioUw{6A}u6d?V5&9&BC!EQcI5LN@yoUVnesWExI zKEpPQ)3R|KCqd|<;5!0+mu|z1fm-G&)RYMXPMJey3mxPX&y`88rb|22I{b`k)TK(4 zA=Ih%c5;?XIc=Ch7};!@b1>q0yNn{Yf zhm?||#REtrYKg+f$D^mJw&^U^19Ht9MT!+mmB4v8@J6YnqiryZ_4VWDO~B5Ftj-3y zgrX;qre^Cu%(m|}ct#7_yq=|~Gbt%)`;#1JuXGeO&mjr(wwiwoTa8tyiDR@6D4(=kJuMw?o8v@y4B0Z1?iskbxUgVw;ln;E9~j$3I4|kw zsX5&D8)i)AFIY$l8Q9tFT|5$ypmc6Dt@9FnGcRF3IjyP3OBf{!#=f81`krlpUW$w+ z(_Y+nD&mH#x$xJT+f?jUFO?Y?LcvWrISt;gtu`Wz4V%t3Y%`j*N`YpIuvc4o$(V5=qDGZVO~5NHAg);NYfuCb*DD z4Iyagz}3@qniP(-1})lo2dpO;DH0q)LQtn)locK`X%r+-3<<5Q>}hH$u+UR^+2WF= zks>)ozd0yP5}_oL92gP?B&mk0nNgYC_>f~6vx^ubS#W~6=IigFgZfIDL_Kw#0cdEE z{df;zM)%{9j>+?PPv&{Z$|#+ZkPuNKv@qbH83Ir>=qV{ul-OrwY3}`_<=K6h=38QAQ!NPPsFU?vugA7w$ zI@>-B?m10yxc*;L0k|H4a9TBceSx(rD`$5MX>1#BH+SGP38w_Zb)v9#?JyjYTDUk_ z($e!E&%=HwWiXA8_#br1hh1>WV{0XuWGjz>)J+TNW^7mz8D;CN@zt_ZO_nUPqf&Gj z%wgJlnoMPaWkE_6CY=L!potwmiE@ zKkT~5l&a=!b~@R|Z6UjY?V#7_z^B^xbFxi2f3e0H^MDnt|D^rNWHjT#ztMH5eGF>& z+W6{~o?R)!43GL#C0zd9l5?B1>)gXkFnF>kN{@g;%M$!c^PbmrN6v3f!t=|(%| z7SeGWOnQoap*wZSPqO;o{-`@A{^Y6C8Ed8IBKb9&c6MJart>+T7uc0G$8h$q<2y@h zYiO$7B?ew@T9H$8;WJj()w@2ZFS=tCHhG_;T%N9gjb0elUtI`H+7+=>{VTq7Z2~E^I;~Uo+uZE7^RSw>5 z4_Cq5c?@~(BqJ~50aBENbe?F9BH}X~j=B}xTcO^(!B(hLuwHKUpKd% z(^J4(NeU%e3|eW7c8@HAvN1e(T7%ADKr+&U7B4PBgyi*#mMn41lxan4XoLdateB5L zegZA3V3#6w!n>o>27DMs3I&02T|yGXMmKPf@%#}|mb_Sq{(0(McQLLgi78P63;G>W zj9e-j#yB!Xgfe0%xv`NTXCF$aMhjimNQn^}Dr`=N8&5fa1S1-JZP5FcFnJtZTT2Uy z=lmHWg&LhlS7dz8ngJ~{(~&9C+fnDwG9W1ilm!h=fe9O!BR#@h=aPiD19ZXbqTtpop@_!jjoJYKEooa8=R9*_#9|`Es&Lhg*Zg*-IC&b0X!NbGb z?lgeQ4}XMHhyvn}2PV_$sZ=)ZBkPlrnEKuxG3hbVR(b2ZH;;xq?0@#kVsR-b@Ne;W zwKCG<7w;ADaIbE?UmAOvS(v;ot~5J`PaI~BF3{7tJ1#FTc=Hz!fcsvy*a_u;Pa^$l&fn`0?runzX3Y zDOH_sGSPBD%&HYDDygMra^B`JVMGS@MT*=;j@iy!e|5d##>S2>p23ER8qH5VHHpy2 z4V(cM{`M#0MeCKS^lg27`{@9rL`by>){ZlB@Nfx6pbHht<3x(pI=8FUX~>3R7Bk^G zN5VgOQ#1gIXh+9=YpPm@&8vEi=&G>EcCS-lfn<~mYhLeNI&eI?6^YEA`-RAz+jdNu zfeJ=Tm%}KM94KU>MVj%QMb#P^F^WnRYBd>-a<5PESTf{Eake&A zPO}`-5Zwy2g``t?<5K|CDntyL5Lhy4!&17$k_=kKbhu~4RP=O4umEJ)P2BIl0*h&} z%C+ZTMI=%r1JGS9^=wFR0p!K2=cOc;JOrqFCF|ATU^C5|!l|BM^fBd0LgbC);7wp= z0uBI;=6V=oXq8f>WfkU2mH;t8x{?8x0!-?o zQ#~C!q+pIy3{bK z@gZ)uc1Q`)s@>(~HKTS+h(eR|DMF}JASre@asfdIiDGd-R@AVpm|0a;2y{_t*M5dx zu~dnJV&g|1)OM4FMd8+RP*v z8(EurdI(OCQZl=?bg<+E^$3VqLU<;8(Pv_0E7Yb>rYHB9v3nT?OaA;|&!UsXhQfb{lpIFdh zKrTWMKpyvjXh^GR0ey$XNmDh4t-n{WXDLitbdopRDi-lIy6Sx~6{n70adK{bY1^_< zrfg*JCNpKGJhB9+RI6Geo%}%u-7_-~{e?8bLEp+|SL38EJ1xU#M9h_s5R34rLgP`R znDxkOc-3J{D<@3s-Ln?t4>~4XnXoZo3udnl7zv|$D~jsvAv_fiz*zBMh@@Tv?~85_ zWW<+73@oODU0|RS(a(=WOdld1Gd|P z#T*k%$#OR;45UnjX-jx0n(;AMg(-bs`Z|HZA&BMS#5NbcFAw79{DL0E;4bjUl}Rs0 z+>`t)NV{I$2*$OK3|Ut^SFx(el3a5$pRe=mGMs%n_rJr=Av1ymX zmlKm_*(={bT)gB7U0P?Q?cDy4oEzF|HVyqqqQ-)UK3<&pd;)DHl={) z4|N>@>VLt}lA>}VwL%6#|C2@y1qJml-ul0r|2_ZWtxa7F={;<1VpC+}vl)=WZoknZ zwyW+T6W}onO9?6oTC7UTl0TK`GH)%)qf~loSKh=9cMk)C?GTqEGHtWrpbwCx3NcEY)DC~ zZo^397k8jqArwkyK@?h^u1P$5qxN#FDnf-DN`$e@p8z>3qA}#mP@0?9Udp0U&J)eXbF-{q9{5Y^kxaQ^nKl zbJ_Hq_H|ZV)Ea+TVIlpTdG2@E(Xozt%@NXn-dNsKjoPR0!3zG_ZQgp)NC1GDdgjfx_Y3HVWJA`3a8nvO!z+b>%SRv1{4%f67jiG9OK8l;s>Ru#I* zFS&^C%)eZ=OBIDRSjGO9W`Jx&Yd-sH{0kRhO!9=zbR0XE_ZY}Dpn#5^a@;bFf>Jw? zML)SIgqckfMThIB0q9n?PTP0W=qoefSKFIkl1I3XSOb4Ga*Nm4Prv*p>K69-?wQRm za41M`#FXkCsg{7U-;akahiKXqnrJW2e%0amO?sTWR3b(isdq){cN;&2AFvA2kWhc1 zA~@E8fq-3nCtuDL^TM z+qZ^30sj$RG@^}K6$_Q>pHI9+UaCQK5A2SXfw$IO{~xAE{|OKegi z1}xqs7J%p}+s29`(2%FLiKi<)mZrYLNb;vOv>i#XMkT7~jHiR6+(wOBy=qy>$(@(Q zAVHdv2qie4bStM(@>1y`#%#2RR@{E3JjD+SUtWWrma(hIGQITp=W>2a{7s|Q!)CtJ zkI2^T&bYUq0|j)ETcG z3X%qk8E5yk^%HVNKd=F_h=(8G{+5r#;ny(qmdkCIfO=DAR|fZF#DqCz{&0-LSZ9C- zmJ`0X!F}=Bv`CuAWn5q;RX$6XzxNVpL167tq;^k18 zA2|YCjuP3Z^V-Pu;B6>I6V8=v4t4)jQ;$xC8LH&?xC3WnAzEgfC!@P$kXH{B7|@I3 zy(og-;zAcERMOYwpxdJ3!fsIxSXrUD98jiU2weg+rwB<@Qp5*j6NuOa_I)MBAF0+M zZUY!iWuQ%jT)-*y%`J|X#IW2k2e_RMxMc>l9{Ta#&)!%87P&YfHeUtJF*$h5VE0No z>+;0UuQ$KV#=_Q8H(nDDwxeLRD&f%}R>luUO8LKVP!mo^ZNVYr7uJZA&>VSxgI#%! z8{-J%(d=awN$L$Yy^AK5L;zk0171=U{_;vk{m9R(#|LKm2D{g)Rm8qP;RkQE3lBnL zNdw=^*JQj}6yt(naKTxH>{vD^1LE7&;s5 z!r6ij4rQ2HmB*ME{>u?OAySzmr(#KU2M5I?b?0WtHCzdSbeGT%y>4k&!;*DCGXO{s zs$k5h{^670^`UM-5mOSI!h$+6JNxt`TLmPI5cf&gbn;4HQU!)qQ_z@yP3Wu)@o8MW z6s*jLSgNE@XB{x;nUPPVlzUAtT&o}L+h#!Tgph2h{od^*@W^!ip|*VN-z!xmXrh1C z|2EfY8BMkqob9;mMywh64lG3M;G26N73nT-Ek}JnU&g-dI9E@cx3rco`?~{(1@~xM zD2wl|T#v#4wMNlw+?#%8bc9gt?qyxIhs-1B;=qhG_~7`BF!JDhoXgLP znw0(uUAg%II=9r9QEPOMR+!j7in+MYCV^@yjTLin-iy- z$tR-f$)Dv`_GfX_@=hhQnltG>FJ8Ya4TIIu*rqAWet{28yFRw&PqJLL%(WrV_c%qB zv9;c)-q>@Nmo-PjmDr%2x^~Mp7S_wxdX|dz$eR-T#><-3x_rFp=gv~CSc_Z58_pP* z{hXd3)lnV`i2pU4PN4=JW(XjlEwcY#vw;Ny0y4I@bNO#?hKH@qweEIOF-Ka@ZaEQ8C+c#y zE%v>RF+I1HqP7$-!=g-|Ifd>_552?JG-;3{)`DJSXx1;;B`Py%DvN5yF zQl}0(8XxYqH+s}6AoV$~n)TsU2lHLk7qxXZ(TXAGg1)KVzi_`=TD!KTKHj;&t@w4o zN}p$9COtaiyw$`oJ(=2klikZXK9!aiwxYZ^!14>3TH6PMh3*T_b5K*Dq(?)mUi&sAp)#H6)EqwYj93 zp6ZdMvua|w&3Y(0zsl1e#t#XUWI{rFb}w}Lte)tTV-eHvn+8Yb#Vwn6BB+PT1(0(C z`AV9|A||OnnOpMQjr^f@S0OTP-sWzb4_7(|{YKey5_~))oMAJhs_#BXih?5O;3bM# zIp%R?squf`53kq!957bM8@2O2&ps4w-f_o8U{|fTU;*}_3tBy!8t4`~3vry=aviP`F!Q(ot(#@yDOpc*18rmP z*$*5)^_JlYBCb?>Sd&O&zpye2*6)CfEJ_Z_rlH4J>Nx|8IXg`t4r$4di=M9mM9Up@ zk35n_xF0C+W}F)xGm+jJ%&$F3^dX%`){O_X^=L7(zjZHp3Ed+qZ-)ji*179> zi4r8+!QyFi91Z)9)5Qmekv4m*(NQl8bv6&PEpljG$wsV;1)ZNmvG4ry`&yOeh+Ay= z!S1ZCxFcDm`Qz$W<%(-3@jB0b^lj330I3h2`btekz2vc8(VAvc#Wv!M{mM_Zn>$(Z zDWz+c1|o3jx035a2;c7CHycFOWZF^$1+B9|X1N<##?*<4h;Ph0fio2$}{ z`$V>Z9TV-gp%^XxxR*%>0top*$kAMWB(7XdJrN~yVC0n)F3>Q?x{~IBD5rv zLG4A`<0VdezF~R<=`{)nh}si_fj#^=z7@jz%%7yU%M8BIzlI0~U3tMul6OtN>fa1xCDGqrZVJh)Zs!vBhjy8A-U4+vmHQ$Z z`=G9&hWZgS<|3R|p}wJT5@9-Om8El)gg)1+dpduIPPh3Oa#S#J1o>O)L)H|#l#kOAGjC6Tq07kNQF z3YdyycZzGDUgczsnny!}rvTLynk4NB|;Pu*92oF;g!X$NNw76~aY!-UQym;7;w^@JgG|425$n`I}&pCW7Kwv!l1`s>_ zIQ+g!yYG-2S62JG1kKBYDwRsBo<5Cg*o;HiQ3s~R{&3nutV^5bGC*h_zf7*IU zLUwy-k?!2+VRc+|v;5PNepQ>?+U=%dB3D+U$)r(~p_WJ*Jbx5BzALWOkhHIg@l>Ud zk+i*3K7c0a69@1_G%i0o_B*{bu+Cc1|>=&fK4D@3w*59SG_T`t%&u7)5Ip1#aYCYiO z4|cWw{IL9b*Q^#7*@sXwNgkn~G5+joC9V49Z*QGRXz!+h0L9rdu#cMcc`Oks2ajc{ zF5E(Hhf{R5m>@asSJ-@01Z{paoS8zKLV&aGj4HLXtBM-OW=2lt#g|ptEwwsUGIYl4 z@(wN=M?prqXYsHBZ<@>2VB+YqGJ z)q9}aIodn0(bhoYHgwXtcks1%p3?GkAxJW*Q%9~CW$>y?lY18&XB_d=pvR<=DV9%0 zoAoNx*Cg%@LeKT$nfdd;8$RM?ze#?!qO-&*wjJB-dE;X^_dfc29UlL0>l5j!K)u;UlRh-@AOL54>GK84(o1%J`Yt z>}WZRSn^zmx`{a1_nh1vqru{4;<3+Ma%om*ENTyVYu0dQSC-&U{a#+<+(nD;)$uiv zk03l6N9UhskEj8r?0&*UOcet!EY@Im7FCuN)pnELr8#-TO-DE$?7EPWKi=!kEZipR zmp$-n{JSZz^~k=kfI7t=Du0FR@avYEcPd|TSd-uaKO{QUd>&Y6 zRNNIQLd6HFqUNq9hSjsgx}l}BPKQOxyv0<60;qrN7vxpXSZY6WA6Y+I6S z5CgEm`^*#~^+P}(JSNLOCBkIqR%})=kIIX)lj+0_+R&5`LudZR0{}aqS95u^6zzD< zDJv(<9QbyF)1;`g$WqPe0B!}YQh}~+!_mlRtdK^?d=e6Qe{K{p_wvf4Js!7~n8p>L zaYXh%dMU742fD$Df`}#(2eg3;CDsj#2ENvQ z%}K`Fo~^&BI3wCf|L|o90pl ztRkbUs!AQi6WkNrlTXi+kCJ{*2r@fxlH%k!qGWIPCxhn?Hn~7R-G9sj>T)+PsgrE| zc+<1L@2c9RPKbK-}g?YpX!%3{T;a9t!SYI4s;f(vvOhKWPGF)HS0vO#k=>2(Kk*1E1%7wpTT zy+K`Hg@&*IR zQhU!`Xr&pGU1&V3RiJN{ZHBG32(~s10y_I+s2iEPU=58hxudsfLV+DmhE+8;0EYGx zfLKMd=2Ak&d%>V1NrqasQB>YZ>b$q379Nfh z{d@x>|K3hGZmOdcM$iVCjl&vMZv_rb_kHH1R1W@nY5R{L?OPY zP0Sj6Fb7mIO@EXzyPjgNgUxnl-e)-Tw_Rrd4xjG{AGdNK!I`UQ%3kIdMo?y5-B;={ zbZxKBu;MGe0)Cv)iW?9%=1Cd?l56BO5Pl}fCa_7D^hjZDL2JgFS|M(&9(`>fdn9E# zZT%fN=}r;#Q36U+0^Fg2OO+n~cKQ+#H=RJ)n*LbOwynOFt@bKii26)M$ z$_!vRa+(6uBBoUW9zf{tznkVY!TP+^kk%B5TdP-G`HID=RXkvFGJp>zdgUJ+>&7=_ z3|6JG_Y1^75&UnF^c`ldAF3O2iR)S*JktNzb~ta2C6;CMioUFbD24}QUBjP0>DcyJ z{&PM#dj40~B#Oh9#3loY1e62*pKYY_Do_{h#`5(HIn-u%&j70bdF=ax*eiv2&#Wv|%30Pa>ps%8f!F0mFD?yof7UmDG7NmvMmbD=jU^xG%^=T&_Q0g6RfFi!yf ze`fjuZux*998W{5zC1lq4nSlVm;4Z>2Jv$;t%zPac~~4;u19Bw`d#;SA=Nm*QGotw z6G54K=f$I50N70i2Et_LT=WF3`@;|CvZf?bFWW{~2&&@aMVS(*E@C$Vi-6dnF^z{LOB&YI-1@<$;DV*ck* z9d1f@sKo$%Jo`4pbf3;i+8zSlL6d&4uYs`75KaJA09L>oLSWO^Cpw5B4MsY=Oy?_r zI$PZ6A;-QXI;35yyr14m+8hR~6NvafI!weFW0XiifhE)7AglTN!^4QG7tJ9KF&!5Y zRW(&%4B}v?m>(qbh{aAfcpZ>v%sPlIorjtKYVWUbp^&41BgHC6+5|MO>~;|9pURE42N+wVN2#J}tP z>G%W#=zqMR162dmNNl>JvC?}!Ds72a1Aiaf;Fm%eHseI3LaeeM-iP0V5n=rE;wntL zg4m~#%z51!Q@>Ttj}W@{E14Teb6QK3~j(h0)mKYpxhTj7v8r`wZa{h+mtH>RwTc87XtZdal#R`0=_96r;8 z85NEVSL1J!lHOV$>81lj?)+Upu3yP_r=DM!fwd8Tx!ylDud_0}X)1d|D2im-!9M&y z)IX9v3l&5Gl|m@Vgc`{!{G42w9V;Yv<*I;szh}Bnl3I|j?i2_y?j>K0B)hdr(PfxF z%hZVxF1C|?G=qDy&Z~@_d9_5ypwXq2fAOOa*Cd#SPWaUN0AbnH#fo$auhavd}N|A*7gZ-Q3a%$3E}4K>Y*% z$%j^PQWT8qY*BT?XU;sG;Y?nxWO-+STzKL)p(4yHow$*FOVNB(q|bN61?Y?ww5Dl%j1R@Kid%MR+!XA*iW`^_j!Up6o$4D9iHFhprvM40l66ne`Om{i>$q3#uI zJ;NYz`F+}A(i2LGYm0fom?*TCXa;-Tja(*`|gF?=XB?$DY7W#_Zhfw^1_ z|Cc1?JtqI)3xyrws9nWjsc25oY*s<_j+@a*r4^P=3E-D$>-B- z+}Hl6P)UTV<$?RdLRnhthtH26;QxM=1$yiQ@iBmaYW;x!KO@ZlmM1&Ac-ol$8(?m0 z>pO3@A^YE!@7V%smMrgKlIQjej1iSD1 zd4HEnCjw}TqBTTJrI?S=ttKt^`;MwLuntnSCv1o%`lCd}l(cA7nvgiBFC<~Jy$f5J zNWJRo7g-y5wdcKA-IyNSPLht_a#N?!KqZoLe&UpxYk?-+gYZtw&yyzR=a58SsnEvdlTD3R!TJZG-ZWt=KTt>5E+IhUu$klCdzm}SwGmp zGuf3W``xrw9DIemuNd4W@hJe>GY5$fzf(BL?o}QWzk=fdnG<4S!}2uNd0L6mPq=iq zmCk~x#2NrNOs2nLvQ!$;D)r$oC2BU)9p|_)1$QjtD?xs&@Yej+E$+#bD=njP*D_rl_CGC{Uz&V8;1WFGoahf z59~BUHy$*(Ieu!@Ff1(tznsF2s%7ri0 zec~Z57n^?4)H$B6#`LSEaj~}sH(WEtA=_zFX7P!2+@3=|XnU@uV9a7Bp7Ux3J9-US zLdmy(@>{+*OE&m3p6>4T^DD7MOcI=q&)tVNn6uhh3{^DCX7%{EU;)-xI% zdcLtc`X58c?a6?c<77Ctf$W4ZjC3PGt|srH+ErsxrJxZSN-I@gtNI&YfBT|*{pF2W zTw^WCK>JR~UsP2mLvJ$fCW!%#_pXxm;T<5jLDd;3Lr1n(ENn zM(CIE3KL-ycj9QRJ_hYpg;<%$w6w)I(qf3Dt4NrR2PEC;7-TXI4HyX-j z>SSqoS}v41&m7#d|OA{UJh2S@CbZj~M zp9GbL68u7l83Q1#WY`zzv1HBKCoGEfN&pR3R0ToA-bpDy>+6EWTHO2yZWmN^A)~ zFez-|#S|iRc=`*Wa2*=C!y^$YHy^HWeEmw4(4hXD=5%3p>4tNg-_UBJ}w5 z6#~^g#e)w)5q$O)pZc55Nwxl6ICQnY2jmKVc?JfZO;toyNF~@6d>)@wRm#1ZN}sN>A7D+EyuOXTe3*hVkN<4PE2eD-~239Q-%rvqjk_t@k!!=cmC@ zW8{Vsia=G!sJYzR5uUI)cpb>`Mtn3~e$IA1>q+l|LL+I>`=S zc}e)K#IoQA4nk6K^FW;C!2j8cgwH=uXPTExH;TVd@_V=Y>dbfU@#`nK$4Kn{zW0}y ztq|X`ADh+M4EtnX%;EnsMYc?hq*NDvQJ`qvu(`SDC$;tDNo^2D^ z5`VZPXM#$g$^80QGYR1*Y18lS`k*7Tf=Ak^zqVhSZ@1fgxzZVHG&|@2K4r-Di|g#o z6pnMA7IKRpGc7*$A~NXe=f*3#?_XEdXjatNl=x2x+ZXjB&33NfGKCY1k1Ndz3YzlC z`^f9MkF}G8cE3N`bgzi61wZT>@UyPDhtvY}FR>r02%zx&ZUtQ&1bDAUGw13a6 zfX90l$z3T8I8nCdOaJTxZy#DdsA4{T~`kHyRa-R;E+D~cJtp!^DgeHoSNT%@04}h@h$dUiPt#h zgmBe0EcRlW5otPo|LS(er9onb_bMyH_TRmt)_u(}`uo; zD6u5JNI$u>s0g~mfSCc*js&fJWP&b$#O)UeEXJng&MVBTQ`p0@Y3T^-d4uQQ!|t5$4Le<9Z(sP4+td?4iPZT-$GYL};EUM)I&pU*PtLNZEolkyTX zbBgs#U`)M&ymXi;uqZ`d3yNy@{U`E$Q?J(q8|i^ zPDWV*gsu^}v;y_(5MW~^axV|24_BFl0AHK%mu#q!gwV$T&TR;w+l*-ra!H5V98g(@ z0I68a!6^A4hQVSFq#0IxfyzDvScJte93>&X5JOE^Kd~5!CNMl#9fg zl8VI?L^+Au6i`Wt0JE`}f?RImHV0I4BEV%V=Af3N5R+gLhg=+hN>T*iZN*Wp26(fw Tfs_d_2r!tjFfb&wfiwXCdO{%@baAj^}Z)0_BWo~pXXL4b1XlY|}P)h>@6aWAS008x^t3-wG zy*}qr008Gv000;O002~Ib!}p9VQFl0FLY>iZDMX=X>2ZVZfA*5PDc$28VUda01Zh< zL{b0%0RR910JR7L0000#V75a508y?qJpB8jX@9g~MT! z$pqg70s(`;00rO{C@Ba6{F+<_MiJ_S+k?R%d~zH&y$nKt@8L$4WkaEm{vf>! zP1CbA2Fj2m3131PXw{SlEu_IIY|J8h2D+WsU^biKiBJYZG?XIpNHGH>5si8Y%_Rsd zAvo0wt<;R|R_{V3wT{5F*NY+5xAM%8O#RTF8#ivisK6K`ULlPY0-}o1(b4wy_JV?f zs;Vm85KReHq=o^KggvGay=pJO*GQblWH}s(ghJt9R6>7B(Woxw31D`b1`T227@p@i zmPKrUskBcdDL(ZfsLqtMc_FU@pI)!GtE&sTzPPv;WZSCirF#(2=lS{hwY9a~-QDNU zpRcH>0Ckb75T2znK(dvLB|H^CI4rpRfeBwQEJPzQOoCDrqiABD1cGNwN(_-WE(?y!niz6;R;6)29|bMj8?M=^q{6OfDKd~qM1_I3a|lsFOd2j9UV1EX*v8W|Z4;e&3D!0cwOI5!KC zV1f-+$S7)SYpblROfo$Q{pq?xtSKmh#MH*)3FVd65-EgEW*~q-_Jb-0J+N`(##J_f zlFc3%_mCiT^^c7CBaG3?IUPod!ZmS?JR4aO=WMI2R|tg|n&ManvsmCxNt8xI!QrcY zITp4!FMB!Mpc!8c-UuuohG8-@Gojysg+(#q^M|cwBj_}<+29%*85tcHWx24ZG~^x` zc8AutG~&pqPg8$Nq4y_@0*pS4$fZk{+-|qsZeO|~v_uV{euBl{Gc?vUG{Kqe&b(|~ zGl#&?*9lag;n6)>42!!8n(vFDQeZ*oOeIM&vU4bsuvo1zi*?lR8@ke4lkdpR%t)2} zH#hu#e_2@>mhjoTE}Z)I58nF9|NQi!qvxLg_#?mg<-1bY-8tynzia*R(`_c6zScAF z!jq3^??t3}AmvCFdEf^0_xB?PSWOcM(dfm_zL9V&Bfl6LA}8oRPWkb+sZFP7N)p9r zR0b(7$r8tN@F$Q9MUyf0rufW{emifxl^!{r;U>YsdV){%gDTJ+XUJ(O>`N_b}`cB2D?r53||q z@pw{F)sh%MGy20LzHnAynIuN_X&*$KI%k3zM6>*G&*cLL4mY(lAAA4(mK{626B7;1 z8;%`(%aV~ZKG4hA^B(*7$G}udsyu$7DGCg+fg?M2V5p}sTcBBPx%B{O0PS636iK>= zdR&3nXI^>s@Zk@gxrMK}#}vkiE2762EUzqj5Xi#FoziqycD})|r{&Eua3>Z|CM^)zs7l{oaln z{SWMah?GLSJtr&E5fJ22ukgty@5i(yv$XxQ0uV(pCG{+o0gMKNERL(>DIxz4OW5?u zQ0M-Q78}RNSFc{RI~?6N`kwvNXS*(6EU8~_G#Nl<(hL(*RPT2B?dTgNzI;q5Qp9M; zT4J*o0?@NqLh6`p4x>c|EtFqah+&1*_3Kn_q)k}{RT^#Fx)WNcdHX3_ph;s6P!wgg zT2oWiQf`Kk<~Xj%VH}(ox7ZzEt!vYiXnu1F9jjEa>AsKT<`>G*NIiyuYrC$oh31W7 zB&@B5kr5$^PrWBz2~?}fRf_VuU4=Ho3fMoGW&&Z8mV|MVM3bBfcse9WqSXm#n5@x0 ze3PdBl>rEyXf&FVs#35JR$i2c^$rY-jTr42G)01g7>mV2eGny}$#R-A6qned(Fnyb z@O>~40P0uW1BOG+Zi3YGn$iCX*c2p5Mdg?2sa_Um;s4gIMx z!=!K0SXRig914X}Qq@wOV6-?@UR+>xO>__ShiMaMvQiXHU`jHy2#8oj7SN6Hg(S^$ z&}xX!gd&PZ%!7<+_yr6P!8gH(ftuWm45tG{M(F;~^1*Jmr?$ddD<^1PED`Xef2Os^ z0s-#X>YG5x>K=#^NYi1rj*gBGxCR3Xp^zNK@f6KytH9aXcBOX=a!M zN>W(L5|IK8pz{EZ6FE8A+1XhUA?))9+8 z04}mxRV*fn5j-Z6ip&z2gQe|G-eR`193Q74ZH*}<5g-deAP`vGjdKt^;$Glw|HMQ% z7^G<0bh4-VNRj>tOK}7F%hbAVXUN0^P z42BsQMwDe&clQWQlV&q-vl&y%42iXOomQ68fk}}sOBF!K0A7k3huay3S#$%Ek(zMm zDQtj%X|D?YD`lX8C7emGisA*%pFdYxS_a0P-VsxJS+88T_*YU8&b`*C=quG&oYdl6 zIy=#`K=d@70qkwJ<61F*L(}Hba5&`k&d&^*1sKzqVSWNigw7%!Ak=`p6nITbltfA* zj&Lbc#^4ruz1~GUeL7OPA!f2ihu<1GG;Eq6*MEWwPz&%T*yK=Zee)qUIZAfrZ2+{7 zDH&6UK9+bL6vl+E=Oy|WnkJjf^DM{lJdZ7a8Q>NIb>_UxkdPoFL&Lm*7ouWzR^}3U z0PPJztMZGRgn%329ka5sGBPq2%|+Ujcxp&@8Mj2vIDv?eO!3&{)zjo+t84foB|96@ zqsAeQ4f_27u<1P>F&NZ|-pR7G%?7*yH%PJ!){ZV1FkMk#aXGDWMIwNNVZnanaW#Zi zQnwL#z2n{8!wf?i42;=qa5zj>D}%P?XyujJnE|RLNwVuo+dD^2KK1NNzyJ%UIbD8X zNp)Voe%cguS{xTP`EmM%exqJVTxn>ZG)-T)aAE7#t-$k(;{l-l>EdZ|ScOgT zy5s;sqWkh|%FjcMal74$W`fTs=_8?_XUrw~{gTg1h$8TcjU-uM8lwr=S(M?`G@90; z0~vLjtRhO<%WCf_C-!%Pg$Vyhu^5}!ZU*emI8uH2zvXmd9rdh{hodjG^k#N+Xf z3`GLre48Z{2m&b*BtdcpFf)MaU7iUEBWxxk3^RyHuv=(~9CeLL3Z7-RX||90VdM?5 zn93wxuSbOPM!tV|9P|P(jlp0r8#tg8x8H{ml*MQO(i0_-HyXp?U^ozBNHT+>h2f#A zZRZK2*`A$AnJp&J3>J&NX)`X6wYTHV_UB)C4%7A{Exb?k5kba=?nx&EKlbyV{Ge~d z|M333M?d_)WXqO9o@?DVYiepLYwH@SiZ5R37`WNhbCD0W&B6>tQX0sVg@<=!w z@OXl5cf{)@A`y`E*;e!Uw(~c}eYsBSxL>Hq&pg_8rKYTGnzsnmKoAAE;#N(2G zz|+?^1_H%sRBa@?9k>NJIIbBewzRAmOEPH|0-!J;{jYuPYgevZdF7Q?Hf`EeP*9-6 zw}3()MaTUHqzp-R$_q`0sXZ>kQ_gLytK3w)A|}%VpVt`d9G<}Z0zl~--gcv2M+w% zpZysyL-*CILtR~Vf^hPDmeI(XOft|Z&w!`fH|}_IhA=C0bjWN`bCQ2TQ6LFV$bfMy?Ol7$@Z^*`bA&B@9{^? zoSo$@Ep;`l$*xEdZy*S|B>=u~2yZCMyVTJ>V9(yQX`97Zyso^muB`ao507|a*p`My zo84(P85Egug6$-Vfnd-*Hg+=_#jV!b4I4~m^U%=H#fukJDSn$`2IQ2@W;=K8+&k~Q z)7sklg)e;J8{hcGt)Fos8uqtc7Gh4Eo){=qDb1^xb2}x_f(~kx*EaGo6-A4b8b3_M0v* zuE@2uEss1{C`E;h=O2Ce;K}yR zo*NuPWo2Z5NPleq!#E~&_V(A7SAmjp2SX7>i82gnw;RgK>p%l=9Fi%y8Wzzx$XEvr z@YSz=bzy;X_WpwzRZ~gth?0@JXhQ+sS z-E-vV(GA;ozx=ZE)whmfNv70N##f&wTsoGE0j3Z=N)VLDm7+!g5hVGEPrOi7U5zEI zOibH#u5PDHDKjY3RR5HD64aZ6gM%)Y>x*CfVoy)ccfRu-C;;*geqtz!0jJODgm1!; zNWklLc|6W~j*dhtWQmN)Jj)m78F@}6%St2N=tf2V# z`F4jh`@XFkML~#&q993)4b~7z_R|bkl4Z5oj7B4^&-R^gQBzm1-?IOq`{4IaefD$7om`tJ@F$I1Fx9iC^>JB5tMm~}4nSNAQpo^w z61@r-a699Lak4bo20Cgb6IUCHXiWZ*cSW%2B6P#GaX0L>5rhuG)CeLgl8GXni;e!ZF_ zLR$idyLN2ati*w@WpJQTr$J#%jLNat z_U7hLB+@%JCNT_dGO<>x$?nKD8R>-es7wwJSYmf2t4BfudF|}7(A-Wn+UN&>k?B|CzK?>pC+C+avU&&t{Hr8 zw-^YhVGfeSlOzwVr>LkPC}`0akMDCPu%zWe?Z0?W>TAgwk5Xt+3`Sa#A~57ELoht+ zaM!=%-AuEYZxsk|Mp)K$Mh}fMS%j*sVp^3=M|fUW(TT_Zkd_MK>TBi(!D7 zh#p@c(G%LNBATw&=hwA7C{_%&ft*_iGia0=TOc!_pE@qI4UKzon!0b-4)_yD|H_ps z(9X)r%505@lUJbiwHs7}K1ZbgIfWUJX27ztpad8v0lyzKgU9Q|Lm^g{IfCF-UZ6m2 zD+!qz@55xG*W@kGRwNP$OOgQEfMrRWjkDRDHk*OxCxt~U57EyHB)ONOTT)`7twJo& zyhN|1g7f?QVP7~3=FMzGud4X?LMoQD5_&5S%52PFq;QLoGaEUZmM#tv=e*3IUsqI? z{^LJ?^;4h!3Z|yJYKN9iC;cg8G-E8#9L#`eQkd@=>6If`W&v&9NVsx~O<*h-S32R~uj(mhbBf);9!5LBZl zu|%?HEu|3JUyM;C!CI_1VUtu7Ix-{(1A!nmFu@Xv)xbKfChL66fF-7wDBik%-$N0< z=g5bL^YRND8`t59Fv;0Mtmj>9pOG1`#6-v3%)rR9fgr?KZ0!^Ml*gf=PgPZmu3_0@ zfu^V`AbqrD1ra@%H$Jd@W4J;vW_u=Q!fggd%L7fSTgT-X!_Zoe{e~4^)Omx82AQ0WRe1>PA!S{%0wfA7?oI#H5t{V(@0c=0&r_E6c`=x z;zMfgL@UqOEJj^3XwAMg$$;plySocYWID{{IOjZU+9%@X~Z z#Y)5i=|c!(AVeZ=uYbZ9@P>fs36%7cwL3YA1oQVy>-&Y(O(hSH_Xyki=Mj_f-*NX)MZH zoWnicy*<~>&Kx5n_ud@Yv;T?A49nSLAN2GL6qZ#st>4&pz3qBOS3%{vy4o6)Ki(yT zhvSJvK{GH8tI1}u00(Ni1xUZUudgp7BV*^zT|m?N?t*z$u-olkT%RtG8DI!~KA+iW zRF$42#YO8(#@OPTaTjAHVu8>Y42_Ooyw)ueEMu}7OcpMaH-qX?rIf4=n;0bw{N?jU zZ}bnldM^bf?juh>fAjLmBbVGecJ4lT`0a_YiFMoeQWP;UIxsvoW;C1LKJuQ|8!WA? zf9&a}4!-`MB58Z!Q_mm%<&V6Iqp_;6tiENirFjd9rbm!F)0e_0w{^FfZKqJk~9$x4-ERE*9X{Gq@lXvW=}V0 zp&dJRfDf(BBTd>*JU4C8i5csqU^brtIpWu^U(d``BQ!~hTFKp>cRG|6+Y;%tS(>ZM z0z%X;L_>lnBm{AWWmq1lN?r5;G_0#pZF3 z^!N5zY<7~u%vQVI;iqEJk_vM;=!=N-ww>F=hya!q7)l`0qmhUh3CE%l3KKJV!eq}h zbBx7obR_a<-xld3q)N%^n8FPDG%IMrU%md*tl}E0nX9a*#BLp*lg7+|9y1z^DXD5M z69`l7U^*kinW6rTiIIpu5cK#%zJM36exy~Vz}QI-2L#!j3$0`aNx<8Kf}|?=-Bwp z8$DHx8?dOr7)%qx-NR$v{M@WJ|NGUN=8d&=>pQ!$mM3U-+uny`YLq{A zUtwu57%Z(`hs9*C$ITi*o*F@2;J8Jr8Piml?DP0cHYcV!$9PVDaZzbyBpkv_;P1xr z%j!#O=z!0&c~=H+Fi{L8NecMW7{MD2LexFL6IhOcwF1$bv!HEyv?DrtL&qXWA0bsU z%~O!9g%VAY%c>iB-qhK96Vp}(Q?tr{_X4a_i^Yf5Az$ZsSv?6g7?%)|9dW*S`(T>bW5Ist(s3bf|eCd8`~W%CxQV*!hsn$ zTJsrbhQhJN`z@GK)|%~C@ED2kpkU6@*; zPe!Wd8KwYDzxMa{@7=pM6bcJE5Yda1i9XxN*|MxFO!UdPt+Xf$2WoRcK>@rP z^iQFhZ8!e?ay;&?#45)EAxvimEaBK8kR)Zb+U8;gPTdkJ3JW57gz3BK?ku`NjR#H# zjrj7*FL!o!qC*ER2~H(C{}-Mm24C%k842CpyZ4vc{#kfWM6w?XtPMjxpW`ZI+YiV{j=?GSx3ma+~o zo#?^A!Bx92KKUa?M7!0Vm6@fw1gb%ZYw@vKAd?mtm1c%H01|{{*_2eZ>{}vdWCk?S z!C(j!8A`f8-KJmlvALcBx0W;cJtHa=!jTw~KfA zD}Z^4J|u+wL7!Vt^9O}zCwjEFhHk-<5){LG*v?Sms3 zdiT!l`Y|MnetmE~+uPd(LC`a|qL%BiTWS&>6hXf!xB9-E;3BtDE=Xp7U}uo|uC5m=I$M5t*5 z6oNHBvj36FqFnS=w{@rlqE1SS(*`DF7zS$2&dx^J15`^d5&_}P*_e@`f$N>)jg9Lt zSsEG|3WS0zYlQdk1tY};dDpLX6qZ&xZ9GyAz_g$NmKbKT60txl0L`(JNyb`VTT@?G zgSJc(Yv7n^B$mR#uuA*dVc;O?!KU|#K2_4Q3hUs+aD+2kNHKciz^TgSIvV%f>>i3? zgwe>UXZn#?C>$%VE1^id=USK78!E0Sl0p%RV}L)*R-=J7$hb5yKEa#Kv9Ryh@nh8+ z8Yo5R>l&n35UHR8SPGW|A*rIFJRJ0$KYyL#*ut_xr_FJ*r^oFL6xWuFi>{&IFyX_w z7-yxyXRw22;CPNuPqxR9FO*XfYm0sEkC?3Xlb+!rXHjv)Gj^lX#eM4IWu=z)e)L}TmUSoJX}x~=Mpi*y!}jK& zYwXg6n-4s7|MjyM%GNixA3DlAGt^45Ga>;$Z7>aW_mno(`v?24cZ{+|OI1bIwbPfJ zrNx6cdTEN2W2A3rh-A$T^~Hiw)RGIy6H)g_cvKlz=m=$u8A@|X?G`)ODm^_t$?vtm z9sm+|I2^CP{`%Lx_O-IIvOoO8Km6S1J~w5C8q77L$=Gq_V$+U&&pfi{pT6_$e$w{f zzK2MO+_H1a&CV-1xjFtpVQh4?pt^xhY}3Y4G2ORnu|UfQWDuhi5o>`50A}7y;S{JP z5f@|X@M*3rrXFpkhO)}BiJ?)SU)Zv}Z1CzeIfj*1m54GK^?Qw0b9iDz!6cjA!V%U@ z_cwN281lJ>EE%?I?Hv!l^2FG+OMM+Zp%9o5ifeE*5Qx<8Djm3THHMRA)g=VOc}E6} zW|K#f6->6-%vQ`^Tw3fbDeS(|<`0Ox(NtJpT~?JJ0kaA`2Q8%#G>^ncP+1I4F*rlw zlk)jYSreES$X*9)XWhDWPd)WiOH0e--?hgP*ok9D>I$7#+fMyL7{y70-NZ+uq96pL zqJ)%ZR8s2e>qk9)J@yexg$4kLiH#i&X+&UvQ&0{c2~#Ex=nS*eto3jz8yBJ8sJTHTuF7M7>Tam zx4Usi=<>-kBYFLq6-7pip=rm~Oh;xY*3~ zrG-&hu!4InMT603SzUc)Lj!mPwT<<13tHsR4ObkP}KC# zQZO9K&dDP}IN6IAF!?s#YPOr9g;-*yAI(1Lb4^od_qI9#t0cHRiV5?P~!q+B2 zH4&&~8=_g-!8+_tJ6r=Z_<}z7fZGfIk^xzD6>yTHz*SIVJQXP;KQ|LoB2giVb^)o| z_M>XvOdu)kl%%K}Q6zPLTtrDqlBk+jG7d~bVv$(Hlx;UeW$)kwDXNm*$>f5hw`7cGqwam(WxKT&P+hHfiZLPi?P0$3o-gmD8-aaz7yhQgIwX9OlwgH8cNLW#g5)P*oWP*g$+9i@zl(Z*!jO*R8( zpjc{>XrDs#Sjxf&r!-HO2oFB^;8Loc@hOYIoqa^oor38x`ZF*?f(lj5z&Y%Sy%;Z& zqEavv^n|<~&n;#EZUJT>)%CDR2WF~*Qz!axoajxM$;R4jMyoN?2>f$P(#MzOu;n27 zIglJ$x0H~I6xMSGACYvYV77UN>CAwk^xr_$YL-WZq)DsIYOz|TGed}x5Hmm-HMk%; zKhf*f7ly~T3$F~(&+y7ierT%dxr2{Lx`VKg#2b^wQj6t7Yq9e(gFp))@fS$+X#IHl zL+@GKC9yzj96+GAd&?~eHy<-xXuD8TQv-(6LWn+1@l$a3!~(5l*zNYE+&(umj8BY% z$d9`avP$b|qF*>@nogTQ%Z^kjS!^a|K;Hn-OJY=20g`rNc6!s+!l0~JZqR!f%aj*L zr*9N+cKXPH^WCoPo12Qus;$P<>~p>Aq1l{ckz^quNyg)h-BUm!B{iiOx`+nem z)Hkk)SnxXmYc4IyU5Ql57#+HK6Q9u6rcn)M+mlZ{dF1HHwBfUGtk4lj_c&6{0LgQv z@fZOND=97m$GEJlGHw4a9QX8|C2+g3K>9wi%a<=>`et11$Rm9( z2-^4$e-;!JfD%9(N2Y9#!nN#CB_$rSvFNg$%W_YZ#=wXOTi6V3ng zkH7WF-+AHkxwfH+&_AA%Jt}Z{7x3?i*;4*e^ zZNY9QhtEBTHE~4JU5w;On1x4Z>P|4?oT9Sg*7KKs{+6e_s@CiCG_ICCiyx zTsYvq)IIRvzI_v}fwGGF>av2Lz54nWKL4rj{lj-ck@AM-#w!;)4;_3j#v3xSvUl#> z{lEY45B7fiIlZ_L*BZ?Rp9d$qR{3l2;Fr~IMbm&4sSF=k; zngM12B6qMdK?!7KW!=_YvIdHxwrtr77jX*;!wHIffn!6#&_nz8V~Q*Y5k);yeAAQ9 zzU24&fNNw~YHX-yXbMe?raj;(NLH}WIZNEHrz6b(GXP27f`Wpco*rNTJxdfer(i}S zR8diJ@Zdp?MQlTf)3@|f*SglqoeD?PC78e(*DbTd!RdcWbqG>2xQc{9VJ1b=&&~s)@$-{gv zL+@}DFan)?<;oQh4Mw9eCDI|y05bzoVN+AnxpU`Omd(k@QIasP9!UOkwgNNOlh`XW zH6QJ5Rk9pq7Q)QS+887nu3o(gpCDyY6+D|}fEkc1>DRAc-__OC*49>9T52|%^}Q^s z-Nt(b`aq11j)LsZ%gd{*tkjLkv%kBL96({9Xw#< zrQL&5I&(L8`6j!dF&2vjJ&5DD{QP{l&=*OT&I4%%m>K$N)TK+8VB~Uhb76$^)7|dP zDV@0+yi7fVzR@c|lBmAEJ|(MQSegN51M1&%=gxu10qRfmwVugpQnBe5X?rQP)1>O{Yw>7ZmY6w|3c;WS{A9m@2SP6{WxU@VG? ziaI(vMn^|coZ{SXF)z{#FauCuPfScSHa2QbFo|QaG1o*!MuvNI@Otm?hL*<57u(JD z?7ZyEm>juu8Vn^JNG$}B`dt%b%+nfbvI5r>(XnZv`w zhyl_*z;eOo^P!|Y(8FTT^Cw^V>Q}$<|9tmvzPao1r{Da^e;>bi8$WuY?ZJok z4U7zJZCT$x?hXb6QCZ%xd*}J~3wb3K4?ggaDXYZD(bH4?q>);HU`kcu$}|Ja03>YU z2W1n`MX}?jFMjchU--?T_iLYh_P~4JcVy*O6z9D9)7M+JHs%(Woc!?6+wUGObJ8I} zKsBWO;AO>~ogF%PwQu^u;o1SpKu_S96pcRn{7WTeCAH)a!_v$I1d&loPmRaz~e=zn!W zn&FB|i=fkUi^?7^sX&eYSYau;SiceFxK(UUC%SQZbbvlT1pjI?NCb*(L7&<;7!n1@ zEle}R+)(c5?9j-t@A7T3eVD+;a2<6(rIE>=UVIP+r^!o9U3J~c>VR) zpMCb(haP(9%{Sj%`8F&BXc-bq(o}>7^y@7*H<8_0nV;yx!Js=Fnh-@VL56vb3PxfR zE(#~i9A`5a97dx#walQCd)(5xE2tm4)3ZaP0jR2~0t!QW`_U;WE8mj2F>m6H1Tm>J zs$FCA5xu~%n9WA>JWG%cbzK=(K!Zf1;ZVry_jS8mG>*@~4C*QJnwnYYdNV3BF*-av z=EbmRQCYQ#XBNQ&Q$hn5%|+h zUEoQ16z3#*yEf`@Q%uoz*kIbbGc3n)yv;E`GbBhqhG=LZ%mB}DWIF%(JO5}ctp8m7 zy2b4$oT3RAJy^OTP5yHcxWlqTC*%i~w2q+~&Hvxu|9w?uW#ZQu>fyDcqxE@)xrrVm zG!Szr{AxV03ON%k5 z2GjU~xc%-$b$Y$tQ>RX$$+3j*PN4QrQo2x4ZXN%vtU5EmqqS(>S&3efZ_psP>m%8(})7J`7NT+vS&%?0AbbbrWu=!wb|9)6IEvL z1OxJfi^Omv%i4J!l!KLJ8LdY#tz&0k1|)8`-uD1bs@Y_T1@|f{2KZ+}+UG(^_(d^K36Fuq?e;_#F3wZoNe?$Tz zpm5B>(KfTeZZW|tB`0KFVP?>ui{^RYLK}PamuZrILI}HQCQ4Hg1CM8A!AMvzi~(X+ zFcMLP)>Iipm3mE(6iLV3VhqR;K2;KtB@yZ*DNf@S87F9Xrq-62D&GlGs|Ow}LCp+H zu$;kWXSMr6fxuu)8S#0kNQ5Rxi_z#X7%U0RfRxoO8ig0(jq0+I9@UpT5jO`Ay&^Wi zG}8r<9eg83g-aJN=a*Jw+AZy8&oE|3O=amo-;H6HZ$nE{*R`v@P}IP)(P+eEwq<2y z8Vo#^cqnLqB@Wga$_}m54Wd+C3C>LPf+RxYX^!D+yqN_)R=0Wx5tlopj194P%*@Ws z3>cOa6EZhHBsJ{TjWgb*e~F%=XT#O`IUEl9{jPAxCCB^}73Fy%JBvB`;aT3AneAX+ zeYI`p?kzo?-C6nhF(GiZYdF(xba?{h6$Ldlr6waE^7#&aaL!`06qn?Tk9n;&v&9Uw zK^x5uf{2`KJux1{S{e!uyxUsUP*2Aq?vPwjl<}L^3r@S$?eUdY6_k}0WM$gLaPY0S z-jxZup|PqsKkrKW#r~1;&D)!eY{={F^SFUw3=C^GnVd$Wm7++U2NpNMfX**FeE2Zb zvuoEbY%<$iYCG|(gRLgsnw?=D9C1ma)W324yKlYu=uPC3;E|P}<=^l0>LEXn8fw z9i~XP#|(HvIn2|cO7fQKq`jy5w1$2=lo=cx{QB3w9*IOe9?vt+JX2DV)!o(ZaNu@3 zlbdItX(I@G;Osd zZo4+NeM{ZJ_s{HqY#%MS-+1dh5fg6?`Li;!Xx?ZwvkGTC(R%z{Z?veWC@>Kia|u~h z1-wDlj7l*S9Q1m-dV4^*S}QA?3YR)?YZ=cPkwe?WkJY0A;nBPx32&3 zkA9iFkz(nZ9AEj9;%6gzJw#rW<$-`V820$XenFg-=w(qSSEMD7kqmVc*qqE@F<4DT z1JA2VPPfQ6mN<%T4rV}KC)%Gx_e9dd5RA9q@4tNc^7p_0eNeA|{^x%V#;(zrSyH+| zk^*6PCn3bL0boXs=b%Q)!2iyR55NE3k@ogZi!-aK(DCk}(^=U$UEKpFi!~!7Cipz< z7drCyZOkmp&(Ahpy)xwU3-uL+-6LL`*{R@UX-STzGx6tMe5A9z{mP}TU<7;Qp-mqg zIz}?4-k$!dB4<`shJokKUhJr=&gWUP&0&*C7MzoU0`vH|zo4ePzN#=L2i49GN#QV1 zEsNpKtZXyO!vCtJVYOPp+FnF(9@GgBs;;hPS@y*jU);NQFEG-rUnME8xcpPkKmEh+ z{nM*I`DxhIpI^11w$S{q|Nh?vMJ4b4>J8<=y@gq}P&kC)#K6t2qbJX`G&OKEiKRjV ztWYe_LWthu^LzXuw?7mVWeFoFhNoFG=VZ+^t;CRB1f3OyYt<@3bc!kZ)+)_sQt){C zWJOl}0t~r}Jje0|UgZ!a<`aam@t`u~V+h5_kTxS{vzV-A6EQn8pikFuf7``QoWwS5 z-C^b_corghEHOheF)=YVHs*G_MNtIXw6wJJPyh5!UwGk#ii(QX)>aOPUCZ+fH-^KJ zV9-16@&K`scx2y$TXS>l4?n!+!lj;vAKKB=)&1$;c@B#Nul0;<+ECTxunED?m5!Tl z|LQPrHs+U=++S5JM`IjIMWrZ)1$N%Ijb;qsJwN{O{SK$IFw5L}^!)Z+o1-D`!;d{s zoa;D#>Qao#+r6g&Y+au}a`@nJnm4pGm9s2aT~WZY-1z8lAfk*6PlUsyf|*zrd}8?D zNYj)q4ge^Z(W2?&Ti^N?mXz08C*0bGjg<{5@jUm+r$0RWfuUyarlz_>2M@gXsn4vd zt!O=b$m{br)K-1u(FbUTf8-)QwdoJhbHyJ$ey^{u2Wf}&`!~Mv4de=2EarqSpjK-% zas~tVpxG)j2oWJ1xN&2QB(s~EGBGUFxW1ND-_4OH=4lsBS~Bm1JE9a-4c^RB+cBncTX%9f%Q_w6jf?9S!r-TohF)|^zKQKMYjU)7C{{PL=Z3lj3BfYMk&)c|yph2Hc(J30&epbU4NmmCJmLC= z`ip0d+j9zXb93FJ1HCu;G7CyfJP9H=Cd)>%h2wb`9a)seM~947Xa9{0(HO~4cqkmn zFUX%58-WW&ib@Jm-@M*6(%*f((`2=mR@O2k)_JXiGujJFON3CcFA(mT7^kpdlcpI| zW-!GygBAco?%VeW=O~Co!acn=J#Ke690oTRDR(^qzwTY2@kS=5-ygt|QevUN1T>*( zG;rX?I-MD^tny?y?7P|Lj>UW=>1AlyY^FGlA}LD2WTOd4AE+V_pAA=!4c2TwOwrc1 zR9e@BYfLQzIx;4L5TGbRiG@Mzz^|epLf=QDq$G2)45B0lB)#3PNqSs8dLz+t49$M( zb5{CLE}HsLRip;^rlqCj+_`h$ROqt{`YaP3Uk8x2fTLR%s3mBsSD#P@ls@|28xhLx_q#hUx5o&svbM2hWAmY3{k*niM}0%( zxBuern(zB)&xI2;n|8JxIoP!8fpf+QTA z3i>Se%)y%{p851=d6wZhUR_?1<)|bM2$4RY7netAMar<6DoP4{K5s}?yo24|i9sOx zva&KD`eZLcUm3*{r|#(9l^zR(#mNX$5bY*01N><=^ClCAG(#jJhC<-_bB~%Td#589J8ENsl5mQmLY9RneXiH#1C?thKSX2a^W8*lR$qeQVuQEom zq}SMKMxtLjWlT3$KnZNwvSsNtC)d9?tWYcvNEepEd72QxXd!WAS8GPOs>{{MU@=x+ zRq^Iee^gphA%p^DRW*GbZJaeDCqHNKX19R{Z_o-#Wb6HpzW3HIhlB25l#qM_M9VI- zksTZC&nfeFUp^m-h}W-P-ne zM&UF^a|W>gHf-NpTD8t<%ao&`!|(jEdCx}zqdlG7{SSWhnJec{5A}8q_jXD`IJ*#p zZyC6_Fesf+yR)24AMO|%1 zcD@jawjO%lY|kpMZQzaEeUCj!;<5glH**ULB{>#}Mx}_LhysgA;15_$AS$iAA?^Ug z%w+=InkFX*u-XkKbH27mR$Y|$1-*fZfGArG24{5xMp7)#RyViAL`kWsCJ9^+g!>2~S6t~M)T2m)WlFiy!THUNV$m(Q~DjLfw>fuq6 z3{FBMCW^epmRFDqdqz|cf`0Jukj|2{YE>_gqQHhx&q9>tNOB$> zH2XsUD-a}3v4vH2kx+1SU;vk+3>J$^`b?)`CZb13&Ln(WM4t?l+3$9dcuM@=8JU4) z9Zsh+7z_>#4Odj;Yc7GJIgY?TM4IU5#mdD3O|cWEG6O-F%ogB>0!=f7fE$8gZy@CJ zd4dX|kiZNa#jrF54j!q~sBV4eHET5~Xx;!cj%g!wfiKQm5lh z+1cv%5h3jL2jG97cOnpH29>>Icx2n!HC(Ye9h)87wr$(CZFcOWW81dXNjkP|+sSwJ zKF|L4InTTIx!!k-e^qslxoXy|an)LD)Vy(pvPWUi)AAy}4b!I{1iE$ zsMKffyRGYM*!&JKt+=r9 zG1VB{n2u(~!)T7kSR#*z%&oa}TL$mVppfKg1&z#|qaaSDMI^Wc~~Ch&X@xM|_zI zBkK_+(Th<5#fBYpY~3n2IX$p7sy56=Yr!+je8$1f+Y#QK0C8EwS zw7j{CZ3izO0ob?u^}yk}9nLgPNNY9$f%IG@SQ^%(5Ex!P@j%S?!n%vBR67|o>aU!^5YR)0KHj43||tT~ir*;X>XS?{^QEA`qjM z9y)nN6$TE5Absg05iasD?eL9bn%>`c!I9yKj9I3`4YVvNd2Vszn6TKQVS)vCvSpQS z0eYzAjkz13@f~$(dGiSmM)HYv%%@ojx|?MEx&~a|USD9-4d_Sq9gb&nMmYTyo-ABB zF185c4*;W#ji}&QjX*|`T?FktB)NUyfw?U$EtQqhyi)rrjbWf4=pl6jgYmLf@(twj z@3!a%2L}hp-K*L_M@#J#sAl6Kf}1p~CtnnGbadq9<0wpYIKAuI#88PxJd2Y9l-Y>j zw22_Fs=-Qmakfly&svuPG)u~1tjBz%Hec)5$P$oRXxp;d;kNCbWMD%V%n8^(>?@;a z2-p%vm{?d;l#~icA!I+Cqp|J<>khmJ1bk*l15BtGZMhv8Cp(N$KXM9LO(d)tKY~n> zJv}`5h7C${GLlRHLQ?(x{l0gizMz$#7p~puNl8f^I7ZX}XG*8*<$6=|w2AR??Jbe< zVA+*ln&M@nzt(@ak5w2M9#n!#X6)?RvDMYrE7qB`aqs|!?Jv7OyQD#y9+&{hv4Ygp z)M&Iich6p_si|rLmSK62TK*F=vkdKGa_C*t6JGIKz|cL9bV_91cZm!~#-@}YXeA%y z#<6~Ws|k1wcSFClO@gr)T_^c4npa`x;Lzx3O2g6Bq7?~Hxbc047^cmTk9<~}tP2YZ zzx4ZM!d5fAd#fx0FJ!MSEwRzl1HE5dyG;#{X{l8%=+Qy8q5mr2!v_$8ZuWTgGAH`m z2V8E?cj(YcY{hAT>p7sXh zjO^6|B$H863X*^%8_CdujpYGdnID%I?e3@OPK=H9Gu3i5zt@M)#h7~?IdSkGR`D}C zSL=sIBq5Ic@BsH?@n7D$c1xJWk)>5pSBJ_Ljls>V1$PstOW=DU*sp2h^4p85%2(}~ z*QD>hjdl!hUQVMZ6YGSUTU=gy zD}q_CukHm%JmJ~Hc*(c*x?TOes`PzX(49Mb`a$ls+oyMy3W4=`ruq4iMC<#pifRH>G^o^aoYV>{&}6m%YL8!$a>Z>X}(=!&bVn_P>ZcbLyFEL9-e`I>HcYV zniQQC9@V!Er^%^3+GXu*$ja&a5y7e+egpo-CLTht)5>T%nZfD!vgZB`ds+rrDwMCN zRzNMw86fQfA-D~J>$A7{ai7h4v9apDEtJTVVxrIM`My5sbMUC^dA>=f>F!?jJToX+ z73W_SE%|uqy*04?=veO%0s(PWq`*|&VMa{1{+xB;PhIe<)34i zST=PAw{t1dbzBWSor#f6Grz{1*tEmj0;QhWcB4yUR}B|@>Lqyp7iaTj6;0(RB~7c# z^~`Y7MMLe)YiJCLit^{iVb>HlHrW`{`^Gf}q{;CS zS5$>bz*OrNM1Sz~PDv9-Ic0fH`>15Nv3iJ&=UuJnyv%rAvPxNkx^7u%e}lAMJW-|P z@=}97B0|oWo`@+BGb^!prBI}ZY^~y|id@^NpQRZ&ALUuBCBffLMNii$^7ozijw=!Z zSLH|`o)`%rnfl|2;@-?U$1%5i8Ef~Ba)i(c^W?$Cp@FlVFbRV)`bk>{o0zt2J<4|^9UygC)c z1Ib+`CM7jCHQ{nNku8hKAs$cN^)N6Ac<7dcvz zi@@MC8KYvE1st|UTP}{4TcD6jFiJ5YGo(s&)u!9xay?&dSXYux06EUMSs57g8PtaK z^XHFXBM8Km7&WF14-HwlK)j7ygNOI@^LGm{Kb@SJYRWhWs0~u~3C6Pjn!1TZe z-SoZ;%&WWE*&*N}aR7Jh;pU(_sH~!b4IyAD$FR9;;Ra7oPtJ3b;&}2o?FxMuGnnh4 z*NIs?bc8{m$BtG~Q4JB)FYf<(o@=(Cx0UpR>*B^_NqY0GynF3)&Nj&yLu6E1sRk z7ntl7-7zt45WxQVMknX~JPq(h+(&a=rZey#D=l)tNGnnoIC zdXtTn1*hjuqi%&)z-+vym+be!t>AuMpHtTHap{k+YkJ$7 zih`evENknYj<-dX{f1TH@n<02Jae;)@1EW7qyD-dX9ACq-y-ri^pcZyD=-QRTZs#m zGgGQU-mCE5_pF@Fd+#qcXnl^$6A7BLc|JB)ZADNXV!G~vAAR3f;+j58@wz`ZyFPs# zCCgzLus1*2yRq({7Fm@f*>FM=rSlzBYCKv8uLpR=)P1I-6cwZ$pqfRq<@8*%w<|{Uxj_=}B zeVuDO)`!x7$7sJ|*WVJ^+P@6hW_8-_BhhwgX(x9t3$-Nq}@2w_jlFi{Ax%%@Mc^1+6Y3um!f*L%B}m)GUo3BdTE&2d0@!#Zc; z?RK)3^^VtScc4U;ceio2-eTWhy0DAIYC}vXRe+!>;vAc+h#iuxrgKI&LD1S6Mx+d%>la2Do$) z&{o)T~N^rv8jI26`oSf(nPD@7dvDMQX!}BRM*%&bqVqzImPq%1)?xE_aeGC%g z<8>Qy&=0W*#(w^bri^BSXI|uDx$%A=%+NWC8hdP){<%JT-0g6D(RH&~)dY4BjJ<=n zzL2HZ6aVUMmKHIxO}nV1AhPiw3Drbcuxe+fuION>DC$%_ywBV)Im)`*t1%}h=W?aB z?j)qGtk0@Fn51@Pg~L>(S8n-dkN>##UDKF>0gDd7K?X zWk_%Ny{F$DY!HLRuxK0`-4O*P`K$X@H8C98?l|_m54pjK&BQqVY^YI~he7qB_ z+IsVI?}$&;wEJ=JP{%Ke&URB^_UZlgq5EY;6VCUJUf_PPrmigQQFGF2sm9;1MwREe zJ%vV#j9flY(ZaHfLK4P&19}lT%=_vBg0}On&hkh^hTq8B@P3~X2lf>6994#GabuNN zN#jSliC-h+p5DSjH3&}UrT=>mJo7yBm9)8j77J;Ex+-nD24=7RQs-kYH#0=wzG$ZNR(vd?Zt0Stv1@U%gOL~ehDXKdlf2p zr|byayhQ)uR-a zm0@;j%(i(PHIQbxVRubZRzA<9G#!|jQbaj_A3Z(6nxcj~M9qt6fNBu&Ck=?6DvqpB zb|#P`hIxm7P%~H|%q%a63+LlPXhEql@25}#HaxrILI)BvVF&n)VqO{1UD_-=_LWU;UDRplNv zV?aJyN83%%TH^;st_!jf!7#EGIaQ^S=?#mJzBF&AO+`;I$!tg? zq(+U58ZkJ(^AgU`Emx*Q5&U%SV)he1MUHO(JX^Gd(==tF&?w~1QGOxc^-xhlH%bL* z0VV>L7A=A``2KTsnv3sAi-wuyat1$+m76LX=nMH3O-7i|ox=W$w#6OoPo!I-r0$eQt+iVz`cvs3SLXhaniCT0TE89sanjc@l@~eOaXOZwQZmhblhjHH$6}|w` zp9Bcg!FCLiExTS*K7G-v3bVOI0Lm3a8QZts%!BiT^YnZEesv}`cS%EVMK%bwcIWF(jI{Y{{>^RUHUH>OT1uKQBZ@jTvx7-AIf~e^2 zS|oll2CN4(7e-5kC;qP7kvuJk>QaX=;qe}c$VEL!O5S;HzAziCwA54v$WH$^ZCzxB z?;{8zxO*T!L!9C|pjgwMRojHmd&GXG0WbL+4X~>++BHpiQ+%ZuCbm;XDUh#olctXR ztX2gdulIOJ;z!aoUm^jE?9X3F32#gK=8dD@3K(AW)x*)PR}URG94t3m5rS&UXKiB=7RoclugE5Y7&fE@v0Lq2RyT7V-!RlUeQrcKrVu4?#I9R||Q-FTW$fM!m9ccX?rqG0XF$R*wSep2L-lEE3WT ze@GZ{PQ($9Jf*IAW&U1d57f>%oCi0E$vmrYF1Yx`C9%!PQ;Ak=;x$qhtZnnGRFWv< zPlP!WQN>q4NDn`RZJ@`Hb|a9tkdmt;R<3kM!tE@{bTcLXRV?T%4{ul2AtztUOQ=hg zW;iduh!oAE7d(Qhkox+u6>hUBW+IQ_59vU+lpLV7Ln@k{c5G73^-^)8-0`AujC@To zGm9ULP#pHC%Gjw+GlpM-Hzv#aaTwVk`>o#ha-_~wF_cIY&YkikCH+~0dh$Up(TXH1 z${>JZ(6JK&FBU}dwNIN7_nj@EQ!74!#5%k!nliu@M4r9`-O3|_suM4=#p4qA#Vxsd zrfn#Gfk~yNEPnZ=wJ<=VhQpHEG#zwIK &7^h)EF&d=UX?*;}K^s5+L1)7jJaS^8 z4rfcNTB(iZ&|?7~oJV!>8}UnKvWZ z_HfoDf75Yg)1zb>&)`<~9Yek7(yO71n|s68!^7)pIP*27Snp<>dN;*f&%Rw->mg&K zk7b;;=vj}YOV&F5z^mZHW0e$k)r(B!Ww(jhhw3Zo?!(NBf%1!ty;6Cl2(GW(rC$mjAhFSd zo`NLUbaZ1L5TGG+)yT15B^2LnOB+-vd%yqyWMTl|pPm1YeTI#l^nbg4RHAfD zIvsq-C!KkmiR{EtYrlI0f(vXx|5NL zJQ*t_9!U%2`Kv$2;P*95S4da(4{N2_zUBIJW_*GAHc8)aESE(xk;A$D8R-#g90FS8iM+!%XxK0Qy4Y=xL0~!QfMm z)kr1k9IL&YEqocejOE_#&LVIZ^V0g6V6sO;#f$RVU+N1CLt0XB=87O9r=YoCWHuSk zHRjjK*=qQHG{K~`ACwhql|O7SfV@uJ_bxa)T9WlTLJKao9&Ra*><&L!z<&5@dbFOq z!=grr?pSQp>0>Lie-&0^_Wl#slMw03#}gpWm{{FVSnl^HfH>O@dn#=a zfj_K-LJ=VLmj$UZ1~q@MmUaJLADv;gJSRW^fHUZSTOTk00Km}J#_9Ja({9#Q=h~<7 z`E1D_-Lm{{E~I51s|-6bGO8+jGLHi)LMFzd%P8yhz8qqa)+-2k@q8Q{FI|q&G;dee&KFRVGU%=TpShuNOGwz8Ux%uA-=|(VUNkq4 zygS~T@d8E$8Z*=F^=J>H@gC26Vn%HJ6Cd)^neGK`F?Y{xT?NSbT|f_JMW z)@xdA<6R3~^Pa|6TixPQsc|@~ud9O1b*d`v8(z~Bepwycn{}RP{5T*MFpiK7(1z(2 z(eXuuFP%hFIYYI$K56~9#Pg_U(^>lhPD@LBBrL+uhNCyj1R-vrqt7|ntQMjZZ)pXG zJ()`z0gk@iOL4gtR;hg|dRwP_lX6Q+x~9&gVwL3KjKF~~B9JDD;@AvPYs~iUk#$j_ zJvj?azsg9tral?}tE24a?z*xtZoToL3Rn4&uHh9QVgerAqh%-bVS87^!M>1j=!L;2Djs z_Iiq`j>23d1>=&$77oZvVfZ0;2x%8mpc_>3lxeCg&v|$eMOF3UZMP13%F<{qd^?ez zg3mJ!THh1NWi>Xmo+ru;w?_je@8csDKYHf^aEttC|*liQ5dc30+rD zY@C8#Hw7;PpqewX_+9B2x)+8Vb8m^~pt(9$`l9H$cI{ z4oKZPQR^y=Z=3XG$*zJCIP+NrW%H)K^2}AMI3ov8y51>lAkkNJ6&0HFyEI9j5eB9k z1ul=0t2mm>W!)2wi5#%&&vvci>$gEPn>=+UK+GHxwv$L{UEXrRQOn+z7!Y%QHLoL% z4pUM-4$e5Vb{};W$*G=mx@w9=rTclaHA+sH8WE|M)i$&hu^uHO!o1W%(&e=%(Vpdp zCc-Z;eUvij?M8O;t-|)`Zx?sEYxK-5!bB7g9m7RS{%G*c2eJk?y0STqBnR$K?MU%x zhzY*hDl=y4EFiW1EF`j8G3ZP_Tb#?VqiO{RD!!;e7kEI0{ayR5ihUZNY9h6q?vy58 zMNu`2%Lg>{@hUO-?=#sUy^_8MEwdQk#=`c6iJTGP^`lD5H)8_9Dp6FFG^k0o$-f&~ zh}lZqW{77r2-6Y;9b48RmvKnB+MH^de|EuUeQYmXM_^3`7)uGetC3!=CLrU7iKRc- zDGz7rF8*Bq(k_H;o&pbFlcyfjIbZBrQoGC9Exuzb_=5Zrx*T}z@l_&MixgGV|1dh0 z^SPa5k9T!KAt2beo!*=Yb?~!AM)~K^&M(XaimUT=LCLv^T*CgaZezA)2Vre2m9Io#jfMXC0 z)4ZZb)zOCR$SB*$RO#8K+b&>}4SC65wo;NM_YdbQwh7{U1MiDO%88Wmn`QBE^Ym1} zYqo)}Lvk^k;or4u{o=^RJfWZcrsjkNi3=6|MhOM?=>x3LxZ>kT($hs4SrWNLZ-M;F zJAubNMI-k6TGj~FI%M11;Re0><+XriO@U2bON3IcQwi+{c*cXb`uX80&1bSa=H)^2 zhDtdFqfUR0RM9U_$ige6MC;u%jblIqV>el&6Mb7l1Q*9)x3*3OO;4DdngGOFvpJ;H;}%61mb7;PhavgD=93ir&q- zU-(D+bizhiPqcB7di`?SLM#b58tGIMV+4(T_2kA?7N ziGeA8LQWe2sEt*NzUiS~C7?5+ij5IY>Y~IB$2nuNRMjf?=f?rmCV~M6R#y_9nJIcr+2-g#&~aLz?@l>+!XCII1P2DEDgpC znNySKN!+VV9C^-YgzTylj`>#6$(F<17qu8%hs)P>rEA57hwQbnBr~sj&z*M+(kHWl z13Y&h_miJv>Em2M=3WXRGSMCjNa7=hzZ#u94rz0#M&gF$f+fMwn#vg zR*H)Wj4DfrSxO7sD36NT%JQg&&x%+gDpVmCkE0ShWwJ?Y&nF8N=%C6mj-*y{mvGrq zNxDG~**=45n;yN)uHwamh2FWAo+d4*ra2B_Bo-e>wv2P;eVtfC`zEoSGAl~-J|>%At;=H?P`s>?S@Tol?1|As z>AZ#4IiM9O)OO_bIJNu}Ws2G{fJ{scsXkks5KV=Pa2zgkDT^9-QPyQqe|jl?J8Cjk zCL=f+#rw!R<7eM+n7~KrUUt*yb(7r@d7Py08XOL1)0=shjVEs8de~`D0UgU1jFGln z$}|g#owC3i6YBWeWW1k&&NGR+Bi%7bf-SN>>3y3CujU|^Bb#pXE4uZYOtan}&2h** zN@SmeUOMDax7}lRaxx-lq|caZ_!+d^5z+7*d}T0O2L8Ti!IuP!zK`|-A8yTd5KHiz z-J2iqrBCn*n%kThmN$HhM%JGCt!h82-rgr;ki?gCWXO{v0tdei&KcAittxhR<)~2o zu&G?n#ojRu;R~KH>3?Lxx4%z_`Y`M=Nxa}=}j#e zrE~NxcH)O-U1njhkrn&^x2=XPX;ZF+x;Ms~Drmrnf>V*zp%qr^;21d{TWaxT=tQjFgE2D6quI58yLcOt4)uSyphS=LGd4EN-ZA!_ z!vpj?*dOEu`EJb}n~R@BFP)$lVpcc(#TvGwU6_%hB#$7~3k5}SLqU{^6bX5_s^FJ$ z{L+&Le=LcnI8-TYzkr0Xc9I4MaZa_FvA{gN2^mTPR^8|`wTEK2@WuMa8{6CIkq-LD z3`_OII^7q61G=!cDJm9s=OyrLH<=8olOI`Y#wTSZdaz^|WEf=6UgXbn_04G$A(v2( zVG4rHp#__~KA7)!VAyzk>U`>a%Zz->52kh~L}B2TwMa*EHA87A^ely=A(ORIiS1%tl204BK zH)0{m>@@+ySOdmFN%i2sYVo;LylMkJU2*S$7`q1x5Q>f+RdHi2hL{=Ve|qS|hbg~s z>&9q-2u}IBFtZ%@Gg88TellA^Omk9Pbt2ngA|N_VHj{_z;|WPBqi=wm4ufuakWn3n zCmr@GwaRvx&kSp!`DgIRtPyw)hD7nIE>Q8mNQ+xc$^Df|g}|wE%%9lkXr_xN)Sn1} z^@_cV?V91vF_chE?pkx>F(C03YZC_B%GNVyr5&eOl>JOg=yV-=GP&6a)Ro|gpNdL= z)ZuL%k8i`hMpS7%VSaYzo_nMcvGPO=sTA`RR=;3WBQW}{in1DBwVG^c@Jw#*#ki5e zcPAvM<{K*u#|(<5o|E4+kO%}i4w|h~L)XTbkcd7~nAy#+%XhJT7qbcnY-hZ%uoevy zP4y)S3o9JgrIQ))9&}2v1Pk$Qv(95kNm#beLn%kF^eZ`Zx6LvP*NnzkH}6Ac?bn~x z!HKahXTAQ?L>LUJvQYGx&E_K{RqLkC85RskR9L6Wdiboj{ok3}HrP5(#Ym{<#=!-j zdF%{HkvFI-jQ1cNS;LQ{CLe&-_uY~yha4fQR1zj;;tZ1I&AuhV@Gq-#T{U~}0e&a989Bwcj;v>V#AO`eXH&2FilP|Ja!+JNVFBkm5?Bltz_ zYwjL!N}Fsy=Yyw|EtIqP0EB^QV&0)gq#t$(BAl{%0@~@{tr=|7;_+$*+aV6FU=$Y2 zicy-8r10~bajcmWz)r;4*IaC%uoNI=Lt;qeO;xK!Xd$e~%g|&X>LzL?h~})AKrGFQ za@!fZcOB0Z_C?S_3Oa~Y_++J&qw;ihuDMV{7LM>f@OhP89Sl1RLyQI;G=tp2 z4k8pc)C`7sF0QpQgd^V4!VO#!yrz%ZS250cWfkc70?ieLe}!);s8Jg*`!62WV-|g%Wl~tW0s*bT2Dihs!?b&$ zu=9xh*6$R&oZiLPG_#fO>;J35*dNxji=Gv0!q{Ci`*Fn&AjMH%r#7@|LIUSWWgH5c zx;^j}9|9i&Ka`PfXEj?@cr9*=g7=#8y1vX}SAE{XWu(2bH;w?@fVY#7kV+gjMij-f z+5Zz+Z5v$-@CUar#rX~w7fQquh?2o0??>;P?8m1UM^h}wY$|gjmA<}c7L`#at!I`g zMQOK|GIKKoDV?~a@p03k{K0t)I%SYZ>)i+_tqLf^xjC#@k7ilqJS|^ne3(Dz368(& zECtE9h!r59ep4tEELG5eh>|Ad2mCKq#Q{TKqqa>tS@!HBZ*Eg9%5P|hB4MDRBKN^g zLg&%v_CRC-|3MCrEDJ!jwU>QB48*ZC@3G;XP32(^&g=omPvAe81@zMe7mgyMQ!WzX zxUpC;Ly_w{Za65=w=KXQ^vevS29Ixo2$Hhl9}@=_@EGSHsX$TruEQtvQwSajxa>FE zP|G;5^-i0KizR)+)8jF`lv5dh%#sHUt;U6Rdj&=PtUfBgL^hxPt(a8St@bg2e$~t}S`Q|;qFyZwJTN>kd>=DXKv*R3H*_%tu!d3cr6u4MO>LfW7Xp-dRoR56>A>7Em`Et)Nwt*ZL%s)UeRsB-eb zoVW#KgKtG}^xk}?e1|ZihnW*Qy5!r_lRLJ%PE1;C)UC%PCG%+YJ#FA`zu9GW+6LyA zB;VU$^Hhiw|6%v|e7es4r|}a5Dm21s?jv;Ujw1qAFcpx#Q(SDwTfnQ7`m2??da`2s zU`7KBfc_vYmcea-H{F7f%qz7a)#GuD;J1ghxElyDAh&EQC^WFRV>e*wCWOTNqB=~4I>O?wnVL99BF zT_5wiqw(WmpP-+hpFE(RR)}IdtASSU+Tj!~9}b7VZBhQgQ{%1j z1>!D)RQ|;w(GtYl`|Vk~r{miDLYu|9QH+}3d^V_Qjeu;K2u+C+0M#<>AG3&GBkRzT z#&cI-_N9Xet$qSwWR@x7PIY59m+gP5lS1FS9?tf``XG37$NUt|Va)4}`gpBy8JFrL zp4aU|k}uK%e&>Csd>`qMDZU zqTY3rGkZ)MTx0yUE}TctjY6=b2uC@xrslW&+1hen@jS}!|$$vW6jRkT9Uhf5=GEFuFjX{)*Ju-b&!b*X5#Bz|cYAD^Md(ZpGN?&M*z)1X+qi1QtkmhaP zYh5mNXjjk}bJbBXZvo_^^UBt*#CNU3c2}av(D<7zM44EfS3_#ZY8GvfnbIJXNxh{r zDL>uqc>Q1}`_Ha8DS7i+QHW*2zI<@|xCei5uUt56+|K@3+cOyqJZt8wub@yPOJ+xL zuCz+T0la40t+eJe1r`ADAyVyF26KfWjY2OrWBf)F?Gd&MV=(6;o&va~jt!jrbldd_ zb2~Qp15W=O#Zy5=ri4sSI2lL~JomJ7nA~YxWQTcd<1n)NrKeti*rg){Mm*%=ENUi- zm970Xn54>KcB264IP##<{r3B{E1o$nu6}GmxpGB<=Z?@Su(cMG6=nRM7#GuSKR&y}>F$str(g;F{i@-W ze@(a5ABioYCl%ZTo5qdxTckbhK<_=#Q)IwBL2&#I(s!c46`gz2IHx_b@OV!`TBVsS#{N%%6z?JqTW+%okjI@tZs?X2gc;YMm@$Gs4Vp1S0&5jR); z9jg3Bj!ZF(yoDhNThds}^wMG@VE7n!$&xGLQ~{LyaZhZu;}#`Li3@x`)e94RfOQ2uhchDl|al}Q?X>B>U=9T z66chF&8Pu1kf5D0#*)@+9n;IyDT}MnAuDpj_jXE%Up>WipHZLoqWm!G4C^))0%Fcb zQ0&zbFEh_xWG+NhNAh{Ap+%7}v23Dj`1XDnxY|}+k7Cii^xBsb z`ASB@;1aV*t)k%P3~E|yd6@$nVZR72f5OvJD@6dNor`Njk|k|-!owY)H`wGKHuV)m z=*It8>^wbkwXIgqE#DGagvCGepfcDf8%$?jzq$^^?Y+8Aj%xV?{g-%TCp}!#moh|| z33-f&8e|Fm<7LrZCixXV?Es_lMq&D|Y6)s7!&D*KYZ!N1zbHy>F51nU!d7*5qEo79 zoh2g$SMI+K#EuW;Ivj*7YbIwX?xPb#pCVV|!H)FH{7icN{CsbQVzWe`Jh;n9G{z;B z%wg*CfdPLKTrQTu9#;nAa%-1P31~NDP-MOQHF590i&u9CiA@sI!;go#5lM^`88WLt(aCOWa=#G;P@E| z!6Vb1x=0{3QK?xpWYruT=mO~Khhu<16R@e;hpo2QojlJm=5NxzY__}h$XwEUE+*#wE8olM^~I!-ga zMWhaqawoRf`b}5Wa7w`Nu;8PR`F?q@&B82(J~WH_otluMM7RRau+KVem#l)pW2f2_ zr|p(A=vcF}L}-wXiInpFvl^cCi-TYHZIQ_ z!GcR-HreMXcaihsP6rS6s|z9l@WV>m9@TUbABO9s{L+1EXkYN zoW5c-4Q__jwV)Z2wXbzbJ&8m0{?V=5{(<%TH&Y9-&JA$O_h_at(Bd=jj`2#sr$Rpe%Jl? z(eVCeG;?cxQ)5S30dps7eLKhh$01FuZ1tUt|6kvrqm#aqv*Uk1)W4e;J6k(vyWeyA zUyc&{Z{DJvgRQB9vE#pu_;*=F(!Z(HH#9W1GIr2+vUQ*}bars~pR@u9;P<-t{i*+M zWc?fe9E3pu007d$O8nH~G9t9r`Zne!#*TkP9x4@AZTj)QcwedtbwD)nNSB!7QJllH z!RZZ%$MxV_l^tBB2%Q+GF;r@7dewMO^!^yX!1i(ukfJdl)Y*?H0a#n%JVZUQbu8K% zG;3Q2@Gk<@c7e_>g*pXCkzqJ9?TO_>@ugKpL)Anzf$}YEp59kD3A=(X7_^t!cx2wn z#k{+>D7y~6a$xYV2vEnZMT0a<%yAK!1&NPvTDnmwl3MLsscpO_db1oKQ9<2*kLj-s zl4Y7m*pVbddVt~4qT?z@b6iz_4I&!zJ3v8Ug$*1uq*@Sh>q z|6Td-ftWuQ&HoQGH2~nxkktRK`u8)n-(i`*@Ll6SqBH-y@Zas~_lewJAh!GW)4D(F z{?3+u+sj{2cmHqJ_MacoUsZrVzQcbZ=5JK~-Rk~X=Rc2T_#brsXiJA&c9pdKkNK9&%dzscRK%J zrTk07g~_{QOLErgrAe79?yW z%p}T85|%d3rjAS!HipioVy4Cb6H`G!1Se-lQ$t$>_smTlX`oIUQuj(tsgwVOg}%Q8 zWOE0hfn5U5DCuZfyfm`~+^7!tQu0lTp=OKf(9GDMs6HZ*CO%(E#`M0jrbqw7>-P%v zy-oGl@uBnm8un9*pWeqy=&2v$i_GWyLrr8pQ?)*mSR!PPJ}&H~7hy2O$HR7t{>QJ? zqaLBpvdR3MrX?PIKPKWgRoM4?BUSfi^{gqjXd6uV$yBB4(c%VQS0uGR4|;X6Eb2qb z&vb93m}w>^y^oR7?S_A-(^$)ADYeZod;=^GyoQ)hy#`27ZW}WBS$AHI1R6hjbbPlo zxoS$QJB&kh$cV?@T>@Mf)!bNCMwqY*tY3m8T zN)KlGi5)yIjkzBWk4!?kTis^}N@^CrJ!8aS@pFPmF4fU<t`G zb(Xq&u^xO^ROl@9j@47;b;XEpPUY!f|MW4Z)8C>kG816Wjtf^$rN>F8g@tARU#BaL_h93*G z-A4}vI1*hC29VEG_E`96__4E{GR=8t_#v{LGHtO{3Y9s<6pq}dBp4gL!(Z2358@&v z`hfWqdHAT=V`=eRiMR>^;`0%g$JiQnoKcZW?t)=6S%Y8+>LZ=9nJs$D*z+JumU1CO zoo+Hmkn!w2jW~E?@Mhb_JbH~6OCgOjG9~#HjA9^xaB_#mB{K#6Yop9LxlqIsAekXt zm7xto`bt&Ex;CR@=D%v?QBO=7^Py;6%9@!h-)}=pB#ICmW!RF)4c^xb1pcl0TO8%!prP%PkIxGs?%TMon zo6ZW)%1`fCTFm+b<>&X$tY`h*^7G>jm5e)6abS;9#@5zc@9s*`nEsMU^M*Bde8+?SQ*&UiG3Jv1o&c^7diXj+g+0YfG{wdX_UIUwu zf@GX12M6zAl?g-xFa-#$JZwMTh{%(7zpel_N^nkiu3o9BmrTHV9yi4mCOL419A81B5B;H}{Ikbt9kz*A&r z;|(L1Or3*}S)NWXXOCs?6mgHYbtFn541%2>)KbP<$@!gddv%4+t&?!`6h^q5W1MjF zM00ujYtEtD_XqEMuhRuvVsdK$xqM|0g-})jCc)up2__vPzpv4I;yBVNlais)y95$n zRbeIu0rH3TH{^#LEk~SidBSOBXB@HShTx1+#B|#4d`!GyKM)BHDdChrhXEtmDFkAs z)^RXrod+c4B+`P$hm>$x8CHw_Y|8c!jDCD)5Z1lBg*&y}z@c$&H$oXirb8|pl(0yo z1%>B*$0jpaRl8s(qQs%B5k>ph#Vpa5sQt=806)UqeiU3)@btbJyMMCyp_2T3!?M&w zi6g;H**>h?;jtQ@^&NY5Vn(4kQqeRU$J&KE4a; zeH3y&o(t-H6!Mc}#dRw#VXpLFkGm-3*$7+yRvGR?_^E{Ok?A9dD$sO+8m|T>VZdKp zO4!TqgsmgJT`MCA>(v@op@rP|&3IQQ)f2pT`0;z$-Qwbj1lz7GSmY523S2IHc-GWN z@SV|LS3JBtjjbng$dPi0m-~y86=&ld)%;1KDbW3EW*jScVbP?N0c?>5($`xzS@_#Y zl)YIp#Y`b>$|}Kjh|ukknoe#LB_TXOfHIE3%bTD9d%l5``5&SA=HxiYZKf@%fbE)Y zsgYVYX>LfvL9ZYIi-b`KMyZfGh)wL9g%h##=4?<4e~!f5;x2B%n}z3JfpO&fmXW5K z6_-?dZxqaZR4T;Ka76Sk>f!8KSv8_j;uPrF_Ps#q>-;)Wgni*rR1cm;N#@#GVX1df@S>>NX}lRFp2qX!(d=EHBO6* zZQsDbT*qNBhuca=kY41`{TM=t3`3R^1^?8oG16A_>dD7xXi1AHX)`j9(2}WFqaqZ3 zd`((hgV=>Od_46h97|9}g!wCY_bV0NnQTbYnXG}{SofC@E|z}%X)4?nVYkXCL*(Ib z+N)%-C!FE1$B40GHI^Vkc)L;R0;q1LWd|Etz3?P~wK`+jx=MK0@~gO{s7F3P%*k)z z9=us2-kvt<&|PF#)7%XIp=tut`50wW+a>$V)E^58UUTJz&|28njYoi5>+q}VM45qknH(I{xN#oAOxj%K*N-b^Q?MFx5EcVYZ1AS4&@#M2 zU&t*j0ihSx0N}nx#i@V^<^?ZWfST9n92O&^4gD5+y0-h;Op&QM5ZcImWNDhyc}v+8 z!QIb1v~_Fs1srX&9pB$pkKzJG8j4R6Q0LTxqQM7j%r-Jiz)p3NS~3|G;GNm-UymKu zheBVP;)RU0JUE#X_xF}2fSRmU_2yAyac3&$=lPY zc%CX0p+qeno-EdqqH8r2KfwFY18}Oc)h*+2P7Fo$yHBN!2Qg#P^t`>az63m3+-EM> zGIe)f`D8M-9CK5&n1#kMBE4)2a!&3#FuNVoRbIQ77(0Z_HNTU41QR>HESpEroVmbtNEC4yIU&UoYrDN7 z#gS)SP&tLRZ&^i9Y)A}|amT$@f1f#M5bafk3nla52&Id!Gns|xjM@)uze+ROI{Q<8T z={i&ED4Dyh$Az~eQ+fKb`v@!Nlr6F%jx*8V??gL8QNarP)M9T&2fCiNs%LN%%~_7+ z9^bi_rOByxeP`G2ZNq08BcoFUA@rv3h+EyF)WO5IfNF_x04iinqUIN?mU7qU51POm zGP*}T1~nNdhb_zYuj4|mOBc$&+1s6`2F0Y>YyTH!BK#;~Cs+rIC#;H)wXAPZfo zM`@f#^uj>%^?T30oI3##jW3n+)E65H0i4(#!MOcl0#Im$c$_}dFB6jRapedxI$jy^ zWM{KTg_>hQo4MV?5ZU$>2HsP({XY!6lIMRYv;O7}6zo#!ZsdP6r^qR3q4b7TZ!e?pFBy@| z$$N|^v;Q)Xp#CpOj+H-rgByF`-UgiEDB75q934b=&pU)Vb{Vwt#f7Yy|- z8*rjrK`G4NJd}njUq2tdD0i-ClK8ntJEx@GwCMzB`8_hi8LQ+}JXsFYUth5(@-ZYP zUM_!xoidojpiRCYpF6BFE++=sG5dw@o%|)2^nwskZ1q`^UFWp&ulQL`0g!5;3KC~;WR+w= z9t|{wZzp1j9rUedsaGQ)N2nG78l%AuIy71X7dPrnSps$8jO><;E_ja=atp34qhR?r zeRtgR6ViacT*rx9O@^r7=Hgmh%f+$dfblcTV7s_#1k&cGCMG{7$XpcflX<*(ff4`J z9U-^F5W*NuX8~${lUc!KKtmtN_%{b)t`SbbkDn0_-{&m&1LH(SgzQm+wS@z};0&wh zxl8#!id=vjUA{*GOa$%5kw+UBd>`9+od3#ao%5atFmh!ke_H-=NqrakdlSixFLfK? zWhNr5_^Ai1@eh>#s`~bULqbIKN11od+!Pfyf+yEuIKns2d6@kjnrg?Grh(_{@{I{H zv;3qtJrZ#As%s*$qEO`N+6ga=`kl9zP47jw!uS!(LQ=k?x~-x+dW0VqNcFD9&9O}& z@7Mi-V%M&xiePGI^52eush!C`lL6tMxA5Y06j7wV`dk zs^59;@KnvKe`5^spkQL|kY+SNt3NS8hvkc1o7@JTWX5ZCKiIkny4!E_G?%MV{yt6o zd%Gszll{RlwUF{mzkt5sk!BSB{qv?}A*Vaz!^1J*;}vvSqS=#bM@^{eUxUG7Q!^C~ z<@*X;P~85__Wl^Uy`s6X=Ak}*XZ^ktdUAKTuy^WBn#4xX;lvt0!K0cw)7_^*t{WXz z<5v^SjYLpfE$6J!=jq6z>NK=6He>>=qvQOg5>L&|+Bo(2Z(4}Ka@b@iRE_al`USC~ zkf8j6qd_|KT0ITRR1)FOHr$xWoAxU>6{S9fEtYDG0{QFDCja4|bK& zGlyzZNLN82e|6dl39t5<9M;@?9Ik% zcGC1r7y#N=&qnz%Ul3BZI$Jx=WLayL)gExOwDdz3VCUM?CB=~Bu8wt8MA!1HX&p^Y zu-@aDc!c4TaicQ=(IAmstg}$@?Xo~CbMO=w%dD?>b1BK)=co5v`&>X?wM?p}5LsmM zGJnDJa9pTWB&pajsRd;r(L*RIl)@@+YyxAXe1w0p{|S*K5gkCFwZGD@?RqHnOJ78A zOrdWJ-fiSQ6i$Zqc;iFc&h;C=#WGB}j$n|Bo z_=u1o#Jk`Hp-G}jFA-PIN?(tE*0JP8NJT*?3$&l|+O`=|cJLvQvQc zY1`@T=MC#US!;`H~y7*(OK+gL-n_=WCGOe`W=TLcZhup#*+k^ zT?5Ezv{$)G7l?{VH_9g{tNa8H@&Y^mUkU~m%zMJ5ePZ%X%tG>Ln7UBaq8m*2!}aS| z)=eU%EpguYTob{|#rhGgHx0TG{gfwDN8hw`YkX>SPDZv*M~?l$^tg(|6mU;zdyOLH zEJJz00oXv*78c2+;Xle<>`kQm(q~rIjD+SoUD0F3912i(@XLF7ietYP@K~*oSU7^4 zkpMCSYt>nn1hwMMU<`LcNB1gr?P`3lD)y)+*O7aEDIa={{zM2g#5 z5W8f0z*e)WD0UiD{!LueOp+65F#zCXuLR_!v4pr=%y1v$x_3>Os|(_|On4b|tXtOI zOf8RQ17aq^7@|ue4BWtuw18W#B!bHr8>Lcg;q8{ND8t)Lt$_!|Mz-x18h0Ok;|C%< z#hEQ$NO}s*buv~e8Lxq>PGtH!hWh_`{^32=eJ9+5XC%(!dCa-3($zlnoxYvY)apGU zl;JGQWjSO@u<7aG{iY`K*J2(>FzouWi5^{RJ&7DjF)T(?H`=z_ACca~ygteu-r-MA zNv(8zk1pc0l=FbI^WhsI8}X}jm$^fgZ5rM`edG&2Q60i)8xIL*Ma7m#PVaN3d- zI~SHoH9R~coBx{X!L(KDHJYQ6#aG>CA*vY;_E9&sg0!Af&jDk*3s7!^j9TI6sC+Vl zCYRgEKW8t-ghV0waX!Jx4EpZ16KMhx4tK_-5xx@Y&FJfkYB_A=07_=E=@f zi$9Q~b(B26g{zt5m5BhIq6d9z+0d14(W1}ALPhZFh#q}+$vmmd?R!knqD8Y%4jKZN zykad@Zu0c3)%h1sjWt?B)&Try09lcIi<)Ti2>V^x4~4rqPFCa2^u2d}-5bdO6~yxN zhb=*gAQmb%6;uyaG)JBKvI@w70Q_E;C=a)xw4bt73A!1)Y-F9bSI-eE!A%XW1LOMz35M0M(N{-C0$BKL@$glAzp=^{GPnt@r<-eoyR;v zA1`-8z`Li!?1=(n-wPBx=cLb@9L3LviV;5{5Zvb_)8~ESQ_#o?H5Q)o$IWcLK~JSb zYi`cR@iDNc-QcZf`}5%?`7M(ac>Q#GAG)m@{qbC8=3NX&=^au5O5epbuL3*1%feS< zskiBL`DAgNJ->u#+U=kG$*;SQ3~a6dt(5#7c^~_Od;C!u^R|JdH=)}tI{p%^f02GC zKHC7CuTIwAwl}=^e7R#HwpZhfysaiwYAD$_tpQl!7nOJWmJ|E=mV>PJjZ|t}{XT#r zLhs<0@5k4-#Gh9@pcKVBo}?7TXQx2yr_3vu=Nk1Sy^r_9ZGrc@TevJ7(7Xy@=KiFA zCyxvyOMdR)gEob(E90@`_*+ZM>>j?nUaN?Kwo4H7R}C~lzM?tFp!oOg%v?HaGduk!H_4v;0*3Q}fFa%z+1k!{oUd<(5e8#<8}@Zm5B<6m9bwmrS5^IA{m{U{xor zhfE4_rcBgP&iw@aj?o^!Id^}bFunNxb%4S)AgT?xE>oGngGhqZcVKPd_&jf9s_U5c znySE{%evyGlEUJ?)cQ92iJ(aPd+zba?HBGwx3SrimEVl@8kwM5HUYP5Y6hrul?sb`uvUPbh&J#r75^vCTd?bPBt^^0(wOE9dsScXyFp_X?uRv)=73 zSUY}(3pn+duEe-)UA*bd_T=rf^P>Lb9ujd+TwlvTwLk&@Z->0`u+(# zcd?epR584t0k<{3W|~n0H^i{rF~Rxh*)T<@+uSx_ww^H496i;{kz>0)zf+CWmNeB& z>1I3H1ZZA~(9!kyYg4iQn9ik+cOjE{d@<2P)vg22wPL;L+@}0jr|wm!#^n|qaltLT zs>AZC!@^)`A*f#Zm>yfhu>e8<>0z)^oGxrl49Un<0JopVaDGikNnhj-DEG|B$zl|dgUYLaLmrh2* zr(Jcxm*SudkBx&bMwE37sY6+Y+F@R*1nb{|2Qr<~M)rSu)yfb1DH_e?-Q8O3n`C<# z3pR?qul$&v{3}Xt!imEa8dw#fLf2UzRU9#0S?SQEV^3TcCU$3axJIi3*^@|_Dx(g>ekV*}^y3jW!8Y|7b?mwfn zyFp7`LEdyBE(1$l82Nb1Rd5^IJU6K~Y&ITOn#TQyb!bzE(PcGb9;A*p9tEx4Tu|>G!D)z>8*d)L_42i-l&`07 z$wsNzH!CRgOUz80`a-DjhRBq?Z;r@Zx7JnnX`*bChF?;RStrq;Cee`olQQO-q=iXU z*}8>?hnft3@cwB930{2s5y%0Tx>CP_R*7Sv#})Y(y3;hsj@0CFQk0#!6CcLC#mc<} zZD%uKcT?8ijq8S^Hs;RE{Si$=Qi8J+?&*{onY}Y~#_(<`UHN83QO0zlU?v^ic9~+i zIT9y>BV7EP)N!qnwxb}(k)zTSxFnkBJduKKz@R9wt<+g>k zqsL{5Ur8;teoD35)kut*iUj!LhlLiu8$G2j^tEGGeoS8|EW%I$k~k;~(Fx%RM{Iz( z29h}F2M(BH$MjcKhzF88?qa;*7L*<=2lJ#h5;I_UA{Xn5wG^-=uw(j*OAq$mKSgBI z9hV|BoX7}=DF_zU9cABqmQzPDOuNAaznXYr#pErj9ccxl{|VOxznVJun$5~pom1em z3k2;eF=aae_ZFoUm7^rvJ5xIJgxmDxElMlh5UDyxi4=bs^@{DsY%+>F?)P@XXh^04 zBbG`u=*qdXOjShhplg;BPIZ*8tb!7o;1hU5@2K{vCFp!S;sQIT`(S24i7H6q0=0eP zMZ7B(D>gw1TS(#p&VA!Wtp5tnA&CnfY*HWnfl7S1V1XUu#Vw>;v@4bqnvmzOPdF~B z1KwXcqj-nSwv>A(EXLe%8DKi2lA}`Q{)KDqxD0TeQOVKX_Z}@aol#U?_eA+POa-z7o*;=In@1xJc1CbF(WLctQnv9tifP z<+|mPzteo$Si-RkWYfMH87hARTCw@wdt;6IEm`T&G${C#|86<*ghW z*ht5nxGhghM$209XI#=A4op)1J|MAKQWz|41SX}Z zHoWb6+Y1yiYkuH7nW8fI@e_A5(cTg|3vTHvuicb5eeH|aD$uuP=+9{k?xpkyssG41*m+ zTu*fhn@vO)JsXa4!kLf%SrOoX?4qT-d6Zk|#Jv<@;iY*d*lI2;UlI`von(Vbt;^Pgv3k|Sd6Ep)X(1YuZU=E5->8~}(D7K_P|L-6_e*#hDP(D3K?EEX;#yIq1*+JsVJHIZXeC&K;S zK8Q6@_zC2^v9Zng37%qbV|Vn1V$ks5CnBfF!4-(h@;iJL$!`|95uzO|sIw54sK0}x zA(0llM}*Hgasz&}Xmi4n;T^462?qw~%AKk~GXx4x=m!S+OlDjE?hn)ru57~>bz_?G zDhug(S`?(x{J=cI{XL(DQg;Iac0Lp8DlN zLzh@BF}N}pgEoT*6rQNI@!m!dUkr#$q?N{U@50708?q z*C1Ln!aEx^0xN<1pykPQ(P%PWi;!pyA3-urq(!1Rf1QqXoG1%nJ7+t&2n+v^C4Zf; zTNW(H0FKZCmMB;nm}P>PXJ))!JPj9~`auf_XdPzl4`!$e#(-n&U%^7TAf}Goe}7&i z`IKVzoYI?0Yw(;xu&X~WF?!J$Il4SSYD}EN{2vfAk9qPWLOHQ%5{E|lc=u}%!$}T~ z?3VJeriK9r;(P`S$OMr`Q0$QGNAr4??GugIQp12jgjL9%F-N)76+u|n4&3DnN4esi z1TlmhycTFoq2#L3$X0_BeOguju&c3d#f-)TH)i2ariUzvRM02ZNf0Byb&9kXW&u%^ z5+sF&lzZJ&-i$vbEN8$C2Jw^N z+KE|P2+nD6rca0d^X!$!yQTk15lsj#L6b{)aAH@REJG+NEMArtmj3Ul|fRbwFL%_Uz4R7p2(2WP0VeghVb)F>?L{>fB7mj}7_GX$q z%9ZdY==L&m7Dgiu1+?N4{u&(F^>UVg!@PFzuU~@vcNfjV*&mHK^pX&d&|q+4cRgAH z4)xmc3ArfE0rlDuqA~^gU&vW1e|*!GVyrUmZ}LQ2U-K!ij5|fm#$5n>yB*MlqAo(b z3Zq_=-q%af1Xq-G(BsBZI9@wB>M&7QW!pU6xDRTC(@0!S>mq!eNm)MF`cF)X$4A0VXBTkPBT z+3AT*6tjJnZkJAeD0}wLG|@M)F6`NN`XxCg-(2sty3>orx03|jAsn=GpJpAfORF5j zX2NHy9N5b~YXYnyt7)XQZ2+h-9&!A#&l_~hEc&^#g97}YQm*W6lTW-0Af4@i=_J=S zJVo~s^VnvC;#FFmZ0rko!jR+nz>V%#&lFI7cj68sD9tXkSMgMM_G02QW#n9DAnU~c zU+fsqdp|*WWTin&&b?n)5|u8oo4hkHko9*=(I|8g;(p&f7xNH0Rr)E!}RK6_sTEc5fxd zUi#;=+a#}Dm2Qq4K!qDRW@3jxws+e?Ztez=BuxHaT2J2Wzg0zQ)agllZERDfN7UVg_AnSKK#0*%yMWeAdU(V5hgp z(Zss8WT&X;@!4!j5W!FZtLaR9Jpb{=V|wiS7rRt9D6UCgD6UDq0DLN;0Q?f%*0pgT z4MksS`wo5`bo>d(i87Lo1a_1xFv_1w!l z+u8R!+gaBaP7B``P76LCe0Cupe0s&ei6zBtShf)9i9`G^KY}s<_X{l|QyEOp$Ls!o z!Lq(DoFtt^?{~Jsd_MTOIo;71>v!5)ird3mmTjQ-J6i}oAN*b(RxQ)!#+>eGfsPD@ zr{nci7%ORn|34`tI5fQtTj+>D!keNK1udv1oc-WH^n$q8YvoEPXBHyz5k zXh#_UGl&lI<3L&g4HaG#?#^T8I?w_%u}B=` z-nNDu1OJxLEi?B(F^zS9s?VW!Wj}h}w(r~}GsiKG`9t=~0&QC3??t(vKg!rcQOkWP z%xv@`+0MpzsN)2butS2nEC)I&VADBFrz~z@jH~m^2qZCrsJs~1Cqk1cUNno?ziVVS z-3nsiMrHnT$B}cJNJmD5!fDlB?&?JAZR{|WhnEnPP^RC&5fE}@v2e*fRAnsICOooC z4UH4~h7XNKXpy-dZHY!`!N1vmM4KNvSK5Uighps@vlD%UhGg+&um5PRBJ|jRBiFo> zu~-Y~NV7gPZn26>Vy03zN_;U4)&#Vk>hO5$P1eZa?>T>IG;0BoFatxWbQwKc3WJg$ z-^0$?FCM!K#0%Or3IPeJd%_jaP6MKZ%zoK@wG#tgg*Kw$;qR06-mKsdU@uH+1>qg< z8?rb18qN}@m7=|InWMe}1U)*`ep$qPLmI}s+w(M8lCv?wf$v{Zw`i#s+9 zne!j^Y5&@IE59$RE5~(;WJypFw41eK$BZ)z5pLVV!jKCoqdikN*F!{xT>v>xV~nj* z1zVhTVHmRHFCp)h3Z(}92g({b$qbdeb`O=J0+J?xBL$)gHWzkTC(w+;e@xP#`3;Ti zEefGjaK|grS9!sAt;h`B8P07mENx_XV!b-K=eBoXs7esbB-TwuA;$Ex*ijwc`&yK1={{u-y5Zbkag>wP4!Y}Y? zZn2T`FO|WOvNAlvMa*kQD?AA}4$02U^uD89a9D~;!@Z+bj609FqUlVl7MY#`0~Xq~ z zcqSFlVeUNM*xN@CWO~Tk&s@Cz=bG#r&HN=IDzJzAFs2t%)<$gi-0n#GnJ}8;G^U#s zLSQ4|_a*agqQ%1+lXT&%s;dpNcy>W`?xwlYc6>6u<1)1Fv1-XrM|o~pEI zFz-6;t@EGT^ge#wT{*OkmWzWB1IspD#h1wIH(PWxL^c(zB#g3yg9?jV0_g?|b{>4X zbaBz?xK^ONtNBymN$QH|8?@l2vg=DfLHlimNVzf@4;$Y@xfu@t!}qu1+S6Ex$MW5m zU$-$=bS~38vN!xC+M3HG)-fE3Y=eSMIY(s~DaJ_PiD0$p3;rKEjKz}$mP^xdF0xkU zfd!%6y>mm8>eN+Q6o>eeD{NI-eA!k^WrmKZ$H&Q$X-sKa91%wb-J@1Zo?>vRmBm_Weesijhh58>t8lo{fDGn z)(pLV-Gz2bWNF*ExQq<(k4WIi2eT^4o9;gEwC!^7e>B`FhZ$b;rriBcSQ{p_%!R{w znvreofrDA|eyYpY^Ve2-trFl}4I;Oh<`P(OWVxkGslXb04p{?x4-ot}(Z`-chQQwc zJ4M3|yhUqywo%D=w(YxfU6#l+x9z*Xxc(O{{zKG^B}no5?p<%gi*gY$IDn(U$#XKW zj)(c)dtyj614d=0#XD*$u;oUa9-H}?bqNXZM)zBP+GhzYFSkiOho3I5_j}~fW5frZLS(Wn=Be`{R1f4~p6KRcuth1*L{VV5 zB?Qt+b5rwB4+rb2ZP{u)>Y~{8wRioXgI@G#xaa;nXf!e&f6qE3Y^i4{QiPR^%Ivm5 zl;6x`37P7IJ0*=@^51g2XLdAdVsU@G?Yy*NkA?PB7`Rb9zc_TjULzst7y3S-pI;y# zmz07@FPEgtM=ToRNB994tKnF)u_A{2Xv*+)LAa`M%GWq9IAC_6-||LDxIl%0*ZRs~zBrGWe5JK>8rJrW$cDt9!r&ZCVLu z4-3Lp!P=W+a~sw#K}7XhY3FGYj?xALMJk|2(Fld>d1F<+W0N;;4I7BT!Birf`9i@Z zgf0bSN<61pxsa6T2^Oz@m%P07N;P4RwGx94pN!B3RnCr_nXjpjaUNUyOK{OKBKd{P z3lHQ-z)y1FX-^PX?$>ZFX6!0vL7gJG)V_HkS8^>TfVQCote0+5FkFeDVdKc&PHW#N zwR%TlLHeZqJy;VxRK$6Ly+5Ub{{TtZX4o-8fn#P>q~X{OEcKyHESPf;`I^>@Kn8y) zHRxepK?9RRfH8AE_NLa_bTlasC68RI_&i_J5TI4Rubq!56tXEGKB`g(uT`Ajd(YDkq4ew@6C}j&|>m zB+XZ8a7N_kL=KrRvU2?!%ibXg=L9jdF>K$>NLWr#Dkl_~9`etoH_|;2i3SrK=^n_S zP7gV`ca(m4q7VJru~vf#&eVKm$mA51dq(p5pZ5d*eTD%-CB)bZx5e2$#_$VwDw&r+ zY9;VrvF)uDM2-prLJz!fXZ8%}+vaH#!+8#l1laiy9H9}zdDij%gI3GGBPbl6CShY( z3N(W@;bK?{bdxsWC~Xd~&Py=&^_g+lJM!Ntv)&gOhwnKUvSBaqKMQc^UzdP_)0mBH zFt`B8-a{?HPY7{(3?cSk@V-3moWi8dffg#+p27@g8Z9H;b$YqZBu3})`m@3ES#G*< zX#dDH4tLvR-O1h@T73IPb{t}`V%Nq56cTv9R&V&T{rRw2!|0e8TzFZ@;0Q4;-Fw@j zUxUlBnyeyn;s4iB-%N+Xb;{6`l3aIZolsp{bHArtEXATqMi4b|b?^5y`cMf6PI`^a zGViNnMCcarU&a(Y(7W)W6)+@Lnr0m^^-&O(^>&WlWtWFtJHyEdMfc_}(+F+ikuq ztmtJf|H5`XKeJY!sK0&1W9d_;-RX87yeGK0c3A_ky8P688JxOl9&uBT|A>D!4@yZE zEYY7pq3Y4~yRDFpd^XhfdVd+To7E?NM3e(6Iv%~mVk|2jeo5mkoRISLP|L} ze@V^n^!`ZU;b}^ChSYC*NVV_?c(73)uZiFBTk4j&QqSQ)x;DD|VVU|?ImH~Mnb0lQ zc<0be2U_9MiNSDU)y{M~=Vmegv}pJ0c<38UyaN-2#B6_P-51%z9w`nAsdupbbkg+| z^>njJe?V4IkwZ4O$~WPb7<+thS+XB_!|4zaOlR^C=W*0Krm7ibu)e&&IlD(=TMKt9?s|;@rMnu;t`=@QlQ%_cD@mW=JsNdClb>m7 za`KLTy-iJ&oH7YPhRrIj1W`1Imjh#14S(si)3ahDGeNo=!>$TmeY2YoZOOCY5`^x0 zdovf;DR^s}_%vM&N8#KyQ;g4ZS_^)`o#qO&COT`$p}m;MP&0|Lm_A!8uevUw;2CJz ztN~c{I5)l-dwx#)^V~e^eN0|u!N}F;Idp=!jHz#NXuP<0IGpc*gMM!onDQD~L1xIM zZ}GRVj$SJkdn~3Q3v)2D>go)KfxvGmKey>kf7{X6vc#vqm^UnQV9m%(Z-2~(NPNk6 zr@KTkBI@)&G2|rX;D4(iz(s_gkbKSq)z8b-R+cHBQ?r%6$>X#7Q|6s3#B~X!opaT0 zbFP0D1wCGFU%G`{gvxEp5@h<>P4E^JbJ{=(<(iK>;tx=sm&7j`in6(&JY&!))_o zxF5#}JoW6Bk>o+;hn;pr61J+ICVjiNpj(H2myR|Kd4k)LKAxo=HoT?*l)`%uS@ z@Nq7~xbFq5QJZcog5bk3mLa{wu}I(ya>eYq{v{kUl}y4mR0#Z8RY$xsD7hBDOnDf7 z9peD#!rZFbUvrYp{ph~CDf62i^*Tri*Ic8a7t+Zl8f}yQ(D6O+b6Nm8w$V?&1*wV2 zjZj}8`=f5XE6x6$DL9vx;OVdZ9JX8->W3ZHv@(^38X@x=4E;bjh< z4oWkGJ@A1Br*XBBaQ}twWbv{NrJ;X=r6Ns)E@DmK?gIT<1#V1$icdjduFSd z_A2vQ)w7YKt!ej3xuWmq_F2_;|0!@d&AemnW!us7c{s!EG$ZhsdHL$n&Bn>e-WCU0 zCH#fwhgH_SP?b})_}8vT)Mhn$%fyCI)h^h5p?>YMN`2K1XUq0)q)?9*Gx_P@@#3Hg zFv;L{=&zshN$bZan#1NKiFcIz#40ct4T-!+Kgs8Jd@X+4?$^_=iwfB5!#?Kr`?j* zUpu%fO)fzNl#!P2AQdon2IJg)P7_ay=^oK^>WO#EVRd3${4jh$PF<+%%o4Fa8 z)!3RK-kTnNtK2>`~uRIj0klcg#`y!`vZp zFBcG&!dTQ9qF8~nTG3N^$OuF;c0mD$BUfr~M86*S_5LX2hQiOti)&xQ@0`-S;A@GG zx2n}nAXcLv2|?Ay^nFZ?PLS_@NsjH04(7=vvS%>KQ8G!i5A2#QQ0vtRaShYWm&ZWS zC>?a&TX6ukLfT4R#u3Nyjl#Ke5YJrhmE_--f05$RLaF6i5HO;&gWWo=`1A8H`3B_9 z^%BLjLjPqI@GA!55VHCUcez;TPM0b>wBr7?RXs9Ikz&Ct%M`od_Xp@NQ@XtzKZ)YQ zu?9+E&P8A6XyyZ!p(%!IxYeHU&o=t~zVg?MJUaP_j*OZl2LE_=BC$3RErrfIWY6!# zKrS#@+Zdz7Y~>~7nn~JD$L9O-T(7-W8ONvOP*SIP(e7O4&PYr;d5THKCmq)^wfik!L=FlEAWBbzp^$5U8J_K6a?*%L7LL`A(bs=VfsMGL;O^Biy=>R1 zjWQRY;Y`CiNFilt*lC(CAHja51m4QN+Zqr^o~4&nQ(H!&ap{NYwVukaVKZSyK*%d;baUBzJaHKq_!WB%IB72ljD=4mIU@0w|ZyJ z{DznJPN-#fuWD7ht$nJcGK19eFL1E1=a=pyucO>Trwd)RC2KY*1db61&}VPtEI2OI zB`_&=xUH4BU1{UoSD$E?@GowCpt{i<1Q65xZf!C)+~(aKeC_G^^sqDAZfpKsp|()^ zm38+LChbTmVt}#vR)?=$dwu%zCwi@dC>a_cmw;Hv8%N{(xNX*0Fr`^&_^S@g$X}7^ zlULjMHSB4^ju9h^ZXDA8>bTsUc+==#ft61F&1hBwh76N z-!Y$C<0V0Er~A+URoq)gRkd}0;{p;MBt#mKLpP@nDc#+j!l9(31Vlg(0qGJ!NLSG-e|OsUA*j5o7an$r5Pkrclk!(bzGyh9Y|kTMA2-|KmD^mAThD0@al#Qk|?z zZv@Z@jylxa?@>BuPdq9pF~)ZyRiR(D=J8D)1<;K;L|&5P;b@`HAFH+uM|cc=^JHrA zR-7{yf_PX9S(#S7OvsrgRzPUw=rmf1=Re-!(eG}<{QX2g%Xz`NL`Oe#=g}1Hy3Nq% zMqOXiMkL4XqZ=en(L%A~5Boin`4%n6WdNDfK8+Vi`XFoTy(Fx|Qd4+|Wf>)Y;K(5E zymJ)39)nCAJKBVf?76b!)qJuuIi|kw#%tV|aF54JrpST& zR({TjW-B%c##ZFq^7Jp>%ap=(qggeax~fSPzLojv<>;_JYw+LIR`Pv$Kd7yPL4{wR zn^Fn*t^B~*ZtM9Bk?pr_J5M32ws0b<1Jc5~hn6JH35AH{6o4`4Vo2 z_f<_6Q*t6yb`vC2j=(?%gvMBno5(+YB3h!%l}}djfR5z(<#{?FKmyqw$Kgx zQ$mQI54kJf?@@E}bG%vZIbmPe)iY87)T_=uQoZoHQl3f(aP_)uq!C=7!h~j)O5fLc z_XQH>u+XP)k>QGa%{$pv8Q|OT?nP4wOOH8z*6ADc>IQj+En#JG`TGGHRLcHQg68=% z!`_eS7&Ma&+#j&6lu)F{TCkIOUq4r8UP<1#p?F5Yditz?smnPX>;rLUYlo*m9{ zqry^Zp85(FihnpVsz^!mGB{lrhYT|?$g?Vgem=_ZcM~LQOzi5O$>VkGMlvK`@g#a% zh`!9PKpYdbBE%|Vk|t-xA*C6d#+!SXY-gzGDfpu3>I-_w>6w3f5IVC+OH}@#g9(dx z@8_z|?*>#8)Z2Rq9T@}Q;dUmc#WY)yjwCKwJ%d^tKPb*PG{*cew)(KZPUsAzdXG3lYO0n{2Z<)Hi- z3yspk6VIog+_)Gj^b5o``V~H!lRse!u;RjgIjL-uyGZ(oxn7xttP8QE8`sNNeJD3T zG~L8WFLx&SMaO28V`TJ29EC1k!B>Pnl}=aDW6h22fg*0(-<9#n$Qe=7*YEL%ym<@* z7MSaCnUaqnS3Fxs z?QFSV*l?v7lzIinVX{Dk`uMApZP}H)@Ve6vVV0A4&I?yxjk&J(-HUr^VSz&)P@DJS zZ2!1!W~RCiS$EDCn!%IXnVXn-R`Ttv_nFzuKBMfu^Ovi7jg5N`(fD9^4@y$XQRN@=NeC=8OKG7VExr_jyqui03!+ z`|?!M5{;o;+mUkFf^$-_ypoY&%3*7}@-pqpfFy!%4~yxF6U0A?|JD(mucbQKwwfz` zwe$X9JU5a5X0Fqy*gsM5;&{s9=5RE!uY$#%^;zdi=Odm8tmX+prfy$4afbZVIk!$LSV* zPyH*y3pFE>@FY?_+LU=L)xI0Z|Ke{)x|lL;rv)X|tIke#$l_RKJCkDh_3 zDV}-E1)2%q%K=wpI%)9j`HJn?j~M$h!#f`RVttf!G@_IF1DXd{HvfWfqqf&GHR8^g zYI?=&jJcj4g;OAtUjB0lkGnbN=roltuPy)bDDkXlCEss@!#m!e$f`FdG=}ad@;$LJ zk4}mWc@Y&TbS#{E6~(adW!k~2%1hiKmvcmr&dlv__Y(sztE=U2N~=(>P?OLB*>5au zK~dSvSkD=lh{U@YLf%E=5Oqkc5vfHpG0l7&PAH>Wdz-N(vl9JYxT=0(-&ZlDzBH8z z<5+Z?a}wb_Q{zv?{M76B&864%1|rs;PiuB%c47Qrm&RMe-6y5W{&cq#DkYL{)k42f z@gyYNn-%6X+cOmW*!O)!(R2-W0-~|sh6e++zZ6k^^N`o;W5R-kNLY%5(`9~9?1Iq` z*|4mYiM6DUrFvgq>8?qEulnO=eisqVH@xt{l;pG3%v!TLq>95De<|}zw2F{cu`>ox zg-%P8>!Vkt_fAK(M7?d~L98lF9lb~SLD|c22LfEGOtER&d9CS3`!8r^e>saU9{FaB zZ2Wp&>}105j)07=>=EDRS-Vu~4{ltME&C5d6Unn=`k6#E2=l_Z%_1R}rQ3!lx8h-b=`g=R{nrmLMM59)12!NerZje_fp_^9U)!$k`lFB9gc~{a7W1>lj zt-rUOURTOGz~r+P^JLH~2mbb_Ug5hjr!nT?t^xB_L!fGl_TWog(YS5Ykjb{oKG#iH zf4B7q9Qm7hw`XA~Ge>L8%tBWga%aC5y}e&Xfp)fl!D|3ZG7|W8azB+vZ~+w7(}}r0S|$(6wYg%fe>cWL)M+<2fB$zis>AccIYm{JxPg_21tFE1J;p?F6#lAAGo3}Hj@UxL%lPyH)9Q~1cCCWh*?}J~ zqb;x0tJgMlP?yZ!t@hV@aKY*HeVMOsFF9FyOddB@a(WD!uMbiWO4G&kpJ`HR>Abn` zzIgf_{_+V6{w%3{9=XQs+Zteud@!r@?7rEnSH}l=erj|bXn9Mka&InZZ?rUV(4#j( zJ8jxcmRNA+Ha=wxIP&2FqBuMrd;vx?;$Tv6V$r9OML$B1X+)oA9m^hzk2JKT2PGT^ zbTg%k42q}^V(p1|d=nzF(g_(*Gm+mZQ)yhyiNL2ZU*R>n>4Ihlx$6u+5+qQ_+FPy- zykC$_-%7|jY>8~8f`wuZ%ICE67;ziva+FhOk#mz5I7kiz5vm+Ay-{YVa5A6ecqfaj z>mjrQ5Xg#)(Y|i4Am29+Wq(_Hp}-paLo*KVrPW?;IC;&zi#-l*~C z&d-T#CEmrT;HkN6sFTnHP+||uA5Aw;vZo}Qy5O@r6^pHWaBby5X>s%=bjMXHtG8k& zBY!!7qJjtn3oj+lh6oC*t5k!Q4xeJtHhIe*j2nGvD>Lr$!LQ!2{BSY=ybdlbl<9-los(W8yppFTR;DYq zGO6{uw5CopH4~Y?XWL*{^j59(GvQ-EXQ%Uyd!I;tD{|2UC6b9Br9KFdA2>dFT0>U# zE9YGVj9X(q|d!=5Yns+cwcqV*2E&@oS zrGVr~sC9L-dg%{NpW8C#wv1#X7%`{^Eln~F2kJ@V&fPy+wJxJ9qTe`mX`(E_aQUS$ zZN3k_DK3_oW)xs`wKf_^9yJ`CUEI-{KcyNeOTApBQ;$KJ?PyJnT=pu^7x``JDo4^! z*bLLprVi~sDPj!AA#J87LO!z`p=3yV%z6JY{)DevU9UND!Scf6-i#FASC*tMqJjKm zVa>c~AHw@N)?>y45pVz?D1BEO?Fsa4ZJ~fK8Hzm#Pu2oym?01q8cgK?@jfpfBYT>8 zHJ^@aQr1pF84tJZO)f;;rpqsF1zQ_(>_WGUq2G zF~2}kapNAy&fmawWNqZ*Oy%o5Ber>h=^|kR5YyaT?zP>A@iX)n zXss#az`w>C~dILTSH@_$BC^4^RXB7Co7AtdagihdiSZ+caH=)>+pG9Up1Ue!? z{Y8TAxbuiZy4_FBElrJ(33PVGV8y1xv^Zf}KHMk8PKMunhbOdq&ApZkbt544K-hS}ll=OIZ4OzL}q)Q@&bZ4P@ zRgZUbt@eWg49|dDElPVz2TGPt<0|ALiRLjynAe(v42n0Jj%3U}MWj-Fi3^?J2@WBV zd|}xq(4#H9?=LU|uWRNBm!z_L8js-p^|&yK2H+jk|lKK8zpmOeH-=9ff@zq>9JPwGB=kI9*abBR}D6j?mu z%Pv^{CivR{-t&mYJaABF?0$eVcI39noKF0ea=VU8u+F1$xGg}pk^tom(l%yhmMbRejD^tG{wuym=Q-?p8Gzt~sfR#`)MX660gJn0=GEODFC zs@FO$G%6bHVIC-G!j9k zi=mj>LphsgUVS58+c%JRlUsn^`9cbl`uk zNV>ob-~K3$7ew5;R(zAiQO6OikSd`xdHwuY?&HH^--jclvt9ugMXzSpf?rGV3{?6% z89b5k9uYm;@&Dxdab%N@^VglUn6yAI)>q?|?<*<4VKBHI=gZ<*%$)7zy1I zI}FvlgRo}<&33p6yIREQsCu%?k(QhBotrISIaZJ2uIw+~T|d!x@N(JVh7@wcKk6*hx5dhPEtNm(1G58+x7x|Ef!{`9qgD-NaN*m+XWrN`j)@>(>4jtzZqadez3RBPgt5X zn^CA)s;Oi!OaLO*S2p5SZ{ldDl&!|CPNP-eg^=ssMd%jy6&DNNDznJWXBy;*ZYM{P z_H3nY0se;RsGod|>7y$Hs{O09rAN7Vv%UTevDdE9Cv(F^egPLpXj0l$%|8qTJf^vs zz&9E8-OX=(fs(-6^3(z#D!y06ph<&6bLxR*^5SEz6;1FuNuw8fF%$GAt+ zcwT?D&zG7w&hj)h6|J~3og06b+bVU|YEBIGDz~>0ncNFk+#lxVRtM!;#{_6GbgWrf zQS3LF^H$ajOpX6?9cAjmbRl7NR8$Dn7u9lKBB~slDQ?-4jVv7JdvC|-#Fb;PnWM+B z!k1TA^0M@GiIZ!zzRFU*-r{j*FIJ_#0rMC)XMRp$gh8^dg-epRLe96|&l`Kwg1=lF z94fvy>TTsj52-;V^M)`;)jihly`TrHjErliBqgl-o-r!08Y-!Y`F|fADsE3wU9s!7 z=wiW(3HEATnm4S}Gh8j)<4}-|tjGGvx*wCUe9l&+Z=knhW$FBVu@g(FAaUrI;cxn% z;eImNGO_)_@F?O2{6t<9 zdd4NRS5j69<$}jz#pB@00S3V8|QnNsHJ}%1maCb zJhU#%wsKUF{iw;U>7GFO_S2*_4c&(?A|#AHw);&Uor)WoQP&fh}0!Z(i?x@N!n z2hWg%I=|o7TFXH*O@FB2wyr-CLttOQ5R1mMBHBm9y&zmpCn;S2XxN=n1kcdrqjjdB zG1@w8LT@QQCz7=0jVcDTK`znn^l&CFF#%b+C^0ucH0r^1m;1Ov9HWrLnZ}(Eh&?*Q1O`PkKioSLPwiBjAG{8cEizR_>rRhgnJj&6eF~k>o zPZt?&d-RsAojtR`d@)u&0Z_?Xjp>hxI*7`=^ev_HdC(Hu&g=M6QAVdRL&u+&(Qz+7 z>UVsm`#n>r4<{Q-CkTzh9k%cD;2zw{e6KdjmsPDR%0!Jqo=FbH$QylNz8E8~^bu`b z-nd%aTu#+T)`xbb=j&&=tYu=-&T33lCW_IW7q+U)mc%LnvsjclgISO$^s=&l(qULx zMq}r#_IGEqZRpHsO7x}Z*k4Sf;8kPc_hN94?^G0iLNo(rEMh5Q$O7wdarS;z7sdv* z`9{^spllf1pE9Vlw14ylMyJ0AnEbwZiil$Z@cb0U*o9Tt-KT z)d?Vb5Ve#T<*S+-c^MTi%r&yo?P-qoO*mQIT|&?!Cv0^(b`&jd)sDt5`oIx@X3WZn z!IUBLqOoTGqej&RRgUdNq8?r-M@Vi7EMBL8@_xg+tl_r{xlT4XRu2S%QPhYUOd_O0 zTZi>3Y91q>3K~i2VIVh6h*k%Waie4)z+;ljtLq-3#T@k*PGo+agz$OjssehR2h`+y zVf?u=rCS7_@hsZpjTpj4XHt0vr%~J$*=Qqnl=K5W{Yy?Q1cSE$?I#fqKHC+Zo&7Vj zoOQvCP14S7$J(1~l+FDiyDKHmjGiPW80OBqT&Z1Be^8dZM0 z@>&PV6!-^sCo?-27_|GsCqdU@lcnD^g}Yi zK?E+%85ObM;1Mp(*$ZMpB6d%#G5njVRI9_HSH)=}vTx-&UHG+fsEk)@ranl})wMnC;L@DTxVz>XJ2=+B_m$=ag|RMr)vEVFq2EXh zKJ(_8IXQE)C1mq+Xo9f>X1)v5(ypSpgC`12mfCZ)Il=#4CFN*G@f^l@(&ogHgDeFx zNO|$?*w&bv$I-+@h$yJx2%NtrY=K4j%<^c2R=t#(tteDbL3x(y?Y>P602F6`MWmgA zz9HaXd(Up##vWsVAJe$XMCtmTr5Eq1%gfBxH5i*7_fR-h#-fOv3Fk=b3B4)63Ls$h z9-(1lfe^^EaOQ@-QGEa}rXEIT8D@-=S#lIaM-Ic=-xvPGyXr$}Ef|c%KbFMEbj?J_ zSq{H)lQHTN`{7+*Tk``$K&$z2P$=cZ!}3yHrj03+UJS}psWRo>4-~b!Se(XJBOzbe zj=Q%OUOZ}6T^aHyRocm|P`Akq%{fl&Ufl<8jg?#-2@Ssr>JAUxH;FqV9Zk0o3kGX4Jq^)!Q|2H zTvWLz2cOn`iIUjjqY2nXBRANL{w-~;#TN9lIL)_FZS3?&YOAW1Y~qqK*SN|cxcWg9G2TE9c1Y=VbA@&?l77aX zN?^l7#PJlRD-m=#mMWc;Z&C)*7)g@Co03BfZgz39xw0L|N~U*N z$RN5Esm?9bWpm^WFq{SZ*DooCJD|@I!4!y1#r2i;pDdZ@xm3VV3BUc^4;?gON!=dm zQ%4I``hv6G?a?W+9c>~FO^NbSMk&6HZd`F~BbIW%Un~?x5iFda{qPL9-*-8IwDXD05ma;2@zQC( zoPqg_GNsU*Zkat0DERTAW!jP=+9|0odSgD*9K;-)y?srMaa1_CnG#vJ&Z@!D)U~;O zG_Y#^(V3HfudOEUryW_>eHZy1j3pODjw}WPw{pPTs8$_^^|_Nv!_;N6ID0_C^<;Wu zr75_@!+fB!d4(>eb=Xt7Kz2Pm_oiFg#kP?2+%sb!j)47w5kay95rC;YnpP2j%@;oG zdCOP&?isreFU5KlksFk1o2hxz>}%YU<9CALrn5ZQt;E0O;lYinCNT}n{?7=^S#08G zhQ6;A|G+UGsB^dEYL3M2mwj6r5B^$g(O^TXf8e>X8YTmV;cl|z}qvUkY70pj-P09S|5jEM_syd@qQ>q$;N;~l~ zLuz)!%)o?>1Lu2}O!;Kq5R2GJx>hkk0WJ3qQ{wCdW!8lIW6v8DwB*rhIzJJ!e1*<+ znthe(>@%xKd7jj(nG@4!h)gXxq0u+5scaM ztC8QAt!&AF1njZxktk-+YdPJtvQb)LOxD1`=#epFqnc8ocsjnOA~QeV4D^ zhwtlLk&*_Yee*o#{gzVNxPNpJ={++RM9sz_wB1OV*u9z4FJ)p?4<9BOborHA!cMTS zi^;zNKW^Jk@qDX6RyedtZG3XFYZTLbWGE`(8@ff(+w!Rq#qsN* z65XzgyjkI+FRgY*^6=tymy&b+0<^j-ji7*$x5?L?W~{F%+t-eM%m!KX^M*fnEuBu8 z4)00{MlT+&8SKc#{Lu57#LAFgOhzm5;fBr+tr8B@f}{~j^z6lcLFU0LLs!b<6Ys*| zx1Ji*r|#9dOJL<|Jo&k!AK%mw(cl7RHBSesh#P&HVbxO9q&1 zei9fguOK>d(pv*6iA2F#*H6dle{3ASIYHedor?^ndi5{GjtLdfFc_56Fx)g;0oPs} z`C)18lVDf^4*4$G>kYAwtf@)Yj7GJhidNZWZt9ERsYS%A;LE;Eb{RkDW_@81gTYeS z@lL;1RNy6FJQQy zsgWb1oHnTOc(o9)#w9ZrStuavdoydm6f`j&JJL-shl;um{VqRZyLw z&h8s$OUfdj9pSf%el5HG-(z-mKj*14#ZAp8by_-=**?+4(OFY5}S(>AjVJMqi_EhD5H->AW@CG5s%h?(h2tUB1HRO%c9!R0`WA z^H1CWro4(s4qogd>$p*Ex0qN|s$R?&6;xnONn|AvTmxDd&&R9o?@8q}wUKp52K#Br{UwdoC@n%jRG!)7*GY_wnmcf-c4Ai1nj{Bv+VnoNNVFpqPQ z8K6@>NSCkLE{j{ON6St;m`m7}Ni*jXw`=M@?LwxrO~;9H{lfTmIw!*1p92<|iB&32 zrvR|3m98a|X_jzolTa-p3>RG|R(&jIr`7&kuCTZ&Mx8xlovRebj*aBEW}qA`2|y>C z&;rR?FKvg2zywrF&>oOL?F$*Yze_L6nkheT zIJK*EXn%S@)o3MP-J+4A=AUG~h<#=fD#wGiM_ToW0TrVt4T)Cai%L|K z43j35tZ5Hq$OF^8Wzgx^ZMkpahA$%zY$iHvDh5WSGK-NA6-875iEuqzb%QqPAY?M| zV!|yt6-lB8L?P1(TBy8grbLl$Zqj0LQak$HgQw>wBX&{2yEQq$DbGh^S_d*B#PsK z{=c|3b>9d*JrkNIxXOD_wQV$fgBgjfE&o5PPr6%obh|(a0)rvp|K4I3@~_hb1O!0p z?hYuBq`jT9ILgV?(aOQu-tq2H(Zm)75*4|<*u_!SCYmm4CU#DM+j9*qoPcn^osy`i zy_+5<1PlcOIpGil5Q6091wtT5qyb3A*~G@mRK(801_cBIKq5}2C_85$FO&xWy0vus zzzKyR03b0F2Wga*g{AXdM*v988D*;pyv9^wNgFL^I24^?6lTBGhM7x2jf1R<7ksF1 znaHujWBS#alh7QETBlaS)Ehu&`}eQ1{I2tS2;0CO2mFa#7VGlKt#yuvGv0H#QLJrh z7nMP*>RJ71+44!_IB_K7j&=9r%yf##571o~Ime?Br$M!7La;I4F@0|=Z|0o_s#fA( zoMVjF665XXVGzL*u6NWPR_FY~!-uaNhW@x$GuL*&abN43^%Jh2zCm2o9j4XCxt;R| zeW`zqJ~W3jG-oP=xT>ofm@`&-c*VI+e*U`Ka@C&De$^jV`MYMljEPHMpqmb#p6a@6l^BF#(Un{y~=4} zOAkrb^?D;Fe-(3O5w($?tRL`YACsEa036R2@qZnjh@HL?%6cjK@lx>KvB!Gk{{;BI z9EtI7WP{XPo;%;dT;0(Hbte@yaYEf<{(l6xm7|lhn5Bv1Eo|jY{``-C13+3kuV4l2>}DSVGtlUoCnB*gaVN~a3BJ~4Mf7=KqxO9$j!qIM1o;JBrgvT z2H^(s^6&tV5GWAJeX9cl@^Zt0NC@Ok9|=JK5l}FY2f_`6@j`(JIPaYfj28@qLm)r| zFXVO%01}40)4!E-bA#`6{umDlgdw1JdI&HW2>VkH0fhsRP~JN^5(Wn#d3b;b82CL@p7ga0D=e|p`eP1(c(1yVL~ye%FO@ZFM1 zRg{yxi=!#Z33z8i6{LVNvog6YOSkPj@Z0G?7=WZ4?OhxM1VCyabw?9BCx_cHOx;0Z zY9MixtCcBARZ0{j19WzDK?w?i#O&>yQFhKwz`IqOeXd$`}y2?)JS>wm-JU+ZWE1pe#?2B}*)+n@v_ zB_t)mU<3>dMv8&KNM0})2?v9b2rw8527_S`!MnGKv)Xxs}p~L`5&|Pe;7UN zUryZO;D2}GAJN}Y=+Ef?jUOk#KR5;W52=8EVbyK1kh3y#(gWT_1iYIH;GHkGO@9L5 zuYvyM$mBbFVkXWeHue^GymtDN(EyN=1IkXs)Y;13?vJzr@A7KrVq^2i57+9GZ$0TzuW%>G3cLq z2rw8f2?isPf8Mw7yM^Mv;0FhTp%P&5T_WyGS=rl(n>eFb#QC6L2m%a;f+4(61Om(s zhBAY}%&dQz3^zxVIRFfV!U5oaUqB=R0Yd=Ifq%$gU@$irc-IQF`%?x6gKyb#d;LcS zhe8p5lfl5y+dBPsJvj33G9;Ym?=lz|hT!>!9s?Lv9M+2Y1ejNxTa1TW5b(dN+;ULd-c-yI zW%|O&#TE#HAs~`q5pgIKf#gL(CB((JC7?VK5U7M06v-pOgSaJ;le3AV^Bpu{5Eujw LU}Tg~k_7yJ@cJ{z literal 26949 zcmdS9Q*fqVxAq%!Cmo|>+qP}nPRF+0v2As18&7Q8ww*lTuKvI8e%DvE*50*FzoXwA zqh{5-=R6r>T=zw)ARBAj}O|(+0n$%2HHJqQ%Abu7d!mcat+6MPnCu*E;ilw z57=L+lKnJ&m6l|feO=5lrttv&CGyo_RTsP~(;tOU&V_WNDZeyRe&w>6W%%-cm@&;K za;{T)aFtLVDC}&1I=r5<&jaLGZv6lsj!f_U51vMUU|&p-F*@m85Frkh{c?Oii)(NF z{D9RHLS~Lh_YYpc$m{VuK#tJZ&&em8@m9jghAq`9`VSWts%i3wL)|dOMp|JLB z;aP`)mktj=HC^AXHds1J2krE-M0tcc7TY`j<>C2sJy!IE!RHlNVkVq=V|{Tr%8PCG z9{ga}cW9T}&F}NlNa!y+NJST76uq378QB`xDZ}HDi4}4&Qc8fSL?v|GMu58-7OJs7 zcF|iW-HkSJCk5UR;$DP%u4$;p8ywDiUu0OkBGM*H7kJX@UqjAa@+7^R?i4R~nqY{J z;v4L7U^R&IW*AHuX&)KIM7MJuP3J|e*Hp$uze1Nf0#c@LSL?!Gci2)tPk|P}%^!~b zW^MX2BDN5$HntEPpD`r+*OdL-oi<~D0sYTIm14vwCOakd31m>nkVwXsB%_QyI;Y=C zsQ1TtTP?%;Drmj18@v7`9M&4{dr<^V13$+`U}A%MW8JMWJ$4yu=RxHqy5NL(8U;0D z3Frz_;H}y9qU*82`_u!NcUl`DI-Ai{1Y7ddt-X9*9~W*1Kx5jX-^!;vMuZ#fLSlU+buW+6AvVbp~+ zqHdyd?C>*Nm{X#2EcqiFMV&(r1gYYwwSv)&Kb)ziC_+rw;_aXp27H*@La#SG&5B-Q zwF>he^TE3Sn!E^z*>)pa_B%DI8ZH<#H)8K1DGnc$%*iWBr80>a+rf=6xQJkG>^gtagZeRHj{&fxcAoTm z1gxFgF1V4H3zk1y5M->9&&;(`5M(Tp&-a~E5Fr_b$;ZV91SO0d8>u)HOm~N)_qjz3 zK29D)-!~mj6{%=5n{E&_#$rIy6p*yFBV!yvRxw#O*u&1V4qI4!xasjL~OJi6nESUsj04J#wv{D`=2P$a!GDe6JmFR z`oLJ4D(xdKJG3v{Ib=DG(r0Nkpd;-k3O)jh8uLK$3fZE`%fG`*MF#h0Ai*T%&1?3w zBf+f9X!lGJqfvAeB~H$Ueb2h2-SJLzewH}lP)cq8iCmS3sH&zbmDeES?9vXiq|?`t*cgkSmIfRY-5hGF?} zUPW&|wy=eiiU!f`nd^cg-?Ut_-8SNIQ=?QR3@th|M#-o6)@<8hVAEZ%tevxRm6wqU z)dHG6ZJjrnO7-MX{O&=C*8KRpkXs^*VaSIdCzAau1TDGw+zFGDQ9pTOb;qQZlsj*~ z8#EN}tW7s<$cJy-zgA<#7tCQ*&g?6xGZ2zI=xEifX7G7fQ<>3hQy30+Y5Tsn!)c%H}$ZeX?~QT#hvKGtqo(c$St!MD@>O9-A6Z zq=6dxQjy&#F^{4!4RhCa)iBNHs;*0jJ!WpPRc~R#x^2=|x9Nd=GI=`l5S#06v8W6hilx{mS~0zU z_OF$-Wte##h;FzDy?p;{DoLu7j08j^gH@iZLw{`;^mU~e$zcUWXu@mlh&dD%fGxQ+ z@42;4^*Gz~RxpTFozpOPvqYIF`i_J-fa=;kaOj!53K4PeX(03}`=PF$1wH*t&Mz%`gq#~rC%>Cc6B>z-S^`Yh0UgzUXZ~KzMN!Z1 zKqkN}!O2Xaa~P}RoBu}&^RVg${g0bLA3?|?ac3E|G4K`L;QnR(IrTvXpRPLUMzNR@ zSwq>JF^v=!!q7B5g|r7GPC(3Xw@PFXy$vp$}5OL>IAq*Nq2%cOZ3YqYmh!7F| zU4*juFSS7{$$;~4i3n^$QY)-t69f)w*c^Ca&Z1`v8vi?CDqPGPm-tLxI%lZ|KYeQ2 zuIRch^I^z4b;FEFS%6{vWJRW~a`mCvyNZpCK4C#Kyu75%su8wDV^WMR&|L8?Aw>Xn zzrN>>?yAAuoCxD65A~2m_FX|)1u^!hU6DfBr5>eWl3X=pu)g69OJ$0zM?5*b()oy* zbd_AL7T?qB=0^s)hrV6y?A@(Z^QIoK)n@+V&S8AX>X3M?P(&Bk=seLv3Bm0|-lI&= zzIf3<@11W;H8xgUr7@NqB{rx5a(C;@V-{b91L<`>Y&wE{!qpG!7W9wV=>BT2-<_m6 zIDBgCV`~Evx5hM~KITyelnaf1sM05#Pd2;N+Su*L+M4Efs@XpjxEeGQO$j88y;31P z)o!_jNkR>+n!xROrNhl|W+%ZGQ{U2Tu6}_uqCI6+T57sEXj@V{_c_#agqvq{ZI3}; z4fTpf?BcuC%F!Y6j}m=+=(J$>X$+ z6|YZieL|L(8aVh;dyFTgMSE!s1%=YWrYTCDDeY7g_$|s*fjk=qD!mX!=2il@p}V^{ zzxY(Q_K~u=w@>saKhsESb$9KQwFzD}+E%qy5A*nIH-m6%X3KBWkAd^u zS)y101l(KM`v`ak%&0mZ#g*G-JkI^uJZ9HF4y93nF~SUy;7Kses{W1RRBX{3Sn<%Y`J%{hAL2ABB#bviynHN~l8Wz7sjb4Eq{Iu!RQxlMWc^b-?WzM*R7`hj@!VrKLdeN&XT|4cj%c zqq0SCu)2<2Fg)fdobGKsD_1uI-V9sE z+ad-0tHkk*@4ahhr&KEbgD$Q-*lfNoj@}DG)bU(UODk*p4&K3=Jpizzj0a3nKBoj8 zDeVzUkM_1s>wfkX3@W=#9vRaD z15-miSYOyO#J)6Q&kj1@4>EAD&ZoHoO|OE9OFO1q@A8%&;fg!fbn}MicH{O*uW;GK z>IdW>SktkXhbAkzCh%ek%G#Im3E5Op>JkG{ZQJ3-L!ks*EHN= zl~q4>nQ)@O#{8GzopunTf)Q=wWkXP3)~sq@Y*kO5*t3^Mgay{2*w4E3CtNmR%)wT0+0cwRPFp+nf5<55@7WatYC)7~aIX ztQ5lhq!PL#tkdoxfl)@Hfkm94uSApz`R0CkU@NEH)Cv?0sZTI*8L6HBJhf>!Z>(Tj zcV5HQuI|(#QatLl0NkAIhIUuXt6i=mY=2-B>wj|lyq&W1cYQrS^q=}UYlPH7o7n#P zw?trK`|Gc&0R2}?_`6_XWMcSFlI6cN2Ne%{6M7AKBTExwXJ~q57bEAt2{AiI8)$kV zAv1O)WP zhW{-2`2KS-_W#=VZ!v}Ae<7w6Y4*low;^?%Q@?ihj7$=(UO+&AEv-C)c{#cI;{;}L zt-_%x1#&=s`Ic`{*Oy^NYlRpJ_}UFnsl_jP4=%aG_AmGQg*v!n#WGD$;v5swYu={% z;eEdL-&_l0Tx_TE!x#84>cF#^gYqE^)bgs6L%du9-~nHGrcc-OfD@Ws3?q${-HOj$ zb(hy;!x%38j1*#=(EWo#n&HA)9OGb3V%=3;gMT`Rje3HCt085(XvKeH?5gUxN1&zGx?J^&o>!D_71@N6ZP40 zk0|Q@hd^>g#d=H4EbDY5ZP1`ODhe-rneNl;QJJ~a!}cO4d2%|89=BW9C8088UdLI5 zHl^>6G);TS0}XyMkA-NI8ZUqLFK>8YImgcIlHGl?<>mPleR}Rv1JuLP{D`sHx2E#3J9-4 z+mJZ>jWYo`u4z6>u#BHno%leF@O8=*vA2UG`Y949H4;{m*UdAUJs>|jo8?$zj7cxC z5nq_%NlcZ!@S7Nn`hKCy;E|il5a)C>X^ZLaqh#~0v3=h<=G2Vqp(}K5BWj*huP3?Q z<1p3HR}bBryHV+NX#Lm~a(*ZMdupiUI3iOF^KFbUy;Vr*Uf=cJUfdpQidW-(J{scaal5-1 zUEj~*mWL6ia@60D0&}Hv@f0Z1$#^mpC=#@x7eiKm(wARIt5VEP>!6l%xy$7LS5$-3QG`AsoE z>|E1hl{(z5WCOId)-S`jKaA+Qzd>7%7K~=7*(tC zhO+5?&2c5QfOa|hay)*}`}F2*(Rv%O?R9w$Zwp7q-NTWd`llCJmP5;5dWh|(HfB%x z$|O{`fB6y0)-(k+fyz$|t_cn$2?oVvoC$g5Z=$LGR_M>w8}6>yTj~DG+fO2izil?$)l=uhv`a&Z4fFUPpLNLM{)*^>L6jk9-^|Ox%O=_d)DPf+90U5 z$T45QGF_-o*Q(>@O7|AS-Z*MU2|HoTb~&5r-KLhZDIRay+r@kdV?(B@`YU%^j$Nfz#!?VXy@`;g)uA>s15_;D>jdT| z&Eg>y=F5e^E@O8+?hxA{%)NZx{X(;yN2=Tou)b2OE!>WdOgw6R`^Sr?trgD|UZV@W z2Pif(9Tn3d`4aNkeo~%#amgD0x=SR?{Uh$?h>t^afxy^guOM%wSzsq^ys)s}p{MJ&tNNDF4T0JEhB zKJ{~{Dt@LJQWsiw{F??h3zjt zVd$C-+_o0n4292_O^I1#{R_e-_Q+m?2nkoh5sp#VS78JU!a`Gp3to^-km^tz>ksH{ zgad-dOq|MQgR{62bSN?|b))%t74Sd(gL1=Y*Ih0m8#0ua(T@_}+PC7@t!xKl(bacN zBXLMp*6VL{wqYA+KU*6h_st*8hOMAo#nU{jTFnjDL;r4IKWmeo(P0?!6yuMC-1D5-LVg>ul+^!UtZ&nT%_X z{>qjNdo4A-8^ZVWOSnaQUfh@ykyXL2`O?7`Qxd=VeL3o^py5byG#7(spgC0?#U3X~ z)RJIiamBK%NIv(o6Qk;<&21sMWR{|vTCbPs{+Kz!U#rvpfueYf=~QoM6-$!a1myEh zwZ;Mw_%N#l>Q_{u#8G!AoGr+RgfzUQ+v$(=8N6Tw{;xZ2j8Ohh zc5ioJJuLr+*^BoJ|FnC;=Ks+4{B2(THg8Of94!CoaXA00$N9gbd1GYwuP^@p?cNyw z`@b;8|IN>^OWpAplMb8gW%aDO06J-+GRPTb%yqzE0{0O4U$sY zl~+{pY!{M<7+2JbyK}R9@r{zm>rjOypOAK%BhJl@x1 z68<{?@V-lV3mID@Ps399ewc64>#G@UFUbF~KY#7#*!$uKczb-L1#$}G?cXk+BLLd4 zKIWAtUt|!ZUSQS7S^Jr%v?dQl!_yXgdo{Mm`)XXphLVl* z8aqquqVjIva(rL!^6w(j&Z8eM__4X4XRoNIxBJ&$mri5!51h%V@;979sT@>b27N`}_LRVB92@zd`uuUCWz%uusxBX)X=-bMzRlrBt`RII z`}Qa7W0AKUtuFtEGywYj=fk}`ND-gZKIZcyRfACD(;?_`jcfx%k&fq)W2$3{V@J|6 z5-!C=Mv;J)oYGXRv`ja#Lq!|EI^}#>(MBu2i#fj2VU7-~r+u45`Z}p?g=1t%v%;Ds z(_T$R(ykE9@|cbAk$&3v;Yxye{$2eLtqIVz^Yf7D8Tz+jLbCwG+OQ4j(&+moAzbf) z6^|ou&d5a9G5sw~j+PV;J-8jVvtsBl^i$nksw|>gJze}Gze_qq4N&bEzqm`2n`<$; zAGi2M5i9GbNqN7vH+oK0VvA-CpMO)_gr`f0a2#@U`d8zymeo7?(L*Au6o>a}f~tAN() zDG+WpBURSTv~5iIb>dwQUeMb?i#}O8i!uM@LUCY0{tJ3MzDV{Up2!T4|t;;<+8=R^qdZ13AxmInt zov~S={Nj$=)=rPd5<`=0=zg*z9u1R~xW3i;YFm#rJd>+|$$%`{fbrk5HqmA0+BI0( zB+GulvKlpUIp^kmCO>+r?TLLdazyHIn_ikvn029zg1r?z0-U3)aUQ&ywICa&PcP(1 z_@Z2KZ1VdJ?E8GuGk}_d=~8RYP?kHAwQ82uZn6oBs9Jm}rD1&2f9g3(wap73$oWAqYyoda~Xwd0^yDkrknTE+-zC!T9{yj$}8TP}E6z z8ZlT2(f0t-9_-$mA*>xATK`P;7!bR$79K%=_WWI0)}A+}F81pTHAsZ847h8oIP(7X z=EdHhMjLmuHCMO?+KiRh)mPbwi?z1AJ?(J)?QcG31{!YAwO2Odu^{L1u;+?km5^9V z1rQpsPu|baHL6qb!%4Y@<3ZC{Klx&#&H3zPd}OC!;uEK_2OqnTPNs<`ez_kXO%o^C z1O`*A>hss4^moPqd=1J%JWfAf1L9zvC_s-=om$iNt*WMRG7RB~dvFpBV-O8u%+vl! zbr{M~_GI1V$V(s^$Tc4SRDxkB=~hRo54HJ1i5e5T)^AW4c(>fFvI&3u)9go z)1lhm5e;&k1ADCp*4YtX6W~(F-ro`1!^Kp;%iE*R?~u34>kVIEx0I9s@KvJH#`9T9 zGH)QOYt|iDsg$I#M2H_Zm0iWfO{y@Y^{T}CQ>fjY1Y)&{)s2K=pu48WZ;o9F`K_?a z5xMm~2IRS2a@~NJa7~XL>bmH|zsK60_@yB%4%W{4^Hp#K&Q9v{RZs=i3Hk*?^<5IQ)z(VmZiT3vmQi~f2yF*{S_#EQ2Q-Y zDP$+?+0ZxvC+`DdzdI%iLys2*e^B$Tr_e@5)w=zMMmO2M*@8`L4&u;s>AF2HVN|YS zLr48-z%?!Y4YQ@KHs6lfg3)`F7MQOX?a*{7>5kdL9ITc7M-=pGZHRxaVnDNZ&v@w! zoE5A)CTgT%G=@R6>2aaa4t7+mVoRH+K-?|k8JoKNkA33>n`%77#TN{h%nI_~w~Ur> zFBnZ&_ytS;v5J|ujFvDj7)_Y?1?0{SC*5#?3i*Od72ZDyBP3_Dxt;E1nSt z-7#&jtI2=gGOe-c$p6?kYqV*_L#%!K7J!=sF*I$Cc*Bf^!Czq5Gi`4E9u?A^GxLH8 zH>vDTt^D1_4u>Uc+Fw~ml-NV-ws%!^LG7$y_QDSkQU5jo7%M}q>v-5{p8al;m!`?n zmO;0PJnCXDJhY!?Q>!@?ih_=?nlZ8`$9Cpe=x`;2UWv!V%4vR}W@o7(SnO|DxFU_6N4np?k`JGVdL7lA@OTW7WpU9m$JaX zMFGU|Y%YwL6&yXjX(m;zg1_VZZ7T^^5#$IzudQU@qA->3FFjTpPk@9@xJ$+^2Bzo@ zn}OOQQ5zX+VLC`#-*sX%AhV9~W1=-s%*^OFFwB;fz)+k{3@nWe0-Rbdo}esRv8oCS z+rC4_4QQSaKBzPhnBYpv!Y1z(I&KLW|9d(oFoK>Es&#QTk;EN+z)=dhGjg7d+EK*0 z{xakns5JLNE^8RZ0NueHqLpHQ)GQ;LVqdCB@#rVb@>-)R=SZ8(&*l-R`PCOusEQQ* zC>Ce!Dpj~mxw#q|rJT!*9M%=X97arx-zzE zkjO{p#_lE?X*jb1zZ*Bx7Kphdip`*iu_PFVpVPESx*^=vi?-9av;@Faan_;({2|;! zOR;32&GVjkAuMY^;Z)k@>KPGd1*X-&kzp^4v|6ytk-lktBBNXwKj~B;qg@dFn%u-_ z_wv}1W_=$ld14&BD-oD3)%v_+d1N#|r0l~FJ*00Ah$S4?r$NM5p3qWe1pYWNEhkVZi)(n81qb_3?X8k)XAC?&hC++9z7#?9??q7Tol}z4!#dhnH8Lh zZz6-M2wEWGl2|+w=)}WUu7n@xxMN@mnFS3;KQJ)qoieyzH-1w=pcXUol$@ zl6LRV{-ShNFw&h9C*RUpfmjcI$=4C8 zz|FgGL?X;Vs&a#d!YYqyB^17b`;X73@#ay<-E${Wg(zoNj7^ zm*cS9pw(B-G-6-)BO{>M_VjKCYKULN3uoFydpwaI{5JCHyP-%YPIvemTnO0vWB2%w zNcZ3?he72&c(FD@rf@j#;XS60aH$jguev-5)%mRYYxMElpMb_`YI(t*d&f%;g|qp>fy2{ z%F+*&N^8iHZ4)r%XNmNNkO$j8n;1Y-n=W&0HK8j>4 zhv*6}bL+~^l&6?u1Z($T@hAJB~ubb+$`7Twg{h^kcL&L`TZM;P?t;F{n;WEKa!QzA}eF9#^)fAG-*A#M$y8(RkshAw##2sgC<9Wy3 z0#3@`0$!B89*~f`9xyj`DKI*HDPVEzDzG}nGhVRKsd0C3C)&1NcaSX}JW`lR|8#XH z+Ll{?Fnbv32yQxRmRq9(n|oIU^w#cb$jj5Yx;+@rhWvi`8b_>e*X+nUnpn(z3^RsS zM4rp8y_(~EK-$%l@Lc5Jlx!m@|B1)BvY8A@`q7s_o=e?#M5P^{+`at!!py$EleyLKiMiEq3v+`3 zh`GTr-7~31H{f~49khP9*FMMXhdmO!)xZ;#4S+l7t0f2AxAflJw>Yqc52Cf_0^nY! zwjnmBw;_Cv?Sih3?Sh)`TLka#TLjmE>v=%nxPbzILh)7M(Mm^8=0c8;dIXYf%;|*f;wevX_&!5Z7OL;wlhhePKaz;L2LS&hS_@cO<%jfpPW z3?vO2^neURn&+RZa^O74$YgP}L3G#lNpkAefO76GJStY`9`m>=bEafpwaVAzYb4p7 zrYdUdxvR`(uOBfu2(C3H<+%5o#wNJkWnkFhl+yi6GwCRxevuA~*9xD$m%|c?+GOxHt zE2v2WR|U3`Rb%ul@2N*C$VCIU5ocBqyaF5!=)|~qCLj#MD{vOsXr> zz%>&mgTNdG!E!S%nwM>dWpHNHu4yL$u89kK{mm_?*B?T{`YqZ56i#8ylML?b5wQ&e zE-nPKBh8YP;nEp_xe|iKdaWO&7K;K!!>N=d2qOSBlt2?s0IIU>n>i~8YwZy-O9iM5 zA6Ua-EaGnLFGSit{EkC;%D&u)!Nzl+Z|j4Dg@hj}){KRLgdYl_i5nFb zD_nq$yMS`*U`Ztg$tvodl~Fz`D2qg2Qf%m~;50B^h8YEK8RO2$s!d;V6qeeLuKE;J zLArOiV1$H3A6m1i5`>8a=l|$fuD9mQxHMk&w-{FDDo1B~_vD?X*CopTI6qb~CeLM9 znl3}Rb)wSbFHsUO`}L6}g~VU7ba}jhb?fwUmc6S_>ED;=6}jx>TNVU!QSCbkzWD$N{(p1&A1aDi;2CQio(%Wrzx9qY|t`;oK{>> zl+seyt1%PAg1!&gg`m%;R5{ebD)h9CspP&>u(EXfc`P#_W=+k*&NsLX^(Pq4qM#h- ztow{p>*|JVCV6#^I?RcAR5{dMpL03KA1Nv4kA(n?>1{H?UIyhNRnB-RxxNUt>3EiC zvBZYDKNc(pIN@(AzuZFP%VWZM2l#j9X-!J(Ci1y^u?ne36PO!}Yb;al*>=?*lcICI zGt6b>L0E5p0F;lj8cC*|+LKM;>d7>PIfXBnf#Esr9+FE2-LmG3Cvt^-hel&O8ETg} znk|K{R2-fs*f_cq-fnZ{q6yu--u(0BP5Wg}30P!M@j13_X~2{+)q*hiqs&URDsgB3 z(K7+L7Vjw8_u>+5z9BN-`FUD&VYb`M99tkp=BuoHi+2<-1xxY+VzJma=r8X-Y%9?I z64$X|3GDq?`%H~C;T0}pcaA80+o0RRZpoA67JH%vSf%xs>=4yj23eDC$kl&C*vPO# zpCH5e?$frEP{G?Zz*f_J$7V{B?Ay?>7GEEk&%6^YI`g(;n`L~VU7F1t`^3LV6JKbj zVLkWX_*-I*{afY(9AUx(+%dM6I&H!e+_AR7znE$I59>5>6-X|zr`V6Gu|%A2$e@aI z?0^`V-;}3YbYz+=7#_XHhgVqAJi8B#Lo}TpyvGMw<{MmNF0_>vE89HV4~;yTtg8RM z2ZI&eQ_t&c=Hd2q-Hx(l8WO|Q?2$Ayt)FD+g>n1tw3+s2MunL+_D{jM-Dz@dK0m(I zcBH!{8}#_dU3nl5Zf9ZnVeT~2h_t1~C+SQBm}$;wTS{!1Y0l|rN@T#VB`n_6J4W+= zVX~6dyZ(@fK^tx)vn#Wb@AtTPLWgF~@(}wxq8&i?I%qK+BjGx{Y@6(PI7?+cO1Ig6 z<0R3lw5r&{xN3?ep2SXs+p9>^w$H-INiS3~fw4Sv{E)1S=&^aTjdn;5MmNFIG5W)} z>sTD&afs7pvJ4dk^RZRY7Gq9WS^SPOt2mlO(!#@%6ejUBbMzB@l+|b}m7oo7G<G5(y-DYcG!3Vi3fAv7u#XHAsBUJ!-AnQDA`rP+KHOEcNLml!+cTQQF zDdXrvxjNayEZHsDe9Po;v)E<(aR#k&y`r8;Ae&t~_Fi6&nyq+a+hfat8e94Gm;J!V z>G;sh!X&2g%7j-f9+4*839UK;F}WIckQ@K>TeTQCXWlUj&DX+}FP>XG92n8Na@{*A z*IK`dhjeeB%4*X+Ga`_rrtNp+>x2dh!G8 z{pob7k{j%J;`hVGub~CBS$`2!YuLYqZh5FM4C0q)H{$}>k9w?`WB*7>HE^e7H-?oe zhy#lY;~7g3H`jolWZG~&KPsD1GZq7~gkCTjx%_cvK}jmWIaM7Pjm~Xg#hev-&J2#o z?HN2Ya7QeXV$0MZ9nTJpBXk`e_yD1jV&G+BEO8^1t*TSdJ`=tvKFQ9B-4N!^4SRDp zW)yHN?-oft-UW3{6Y*@Hp2A(2p9_?=TMCflfa4xdVP4Ho+L;D5T0c(1=YNJx0Q|WA zpYPtVnxCjd?^bp1R(D8e;55HQG+r>6?q!%AUec_WuoB#n(22b?{t6TX8BIrbD_~*2 zSpQBbVy>TT5X4wni*cwZ>z57;UE-_^nu=u9AThxz zXb-Jj;=~NPiWaKil?I0Mf2%=K?(Hj#i9!Ay>S9ct{fYPXcR&ijlQ6*W6Qkf2h=_D$ zTJ+$txPgDiK`d^dzs1tD;NMDv28sm7a(-+O`NFADmD}IO_Kq34_{ymPu2cT#6i4I> zpKyvU`i?`=_)fWZOh7u0PwWeC)bP&8V2ViN7xls!W&~cTqLI0HH*dWUK<)G15 z4hAAVc-Zs(p0F1VhRWD}Aou$sTKl6zp13#8$o9$ln`}Tj zJgbjE-Va)JvNea2J-ibELkyN}+vsEY`940$8-lk#UN>v#98-Q5+Evjyf{j`9+_&i0 zP_nM1DGOftV>#)Y8j!fk8kvw1>+YQ6s%vY4_muxmHLsEqLQLA(Q=LK{Dq+RQsI^|= z;&zG%-NIj^OVvAs6MS8k)=EGL|80#Wla&&Rr(+=txE$9_^!?%aB(Jr2caDuW@m6a? zEJLQR%>Bny-=)GmnY|BQb_;$Vk?7Uc;3-wfgT=S~K2?Z{{)W?pJjlWrhHQT#i1fT- zi36*0El0dNXN<}2j9dMjL09qaN$mvn072k7O~>izH>=+j+Z8i zSxFMpt3_80hRb`%NzqkFjvlg7iV8tI4@@$R(DPa`7rQPN;zulfT2v_c`t2)Q34xv90v0wI$*G+| z@_1H~mUO#qjh2^8V<)FqGxeeR=mo%1pX8)sIwQoD(KDHa$9Ku}Ww26GpG@nOV?Fuq z9Gh+!iZh*ds>3xauECFGvs3rYKuGG3ES%`qQAzo`a{9tF8EWIF|HIV)3mhI0;J!xuH8i7UyS^s>FS-;Bh4t9noD+$dnO_D;&El(95w#Z!6P% zfY69vPCCf%AbZuU_BJ@*)NAOykF7O>kYA~LVmfG|Iro5gn|<1ZopQBEJ-^MPpPhmH$l!U^ zymGNrrP~GoWgPUk8tW&M7R#$_m`SIl&6}T|(<#4Bw)}n-dk-8FGanL%T+`mSbIx?* zqx0+U8+}eWJqLZ+^MdnX5RDKYC`bO;X7|sQRc>Z;PPo-8HRroj*)g4W-l$;PCI+Cj zz&eeqFeMjq=S4Fo7;tm(%@i8b%@cwkVFuP^(?Wq<&`FtITmp%_GFoB!R;c`z# z0j#tgpOu*Tj@G(;>P7IeI_natr$R#(=D1^FKMnNy4odnAC_ShS`XE`|Y+$-Q3~uU) zJLvs}iG>59MNpUB&^bwz1`mknb=suL!d9nxuM&s@NQ9kALLq8d!Wk1@QkP4}dfD&( zq>~{*_waquB*%;Hu0&Og~*zLy;IiE|X{>7)F%j^Fh4mH37KG79jU`US@wSGcpyV^(}P zgS?Ik%v5jxexWyZ+U%*wRM2VlSj|h+a89Ay5|OLV3zP8zKRuWJsUp3D%c@P~_sG%+#%`~8m5p%D;M8xnBfhozk?+HB{P=yz@+qVx0AEU;|;kAUqr z{#3FdGGeE>FN)G}08q61$kL(DZ>O&4M!z$O382UL9j2?&c2vK(uu;kSe9a<$3BvbD z=sZ$H3ra3Zr8-A0Liq}^U$pVhV8S8x+~O?hk4L;OFZj)1etzHAas4mp;$i&H6{Fme2!ZtqP`4+9j{=I5>^m&a~5au$+BN-og)lAj5+MWb_)Kd^rMPKEbQ zBZDH5Kx-HQp(7wdjPr>T#ZbTxLTc(afc^0!0*vTp1_^f<(@bp85buhMD?w$eZE=%O z@aW_A^SxbJ^*N^_+iU9j^QtY`*2`i{#0+4xC4otKbR@X zH&V%31&gO}Vu`CpO{>I2p`0PHVG(FqjC&I5)*Gd>#(O`J6BCq`<~L3;jc333@uvbW zPHda8uP&|Gy&TLbm4DNiGYfqT;0IkL1aIUgx4CJ5q0g0n`bkU&k_d5q`ppL-@Ga$| zp2(X+u7$DXWt+ok1w5956Uy&iNa>3LCooOsOOm&ImKPEz2StOo0^d6~Msuc>D|KfiW#o8ux0V#o0TVorBM(A zs4RhSn9z<?KgRWyncrvrr2uJpe&{BjsHOjZn=VccZm4KL5 z>8i1x_m-;RP^HpEA%$2<0g6UW=y?S zi~#3i)Ta~ja7+cIqtA=$KsgIPGtZt$wl6pKi3Odkh*(|-WBuer1zkh1;XVnBiCF|S z_BLot!^h})nE#RHVM8D|LH}bQQBQgdVu-Shl?1+)E{MYU-QR#~51lYWhknCabL zf=S@;yS-*`qBo9St&4c5I!Y(5D(+{_=i$9bFM+GG1q<%vcLQ6NDzNsGQ~u^Dq?_vJ z@35*a?pgFKE_RxdJv=_HqW+E^!Cd?jJM`ktrkq(r0bKK+x{P2A{ti8Ta^#Ht?RW?o zkm)^lr>MQ1{zshf#PW`v^a85!3Cg?3R-BZYcyyTNdZdAN(xxiT-!Rl2qBTI!^p6o} zav@K&Byfnaz}o$|vW!AUDSnZ^9khd)tLiAze_PRDKyJj^0I?U&aK<=@y2|YxMu*2B zSTaYh7d;7iP$ocA3I4pQiS&p6#^9{zjrfC^8Z5_K!u_2v7S*|kL>R^om7|hR-_?{r z(;8l5U<33Txe5BrOl3q?Xc8=?6QsNxz6qKHedJt<;-qv8p7C!V3+wK^W0SPX^yYL;xBL`Bs7r?^6cL?*lz+7 zG_C};u>mFY-Sz(|?z@AU`nt81DiN^Jn+H)85OUJ#2!YT$BE1KQlt>621PdSvDpEy2 ziZns#f{1|hCMBR!6jXXunuX?_#Gm)|)pzdq&Hdy1&P;N$*Iqk&?X}jkPGl zjwN37;iQXzCE3!;tJ-Y#eajtQ!3W;3L1DU=H|VdO8H&S`+pW%URqT5!!DFY{wZA62 zYeM+OQ%`|8)zgpX4!W4=+;*eCFF$Lh)e6r%Yz0SPV0-f=tF)Wla{U56r|>3lsGCq++wm&Az zddKl#(ka+x8k1XT_I`_mj*mjMwc2xD3~r|*`O-5Y+&YPD);8uVa%`8KR|T0mPaF-? zRuz=yf096#8L?+-udTIu!mZP2C;B%HN7?PX4p(cYGF-oq5~fx3k}bc*>R7MvYHCKR z2#N6w$0&%jWv{!>)xfZ~Tq3pPc)0zHpkM^_T3+xOalwe*!^%$w4l3mXxQtjjb6aQE zLJ!Z(eS48qP)3DsS%NSkglNiIBqK<@D?Ztx;OBbR zwo>vRO7deoZP+#Tyqvv%27%(6iJCSM?*@bre3E&mzjz7co_aE*-fwnxu_?n){yNVC zV~zWR94B=~`D$kG2Nw5rgmmRU4be9&99lm4pnr7h@e2*n66F)o56*}vh+Z|<8K^Z< zMvzNyWopj!AxM#d;fnSo-ae7a$HrRt>kmxOn^7k-F!8hO@WggGBJC3BB)UaK z^br=b?E-_bU&owYr$E;O zZ57vW2i_g2V7*5_!DDcz?ct;}XJUZ}i2Sn67S+57E~ zpf;ZGoqLbl^Ux`_AiaJOe=YT$h^L=l#EuQPe|_o2zSx<$-|zx*S@H0dETld|Z&|8E z-T=ws24`V3GS9S6wmEih$F(oE(L8*8QT7`Cxh~g}(?xmx-w8>JSWT}yGs?S}gey6Q;&Cc;RK0+zgUR|S5t#gXOG7~v7rk~m^D!+`Z`M;Vw zu$msFYt}FO$~Ms^*z-sEOs>bhktcQ!Lj%tbuiPej6?O}@dN%L*+|fNrV4NY>#es|h z$o?x~OK}%G4Vze;j>X*{tn$gU2T|sOq zwnyXSgPm%~%*2ZS*vr>-#Ja7yHe&rk_U`eeB)%0{t4OyMivshJlh2{n%>;ken@t6j zmV+*@F1^<3L%&1lFeWpw9xGRC(OD@lNma2=`Dol{bzzGAf(lPknUDRUBR3oENe=T5 zBq){GG=E-Hnf~)NA~8)q^iKcw_d-Q8wvd~WTNonng-&9RrOg>M$3khJ>VJfvVmm`aAb_OZvr?3SM}9A5wmkArY?nP@t(uRbPK&hnK=BP4ZIJVc={U&R-;EQRFJei4+ zL!0k9h!#Tc1{Rn$%}Jy;T~La4;L)+2&4{YGvkWZWTf{!@yINVy;_KB8{yruqP7k8; ztV@%P)Hyd#Rq~lX3M@YGEj0N}+lZZ82d_@w@ReDl>JhJFZBejNUZ!)RUh2`uJWpo` z<~K3!b{Je>SS8<$g9Zi84XBH)mFDKS-S^X1T2k^>p)4)SexAO;rZ06$v6^2Wb>X{2 z;>%!Z;iYinki;)NX|Et^Shp#wzAZ8EWAGQph8m$Lp=`*RtEDgU&vJaPh!;8{2<-r2 zHCQJCd0?)Liz|yi-RFHnY!NKGHHIH0dPA%+yByK#jK@149%slX4|Zlx{b;Oks6FdL zK#m%r{CqqU{lo?@tGI%ZMbs@)BmcA{C>t}knj%`^?iZP>`^kE<#t@Pu)=f>iE(o6%~O=F6rhUz(A_sI91yK-#bmE>5{OB_wd&Z3L;@fmD3 zM?Id2TnU>iU@eO{_~h#B!Fb1M&GCy*HwKtXrn}tC#)Ce-AJA9YS`1YT2_tEaYk%^! zn-3^@u|hd9H!XK`XXTwlXf+=Nh%4XYrkZVa%bUSPT{bNi0nv*)d%&}q&!T{#S)8pR z;F|2!d+!d($k1Q!sClXy#)v;&>+ILoa>**bKSe0ZH881cqx@_A=H`6cjY`OL!+8R6 zrsrLnrSGSaO-q3r8=rOkAFfLUui9d`jy^wbqqNmA<=1GD(xB&zWR3Z%s-C}iJ|*I; z;#>Q3K_R|!AEj@+Y2&@!vw(dfhf^#IbiWp;V_F!uC~`(wK(I+Hb*^-{)?Fff7r#qFmI-7DSM=eS z4b}t8grWdD{k2VZrkkehJW}8Fp7EJ2zJAP=q`E^g&bCx~zW#wiNpe;!t0VmA;gmU= zz8L7c9jfU2qxi5Jy0(t1?UTGgiJJ)}n$)|fIXrbkOK8-(LN>H>@~WmeZ}`14>T*Sp zSTU6*!^>@1XZjf`kySH74%=WE*a{U7H-|G_PB~%cyyqZ_>#T+SP_Pw6m-nzDo3m?pvNJZKCT6+6HOCapmsH_i+K~hC(CT^Kzl-;KWK{NU=8;fgE5uw|MTyjS@B- zp5oQX@+^mo?W-$?Mhk$;JJ;5aNCuR=qk@P4>TwtYF zCkVPYX-aBT&(0y$9$axK-uv-@8q<1G5SmdoM@d_$A}u|nck8VGU8LVY z;!KZ6NdCied*`W8rPVPu=jF{N!`h|xM2S$&BVR=GKK7ecu0*vhTc<30NTu$|4#&~i z&G`n5`(_E;I#}S$0Z|-jzUQEdi&+mWe3OScwNXpYR$r+_@;o%U$7<>XJ%_K!QQg7P<^!8;0d>bCx)Bl;S|hCPNj&yH+S72kS3>nZUC)yyko4d!bFGcn#wo^LvYEkJyu2-E9lF})X8za39!nHF7$ zVJ_Z>bn)qp!`=Mv}UJP~pbAE|d zHa@`&%Dle3_<1bq3WF0JUE|h+hfSE=Ie(w6(a0R}3ynMXr(NDZ%a6Y|`LgBoygiON z@l__rtt-yFMSTzLpbWPPOA;xr72=FdE3#KfJrCHf<|-#PRdn^Fa3Td=eDP(-N*N&{ zkLaWAIRiV}_m+f>eYLVeU&6lRF1yYRS=AbhC~^m7KfCu?wyei*J@oP$I11b^@#5*F z3BbQ}*k{`4eow{&?U2qFr7ow!It4hk`3K$+UJTxh8lDiHaelV8Y>QjozXpM@5n~S?le+7(6vEw<^RkbbimcI7>}{4pzR$4wvJ6SLJA7TX5#BqG-9yd$m6r^=dM| zBF>wR5KNjt{o# zjN}w#c29@6D>C7;I`aDwVv6vtoT&8n+?3M0IjzmCw|vzelGY*?5bL})N0sXO@~(eq zn+;GP@*^U|*JJu`b&9RWz=!TYLeT22^YnF58hQ~|jD5H317-+khc4Si#aWABBi$Vi zespdftHD+WEeOzS_v^rU=O#7c0`2`9)$@!dxb+Hxmp&I#)G6cR^>wu#I#2R4w#N>P zrJe0mq@#~AiZ=t zr7HKrR2gMycjcti^Uba5f&;Jg6t@e#WYQPcgKK@f~OrEyZ`t z{6y#?Bs4RR;UxcUTrjgPE2Di}2ji>>l(sL$VeF0t8T z+_8RsR(D!%{ZaSGd3NElZk6NQV>3+UqkHnghfn7py7_j)>tk8or+)o!d&REQ3rgRQ zt&!Sxqx&4D2IIWIU>N+_`cZA?p&$O^FO-#J8>)8p3CBIXriU2MTGNrRVry@%yK&P? zUbeU$H)BpeB+NdEaur4irOmWvGvAc}-efR~?Z2%lNILWEDMw_w!{h9*kBS_2W7tRC zGZz%DSH)GD$&vs=Yw-w$WU{&m5&k=h+`_>=@XP7S?@0`(d z*&s!YZ%{zXJB-4tLSS4xb}ND+uPSy9OF*SI{D6Iy$T;2|PO;F)i=@aRnR-Pm;FB1v zq<_e%m{=C=Ok_AYUN}CtpFvzaJ_1a5$eLr$p)3i>k#WipV$0!t|HdN|c?;KXo>{?% z??0Pa!SpiI?<&PYBlM^imbFo*tJv0H+_b*p2`+!7>U;Qn?(_-Cjs5c@S6#v@;s@hZ zt8+y6h3o3>=XwPR(3O0tE4lk*Gq?2tWqJf>A5mXgS>&s{bu6@N^7Z^kYsv_hup!T> z!=%v#?pON?#Kh>c%qt}y^;v1?Y9)lL`W9XcDSXd8VzG^?WZO*)3eWxiA$BJQrE(=S z3M@8tEwDBiA2A)J6Mut&ysDJ+xacqCPAnikDkv7MsT5^9h0vI&dGci1tE1xvH&kym z@_Jz=vY@6w_uIl@lx~ z{wV}au`64NZYNNcr!{OiiiT6~$R#BNI;#zwM!h@cd18-y;-8-dR!)Our%yzk+1(kK& z1X5<9!Q#r)&TlAT_ngb35-X=Pn8teN{8Jp;TgwcaV^;Ox}N;ZOOKIVVS``9mUcVSAd6njseIZOXa(ZqAWO=T?<=2Bl&3B0kO>kTNYP)k2L z_el4(sOwZdIS%0SWrIDSO0=Q?2{MSD3~hwjEV} zDxyV4K#0>d?z7d*U^*Oe$Qm8L;N}r?>;CA-Yax9_kCjIrD_4|AXa}jGkD+^x-rY;v zAO3@*i?(-4-9N@4Vd#JDAOA`8fFpX6Rq@`AB*FzUkwk0i*?ZujD$3MLLjMBZ)7akA z8$#s_cku?05Sj{h!|lmR5+7!ZKLK_mbJkn#$>#QDIg90oB>C0$HM{Ip0LsHWL)sy zcpv^@<3ggr|G)wE`=+pUoVMpE$e}i4!5BFQ+}6-?qT`{nl4FwB;2&I~bBgk19K3U; zi~f^h?I&^F{!8K5)ev?9e0;HLEA#$ZmGqNHu+UhE46|ypbiX1S;jQE}lK+@EgFin0 zg`#7gPCD>)K|pHj1M;z{`L4(#>ly!Z2Npc#BcSHH3ckW$n#>ipM+7L}F5Jsi`fxbH zoYUO8P%70Z?Tz#1YbzBg^IKa|l3&+$U^&4|+qL@2yu5A?D;&8W8xKSbvCiCnbUn+o z@ZIB{#=e~@(*zxv^pSC0olMzqE;amoGp@R*A#v?Q_3^IYmSpY{5#o#Vq8Iv;Y~Sua z<*J?DMtqY9U>*Z=_Yh^EE6_&Vv|sc}l#lRYpZ36bW^Qi%m3u#5Sg3AjWC&qXZc%$+ zJi9ozKFMbN>tX%SldyF^eY;J5`^})#yxe+j%qD+oRA#-JAU5=t0^cToGyi7NtlR9T z4+O5z0&M6Fn~2?>OWdLMoA*%3yQcmtNmiRdcgJ^)T~^xaSa$?_nz`=75_h>=+~#aq z2F5?um#jk#+bH#oLXtbi{8ocgxR0G5bwlJCUyV8F^qvt)e;fLC`{KUP@oA)9`<-z) zKlEJVe>f-shy9BQgvMP$P3iw8oiic%;Av77dv83g=kLrWki5xQS9=mQVRh{P`HVtB zpyvoqWLIxXGzbD{m^6Swp#c;M0bt)$IDm#B02u@bpg|;nhQR?88U`RS7yyI800h%K!VZ$ z3Pb>KBnpDUz-anZIR-BCQwIeHsB#2?ss{on6byi)spH`Q5{&^+GB5xIM*t`!27<;& z14y_GfJ92u#$XT_0EGr=`qn?;_OB8E&`N|61ZqM&>q(&gMF2p<+`psnH&PWG44{SJ z-xYpQ1j@_P;sMY`7(srq0?NxnO-L{#Ks(Q31T`Tc0qV&X>L(e1TBD4p9Di#5WOZY* zJsB@A4^>k)hJgT$_Qi|OBK zpEu2)Hx=vOYs#MqWMJ=thZ<1n2gv{oO*6{~?@ja}IpVzm+RBWex_BpoJ(bFkT92fW zZ~!HPvi`rKFx32q!VrojdXn*;WN(1#Oseg_=oP6`QY$yrNmSy*|JU|V590i85A6T9 zgJ>K+zgq|H>4aO-4{XygwO$ z)3WBbkkSPIn~CASCjOa+|HH%vB)sph<^MF|&p6Tq{~IF^zl@+|!vDDZpH}?2d>V@W zOlfb(U$BDw3s=Bzz@kK>&r!myNe^TIgfB#~+`h_8^J=xx!=t9Gi z_dlqHK&d}(lpVt8Y?oLYkalp$b$$*2#of7gSfK;(bWL;k4; zqA?)ikFg*cLv{bZ&x@7;|NK5U3MP&ETMr5ULk2@hBmP_m3?+^FOAihHr3Xi0(0|$o zLxY&VY(bzff7%R3pk&a0*aAmjr2q6Y9EteL_i!Xy=C3)BfBOZFLi`mEI0_;CXMEu( z)PIN(`mY$lQPP;dY(~joev2EKWKVF%lc;DjCIsRE1PC=E63GA!hDH#mrl&IzprM7@ zPz5Xjd1W;W4vRv_sDRQ~EE Date: Mon, 20 Apr 2015 22:00:36 +0200 Subject: [PATCH 250/268] Commander: Fix RTL mode switch logic --- src/modules/commander/commander.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3fecd9f58f..948543714f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2237,14 +2237,14 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s /* fallback to LOITER if home position not set */ res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER); - - if (res != TRANSITION_DENIED) { - /* changed successfully or already in this state */ - return res; - } - - /* mode rejected, continue to evaluate the main system mode */ } + + if (res != TRANSITION_DENIED) { + /* changed successfully or already in this state */ + return res; + } + + /* if we get here mode was rejected, continue to evaluate the main system mode */ } /* offboard and RTL switches off or denied, check main mode switch */ From 8279de5a0b9f1ea0d0535c1de856d2344178deac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 21 Apr 2015 06:58:03 +0200 Subject: [PATCH 251/268] MAVLink app: Cleanup RC channel messages / handling --- src/modules/mavlink/mavlink_messages.cpp | 73 +----------------------- 1 file changed, 2 insertions(+), 71 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7d6b60e227..5adfea36b6 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1867,29 +1867,8 @@ protected: struct rc_input_values rc; if (_rc_sub->update(&_rc_time, &rc)) { - const unsigned port_width = 8; - /* deprecated message (but still needed for compatibility!) */ - for (unsigned i = 0; (i * port_width) < rc.channel_count; i++) { - /* channels are sent in MAVLink main loop at a fixed interval */ - mavlink_rc_channels_raw_t msg; - - msg.time_boot_ms = rc.timestamp_publication / 1000; - msg.port = i; - msg.chan1_raw = (rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX; - msg.chan2_raw = (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX; - msg.chan3_raw = (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX; - msg.chan4_raw = (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX; - msg.chan5_raw = (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX; - msg.chan6_raw = (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX; - msg.chan7_raw = (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX; - msg.chan8_raw = (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX; - msg.rssi = rc.rssi; - - _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS_RAW, &msg); - } - - /* new message */ + /* send RC channel data and RSSI */ mavlink_rc_channels_t msg; msg.time_boot_ms = rc.timestamp_publication / 1000; @@ -1913,55 +1892,7 @@ protected: msg.chan17_raw = (rc.channel_count > 16) ? rc.values[16] : UINT16_MAX; msg.chan18_raw = (rc.channel_count > 17) ? rc.values[17] : UINT16_MAX; - /* RSSI has a max value of 100, and when Spektrum or S.BUS are - * available, the RSSI field is invalid, as they do not provide - * an RSSI measurement. Use an out of band magic value to signal - * these digital ports. XXX revise MAVLink spec to address this. - * One option would be to use the top bit to toggle between RSSI - * and input source mode. - * - * Full RSSI field: 0b 1 111 1111 - * - * ^ If bit is set, RSSI encodes type + RSSI - * - * ^ These three bits encode a total of 8 - * digital RC input types. - * 0: PPM, 1: SBUS, 2: Spektrum, 2: ST24 - * ^ These four bits encode a total of - * 16 RSSI levels. 15 = full, 0 = no signal - * - */ - - /* Initialize RSSI with the special mode level flag */ - msg.rssi = (1 << 7); - - /* Set RSSI */ - msg.rssi |= (rc.rssi <= 100) ? ((rc.rssi / 7) + 1) : 15; - - switch (rc.input_source) { - case RC_INPUT_SOURCE_PX4FMU_PPM: - /* fallthrough */ - case RC_INPUT_SOURCE_PX4IO_PPM: - msg.rssi |= (0 << 4); - break; - case RC_INPUT_SOURCE_PX4IO_SPEKTRUM: - msg.rssi |= (1 << 4); - break; - case RC_INPUT_SOURCE_PX4IO_SBUS: - msg.rssi |= (2 << 4); - break; - case RC_INPUT_SOURCE_PX4IO_ST24: - msg.rssi |= (3 << 4); - break; - case RC_INPUT_SOURCE_UNKNOWN: - // do nothing - break; - } - - if (rc.rc_lost) { - /* RSSI is by definition zero */ - msg.rssi = 0; - } + msg.rssi = rc.rssi; _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg); } From 92bdf74423d6feff3fa9ab3e721320138631a86f Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sat, 18 Apr 2015 23:35:28 +0200 Subject: [PATCH 252/268] overwrite rc in rssi with value from pwm input, parameters hardcoded --- src/drivers/px4io/px4io.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 33125699f5..3fa12261e7 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -111,6 +111,10 @@ extern device::Device *PX4IO_serial_interface() weak_function; #define ORB_CHECK_INTERVAL 200000 // 200 ms -> 5 Hz #define IO_POLL_INTERVAL 20000 // 20 ms -> 50 Hz +#define RC_RSSI_PWM_MAX 1000 +#define RC_RSSI_PWM_MIN 1800 +#define RC_RSSI_PWM_CHAN 8 + /** * The PX4IO class. * @@ -1633,6 +1637,12 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) input_rc.values[i] = regs[prolog + i]; } + // get RSSI from channel 8, input range 1000 - 2000 + if (RC_RSSI_PWM_CHAN > -1 && RC_RSSI_PWM_CHAN <= RC_INPUT_MAX_CHANNELS) { + input_rc.rssi = (input_rc.values[RC_RSSI_PWM_CHAN - 1] - RC_RSSI_PWM_MIN) * + ((RC_RSSI_PWM_MAX - RC_RSSI_PWM_MIN) / 255); + } + return ret; } From 9c282cf6d69c60e2e280d1febf21fc96bb6e40cc Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sun, 19 Apr 2015 14:52:33 +0200 Subject: [PATCH 253/268] added parameters to specify range and channel, caping result --- src/drivers/px4io/px4io.cpp | 32 ++++++++++++++++------- src/modules/sensors/sensor_params.c | 39 +++++++++++++++++++++++++++++ 2 files changed, 62 insertions(+), 9 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 3fa12261e7..e5636ff0f3 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -111,10 +111,6 @@ extern device::Device *PX4IO_serial_interface() weak_function; #define ORB_CHECK_INTERVAL 200000 // 200 ms -> 5 Hz #define IO_POLL_INTERVAL 20000 // 20 ms -> 50 Hz -#define RC_RSSI_PWM_MAX 1000 -#define RC_RSSI_PWM_MIN 1800 -#define RC_RSSI_PWM_CHAN 8 - /** * The PX4IO class. * @@ -307,6 +303,10 @@ private: uint64_t _battery_last_timestamp;///< last amp hour calculation timestamp bool _cb_flighttermination; ///< true if the flight termination circuit breaker is enabled + int32_t _rssi_pwm_chan; ///< RSSI PWM input channel + int32_t _rssi_pwm_max; ///< max RSSI input on PWM channel + int32_t _rssi_pwm_min; ///< min RSSI input on PWM channel + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 bool _dsm_vcc_ctl; ///< true if relay 1 controls DSM satellite RX power #endif @@ -528,7 +528,10 @@ PX4IO::PX4IO(device::Device *interface) : _battery_amp_bias(0), _battery_mamphour_total(0), _battery_last_timestamp(0), - _cb_flighttermination(true) + _cb_flighttermination(true), + _rssi_pwm_chan(0), + _rssi_pwm_max(0), + _rssi_pwm_min(0) #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 , _dsm_vcc_ctl(false) #endif @@ -668,6 +671,10 @@ PX4IO::init() if (_max_rc_input > RC_INPUT_MAX_CHANNELS) _max_rc_input = RC_INPUT_MAX_CHANNELS; + param_get(param_find("RC_RSSI_PWM_CHAN"), &_rssi_pwm_chan); + param_get(param_find("RC_RSSI_PWM_MAX"), &_rssi_pwm_max); + param_get(param_find("RC_RSSI_PWM_MIN"), &_rssi_pwm_min); + /* * Check for IO flight state - if FMU was flagged to be in * armed state, FMU is recovering from an in-air reset. @@ -1073,6 +1080,10 @@ PX4IO::task_main() /* Update Circuit breakers */ _cb_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY); + param_get(param_find("RC_RSSI_PWM_CHAN"), &_rssi_pwm_chan); + param_get(param_find("RC_RSSI_PWM_MAX"), &_rssi_pwm_max); + param_get(param_find("RC_RSSI_PWM_MIN"), &_rssi_pwm_min); + } } @@ -1637,10 +1648,13 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) input_rc.values[i] = regs[prolog + i]; } - // get RSSI from channel 8, input range 1000 - 2000 - if (RC_RSSI_PWM_CHAN > -1 && RC_RSSI_PWM_CHAN <= RC_INPUT_MAX_CHANNELS) { - input_rc.rssi = (input_rc.values[RC_RSSI_PWM_CHAN - 1] - RC_RSSI_PWM_MIN) * - ((RC_RSSI_PWM_MAX - RC_RSSI_PWM_MIN) / 255); + /* get RSSI from input channel */ + if (_rssi_pwm_chan > 0 && _rssi_pwm_chan <= RC_INPUT_MAX_CHANNELS && _rssi_pwm_max - _rssi_pwm_min != 0) { + int rssi = (input_rc.values[_rssi_pwm_chan - 1] - _rssi_pwm_min) / + ((_rssi_pwm_max - _rssi_pwm_min) / 100); + rssi = rssi > 100 ? 100 : rssi; + rssi = rssi < 0 ? 0 : rssi; + input_rc.rssi = rssi; } return ret; diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 18e47865b1..d6ab637cdc 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -1360,3 +1360,42 @@ PARAM_DEFINE_FLOAT(RC_ACRO_TH, 0.5f); * */ PARAM_DEFINE_FLOAT(RC_OFFB_TH, 0.5f); + +/** + * PWM input channel that provides RSSI. + * + * 0: do not read RSSI from input channel + * 1-18: read RSSI from specified input channel + * + * Specify the range for RSSI input with RC_RSSI_PWM_MIN and RC_RSSI_PWM_MAX parameters. + * + * @min 0 + * @max 18 + * @group Radio Calibration + * + */ +PARAM_DEFINE_INT32(RC_RSSI_PWM_CHAN, 0); + +/** + * Max input value for RSSI reading. + * + * Only used if RC_RSSI_PWM_CHAN > 0 + * + * @min 0 + * @max 2000 + * @group Radio Calibration + * + */ +PARAM_DEFINE_INT32(RC_RSSI_PWM_MAX, 1000); + +/** + * Min input value for RSSI reading. + * + * Only used if RC_RSSI_PWM_CHAN > 0 + * + * @min 0 + * @max 2000 + * @group Radio Calibration + * + */ +PARAM_DEFINE_INT32(RC_RSSI_PWM_MIN, 2000); From 027919302d9537e4787f1384690a019eb937e458 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 21 Apr 2015 10:40:42 +0200 Subject: [PATCH 254/268] IO RSSI handling: Fix RSSI for all protocols. --- src/modules/px4iofirmware/controls.c | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index e04ffc9400..4ef27718cf 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -99,6 +99,9 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool if (*st24_updated) { + /* ensure ADC RSSI is disabled */ + r_setup_features &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI); + *rssi = st24_rssi; r_raw_rc_count = st24_channel_count; @@ -116,14 +119,14 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool for (unsigned i = 0; i < n_bytes; i++) { /* set updated flag if one complete packet was parsed */ - st24_rssi = RC_INPUT_RSSI_MAX; + sumd_rssi = RC_INPUT_RSSI_MAX; *sumd_updated |= (OK == sumd_decode(bytes[i], &sumd_rssi, &sumd_rx_count, &sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS)); } if (*sumd_updated) { - *rssi = sumd_rssi; + /* not setting RSSI since SUMD does not provide one */ r_raw_rc_count = sumd_channel_count; r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD; @@ -187,8 +190,8 @@ controls_tick() { /* use 1:1 scaling on 3.3V ADC input */ unsigned mV = counts * 3300 / 4096; - /* scale to 0..253 */ - rssi = mV / 13; + /* scale to 0..253 and lowpass */ + rssi = (rssi * 0.99f) + ((mV / 13) * 0.01f); } } #endif @@ -215,17 +218,23 @@ controls_tick() { if (sbus_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; - rssi = 255; + unsigned sbus_rssi = 254; if (sbus_frame_drop) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP; - rssi = 100; + sbus_rssi = 100; } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); } + /* set RSSI to an emulated value if ADC RSSI is off */ + if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) { + rssi = sbus_rssi; + } + if (sbus_failsafe) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; + /* set RSSI to 0 if the decoder senses complete drop, independent of the ADC value */ rssi = 0; } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); From 4440c6383c3ad9030db7de4b828a7c4a1627c100 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 21 Apr 2015 10:48:40 +0200 Subject: [PATCH 255/268] IO RSSI handling: Make 0-RSSI value consistent for all input sources --- src/modules/px4iofirmware/controls.c | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 4ef27718cf..c9702c2ec6 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -196,6 +196,11 @@ controls_tick() { } #endif + /* zero RSSI if signal is lost */ + if (!(r_raw_rc_flags & (PX4IO_P_RAW_RC_FLAGS_RC_OK))) { + rssi = 0; + } + perf_begin(c_gather_dsm); bool dsm_updated, st24_updated, sumd_updated; (void)dsm_port_input(&rssi, &dsm_updated, &st24_updated, &sumd_updated); @@ -227,19 +232,17 @@ controls_tick() { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); } + if (sbus_failsafe) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + } + /* set RSSI to an emulated value if ADC RSSI is off */ if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) { rssi = sbus_rssi; } - if (sbus_failsafe) { - r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; - /* set RSSI to 0 if the decoder senses complete drop, independent of the ADC value */ - rssi = 0; - } else { - r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); - } - } perf_end(c_gather_sbus); From 8f762b57670d8ccf9e37b66699122135fdc624ff Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 21 Apr 2015 12:27:52 +0200 Subject: [PATCH 256/268] more specifications in firefly6 config file --- ROMFS/px4fmu_common/init.d/13002_firefly6 | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/13002_firefly6 b/ROMFS/px4fmu_common/init.d/13002_firefly6 index 29167f1ede..9489bfd9a2 100644 --- a/ROMFS/px4fmu_common/init.d/13002_firefly6 +++ b/ROMFS/px4fmu_common/init.d/13002_firefly6 @@ -8,10 +8,14 @@ sh /etc/init.d/rc.vtol_defaults set MIXER firefly6 -set MIXER_AUX firefly6 +set PWM_OUT 123456 -set PWM_OUT 12345678 -set PWM_AUX_OUT 1234 +set MIXER_AUX firefly6 set PWM_AUX_RATE 50 +set PWM_AUX_OUT 1234 +set PWM_AUX_DISARMED 1000 +set PWM_AUX_MIN 1000 +set PWM_AUX_MAX 2000 + param set VT_MOT_COUNT 6 param set VT_IDLE_PWM_MC 1080 From 05847fd4c7049f09ff6e72a32f10074129f0235f Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 21 Apr 2015 12:35:35 +0200 Subject: [PATCH 257/268] improve serial output during loading of mixers --- ROMFS/px4fmu_common/init.d/rc.interface | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index efdba9506e..7a424970f6 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -45,7 +45,7 @@ then if mixer load $OUTPUT_DEV $MIXER_FILE then - echo "[i] Mixer: $MIXER_FILE" + echo "[i] Mixer: $MIXER_FILE on $OUTPUT_DEV" else echo "[i] Error loading mixer: $MIXER_FILE" tone_alarm $TUNE_ERR @@ -105,7 +105,7 @@ then set MIXER_AUX_FILE none set OUTPUT_AUX_DEV /dev/pwm_output1 - if [ -f $SDCARD_MIXERS_PATH/$MIXER_AUX.mix ] + if [ -f $SDCARD_MIXERS_PATH/$MIXER_AUX.aux.mix ] then set MIXER_AUX_FILE $SDCARD_MIXERS_PATH/$MIXER_AUX.aux.mix else @@ -120,7 +120,12 @@ then then if fmu mode_pwm then - mixer load $OUTPUT_AUX_DEV $MIXER_AUX_FILE + if mixer load $OUTPUT_AUX_DEV $MIXER_AUX_FILE + then + echo "[i] Mixer: $MIXER_AUX_FILE on $OUTPUT_AUX_DEV" + else + echo "[i] Error loading mixer: $MIXER_AUX_FILE" + fi else tone_alarm $TUNE_ERR fi From 15f11ae1e231422cd57cc65cdf89ad714f8a6ed4 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 21 Apr 2015 12:36:46 +0200 Subject: [PATCH 258/268] add control output for tilting rotors --- src/modules/vtol_att_control/vtol_att_control_main.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index defcff8e4a..d9425f2bb3 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -176,7 +176,7 @@ private: bool flag_idle_mc; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode" unsigned _motor_count; // number of motors float _airspeed_tot; - + float _tilt_control; //*****************Member functions*********************************************************************** void task_main(); //main task @@ -241,6 +241,7 @@ VtolAttitudeControl::VtolAttitudeControl() : flag_idle_mc = true; _airspeed_tot = 0.0f; + _tilt_control = 0.0f; memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ @@ -521,6 +522,7 @@ void VtolAttitudeControl::fill_mc_att_control_output() //set neutral position for elevons _actuators_out_1.control[0] = _actuators_mc_in.control[2]; //roll elevon _actuators_out_1.control[1] = _actuators_mc_in.control[1];; //pitch elevon + _actuators_out_1.control[4] = _tilt_control; // for tilt-rotor control } /** From 5e044e5b676747a278513ec7501a472dea25aac6 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 21 Apr 2015 13:16:59 +0200 Subject: [PATCH 259/268] completed auxiliary mixer file for firefly6 --- ROMFS/px4fmu_common/mixers/firefly6.aux.mix | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/mixers/firefly6.aux.mix b/ROMFS/px4fmu_common/mixers/firefly6.aux.mix index 9ed6eeed91..b0b4250926 100644 --- a/ROMFS/px4fmu_common/mixers/firefly6.aux.mix +++ b/ROMFS/px4fmu_common/mixers/firefly6.aux.mix @@ -1,4 +1,14 @@ -# mixer for the FireFly6 elevons +# mixer for the FireFly6 tilt mechansim servo, elevons and landing gear +======================================================================= + +Tilt mechanism servo mixer +--------------------------- +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 4 10000 10000 0 -10000 10000 + +Elevon mixers +------------- M: 2 O: 10000 10000 0 -10000 10000 S: 1 0 7500 7500 0 -10000 10000 @@ -8,3 +18,9 @@ M: 2 O: 10000 10000 0 -10000 10000 S: 1 0 7500 7500 0 -10000 10000 S: 1 1 -8000 -8000 0 -10000 10000 + +Landing gear mixer +------------------ +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 From d544ac09550814550e4fc07a8197f8fffb44d98d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 21 Apr 2015 17:45:29 +0200 Subject: [PATCH 260/268] Sumd: Better magic number for RSSI --- src/lib/rc/sumd.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/rc/sumd.c b/src/lib/rc/sumd.c index a98c986bb7..cea7790ec1 100644 --- a/src/lib/rc/sumd.c +++ b/src/lib/rc/sumd.c @@ -269,7 +269,7 @@ int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe uint8_t _cnt = *rx_count + 1; *rx_count = _cnt; - *rssi = 255; + *rssi = 100; /* received Channels */ if ((uint16_t)_rxpacket.length > max_chan_count) { From 09ae879b8262f5ae9b0cd23f48854d4730172ae2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 21 Apr 2015 17:46:21 +0200 Subject: [PATCH 261/268] RC input: Replace magic numbers with better numbers, cap output to 0-100 --- src/drivers/drv_rc_input.h | 2 +- src/modules/px4iofirmware/controls.c | 9 ++++++--- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index d44728a712..a24d8814fe 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -65,7 +65,7 @@ /** * Maximum RSSI value */ -#define RC_INPUT_RSSI_MAX 255 +#define RC_INPUT_RSSI_MAX 100 /** * @addtogroup topics diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index c9702c2ec6..ac004f2127 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -191,7 +191,10 @@ controls_tick() { unsigned mV = counts * 3300 / 4096; /* scale to 0..253 and lowpass */ - rssi = (rssi * 0.99f) + ((mV / 13) * 0.01f); + rssi = (rssi * 0.99f) + ((mV / (3300 / RC_INPUT_RSSI_MAX)) * 0.01f); + if (rssi > RC_INPUT_RSSI_MAX) { + rssi = RC_INPUT_RSSI_MAX; + } } } #endif @@ -223,11 +226,11 @@ controls_tick() { if (sbus_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; - unsigned sbus_rssi = 254; + unsigned sbus_rssi = RC_INPUT_RSSI_MAX; if (sbus_frame_drop) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP; - sbus_rssi = 100; + sbus_rssi = RC_INPUT_RSSI_MAX / 2; } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); } From 6bf0a2618bf5e2680f61c4d2a1258eed62103f0e Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 21 Apr 2015 12:31:08 -0700 Subject: [PATCH 262/268] Add support for board attribute to parse output This allows for writing parameter meta data which is specific to a board type --- Tools/px4params/dokuwikiout.py | 4 ++-- Tools/px4params/srcparser.py | 37 +++++++++++++++++++-------------- Tools/px4params/xmlout.py | 38 +++++++++++++++++++++------------- Tools/px_process_params.py | 7 ++++++- 4 files changed, 53 insertions(+), 33 deletions(-) diff --git a/Tools/px4params/dokuwikiout.py b/Tools/px4params/dokuwikiout.py index 77e0ef53db..28e487ea62 100644 --- a/Tools/px4params/dokuwikiout.py +++ b/Tools/px4params/dokuwikiout.py @@ -12,11 +12,11 @@ class DokuWikiTablesOutput(): result += "^ Name ^ Description ^ Min ^ Max ^ Default ^\n" result += "^ ::: ^ Comment ^^^^\n" for param in group.GetParams(): - code = param.GetFieldValue("code") + code = param.GetName() + def_val = param.GetDefault() name = param.GetFieldValue("short_desc") min_val = param.GetFieldValue("min") max_val = param.GetFieldValue("max") - def_val = param.GetFieldValue("default") long_desc = param.GetFieldValue("long_desc") if name == code: diff --git a/Tools/px4params/srcparser.py b/Tools/px4params/srcparser.py index 891db7ecd2..0d2413a75f 100644 --- a/Tools/px4params/srcparser.py +++ b/Tools/px4params/srcparser.py @@ -37,20 +37,30 @@ class Parameter(object): # Define sorting order of the fields priority = { - "code": 10, - "type": 9, + "board": 9, "short_desc": 8, "long_desc": 7, - "default": 6, "min": 5, "max": 4, "unit": 3, # all others == 0 (sorted alphabetically) } - def __init__(self): + def __init__(self, name, type, default = ""): self.fields = {} + self.name = name + self.type = type + self.default = default + def GetName(self): + return self.name + + def GetType(self): + return self.type + + def GetDefault(self): + return self.default + def SetField(self, code, value): """ Set named field value @@ -88,7 +98,7 @@ class SourceParser(object): re_is_a_number = re.compile(r'^-?[0-9\.]') re_remove_dots = re.compile(r'\.+$') - valid_tags = set(["group", "min", "max", "unit"]) + valid_tags = set(["group", "board", "min", "max", "unit"]) # Order of parameter groups priority = { @@ -177,15 +187,12 @@ class SourceParser(object): # Non-empty line outside the comment m = self.re_parameter_definition.match(line) if m: - tp, code, defval = m.group(1, 2, 3) + tp, name, defval = m.group(1, 2, 3) # Remove trailing type specifier from numbers: 0.1f => 0.1 if self.re_is_a_number.match(defval): defval = self.re_cut_type_specifier.sub('', defval) - param = Parameter() - param.SetField("code", code) - param.SetField("short_desc", code) - param.SetField("type", tp) - param.SetField("default", defval) + param = Parameter(name, tp, defval) + param.SetField("short_desc", name) # If comment was found before the parameter declaration, # inject its data into the newly created parameter. group = "Miscellaneous" @@ -211,11 +218,9 @@ class SourceParser(object): # Nasty code dup, but this will all go away soon, so quick and dirty (DonLakeFlyer) m = self.re_px4_parameter_definition.match(line) if m: - tp, code = m.group(1, 2) - param = Parameter() - param.SetField("code", code) - param.SetField("short_desc", code) - param.SetField("type", tp) + tp, name = m.group(1, 2) + param = Parameter(name, tp) + param.SetField("short_desc", name) # If comment was found before the parameter declaration, # inject its data into the newly created parameter. group = "Miscellaneous" diff --git a/Tools/px4params/xmlout.py b/Tools/px4params/xmlout.py index 89f495dc02..07cced4786 100644 --- a/Tools/px4params/xmlout.py +++ b/Tools/px4params/xmlout.py @@ -18,26 +18,36 @@ def indent(elem, level=0): class XMLOutput(): - def __init__(self, groups): + def __init__(self, groups, board): xml_parameters = ET.Element("parameters") xml_version = ET.SubElement(xml_parameters, "version") - xml_version.text = "2" + xml_version.text = "3" + last_param_name = "" + board_specific_param_set = False for group in groups: xml_group = ET.SubElement(xml_parameters, "group") xml_group.attrib["name"] = group.GetName() for param in group.GetParams(): - xml_param = ET.SubElement(xml_group, "parameter") - for code in param.GetFieldCodes(): - value = param.GetFieldValue(code) - if code == "code": - xml_param.attrib["name"] = value - elif code == "default": - xml_param.attrib["default"] = value - elif code == "type": - xml_param.attrib["type"] = value - else: - xml_field = ET.SubElement(xml_param, code) - xml_field.text = value + if (last_param_name == param.GetName() and not board_specific_param_set) or last_param_name != param.GetName(): + xml_param = ET.SubElement(xml_group, "parameter") + xml_param.attrib["name"] = param.GetName() + xml_param.attrib["default"] = param.GetDefault() + xml_param.attrib["type"] = param.GetType() + last_param_name = param.GetName() + for code in param.GetFieldCodes(): + value = param.GetFieldValue(code) + if code == "board": + if value == board: + board_specific_param_set = True + xml_field = ET.SubElement(xml_param, code) + xml_field.text = value + else: + xml_group.remove(xml_param) + else: + xml_field = ET.SubElement(xml_param, code) + xml_field.text = value + if last_param_name != param.GetName(): + board_specific_param_set = False indent(xml_parameters) self.xml_document = ET.ElementTree(xml_parameters) diff --git a/Tools/px_process_params.py b/Tools/px_process_params.py index 12128a997e..cb2202d529 100644 --- a/Tools/px_process_params.py +++ b/Tools/px_process_params.py @@ -65,6 +65,11 @@ def main(): metavar="FILENAME", help="Create XML file" " (default FILENAME: parameters.xml)") + parser.add_argument("-b", "--board", + nargs='?', + const="", + metavar="BOARD", + help="Board to create xml parameter xml for") parser.add_argument("-w", "--wiki", nargs='?', const="parameters.wiki", @@ -116,7 +121,7 @@ def main(): # Output to XML file if args.xml: print("Creating XML file " + args.xml) - out = xmlout.XMLOutput(param_groups) + out = xmlout.XMLOutput(param_groups, args.board) out.Save(args.xml) # Output to DokuWiki tables From 638be07c2cf6b02defd2e50a8b2539a2d5fe7359 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 21 Apr 2015 12:31:30 -0700 Subject: [PATCH 263/268] Use new @board attribute for ifdef's --- src/modules/sensors/sensor_params.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index d6ab637cdc..1bebea2060 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -960,6 +960,7 @@ PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000); /** * Scaling factor for battery voltage sensor on FMU v2. * + * @board CONFIG_ARCH_BOARD_PX4FMU_V2 * @group Battery Calibration */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); @@ -969,6 +970,7 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); * * For R70 = 133K, R71 = 10K --> scale = 1.8 * 143 / (4096*10) = 0.0063 * + * @board CONFIG_ARCH_BOARD_AEROCORE * @group Battery Calibration */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0063f); From 08123df83a595c33ed2c62267f3bc64af6090be7 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 21 Apr 2015 12:32:01 -0700 Subject: [PATCH 264/268] Remove PX4_PARAM_DEFINE_* usage to get better meta data --- src/modules/systemlib/circuit_breaker_params.c | 13 ++++++------- src/modules/systemlib/circuit_breaker_params.h | 7 ------- 2 files changed, 6 insertions(+), 14 deletions(-) delete mode 100644 src/modules/systemlib/circuit_breaker_params.h diff --git a/src/modules/systemlib/circuit_breaker_params.c b/src/modules/systemlib/circuit_breaker_params.c index e499ae27ac..e5cc034bc9 100644 --- a/src/modules/systemlib/circuit_breaker_params.c +++ b/src/modules/systemlib/circuit_breaker_params.c @@ -43,7 +43,6 @@ */ #include -#include /** * Circuit breaker for power supply check @@ -56,7 +55,7 @@ * @max 894281 * @group Circuit Breaker */ -PX4_PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK); +PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK, 0); /** * Circuit breaker for rate controller output @@ -69,7 +68,7 @@ PX4_PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK); * @max 140253 * @group Circuit Breaker */ -PX4_PARAM_DEFINE_INT32(CBRK_RATE_CTRL); +PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0); /** * Circuit breaker for IO safety @@ -81,7 +80,7 @@ PX4_PARAM_DEFINE_INT32(CBRK_RATE_CTRL); * @max 22027 * @group Circuit Breaker */ -PX4_PARAM_DEFINE_INT32(CBRK_IO_SAFETY); +PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0); /** * Circuit breaker for airspeed sensor @@ -93,7 +92,7 @@ PX4_PARAM_DEFINE_INT32(CBRK_IO_SAFETY); * @max 162128 * @group Circuit Breaker */ -PX4_PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK); +PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0); /** * Circuit breaker for flight termination @@ -106,7 +105,7 @@ PX4_PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK); * @max 121212 * @group Circuit Breaker */ -PX4_PARAM_DEFINE_INT32(CBRK_FLIGHTTERM); +PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212); /** * Circuit breaker for engine failure detection @@ -120,4 +119,4 @@ PX4_PARAM_DEFINE_INT32(CBRK_FLIGHTTERM); * @max 284953 * @group Circuit Breaker */ -PX4_PARAM_DEFINE_INT32(CBRK_ENGINEFAIL); +PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953); diff --git a/src/modules/systemlib/circuit_breaker_params.h b/src/modules/systemlib/circuit_breaker_params.h deleted file mode 100644 index 768bf7f533..0000000000 --- a/src/modules/systemlib/circuit_breaker_params.h +++ /dev/null @@ -1,7 +0,0 @@ -#define PARAM_CBRK_SUPPLY_CHK_DEFAULT 0 -#define PARAM_CBRK_RATE_CTRL_DEFAULT 0 -#define PARAM_CBRK_IO_SAFETY_DEFAULT 0 -#define PARAM_CBRK_AIRSPD_CHK_DEFAULT 0 -#define PARAM_CBRK_FLIGHTTERM_DEFAULT 121212 -#define PARAM_CBRK_ENGINEFAIL_DEFAULT 284953 -#define PARAM_CBRK_GPSFAIL_DEFAULT 240024 From a885c2c8c9739459361bb7f355a25e0961bb20e5 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 21 Apr 2015 12:32:15 -0700 Subject: [PATCH 265/268] Parameter meta data is not typed to board --- makefiles/firmware.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index 4c10de931c..af3ca249e5 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -494,7 +494,7 @@ $(filter %.S.o,$(OBJS)): $(WORK_DIR)%.S.o: %.S $(GLOBAL_DEPS) $(PRODUCT_BUNDLE): $(PRODUCT_BIN) @$(ECHO) %% Generating $@ ifdef GEN_PARAM_XML - python $(PX4_BASE)/Tools/px_process_params.py --src-path $(PX4_BASE)/src --xml + python $(PX4_BASE)/Tools/px_process_params.py --src-path $(PX4_BASE)/src --board CONFIG_ARCH_BOARD_$(CONFIG_BOARD) --xml $(Q) $(MKFW) --prototype $(IMAGE_DIR)/$(BOARD).prototype \ --git_identity $(PX4_BASE) \ --parameter_xml $(PRODUCT_PARAMXML) \ From 5e584c2942a3087b5250003d05917db8769f7789 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 22 Apr 2015 09:27:03 +0200 Subject: [PATCH 266/268] commander: Better low battery failure feedback --- src/modules/commander/commander.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 948543714f..e0634bb348 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1602,7 +1602,11 @@ int commander_thread_main(int argc, char *argv[]) if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; + mavlink_and_console_log_critical(mavlink_fd, "LOW BATTERY, LOCKING ARMING DOWN"); } + + } else { + mavlink_and_console_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY"); } status_changed = true; From 5b772e5720ed6bc8f90316dc7632b218819543b7 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 22 Apr 2015 09:53:09 +0200 Subject: [PATCH 267/268] update vehicle status before doing preflight checks --- src/modules/commander/commander.cpp | 21 ++++++++------------- src/modules/commander/commander_helper.cpp | 5 +++++ src/modules/commander/commander_helper.h | 1 + 3 files changed, 14 insertions(+), 13 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e0634bb348..65fc8f90e8 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1125,6 +1125,10 @@ int commander_thread_main(int argc, char *argv[]) commander_initialized = true; thread_running = true; + /* update vehicle status to find out vehicle type (required for preflight checks) */ + param_get(_param_sys_type, &(status.system_type)); // get system type + status.is_rotary_wing = is_rotary_wing(&status) || is_vtol(&status); + bool checkAirspeed = false; /* Perform airspeed check only if circuit breaker is not * engaged and it's not a rotary wing */ @@ -1204,15 +1208,7 @@ int commander_thread_main(int argc, char *argv[]) } /* disable manual override for all systems that rely on electronic stabilization */ - if (status.system_type == vehicle_status_s::VEHICLE_TYPE_COAXIAL || - status.system_type == vehicle_status_s::VEHICLE_TYPE_HELICOPTER || - status.system_type == vehicle_status_s::VEHICLE_TYPE_TRICOPTER || - status.system_type == vehicle_status_s::VEHICLE_TYPE_QUADROTOR || - status.system_type == vehicle_status_s::VEHICLE_TYPE_HEXAROTOR || - status.system_type == vehicle_status_s::VEHICLE_TYPE_OCTOROTOR || - (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR && vtol_status.vtol_in_rw_mode) || - (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR && vtol_status.vtol_in_rw_mode)) { - + if (is_rotary_wing(&status) || (is_vtol(&status) && vtol_status.vtol_in_rw_mode)) { status.is_rotary_wing = true; } else { @@ -1220,8 +1216,7 @@ int commander_thread_main(int argc, char *argv[]) } /* set vehicle_status.is_vtol flag */ - status.is_vtol = (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR) || - (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR); + status.is_vtol = is_vtol(&status); /* check and update system / component ID */ param_get(_param_system_id, &(status.system_id)); @@ -1422,8 +1417,8 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status); status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab; - /* Make sure that this is only adjusted if vehicle realy is of type vtol*/ - if ((status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR) || (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR)) { + /* Make sure that this is only adjusted if vehicle really is of type vtol*/ + if (is_vtol(&status)) { status.is_rotary_wing = vtol_status.vtol_in_rw_mode; } } diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index a5e4d19725..5f735b7b7c 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -84,6 +84,11 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status) || (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_COAXIAL); } +bool is_vtol(const struct vehicle_status_s * current_status) { + return current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR || + current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR; +} + static int buzzer = -1; static hrt_abstime blink_msg_end = 0; // end time for currently blinking LED message, 0 if no blink message static hrt_abstime tune_end = 0; // end time of currently played tune, 0 for repeating tunes or silence diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index 0cefedba7c..bf0c0505d2 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -51,6 +51,7 @@ bool is_multirotor(const struct vehicle_status_s *current_status); bool is_rotary_wing(const struct vehicle_status_s *current_status); +bool is_vtol(const struct vehicle_status_s *current_status); int buzzer_init(void); void buzzer_deinit(void); From 67956341e68824942cccac5c5c2c6c425a9c4d97 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 21 Apr 2015 14:59:08 +0200 Subject: [PATCH 268/268] FireFly6: make landing gear manual pass-through --- ROMFS/px4fmu_common/mixers/firefly6.aux.mix | 2 +- src/modules/vtol_att_control/vtol_att_control_main.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/firefly6.aux.mix b/ROMFS/px4fmu_common/mixers/firefly6.aux.mix index b0b4250926..fda8416403 100644 --- a/ROMFS/px4fmu_common/mixers/firefly6.aux.mix +++ b/ROMFS/px4fmu_common/mixers/firefly6.aux.mix @@ -23,4 +23,4 @@ Landing gear mixer ------------------ M: 1 O: 10000 10000 0 -10000 10000 -S: 0 7 10000 10000 0 -10000 10000 +S: 3 6 10000 10000 0 -10000 10000 diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index d9425f2bb3..2ae10bd274 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -765,7 +765,7 @@ void VtolAttitudeControl::task_main() vehicle_battery_poll(); - if (_manual_control_sp.aux1 <= 0.0f) { /* vehicle is in mc mode */ + if (_manual_control_sp.aux1 < 0.0f) { /* vehicle is in mc mode */ _vtol_vehicle_status.vtol_in_rw_mode = true; if (!flag_idle_mc) { /* we want to adjust idle speed for mc mode */