CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/Rover/cruise_learn.cpp
Views: 1798
1
#include "Rover.h"

2


3
// start cruise throttle and speed learning

4
void Rover::cruise_learn_start()

5
{

6
    // if disarmed or no speed available do nothing

7
    float speed;

8
    if (!arming.is_armed() || !g2.attitude_control.get_forward_speed(speed)) {

9
        cruise_learn.learn_start_ms = 0;

10
        gcs().send_text(MAV_SEVERITY_CRITICAL, "Cruise Learning NOT started");

11
        return;

12
    }

13
    // start learning

14
    cruise_learn.speed_filt.reset(speed);

15
    cruise_learn.throttle_filt.reset(g2.motors.get_throttle());

16
    cruise_learn.learn_start_ms = AP_HAL::millis();

17
    cruise_learn.log_count = 0;

18
#if HAL_LOGGING_ENABLED

19
    log_write_cruise_learn();

20
#endif

21
    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
void Rover::cruise_learn_update()

27
{

28
    float speed;

29
    if (cruise_learn.learn_start_ms > 0 && g2.attitude_control.get_forward_speed(speed)) {

30
        // update filters with latest speed and throttle

31
        cruise_learn.speed_filt.apply(speed, 0.02f);

32
        cruise_learn.throttle_filt.apply(g2.motors.get_throttle(), 0.02f);

33
        // 10Hz logging

34
#if HAL_LOGGING_ENABLED

35
        if (cruise_learn.log_count % 5 == 0) {

36
            log_write_cruise_learn();

37
        }

38
        cruise_learn.log_count += 1;

39
#endif

40
        // check how long it took to learn

41
        if (AP_HAL::millis() - cruise_learn.learn_start_ms >= 2000) {

42
            cruise_learn_complete();

43
        }

44
        return;

45
    }

46
}

47


48
// complete cruise learning and save results

49
void Rover::cruise_learn_complete()

50
{

51
    // when switch is moved low, save learned cruise value

52
    if (cruise_learn.learn_start_ms > 0) {

53
        const float thr = cruise_learn.throttle_filt.get();

54
        const float speed = cruise_learn.speed_filt.get();

55
        if (thr >= 10.0f && thr <= 100.0f && is_positive(speed)) {

56
            g.throttle_cruise.set_and_save(thr);

57
            g.speed_cruise.set_and_save(speed);

58
            gcs().send_text(MAV_SEVERITY_CRITICAL, "Cruise Learned: Thr:%d Speed:%3.1f", (int)g.throttle_cruise, (double)g.speed_cruise);

59
        } else {

60
            gcs().send_text(MAV_SEVERITY_CRITICAL, "Cruise Learning failed");

61
        }

62
        cruise_learn.learn_start_ms = 0;

63
#if HAL_LOGGING_ENABLED

64
        log_write_cruise_learn();

65
#endif

66
    }

67
}

68


69
#if HAL_LOGGING_ENABLED

70
// logging for cruise learn

71
void Rover::log_write_cruise_learn() const

72
{

73
// @LoggerMessage: CRSE

74
// @Description: Cruise Learn messages

75
// @URL: https://ardupilot.org/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
    AP::logger().WriteStreaming("CRSE", "TimeUS,State,Speed,Throttle", "Qbff",

82
                       AP_HAL::micros64(),

83
                       cruise_learn.learn_start_ms > 0,

84
                       cruise_learn.speed_filt.get(),

85
                       cruise_learn.throttle_filt.get());

86
}

87
#endif

88


89