Implement assisted autorotation for Heli’s #22871

bnsgeyer · 2023-02-08T22:08:08Z

This PR incorporates helicopter auto rotation, where it assists the pilot in the vertical axis and maintaining speed from entry all the way to touchdown. The pilot still maintains control of the lateral and directional axes to guide the aircraft to a safe touchdown point.

ArduCopter/Copter.cpp

ArduCopter/mode_autorotate.cpp

IamPete1

White space needs fixing throughout.

Guided stuff is a little odd taking roll only, what the use case?

Speeding up the rangefinder read probably has subtle unintended consequences, things like filters now having a different frequency due to the DT change.

bnsgeyer · 2023-02-27T18:50:45Z

@IamPete1 the idea behind the guided stuff is that the autorotational controller will control the vertical and pitch axes to control forward speed and descent rate. So the pilot or autopilot only needs to control the roll and yaw axes

IamPete1 · 2023-02-27T19:28:20Z

The guided stuff is adding support for setting roll via the SET_ATTITUDE_TARGET MAVLink message? Is that how the pilot is controlling rather than via RC? As I say its a little odd.

bnsgeyer · 2023-02-27T19:48:21Z

The guided stuff is adding support for setting roll via the SET_ATTITUDE_TARGET MAVLink message? Is that how the pilot is controlling rather than via RC? As I say its a little odd.

Ok. Good point.
@Ferruccio1984 what is the purpose of the guided mode stuff that you added?

Ferruccio1984 · 2023-02-27T20:45:01Z

Hi @bnsgeyer and @IamPete1 , the reason behind the guided stuff is BVLOS operation. In my use case most of the flight is performed in guided mode and the operator behind the Ground Station has a joystick for manual take-over ( which happens still in guided flight mode, joystick inputs are translated into attitude targets). Following this logic the operator is able to steer the helicopter also in case of autorotation, choosing the best landing spot through live Fpv feedback from the drone. The guided stuff is optional, so I coded it to allow traditional RC control or guided control.

IamPete1 · 2023-02-27T22:43:23Z

I think your probability better to drop the guided stuff from this PR and move it to a future one. As I say its a little odd and removing it make this PR more self contained in heli and auto-rotation code so will need less testing.

I think the same thing is probably true of the rangefinder speed. I think you would at least need to do something to rate limit logs. Lots of the back ends do averaging, you may be able to get it to work just as well by providing a new handle to get the min distance or just the latest in the last loop. Removing the averageing will remove 25ms worth of lag.

tridge · 2023-02-28T00:18:48Z

it is worth looking at the XKF5.HAGL value as a possible height above ground

bnsgeyer · 2023-02-28T00:23:09Z

@Ferruccio1984 See @tridge suggestion above for a EKF signal that could provide a higher quality height estimate that is based off of the rangefinder and other sources including providing some reduction in lag that you will see with rangefinders. If it looks suitable then we can add it in and remove the code that changes the the scheduler rate.

I will pull out the guided mode stuff and clean up the indentation problems.

Ferruccio1984 · 2023-02-28T10:13:15Z

it is worth looking at the XKF5.HAGL value as a possible height above ground

Hi @tridge , I had a look again at last summer test flights, comparing the rangefinder data ( scheduler rate of 100Hz) and the HAGL:

bnsgeyer · 2023-03-01T11:42:04Z

@tridge based on the lag that we are seeing on the HAGL signal, I don’t think it is usable for this application. Please post a link to your branch that uses a complementary filter. Thanks!

…ion, change all measurements to height above ground to be the same variable.

…der measurement for heigh above ground in the autorotation lib. We don't need to check for range finder out of range because inertial nave will interpolate for this case and we check against ground clearance in the autorotation lib.

…t phase

…ve now needed variables and functions.

…read and to ensure flight phase progression based on heights. Also flip flag bools to be init checks.

…e not used elsewhere in the code.

…remove unused define.

…tion

…ptions

…onal

…e and utilise a timer

Ferruccio1984 · 2024-02-10T15:39:36Z

libraries/AC_Autorotation/AC_Autorotation.cpp

