Re: [Paparazzi-devel] force feedback

[address@hidden]

Very cool Martin!  But I'm concerned that you may damage your fancy joystick 
with those harsh navigation commands :o

Anton and I had implemented a sort of threshold slewing filter on the 
navigation to prevent damage to the joystick/airplane when changing blocks or 
when flight plan segments are not perfectly aligned.  It was implemented in an 
optional, complicated, and tunable manner though we never found a need to tune 
it or opt out.  I think it should be "hard coded" so that all can benefit 
without the need for another undocumented variable and it could probably be 
replaced by a more traditional low pass filter like: 
if (launch) {nav_err = Min(nav_err, prev_nav_err + .01)

Anyway, the theory is that any course errors that would result in more than 50% 
of the max allowable navigation should produce an immediate response to the 
first 50% only, then a slow (~1 sec) response to the remainder.  We used 
H_CTL_COURSE_SLEW_INCREMENT = 0.035

#ifdef H_CTL_COURSE_SLEW_INCREMENT
  /* slew severe course changes (i.e. waypoint moves, block changes or 
perpendicular routes) */
  static float h_ctl_course_slew_rate = 0.;
  float nav_angle_saturation = -(h_ctl_roll_max_setpoint/h_ctl_course_pgain);  
/* heading error corresponding to max_roll */
  float half_nav_angle_saturation = nav_angle_saturation / 2.;
  if (launch) {  /* prevent accumulator run-up on the ground */
    if (err > half_nav_angle_saturation) {
      h_ctl_course_slew_rate = Max(h_ctl_course_slew_rate, 0.);
      err = Min(err,(half_nav_angle_saturation + h_ctl_course_slew_rate));
      h_ctl_course_slew_rate +=h_ctl_course_slew_increment;
    }
    else if ( err < -half_nav_angle_saturation) {
      h_ctl_course_slew_rate = Min(h_ctl_course_slew_rate, 0.);
      err = Max(err,(-half_nav_angle_saturation + h_ctl_course_slew_rate));
      h_ctl_course_slew_rate -=h_ctl_course_slew_increment;
    }
    else {
      h_ctl_course_slew_rate = 0.;
    }
  }
#endif


Also, along a similar note.  The reason Mohammad's plane suddenly rolls right 
and crashes at exactly 20m altitude is because the aggressive climb navigation 
gain limiting was "unblended" during the tragic breakup of control.c.  Our 
planes were doing the same thing - fw_h_ctrl.c line 200 should be:

#if defined  AGR_CLIMB 
  /** limit navigation during extreme altitude changes */
  if (AGR_BLEND_START > AGR_BLEND_END && AGR_BLEND_END > 0) { /* prevent divide 
by zero, reversed or negative values */
    if (v_ctl_auto_throttle_submode == V_CTL_AUTO_THROTTLE_AGRESSIVE || 
V_CTL_AUTO_THROTTLE_BLENDED) {
      altitude_error = estimator_z - v_ctl_altitude_setpoint;
      BoundAbs(cmd, h_ctl_roll_max_setpoint);  /* bound cmd before NAV_RATIO 
and again after */
      if (altitude_error < 0) {
      nav_ratio = AGR_CLIMB_NAV_RATIO + (1 - AGR_CLIMB_NAV_RATIO) * (1 - 
(fabs(altitude_error) - AGR_BLEND_END) / (AGR_BLEND_START - AGR_BLEND_END));
      Bound (nav_ratio, AGR_CLIMB_NAV_RATIO, 1);
      }
      else if (altitude_error > 0) {
        nav_ratio = AGR_DESCENT_NAV_RATIO + (1 - AGR_DESCENT_NAV_RATIO) * (1 - 
(fabs(altitude_error) - AGR_BLEND_END) / (AGR_BLEND_START - AGR_BLEND_END));
      Bound (nav_ratio, AGR_DESCENT_NAV_RATIO, 1);
      } 
      cmd *= nav_ratio;
    }
  }

#endif



-- address@hidden wrote:


another way of visualizing the autopilots work

http://www.youtube.com/watch?v=ftki1p5iW1c





_______________________________________________
Paparazzi-devel mailing list
address@hidden
http://lists.nongnu.org/mailman/listinfo/paparazzi-devel
_____________________________________________________________
Boost your productivity with new office software. Click now!
http://thirdpartyoffers.juno.com/TGL2121/fc/Ioyw6i3mEauV05ClgBdlUpSkxNG3umPxXrfptQIouvAcUX6eG44lrH/?count=1234567890