Copter: allow adding custom modes from scripting

[IamPete1]

This allows a script to register a new mode with a given number and name. This is just a wrapper on guided mode, but it allows clearer interaction with the user.

The example re-implements flip mode in a script. It uses mode 100, in MAVProxy you can enter with mode 100.

In order to allow the flip I added a new guided binding set_target_rate_and_throttle. I also found and fixed a issue that the Z controller would throw a flow of control error when moving from guided angle with throttle to guided angle with climb rate.

The internal variables of guided mode are changed to static so they apply to to the new modes too.

There are some more changes needed:

• anywhere that doing a compare on guided mode by reference or number needs to deal with the new modes.

• Come up with a way to communicate to the GCS which modes are available so it could populate its mode list with the new modes (this would also be useful if we want to start removing modes and/or for the custom build server).

• Currently scripting is the only way to register a mode, but there is no reason a companion computer could not do the same with a new MAVLink message.

• In the future we may want to add more state on the custom mode object, for example a "allow_enter" bool which the script could keep updated. This would allow the script to reject entry if the correct conditions are not met. Currently the flip example will enter but then immediately put the mode back again if it does not like it.

• Allow a custom version of guided no-gps as well as full guided.

• Currently the mode will fail to add if the script is restated because it is already there, it might be nice to allow a re-add of the same mode, although this is only a issue for script dev.

[amilcarlucas]

Binary Name      Text [B]         Data [B]     BSS (B)        Total Flash Change [B] (%)      Flash Free After PR (B)
---------------  ---------------  -----------  -------------  ----------------------------  -------------------------
arducopter       1092 (+0.0604%)  0 (0.0000%)  -4 (-0.0015%)  1092 (+0.0603%)                                  153864
antennatracker   356 (+0.0266%)   0 (0.0000%)  4 (+0.0015%)   356 (+0.0266%)                                   624868
arducopter-heli  1124 (+0.0622%)  0 (0.0000%)  4 (+0.0015%)   1124 (+0.0621%)                                  154832
ardusub          356 (+0.0223%)   0 (0.0000%)  -4 (-0.0015%)  356 (+0.0222%)                                   364140
ardurover        356 (+0.0215%)   0 (0.0000%)  -4 (-0.0015%)  356 (+0.0214%)                                   305848
blimp            356 (+0.0262%)   0 (0.0000%)  -4 (-0.0015%)  356 (+0.0261%)                                   602752
arduplane        356 (+0.0197%)   0 (0.0000%)  -4 (-0.0015%)  356 (+0.0197%)                                   157544

[tridge]

could these cause problems? it makes it shared between all modes

[andyp1per]

Feels like a bear trap!

[IamPete1]

The problem is the vast majority of the guided state is static already. This stuff:

ardupilot/ArduCopter/mode_guided.cpp

Lines 9 to 33 in 25e77a5

	static Vector3p guided_pos_target_cm; // position target (used by posvel controller only)
	bool guided_pos_terrain_alt; // true if guided_pos_target_cm.z is an alt above terrain
	static Vector3f guided_vel_target_cms; // velocity target (used by pos_vel_accel controller and vel_accel controller)
	static Vector3f guided_accel_target_cmss; // acceleration target (used by pos_vel_accel controller vel_accel controller and accel controller)
	static uint32_t update_time_ms; // system time of last target update to pos_vel_accel, vel_accel or accel controller
	struct {
	uint32_t update_time_ms;
	Quaternion attitude_quat;
	Vector3f ang_vel_body;
	float yaw_rate_cds;
	float climb_rate_cms; // climb rate in cms. Used if use_thrust is false
	float thrust; // thrust from -1 to 1. Used if use_thrust is true
	bool use_yaw_rate;
	bool use_thrust;
	} static guided_angle_state;
	struct Guided_Limit {
	uint32_t timeout_ms; // timeout (in seconds) from the time that guided is invoked
	float alt_min_cm; // lower altitude limit in cm above home (0 = no limit)
	float alt_max_cm; // upper altitude limit in cm above home (0 = no limit)
	float horiz_max_cm; // horizontal position limit in cm from where guided mode was initiated (0 = no limit)
	uint32_t start_time;// system time in milliseconds that control was handed to the external computer
	Vector3f start_pos; // start position as a distance from home in cm. used for checking horiz_max limit
	} guided_limit;

It either has to be all static or all not. With half and half as it is now you can endup being in the wrong sub mode for the targets you have. Making these static is a smaller change than making the existing stuff not.

Currently there should be no issue because there is nothing in the mode exit function so there is no overlap between one version of guided and the next. So long as everything is reset correctly in the mode init then we start clean (which I think it is already).

As you say, this is a bit nasty for the future, but anything else would require a much larger re-work.

[rmackay9]

This will be a great feature!

[IamPete1]

Investigate AVAILABLE_MODES MAVLink message: https://github.com/mavlink/mavlink/blob/7b35ec95a1995ba7253f1af47ecb48abf93d8433/message_definitions/v1.0/development.xml#L666

[andyp1per]

Feels like a bear trap!

[IamPete1]

I have rebased, now lots of the supporting stuff has been merged in the other PRs this is a much smaller change.

[IamPete1]

This now uses the AVAILABLE_MODES and AVAILABLE_MODES_MONITOR to notify the GCS of the new modes. There is a "allow enter" state which the script can change.