+    // driven the collective high to cushion the landing so the landing detector will not trip until we drive the collective back to zero thrust and below.
+    if ((_time_on_ground > 0) && ((AP_HAL::millis() - _time_on_ground) > MIN_TIME_ON_GROUND)) {
+       // On ground, smoothly lower collective to just bellow zero thrust, to make sure we trip the landing detector
+       float desired_col = _motors_heli->get_coll_mid() * 0.95;


This needs changed to use H_COL_LAND_MIN instead of zero thrust collective, since the landing detector looks for "motors->get_below_land_min_coll()"

IamPete1

There are quite a few "lucky dip" units varables. As a rule if its not SI units we append the unit the variable/define. Also knowing we will be making some effort to more to SI units in the future it would be nice to bring this in using SI as much as possible.

IamPete1 · 2024-02-13T23:04:31Z

ArduCopter/mode.cpp

+    if (!pos_control->is_active_xy() && use_psc_case) {
        return copter.current_loc.alt;
    }


I suspect we can just remove this whole section. I don't understand why it would just return current loc rather than trying to get the terrain value. As far as I can tell the only user that is not in position control is landing gear.

In anycase, removing this section means you can also remove the new bool.

IamPete1 · 2024-02-13T23:06:49Z

ArduCopter/mode.h

-
-    //--- Internal functions ---
-    void warning_message(uint8_t message_n);    //Handles output messages to the terminal
+    uint32_t _last_bad_rpm_ms;


Its a little odd having this bad rpm time stamp in a different structure to the bad_rpm flag. Maybe combine:

struct { uint32_t last_ms; bool bad; } _rpm;

IamPete1 · 2024-02-13T23:11:19Z

libraries/AC_Autorotation/AC_Autorotation.h

+    // Update the touchdown controller
+    void touchdown_controller(void);
+
+    void set_ground_distance(float gnd_dist) { _gnd_hgt = (float)gnd_dist; }


Suggested change

void set_ground_distance(float gnd_dist) { _gnd_hgt = (float)gnd_dist; }

void set_ground_distance(float gnd_dist) { _gnd_hgt = gnd_dist; }

IamPete1 · 2024-02-13T23:18:49Z

ArduCopter/mode_autorotate.cpp

-    g2.arot.init_hs_controller();
-    g2.arot.init_fwd_spd_controller();
+    // Init autorotation controllers object
+    g2.arot.init(motors, copter.rangefinder.ground_clearance_cm_orient(ROTATION_PITCH_270));


Passing in ground clearance here is quite odd. It means that we use the range finder ground clearance for a rangefinder that we may not even be using for the altitude estimate.

I suspect we can just remove for now. Then we can come back and include ground clearance at the copter level before we pass the height above ground estimate down to the arot lib. That way it can only apply the the offset when using the rangefinder.

Also its a bit mad passing in motors in the mode init. You should be able to pass a const reference in the constructor.

IamPete1 · 2024-02-13T23:19:14Z

ArduCopter/mode_autorotate.cpp

-    _flags.straight_ahead_initial = true;
-    _flags.bail_out_initial = true;
-    _msg_flags.bad_rpm = true;
+    // Set all intial flags to on


Suggested change

// Set all intial flags to on

// Set all intial flags off

IamPete1 · 2024-02-13T23:52:43Z

libraries/AC_Autorotation/AC_Autorotation.cpp

@@ -251,54 +321,121 @@ float AC_Autorotation::get_rpm(bool update_counter)
    return current_rpm;
 }

+void AC_Autorotation::initial_flare_estimate()
+{


This bit is much better, but still quite scary.

IamPete1 · 2024-02-13T23:53:59Z

libraries/AC_Autorotation/AC_Autorotation.cpp

-                        (double)_accel_target,
-                        (double)_pitch_target);
+                                "TimeUS,hsp,hse,co,cff,sf,dsf,vp,vff,az,dvz,h",
+                                "Qfffffffffff",


It would be nice to add units and multipliers too.

IamPete1 · 2024-02-13T23:55:38Z

libraries/AC_Autorotation/AC_Autorotation.cpp

