Add Goto Location Action #485

mzahana · 2018-08-08T21:30:13Z

This PR adds goto_location() action which allows to send the vehicle to a specific WGS84 global position. It uses MAV_DO_REPOSITION

TSC21 · 2018-08-08T21:36:39Z

plugins/action/include/plugins/action/action.h

+     * @brief Send command to reposition the vehicle to a specific WGS84 global position
+     *
+     * This sends the vehicle to a specified lattitude/longitude/altitude coordinates.
+     * @param latitude deg * 1E7


Why not pass the latitude and longitude as normal coordinates and then multiply by 1E7 on the ActionResult code?

Possible. I guess that requires to use double instead of float and then multiply by 1E7, then cast to float when passing to command.params, no?

lat/long can be entered in degrees now.

hamishwillee · 2018-08-08T21:55:11Z

How does this differ from the intention of offboard control? ie should this be the unimplemented position setting part of offboard control?

mzahana · 2018-08-08T22:05:03Z

@hamishwillee Offboard does not have GPS setpoints. It's in local frame. So, conversions need to happen. Also, the flight controller already provides this functionality without much coding.

TSC21 · 2018-08-08T23:02:12Z

plugins/action/action_impl.cpp

 {
+    float lat1e7 = (float) (latitude * 1E7);


you don't need this. You can use a floating point literal:

ActionResult ActionImpl::goto_location(float latitude, float longitude, float altitude) { .... command.params.param5 = latitude * 1E7F; command.params.param6 = longitude * 1E7F; .... }

if I keep latitude and longitude as float, I get warnings when I enter big values like 48.1234567 for loosing precision.

mhkabir · 2018-08-09T04:44:24Z

It would be more correct to use double precision for lat/lon.

julianoes · 2018-08-09T07:20:04Z

@mzahana cool, thanks for the contribution! I think that's a handy feature and while it might be overlapping with offboard it's still useful.

julianoes · 2018-08-09T07:20:41Z

plugins/action/action.cpp

@@ -32,6 +32,11 @@ ActionResult Action::land() const
    return _impl->land();
 }

+ActionResult Action::goto_location(double latitude, double longitude, double altitude)


Please always add units if possible. Here I suggest to use latitude_deg, and relative_altitude_m.

julianoes · 2018-08-09T07:23:08Z

plugins/action/action_impl.cpp

+    float lat1e7 = (float) (latitude * 1E7);
+    float long1e7 = (float) (longitude * 1E7);
+
+    MAVLinkCommands::CommandLong command{};


I think this was already said but I this should be a CommandInt for better accuracy.

julianoes · 2018-08-09T07:24:02Z

plugins/action/include/plugins/action/action.h

+     * This sends the vehicle to a specified lattitude/longitude/altitude coordinates.
+     * @param latitude in degrees
+     * @param longitude in degrees
+     * @param altitude Altitude AMSL (meters)


Oh, so this is absolute altitude? Are you sure about that?

@julianoes seems like it is always AMSL: PX4/PX4-Autopilot#8502

Here defined: https://github.com/PX4/Firmware/blob/bgat-master/src/modules/navigator/navigator_main.cpp#L414

TSC21 · 2018-08-09T09:19:07Z

It would be more correct to use double precision for lat/lon.

@mhkabir that's only possible in COMMAND_INT. This is using COMMAND_LONG. Though, as @julianoes, the COMMAND_INT can be used for this case.

mzahana · 2018-08-09T16:55:46Z

@julianoes I am not sure if PX4 uses COMMAND_INT to parse MAV_CMD_DO_REPOSITION.

So, in summary, what is required to merger this PR?

mzahana · 2018-08-09T17:08:10Z

So, this is what I have so far.

ActionResult ActionImpl::goto_location(double latitude_deg, double longitude_deg, float altitude_amsl_m)
{

    MAVLinkCommands::CommandLong command{};

    command.command = MAV_CMD_DO_REPOSITION;
    command.target_component_id = _parent->get_autopilot_id();
    command.params.param1 = -1; /* default speed */
    command.params.param2 = MAV_DO_REPOSITION_FLAGS_CHANGE_MODE;
    // command.param4 = NaN /* Yaw Unchanged */
    command.params.param5 = (float)latitude_deg * 1E7F;
    command.params.param6 = (float)longitude_deg * 1E7F;
    command.params.param7 = altitude_amsl_m;

    return action_result_from_command_result(_parent->send_command(command));
}

Let me know your feedback.

julianoes · 2018-08-13T08:00:58Z

@mzahana it looks to me like command int should be supported:
https://github.com/PX4/Firmware/blob/96995852cb6a2e2e23894c7ea05dd8385c595172/src/modules/navigator/navigator_main.cpp#L349-L351

julianoes · 2018-08-13T08:02:05Z

plugins/action/action_impl.cpp

+ActionResult ActionImpl::goto_location(double latitude_deg, double longitude_deg, float altitude_amsl_m)
+{
+
+    MAVLinkCommands::CommandLong command{};


I suggest you change it to CommandInt and try it in SITL.

mzahana · 2018-08-14T07:45:53Z

@julianoes CommandInt is used and tested in SITL. Works fine.

julianoes

Just a small change and a question. Thanks for the changes.

julianoes · 2018-08-14T08:28:55Z

plugins/action/action.cpp

@@ -32,6 +32,11 @@ ActionResult Action::land() const
    return _impl->land();
 }

+ActionResult Action::goto_location(double latitude_deg, double longitude_deg, float altitude_amsl_m, float yaw_rad)


Please expose yaw in degrees and convert it to rad internally.

