Copter: Improve setting home location when it takes off in indoor mode #18

iamczar · 2017-10-19T14:42:03Z

To test this PR with a real copter:

1.Ready two PC/Phone to connect to the web server on the drone. I used two phones for my test- one connects to the web server using google chrome to see the status text and one connects with the skyviper video viewer app to check where it sets the home location.
2.connect the battery to drone and turn on tx
3.on you pc/phone, connect the 1st PC/Phone to the web server on the drone via wifi.
4.using your any web browser (I used chrome) navigate to 192.168.99.1
5.navigate to system status>>message
6.arm or arm and hover the drone in alt_hold mode
7.keep an eye on the message tab - you should get the "INDOOR MODE:set home loc with horizontal_acc:" message
8.fly forward or in any direction which is considerable far from you i.e. 30meter away before it gets full gps lock/EKF happy
9. wait for the drone to get full gps lock/ EKF happy
10. connect your 2nd PC/Phone to the drone and launch either mission planner if you are using PC or sky viper video viewer app if you are using a phone.
11. check if the home location is near you and make sure it did not set home location in some part of the world.
12. if home is nearby then RTL, if not manually control the drone and land it safely because something is wrong

I have attached my test video and some logs on that flight for your reference
https://drive.google.com/open?id=0B3_WlAMsda15R25KTl96ejJXbDQ

To test this PR with a SITL:

I did sim_vehicle.py --console --out=udp:host:port to connect to my windows machine running mission planner
param set sim_gps_numsats 2
restart sitl
4.start mission planner on windows
5.switch to alt_hold mode
6.arm and hover:
7.you will see the status text message "INDOOR MODE:set home loc with horizontal_acc" - you should see home location get set on mission planner
8.fly the copter just a couple of meters away from where it launched
9.do param set sim_gps_numsats 12
10.wait for it to get full lock/ekf happy
do RTL
check if it goes back to the desired home location. if not then something is wrong

peterbarker

Thanks for the great description of your testing procedure. I think you neglected a few steps (setting TOY_MODE, for example), but I hope any other settings you made weren't important :-)

The HOME_POSITION mavlink message is not broadcast as it should be you set home. That means it has to be fetched by the GCS, which isn't great.

Your setting of the entire location from GPS in the first stage isn't working too well. The GPS location is absolute, the Copter's position relative. In the MAVProxy console you can see the vehicle's altitude goes to ~ -584m, which will probably lead to sub-optimal behaviour.

Your testing should probably extend to doing an RTL if the vehicle sets its home based on GPS but EKF never gets happy.

peterbarker · 2017-10-19T23:28:09Z

ArduCopter/toy_mode.cpp

+    /*
+    * attempt to improve the setting of home location sooner
+    */
+    uint8_t home_state_local = copter.ap.home_state;


Pointless local copy?

const it if there's a good reason to have it that I'm missing :-)

forgot to have default: in my switch. It didnt compile when I had it like this -> switch (copter.ap.home_state). There was three states I was only handling two. so by putting a "default: break;" solves the problem.

peterbarker · 2017-10-19T23:35:38Z

ArduCopter/toy_mode.cpp

+            const struct Location &ekf_origin = copter.inertial_nav.get_origin();
+            indoor_loc.alt = ekf_origin.alt;
+            copter.ahrs.set_home(indoor_loc);
+            copter.set_home_state(HOME_SET_AND_LOCKED);


Probably worth another send_statustext_all here.

peterbarker · 2017-10-19T23:43:04Z

ArduCopter/toy_mode.cpp

+                        copter.ahrs.set_home(indoor_loc);
+                        copter.set_home_state(HOME_SET_NOT_LOCKED);
+                        indoor_loc_set = true;
+                        GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "INDOOR MODE:set home loc with horizontal_acc:%.2f,",toy_horizontal_acc); 


nitpicks: extra comma in string, space after comma, space after colon.

iamczar · 2017-10-20T16:40:26Z

Ah yeah, forgot about the default settings. -
I comment this out in when I test in SITL:

