Line data    Source code

       1             : #include "Rover.h"
       2             : 
       3             : // start cruise throttle and speed learning
       4           0 : void Rover::cruise_learn_start()
       5             : {
       6             :     // if disarmed or no speed available do nothing
       7             :     float speed;
       8           0 :     if (!arming.is_armed() || !g2.attitude_control.get_forward_speed(speed)) {
       9           0 :         cruise_learn.learn_start_ms = 0;
      10           0 :         GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Cruise Learning NOT started");
      11           0 :         return;
      12             :     }
      13             :     // start learning
      14           0 :     cruise_learn.speed_filt.reset(speed);
      15           0 :     cruise_learn.throttle_filt.reset(g2.motors.get_throttle());
      16           0 :     cruise_learn.learn_start_ms = AP_HAL::millis();
      17           0 :     cruise_learn.log_count = 0;
      18             : #if HAL_LOGGING_ENABLED
      19           0 :     log_write_cruise_learn();
      20             : #endif
      21           0 :     GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Cruise Learning started");
      22             : }
      23             : 
      24             : // update cruise learning with latest speed and throttle
      25             : // should be called at 50hz
      26      628801 : void Rover::cruise_learn_update()
      27             : {
      28             :     float speed;
      29      628801 :     if (cruise_learn.learn_start_ms > 0 && g2.attitude_control.get_forward_speed(speed)) {
      30             :         // update filters with latest speed and throttle
      31           0 :         cruise_learn.speed_filt.apply(speed, 0.02f);
      32           0 :         cruise_learn.throttle_filt.apply(g2.motors.get_throttle(), 0.02f);
      33             :         // 10Hz logging
      34             : #if HAL_LOGGING_ENABLED
      35           0 :         if (cruise_learn.log_count % 5 == 0) {
      36           0 :             log_write_cruise_learn();
      37             :         }
      38           0 :         cruise_learn.log_count += 1;
      39             : #endif
      40             :         // check how long it took to learn
      41           0 :         if (AP_HAL::millis() - cruise_learn.learn_start_ms >= 2000) {
      42           0 :             cruise_learn_complete();
      43             :         }
      44           0 :         return;
      45             :     }
      46             : }
      47             : 
      48             : // complete cruise learning and save results
      49           0 : void Rover::cruise_learn_complete()
      50             : {
      51             :     // when switch is moved low, save learned cruise value
      52           0 :     if (cruise_learn.learn_start_ms > 0) {
      53           0 :         const float thr = cruise_learn.throttle_filt.get();
      54           0 :         const float speed = cruise_learn.speed_filt.get();
      55           0 :         if (thr >= 10.0f && thr <= 100.0f && is_positive(speed)) {
      56           0 :             g.throttle_cruise.set_and_save(thr);
      57           0 :             g.speed_cruise.set_and_save(speed);
      58           0 :             GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Cruise Learned: Thr:%d Speed:%3.1f", (int)g.throttle_cruise, (double)g.speed_cruise);
      59             :         } else {
      60           0 :             GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Cruise Learning failed");
      61             :         }
      62           0 :         cruise_learn.learn_start_ms = 0;
      63             : #if HAL_LOGGING_ENABLED
      64           0 :         log_write_cruise_learn();
      65             : #endif
      66             :     }
      67           0 : }
      68             : 
      69             : #if HAL_LOGGING_ENABLED
      70             : // logging for cruise learn
      71           0 : void Rover::log_write_cruise_learn() const
      72             : {
      73             : // @LoggerMessage: CRSE
      74             : // @Description: Cruise Learn messages
      75             : // @URL: https://ardupilot.club/rover/docs/rover-tuning-throttle-and-speed.html
      76             : // @Field: TimeUS: Time since system startup
      77             : // @Field: State: True if Cruise Learn has started
      78             : // @Field: Speed: Determined target Cruise speed in auto missions
      79             : // @Field: Throttle: Determined base throttle percentage to be used in auto missions 
      80             : 
      81           0 :     AP::logger().WriteStreaming("CRSE", "TimeUS,State,Speed,Throttle", "Qbff",
      82             :                        AP_HAL::micros64(),
      83           0 :                        cruise_learn.learn_start_ms > 0,
      84           0 :                        cruise_learn.speed_filt.get(),
      85           0 :                        cruise_learn.throttle_filt.get());
      86           0 : }
      87             : #endif