julianoes · 2018-08-14T08:30:08Z

plugins/action/include/plugins/action/action.h

+     * This sends the vehicle to a specified lattitude/longitude/altitude coordinates.
+     * @param latitude_deg Latitude in degrees
+     * @param longitude_deg Longitude in degrees
+     * @param altitude_amsl_m Altitude AMSL in meters


Are you sure that this command is in absolute altitude and not relative? Could it be that we need to specify the frame when sending the command int?

Yes! It's weird though as the frame component in CommandInt is defined as MAV_FRAME_GLOBAL_RELATIVE_ALT. According to the description in here, the altitude should be relative to home, not MSL!

When I try in the SITL, I had to command 500m to get 10m above home!

Also, I think MAV_FRAME_GLOBAL_RELATIVE_ALT_INT should be used in (https://github.com/Dronecode/DronecodeSDK/blob/27027d4eae32267616d6016cfebdcd13b0703719/core/mavlink_commands.h#L35)

Hm, so the implementation in PX4 is wrong? Could you check that?

My guess is yes. It needs to be fixed on the PX4 side. I just have not figured out yet where it is exactly consumed.

You're right, I filed an issue: PX4/PX4-Autopilot#10246.

@mzahana oh yes that should be _INT, thanks for the note!

julianoes · 2018-08-15T11:20:56Z

plugins/action/action_impl.cpp

 {

    MAVLinkCommands::CommandInt command{};

    command.command = MAV_CMD_DO_REPOSITION;
    command.target_component_id = _parent->get_autopilot_id();
-    command.params.param4 = yaw_rad;
+    command.params.param4 = to_rad_from_deg(yaw_deg);


julianoes · 2018-08-16T07:04:41Z

@mzahana would be good if you could rebase this on develop and force push it.

…e is changed to float;(3) Update functionbrief

julianoes · 2018-08-17T14:35:03Z

@mzahana I hope you don't mind that I rebased and force pushed your branch. I also fixed the style issues and added an integration test.

julianoes · 2018-08-17T14:35:30Z

@TSC21 are you ok if we merge this for now, and then follow up with the firmware issues?

Dismissing your review since the concerns have been addressed.

julianoes · 2018-08-18T11:17:36Z

Thanks a lot @mzahana!

mzahana · 2018-08-18T12:21:51Z

@julianoes Thanks for merging. Sorry I didn't have time to make the last changes rapidly.

hamishwillee · 2018-08-20T07:44:37Z

plugins/action/include/plugins/action/action.h

+     * @brief Send command to reposition the vehicle to a specific WGS84 global position
+     *
+     * This sends the vehicle to a specified lattitude/longitude/altitude coordinates.
+     * @param latitude_deg Latitude in degrees


FYI, I wouldn't necessarily change this now, but one of the benefits of naming with units is that you don't necessarily have to spell everything out in the description. So normally I'd just have Latitude. (and yes, arguably you don't even need that, but good to have something because it shows that you have thought about docs).

Add Goto Location Action

dagar added the in progress label Aug 8, 2018

TSC21 reviewed Aug 8, 2018

View reviewed changes

TSC21 previously requested changes Aug 8, 2018

View reviewed changes

julianoes requested changes Aug 9, 2018

View reviewed changes

julianoes reviewed Aug 13, 2018

View reviewed changes

julianoes reviewed Aug 14, 2018

View reviewed changes

julianoes reviewed Aug 15, 2018

View reviewed changes

julianoes mentioned this pull request Aug 16, 2018

DO_REPOSITION ignores frame, always assumes AMSL PX4/PX4-Autopilot#10246

Open

mzahana and others added 8 commits August 17, 2018 16:14

Add goto_location action

093e509

Update @brief of goto_location action

925fc91

latitude/longitude in degrees instead of deg * 1E7

8598809

(1)Add units to variables names; (2)lat/long are kept double, altitud…

b16d581

…e is changed to float;(3) Update functionbrief

Use CommandInt instead of CommandLong

79bd078

Internal conversion from yaw deg to radians

41a736d

integration_tests: added test for goto_location

a2eb553

action: fix style for goto

52ab740

julianoes approved these changes Aug 17, 2018

View reviewed changes

julianoes merged commit 4029526 into mavlink:develop Aug 18, 2018

julianoes removed the in progress label Aug 18, 2018

hamishwillee reviewed Aug 20, 2018

View reviewed changes

rt-2pm2 pushed a commit to rt-2pm2/DronecodeSDK that referenced this pull request Nov 27, 2018

Merge pull request mavlink#485 from mzahana/develop

799a8d9

Add Goto Location Action

julianoes mentioned this pull request Mar 7, 2019

Inconsistent documentation in Action class. #691

Closed

Add Goto Location Action #485

Add Goto Location Action #485

Conversation

mzahana commented Aug 8, 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

hamishwillee commented Aug 8, 2018

mzahana commented Aug 8, 2018

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mhkabir commented Aug 9, 2018

julianoes commented Aug 9, 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

TSC21 commented Aug 9, 2018 • edited Loading

mzahana commented Aug 9, 2018

mzahana commented Aug 9, 2018

julianoes commented Aug 13, 2018

Choose a reason for hiding this comment

mzahana commented Aug 14, 2018

julianoes 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

julianoes commented Aug 16, 2018

julianoes commented Aug 17, 2018

julianoes commented Aug 17, 2018 • edited Loading

julianoes commented Aug 18, 2018

mzahana commented Aug 18, 2018

Choose a reason for hiding this comment

TSC21 commented Aug 9, 2018 •

edited

Loading

julianoes commented Aug 17, 2018 •

edited

Loading