Copter : Command Go To Rally point : implementation #8624

Kiwa21 · 2018-06-11T12:34:50Z

Using a command_long MAV_CMD_NAV_RALLY_POINT (5100) , this allow to send Copter to :

a specific rally point (defined with its ID) :
use param1 = 1
and param2 = rally_id
the closest rally point (excluding home) :
use param3 = 1
a new rally point ( lat/long/alt)
use param4 = 1
param5 = lat ( ex : -35.363785 )
param6 = lng ( ex : 149.165761 )
param7 = alt ( ex : 10 // meters above home)

khancyr · 2018-06-11T12:49:21Z

ArduCopter/GCS_Mavlink.cpp

+#if AC_RALLY == ENABLED
+        case MAV_CMD_NAV_RALLY_POINT: //not including home
+
+            if(!is_zero(packet.param1)) {           // Go to a specific rally ID


khancyr · 2018-06-11T12:50:29Z

ArduCopter/mode_rtl.cpp

+    if(copter.rally.cmd_go_to_rally_id) {
+        rtl_path.return_target = copter.rally.go_to_rally_id_location(copter.current_loc,copter.rally.go_to_rally_id);
+        copter.rally.cmd_go_to_rally_id = false; //only use once
+    }else if(copter.rally.cmd_go_to_rally_only) {


} else if (

khancyr · 2018-06-11T12:51:32Z

libraries/AP_Rally/AP_Rally.cpp

@@ -55,6 +55,17 @@ AP_Rally::AP_Rally(AP_AHRS &ahrs)
    AP_Param::setup_object_defaults(this, var_info);
 }

+// get a rally point total count from EEPROM
+uint8_t AP_Rally::get_rally_point_total_count(){


khancyr · 2018-06-11T13:00:20Z

libraries/AP_Rally/AP_Rally.cpp

@@ -161,3 +172,40 @@ Location AP_Rally::calc_best_rally_or_home_location(const Location &current_loc,

    return return_loc;
 }
+// return best rally location from current position - not including home or km limit
+Location AP_Rally::calc_best_rally_location(const Location &current_loc, float rtl_home_alt) const


same calc_best_rally_or_home_location ?

nope, this one exclude home from calc but it still verify its validity (rally point inside fence) otherwise it replace by home.

If you don't want to include home then changing the methods to use an argument instead of directly using the parameter is better. find_nearest_rally_point doesn't return false only when including home, there are other parameters in play.

find_nearest_rally_point returns false when home is nearest than any rally points or when all rally points are further than the _rally_limit_km

What do you mean by changing the methods instead of using the parameter?
Do you want me to change the find_nearest_rally_point method to have an extra argument for including home or not ?

Kiwa21 · 2018-06-11T13:20:33Z

Reformatted commits and coding rules.

OXINARF · 2018-06-11T14:39:05Z

libraries/AP_Rally/AP_Rally.cpp

+}
+
+bool AP_Rally::increment_rally_point_total_count() {
+    return AP_Param::set_by_name("RALLY_TOTAL", _rally_point_total_count + 1);


This is a local parameter, there is no reason to be using this costly method. Just setting it like a normal variable works perfectly fine.

In any case, you should instead do a method to add a new rally point that does this internally.

Maybe I'm wrong but if I just increase _rally_point_total_count, it will not be saved in the EEPROM ?
I'll add a method than does increment and add a new rally point in one go, it's what I was doing in GCS_Mavlink with :
uint8_t new_rally_id = copter.rally.get_rally_point_total_count(); copter.rally.increment_rally_point_total_count(); copter.rally.set_rally_point_with_index(new_rally_id, rally_location);

Use set_and_save_ifchanged() https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_Param/AP_Param.h#L674

@Kiwa21 set_by_name doesn't save it either, which is why I suggested that was enough. As @WickedShell there are methods for setting and saving the parameter value.

OXINARF · 2018-06-11T15:01:34Z

libraries/AP_Rally/AP_Rally.cpp

@@ -161,3 +172,40 @@ Location AP_Rally::calc_best_rally_or_home_location(const Location &current_loc,

    return return_loc;
 }
+// return best rally location from current position - not including home or km limit
+Location AP_Rally::calc_best_rally_location(const Location &current_loc, float rtl_home_alt) const


If you don't want to include home then changing the methods to use an argument instead of directly using the parameter is better. find_nearest_rally_point doesn't return false only when including home, there are other parameters in play.

OXINARF · 2018-06-11T17:13:08Z

libraries/AP_Rally/AP_Rally.cpp

+}
+
+//return location of a specific id rally point
+Location AP_Rally::go_to_rally_id_location(const Location &current_loc, uint8_t rally_id) const


I don't see any reason for this method, the caller can already get the specific rally point and if doesn't exist it can then choose what to do.

Agree, the const Location &current_loc is too much, it was a bad copy paste from other method. However, I think the rest of the function is fine.
Maybe I can rename to get_rally_location_with_index to have better consistency with get_rally_point_with_index

What is this needed for? I see no reason to be hard-coding the home behavior when the caller can do the same - if this was going to be repeated in a lot of places then it would make sense but this is going to be used in one place.

Ok, I can do that. I thought it was cleaner to define an alternate method even though it's called once (for now?).

OXINARF · 2018-06-11T17:21:06Z

libraries/AP_Rally/AP_Rally.h

+    // cmd_nav_rally parameters
+    bool cmd_go_to_rally_only = false;
+    bool cmd_go_to_rally_id = false;
+    uint8_t go_to_rally_id = 0;


This is really bad, class variables shouldn't be public.

Ok ! Will make them private and do method to access them

OXINARF · 2018-06-11T17:21:56Z

ArduCopter/GCS_Mavlink.cpp

+                copter.rally.go_to_rally_id = (uint8_t)packet.param2;
+            } else if (!is_zero(packet.param3)) { // Go to nearest rally from loc except home
+                copter.rally.cmd_go_to_rally_only = true;
+            } else if (!is_zero(packet.param4)) { // Go to new rally location (and save it in EEPROM)


Where are all these param definitions? I don't see them in MAVLink...

There is nothing defined in MAVLink.. I can add a MAVLINK specific message to ardupilotmega.xml if you prefer but I don't see any sense to wait for MAVLINK to define all the messages before we can use them. When they'll be ready, we can always refractor

Adding things to code before adding them to MAVLink is part of what has led to problems. Things should be discussed in MAVLink first then we can add them to code.

OXINARF · 2018-06-11T17:28:57Z

ArduCopter/GCS_Mavlink.cpp

@@ -1062,6 +1062,38 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
            break;
 #endif

+#if AC_RALLY == ENABLED
+        case MAV_CMD_NAV_RALLY_POINT: //not including home


MAVLink documentation doesn't mention what this should be used for, but following the history it looks like it was added for the new rally protocol and not exactly to go to a rally point. I think this needs to be clarified in MAVLink first.

Same nothing define.
How long should we wait for Mavlink then...?

Is there any discussion in MAVLink about this?

Nothing more than this mavlink/mavlink#683 from March 2017.

As written in mavlink/mavlink#683 (comment), these new commands were intended to be used in MISSION_ITEM and not in COMMAND_LONG.

I'm happy to change the mavlink message transporting the info if you prefer.
We can define a new message in ardupilotmega.xml with a good definition and I'll rebase on that.
Should it be presented to the next dev call ?

Changing the MAVLink message needs to be discussed in the MAVLink repo. We can add a new MAVLink message, but I don't think that's needed and will only bring more confusion.

Is there some question here someone if looking for me to answer?

OXINARF · 2018-06-11T17:33:36Z

ArduCopter/GCS_Mavlink.cpp

+                copter.rally.go_to_rally_id = new_rally_id;
+            }
+
+            copter.set_mode(RTL, MODE_REASON_MISSION_END);


This seems strange to me, it looks like Guided would be more appropriate, it's a request to go to a point, not an RTL.
Also, this certainly isn't reason mission end.

I disagree, it's still a RTL for aborting / ending mission but with a special condition.
I can change to MODE_REASON_MISSION_COMMAND but I don't know where is the actual enum for this

I'm confused, this isn't handled in a mission...

Sorry, I meant MODE_REASON_GCS_COMMAND. (again I don't know the enums, just found these in the ArduRover set_mode reason.. 23532bf ).
Still I think MISSION_END is not too far from the scope.

Sorry, but I'm still confused, you said this is an RTL because it is for aborting/ending a mission, but this isn't handled as a command in a mission, this is handled as command coming from MAVLink, it doesn't matter what mode the vehicle is in.

It's an outdated snippet of code, it was changed in the last update of this commit!

It still does RTL which was my main point. Anyway, up to @rmackay9 to decide.

peterbarker · 2018-06-12T04:01:09Z

Shouldn't this mavlink command be relevant on the other vehicle types, too?

Kiwa21 · 2018-06-12T12:04:22Z

I've updated the commit with severals review.
@peterbarker I guess it could be directly used for other vehicles yes. I can do the others when this one is over.

xcoder123 · 2018-07-09T09:04:21Z

will this change be also available for the ArduPlane? If so when can we expect it to be merged into the master?

peterbarker · 2021-02-03T09:36:00Z

@Kiwa21 are you still interested in pursuing this PR?

khancyr · 2021-07-14T07:42:03Z

This is heavily conflicting and need a full rework;
feel free to join a devcall, if you want to discuss this. thanks anyway

Kiwa21 changed the title ~~Command Go To Rally point : implementation~~ Copter : Command Go To Rally point : implementation Jun 11, 2018

khancyr added the Copter label Jun 11, 2018

khancyr requested a review from rmackay9 June 11, 2018 12:48

khancyr suggested changes Jun 11, 2018

View reviewed changes

OXINARF requested changes Jun 11, 2018

View reviewed changes

Kiwa21 added 2 commits June 12, 2018 14:01

AP_Rally : implement go to rally functions

4168ae8

ArduCopter : implement go to rally functions

1192b5d

ArduPilot deleted a comment from paytufo Aug 20, 2018

peterbarker added the NeedAnswer label Feb 3, 2021

khancyr closed this Jul 14, 2021

Copter : Command Go To Rally point : implementation #8624

Copter : Command Go To Rally point : implementation #8624

Conversation

Kiwa21 commented Jun 11, 2018

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

Kiwa21 commented Jun 11, 2018

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

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

Choose a reason for hiding this comment

Kiwa21 Jun 11, 2018 • edited

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

peterbarker commented Jun 12, 2018

Kiwa21 commented Jun 12, 2018

xcoder123 commented Jul 9, 2018

peterbarker commented Feb 3, 2021

khancyr commented Jul 14, 2021

Kiwa21 Jun 11, 2018 •

edited