Changeset 177 in flair-src for trunk

Timestamp:

Apr 27, 2017, 5:49:17 PM (8 years ago)

Author:

Sanahuja Guillaume

Message:

use current zvel when switching from starttakingoff to stabilized into the traj generator

Location:

trunk/lib

Files:

• FlairFilter/src/TrajectoryGenerator1D.cpp (modified) (1 diff)
• FlairFilter/src/TrajectoryGenerator1D.h (modified) (1 diff)
• FlairFilter/src/TrajectoryGenerator1D_impl.cpp (modified) (2 diffs)
• FlairFilter/src/unexported/TrajectoryGenerator1D_impl.h (modified) (1 diff)
• FlairMeta/src/UavStateMachine.cpp (modified) (1 diff)

trunk/lib/FlairFilter/src/TrajectoryGenerator1D.cpp

@@ -41 +41 @@
 cvmatrix *TrajectoryGenerator1D::Matrix(void) const { return pimpl_->output; }
 
-void TrajectoryGenerator1D::StartTraj(float start_pos, float end_pos) {
-  pimpl_->StartTraj(start_pos, end_pos);
+void TrajectoryGenerator1D::StartTraj(float startPosition, float endPosition,float startVelocity) {
+  pimpl_->StartTraj(startPosition,endPosition,startVelocity);
 }
 

trunk/lib/FlairFilter/src/TrajectoryGenerator1D.h

@@ -72 +72 @@
   * \brief Start trajectory
   *
-  * \param start_pos start position
-  * \param end_pos end position
+  * \param startPosition start position
+  * \param endPosition end position
+  * \param startVelocity start velocity, should be actual velocity. This value will be saturated 
+  * max velocity defined in GCS.
   */
-  void StartTraj(float start_pos, float end_pos);
+  void StartTraj(float startPosition, float endPosition,float startVelocity=0);
 
   /*!

trunk/lib/FlairFilter/src/TrajectoryGenerator1D_impl.cpp

@@ -79 +79 @@
 }
 
-void TrajectoryGenerator1D_impl::StartTraj(float start_pos, float end_pos) {
+void TrajectoryGenerator1D_impl::StartTraj(float start_pos, float end_pos,float startVelocity) {
   is_started = true;
   is_finished = false;
@@ -88 +88 @@
   pos = start_pos;
   acc = acceleration->Value();
-  v = 0;
+  v = startVelocity;
+  if (fabs(v) > fabs(max_veloctity->Value())) {
+    if (v > 0)
+      v = max_veloctity->Value();
+    else
+      v = -max_veloctity->Value();
+  }
   if (end_position < start_pos) {
     acc = -acc;

trunk/lib/FlairFilter/src/unexported/TrajectoryGenerator1D_impl.h

@@ -35 +35 @@
   ~TrajectoryGenerator1D_impl();
   void Update(flair::core::Time time);
-  void StartTraj(float start_pos, float end_pos);
+  void StartTraj(float start_pos, float end_pos,float startVelocity);
   void StopTraj(void);
   void Reset(void);

trunk/lib/FlairMeta/src/UavStateMachine.cpp

@@ -279 +279 @@
     if (currentAltitude > groundAltitude + 0.03) {
       altitudeTrajectory->StartTraj(currentAltitude,
-                                    desiredTakeoffAltitude->Value());
+                                    desiredTakeoffAltitude->Value(),currentVerticalSpeed);
       altitudeState = AltitudeState_t::Stabilized;
       SignalEvent(Event_t::Stabilized);