+
+    // Check to see if we think the aircraft is on the ground
+    if(_current_sink_rate < 10.0 && _gnd_hgt <= _ground_clearance && _time_on_ground == 0){
+        _time_on_ground = AP_HAL::millis();


you could const uint32_t now = AP_HAL::millis(); Its used again on 673

IamPete1 · 2024-02-13T23:58:52Z

libraries/AC_Autorotation/AC_Autorotation.cpp

+}
+
+
+void AC_Autorotation::update_avg_acc_z(void)


This makes me nervous, it will work differently with different loop rates. I would prefer a low pass. If average is better there is a AverageFilter class that you should use. https://github.com/ArduPilot/ardupilot/blob/master/libraries/Filter/AverageFilter.h

IamPete1 · 2024-02-14T00:00:41Z

libraries/AC_Autorotation/AC_Autorotation.h

+    //--------References to Other Libraries--------
+    AP_InertialNav&    _inav;
+    AP_AHRS&           _ahrs;
+    AP_MotorsHeli*     _motors_heli;


Suggested change

AP_MotorsHeli* _motors_heli;

AP_MotorsHeli*& _motors_heli;

You can refence the copter pointer and set it in the constructor. You will have to check for nullprt before use.

… to prevent a race on mode init.

…n blade pitch angle

…ermine unhealthy. Col defaults to ang of -2 if no RPM

post touchdown condition requires collective to be lowered below land_col_min_pct in order to get the disarm logic to disarm the aircraft

-collective gets lowered below land_col_min_pct in order to get the aircraft disarmed -adjusted thresholds of lfare update and flare complete check

bnsgeyer added Enhancement TradHeli labels Feb 8, 2023

bnsgeyer self-assigned this Feb 8, 2023

bnsgeyer force-pushed the pr-autorotation branch 5 times, most recently from 6d4ddd6 to d87df0c Compare February 13, 2023 23:41

bnsgeyer force-pushed the pr-autorotation branch from d87df0c to a67d3c9 Compare February 18, 2023 18:34

bnsgeyer added the DevCallTopic label Feb 26, 2023

bnsgeyer requested review from MattKear and rmackay9 February 26, 2023 17:03

IamPete1 reviewed Feb 27, 2023

View reviewed changes

ArduCopter/Copter.cpp Outdated Show resolved Hide resolved

IamPete1 reviewed Feb 27, 2023

View reviewed changes

ArduCopter/mode_autorotate.cpp Outdated Show resolved Hide resolved

IamPete1 reviewed Feb 27, 2023

View reviewed changes

ArduCopter/mode_autorotate.cpp Outdated Show resolved Hide resolved

IamPete1 reviewed Feb 27, 2023

View reviewed changes

ArduCopter/mode_autorotate.cpp Outdated Show resolved Hide resolved

IamPete1 requested changes Feb 27, 2023

View reviewed changes

Hwurzburg added the WikiNeeded needs wiki update label Feb 28, 2023

CraigElder removed the DevCallTopic label Feb 28, 2023

bnsgeyer force-pushed the pr-autorotation branch 2 times, most recently from b9871c9 to 3870277 Compare June 30, 2023 03:27

MattKear added 18 commits February 8, 2024 22:15

AC_Autorotation: Let copter do all of the height above ground estimat…

ee06b79

…ion, change all measurements to height above ground to be the same variable.

Copter: Autorotation Mode: Make the hover autorotation it's own fligh…

c2b8589

…t phase

AC_Autorotation: Add helpers for state machine logic tidy up and remo…

ee7b895

…ve now needed variables and functions.

Copter: Autorotation Mode: Rework logic machine to make it easier to …

a42288d

…read and to ensure flight phase progression based on heights. Also flip flag bools to be init checks.

AC_Autorotation: Make param defaults numeric values if the defines ar…

29c2d35

…e not used elsewhere in the code.

AC_Autorotation: Added Comments and whitespace fixups

ad64639

AC_Autorotation: Fix data type for time_on_ground

f101a21

Copter: Autorotation Mode: Whitespace changes, comments updated, and …

61c6e60

…remove unused define.

Copter: Autorotation Mode: Use standard earth_to_body2D velocity rota…

0798ca6

…tion

AC_Autorotation: Add options param and give roll-yaw control scheme o…

ba3fbaf

…ptions

Copter: Autorotation mode: Make control schemes for roll and yaw opti…

a1b0725

…onal