AHRS_ORIENTATION 12
COMPASS_EXTERNAL 1
COMPASS_ORIENT 10
COMPASS_USE2 0
COMPASS_USE3 0
COMPASS_OFFS_MAX 2000
COMPASS_DIA_X 1.20
COMPASS_DIA_Y 1.11
COMPASS_DIA_Z 0.89
COMPASS_ODI_X -0.051
COMPASS_ODI_Y 0.141
COMPASS_ODI_Z 0.091

BATT_MONITOR 3
BATT_VOLT_PIN 4
BATT_VOLT_MULT 0.8485
BATT_LOW_TIMER 5

non-standard motor ordering
SERVO1_FUNCTION 36
SERVO2_FUNCTION 33
SERVO3_FUNCTION 34
SERVO4_FUNCTION 35

logging
LOG_BACKEND_TYPE 2
LOG_FILE_BUFSIZE 1

I load the default param in mk/PX4/ROMFS/defaults.parm

With regards to doing a RTL test when EKF is not happy - I find that it wont allow me to change to RTL.

It only allows me to RTL when EKF is happy and by that time the altitude gets reset to based on copter.inertial_nav.get_origin(). But it will show an incorrect altitude to the app. I'll look into getting the alt from other sources. Any suggestion?

Doing another review.

peterbarker · 2017-10-31T05:24:47Z

It only allows me to RTL when EKF is happy and by that time the altitude gets reset to based on
copter.inertial_nav.get_origin(). But it will show an incorrect altitude to the app. I'll look into getting the alt > from other sources. Any suggestion?

Possibly barometric altitude, or gps altitude if by some miracle it thinks it has good altitude. Whatever you choose has to be absolute, 'though - I don't think much good can come of putting a "0" when it should be several hundred meters (which is what it currently does) :-)

peterbarker

Need to resolve the height issue in the set/unlocked state.

peterbarker · 2017-10-31T03:53:28Z

ArduCopter/toy_mode.cpp

+            copter.set_home_state(HOME_SET_AND_LOCKED);
+            // send new home location to GCS
+            GCS_MAVLINK::send_home_all(indoor_loc);
+            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "TMODE: EKF OK reset altitude to:%.2f", ekf_origin.alt); 


reset home altitude

Consistency of prefix would be nice. TMODE: IM:, perhaps.

peterbarker · 2017-10-31T04:07:56Z

ArduCopter/toy_mode.cpp