AC_Autorotation: Remove sink derivative and only use filtered accel.

3384684

Copter: Autorotation Mode: Remove calls to update derive sink rate

64829ac

Copter: Autorotation mode: Simplify rpm warning message code structur…

72553f7

…e and utilise a timer

Copter: Autorotation Mode: Update comments and white space change

7fddf29

AC_Autorotation: update comments

bbebd62

AC_Autorotation: Remove unused variables

a895d15

Ferruccio1984 reviewed Feb 10, 2024

View reviewed changes

IamPete1 reviewed Feb 14, 2024

View reviewed changes

MattKear and others added 8 commits February 15, 2024 22:58

AC_Autorotation: fix options param index

aaee063

Copter: Always update the ground distance in the autorotation library…

9e110c9

… to prevent a race on mode init.

AP_Motors: Heli: Add helper to calculated norm collective from a give…

7fd07b4

…n blade pitch angle

AC_Autorotation: Simplify bad RPM handling, leaving to RPM lib to det…

533e202

…ermine unhealthy. Col defaults to ang of -2 if no RPM

Copter: Autorotation mode: remove bad rpm handling

6547e24

AP_Motors: temporary fix to prevent I-term decay during autorotations

6f3eff1

AP_Motors: return collective land position

2cde571

post touchdown condition requires collective to be lowered below land_col_min_pct in order to get the disarm logic to disarm the aircraft

AC_Autorotation: fix for post touchdown disarming

241959e

-collective gets lowered below land_col_min_pct in order to get the aircraft disarmed -adjusted thresholds of lfare update and flare complete check

bnsgeyer force-pushed the pr-autorotation branch from 7c8274a to 241959e Compare February 16, 2024 03:59

MattKear mentioned this pull request Feb 23, 2024

Heli: Create Copter Level Param Group for General Heli Params #26306

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Implement assisted autorotation for Heli’s #22871

Implement assisted autorotation for Heli’s #22871

bnsgeyer commented Feb 8, 2023

IamPete1 left a comment •

edited

Loading

bnsgeyer commented Feb 27, 2023

IamPete1 commented Feb 27, 2023

bnsgeyer commented Feb 27, 2023

Ferruccio1984 commented Feb 27, 2023

IamPete1 commented Feb 27, 2023 •

edited

Loading

tridge commented Feb 28, 2023

bnsgeyer commented Feb 28, 2023

Ferruccio1984 commented Feb 28, 2023 •

edited

Loading

bnsgeyer commented Mar 1, 2023

Ferruccio1984 Feb 10, 2024

IamPete1 left a comment

IamPete1 Feb 13, 2024

IamPete1 Feb 13, 2024

IamPete1 Feb 13, 2024

IamPete1 Feb 13, 2024

IamPete1 Feb 13, 2024

IamPete1 Feb 13, 2024

IamPete1 Feb 13, 2024

IamPete1 Feb 13, 2024

IamPete1 Feb 13, 2024

IamPete1 Feb 13, 2024

IamPete1 Feb 14, 2024

	void set_ground_distance(float gnd_dist) { _gnd_hgt = (float)gnd_dist; }
	void set_ground_distance(float gnd_dist) { _gnd_hgt = gnd_dist; }

Implement assisted autorotation for Heli’s #22871

Are you sure you want to change the base?

Implement assisted autorotation for Heli’s #22871

Conversation

bnsgeyer commented Feb 8, 2023

IamPete1 left a comment • edited Loading

Choose a reason for hiding this comment

bnsgeyer commented Feb 27, 2023

IamPete1 commented Feb 27, 2023

bnsgeyer commented Feb 27, 2023

Ferruccio1984 commented Feb 27, 2023

IamPete1 commented Feb 27, 2023 • edited Loading

tridge commented Feb 28, 2023

bnsgeyer commented Feb 28, 2023

Ferruccio1984 commented Feb 28, 2023 • edited Loading

bnsgeyer commented Mar 1, 2023

Choose a reason for hiding this comment

IamPete1 left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

IamPete1 left a comment •

edited

Loading

IamPete1 commented Feb 27, 2023 •

edited

Loading

Ferruccio1984 commented Feb 28, 2023 •

edited

Loading