+    */
+    switch (copter.ap.home_state) {
+    case HOME_UNSET:
+        if (copter.motors->armed() && !copter.ekf_position_ok()) {


This looks a lot nicer, but I don't think I explained the formatting I was suggesting to avoid these deeply-nested blocks.

This (rough) reformatting is what I meant:
peterbarker/ardupilot@c226870

peterbarker · 2017-10-31T04:21:32Z

ArduCopter/toy_mode.cpp

+                        indoor_loc_set = true;
+                        // send new home location to GCS
+                        GCS_MAVLINK::send_home_all(indoor_loc);
+                        GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "INDOOR MODE: set home loc with horizontal_acc: %.2f", toy_horizontal_acc); 


There's a 50-character limit on statustext message...

peterbarker · 2017-10-31T04:23:25Z

ArduCopter/toy_mode.cpp

+        break;
+    case HOME_SET_NOT_LOCKED:
+        // as soon as EKF is happy and indoor_loc_set was set to true then reset the altitude based on origin but retain the original lat lng during flight
+        if (copter.motors->armed() && copter.ekf_position_ok() && indoor_loc_set) {


Nitpick. Put the inexpensive check first.

… in indoor mode. It broaadcasts the home location to GCS when home is set in indoor mode

…indoor mode

iamczar · 2017-11-09T16:54:51Z

I saw this example for trying to resolve the height issue when it is at HOME_SET_NOT_LOCKED state on my code. I'm struggling to set the HomeAtl to an absolute altitude without changing relative altitude or vise versa.
https://github.com/SkyRocketToys/ardupilot/blob/skyviper-stable/ArduCopter/commands.cpp#L28

I flew the skyviper-stable and found that the HomeAlt remains zero when ekf_position return true while the copter is in the air instead of a few hundred meters. It seems to only set HomeAlt when its disarmed. I'm still looking for that check if its true.

tridge · 2017-11-21T11:16:55Z

I think this patch is probably correct, but I'd be more confident of it not having unintended side effects if it worked a bit differently. Specifically:

keep track of the best estimate of home in separate variables, possibly in toy_mode.h
at the time where home is currently initialised in-flight (when we get good EKF lock in flight), replace the location of home with the one from the tracking variables in the first step, but only if home has not been previously set

The key is that it is easier to reason about the patch not having unintended consequences, as the "home is set" state remains as it is now, and the only thing that changes is the source of the location used when home is being set in-flight

…hen GPS lock is achieved the best est home loc is set as home loc. This only happens in flight

iamczar · 2017-11-22T11:57:23Z

Hi @tridge, I have changed the working of the code a bit where the best estimated home location is stored when the copter takes off without GPS lock. It then sets it as the home location when it gets GPS lock given that a best estimated home location is stored otherwise it will stick with whatever EKF sets it too. This will only happen in flight. Is this what you mean?

tridge · 2017-11-29T23:35:43Z

The place where we should hook in is where the copter code sets the GPS locations in-flight. That is the only case where it may currently set a bad home.
The key function is Copter::set_home_to_current_location_inflight(), here:
https://github.com/SkyRocketToys/ardupilot/blob/skyviper-stable/ArduCopter/commands.cpp#L28
So what I was really expecting was two parts:

updating the best estimate of the home when disarmed. That can be done in toy_mode.cpp, and is very similar to what you're doing in your patch, except it doesn't actually set home, it just updates a local variable
when in Copter::set_home_to_current_location_inflight() check if we have a better estimate of the home position from toy_mode and use it

For the 2nd part, add a new ToyMode function called get_home_estimate() and do this:

if (toy_mode.get_home_estimate(temp_loc) || inertial_nav.get_location(temp_loc)) {
    const struct Location &ekf_origin = inertial_nav.get_origin();
    temp_loc.alt = ekf_origin.alt;
    set_home(temp_loc, false);
}

that is the "minimal change" to the logic. It just changes the inertial_nav.get_location() to be toy_mode.get_home_estimate() if available. The really key thing is that it clearly only changes behavior for the case where ArduPilot is getting the home position while flying, which is the only situation that is currently a problem.

…ill trying to resolve the altitude problem

iamczar · 2017-11-30T16:13:48Z

Hi @tridge ,

I find the checks at set_home(const Location& loc, bool lock). Will not accept results from toy_mode.get_home_estimate()'s - https://github.com/SkyRocketToys/ardupilot/blob/czar-improve-hybrid-home/ArduCopter/commands.cpp#L65

I'm doing this instead but having troubles resetting the home altitude when EKF becomes happy

    // get current location from EKF
    Location temp_loc;
    if (inertial_nav.get_location(temp_loc)) {
        const struct Location &ekf_origin = inertial_nav.get_origin();
        temp_loc.alt = ekf_origin.alt;
        set_home(temp_loc, false);
    // get current location from toy mode
    } else if (g2.toy_mode.get_home_estimate(temp_loc)) {
        temp_loc.alt = copter.inertial_nav.get_altitude() * 0.01 - copter.arming_altitude_m;
        ahrs.set_home(temp_loc);
        set_home_state(HOME_SET_NOT_LOCKED);
        //send home location to GCS
        GCS_MAVLINK::send_home_all(temp_loc);
        GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Tmode:IF: home set by TMODE");
    }

when I test it in sitl the altitude becomes +600m when EKF status becomes good.

with regards to this point.

updating the best estimate of the home when disarmed. That can be done in toy_mode.cpp, and is very similar to what you're doing in your patch, except it doesn't actually set home, it just updates a local variable

Should we not leave this to EKF? - https://github.com/SkyRocketToys/ardupilot/blob/czar-improve-hybrid-home/ArduCopter/commands.cpp#L23

tridge · 2017-11-30T21:29:17Z

temp_loc.alt is in centimeters, not meters
Also, you need to hook in at set_home_to_current_location_inflight() not set_home(), and I think you need to set the altitude using the EKF origin, just as it does for the current code

…her arm or disarmed and if inflight the best estimated home by toymode will be compared in set_home_to_current_location_inflight to see which one is better.

…t HOME_UNSET and the copter is disarmed

…med. In flight set_home_to_current_location_inflight() will use get_home_estimate rather than inertial_nav.get_location() if a location is available

tridge · 2017-12-05T10:14:15Z

that is looking much better, thanks Czar!
I'll do some testing on this soon

peterbarker previously requested changes Oct 20, 2017

View reviewed changes

iamczar closed this Oct 20, 2017

iamczar reopened this Oct 20, 2017

peterbarker suggested changes Oct 31, 2017

View reviewed changes

iamczar added 3 commits November 7, 2017 12:54

Copter: Improve setting home location when it takes off in indoor mode

57866ee

Copter: the relative altitude is set when it does a set home location…

3c05baf

… in indoor mode. It broaadcasts the home location to GCS when home is set in indoor mode

Copter: reformatted the checks for improve-setting-home-loc while in …

69d4311

…indoor mode

iamczar force-pushed the czar-improve-hybrid-home branch from 4aedad9 to 69d4311 Compare November 7, 2017 16:19

Copter: removed some trailing whitespaces

f1658cc

iamczar requested a review from tridge November 9, 2017 16:14

Copter: best est home loc is stored if copter flies without GPS and w…

1683a6d

…hen GPS lock is achieved the best est home loc is set as home loc. This only happens in flight

Copter: home is set in set_home_to_current_location_inflight() but st…

1fa706b

…ill trying to resolve the altitude problem

iamczar added 3 commits December 1, 2017 16:02

Copter: the home location will be set in toymode if the copter is eit…

3b88a0b

…her arm or disarmed and if inflight the best estimated home by toymode will be compared in set_home_to_current_location_inflight to see which one is better.

Copter: tmode_best_est_home_set will only reset when home state is no…

e69e2e9

…t HOME_UNSET and the copter is disarmed

Copter: toy mode will try to set best home loc during disarmed and ar…

29979cd

…med. In flight set_home_to_current_location_inflight() will use get_home_estimate rather than inertial_nav.get_location() if a location is available

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Copter: Improve setting home location when it takes off in indoor mode #18

Copter: Improve setting home location when it takes off in indoor mode #18

iamczar commented Oct 19, 2017

peterbarker left a comment

peterbarker Oct 19, 2017

iamczar Oct 20, 2017

peterbarker Oct 19, 2017

peterbarker Oct 19, 2017

iamczar commented Oct 20, 2017 •

edited

Loading

peterbarker commented Oct 31, 2017

peterbarker left a comment

peterbarker Oct 31, 2017

peterbarker Oct 31, 2017

peterbarker Oct 31, 2017

peterbarker Oct 31, 2017

peterbarker Oct 31, 2017

iamczar commented Nov 9, 2017

tridge commented Nov 21, 2017

iamczar commented Nov 22, 2017

tridge commented Nov 29, 2017

iamczar commented Nov 30, 2017 •

edited

Loading

tridge commented Nov 30, 2017

tridge commented Dec 5, 2017

Copter: Improve setting home location when it takes off in indoor mode #18

Are you sure you want to change the base?

Copter: Improve setting home location when it takes off in indoor mode #18

Conversation

iamczar commented Oct 19, 2017

peterbarker 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

iamczar commented Oct 20, 2017 • edited Loading

peterbarker commented Oct 31, 2017

peterbarker 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

iamczar commented Nov 9, 2017

tridge commented Nov 21, 2017

iamczar commented Nov 22, 2017

tridge commented Nov 29, 2017

iamczar commented Nov 30, 2017 • edited Loading

tridge commented Nov 30, 2017

tridge commented Dec 5, 2017

iamczar commented Oct 20, 2017 •

edited

Loading

iamczar commented Nov 30, 2017 •

edited

Loading