New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

offboard: automatically send setpoins at set rate #134

Merged

julianoes merged 18 commits into develop from improve-offboard

Oct 30, 2017

Collaborator

julianoes commented Oct 25, 2017

This changes the offboard API so that it is no longer required to send
setpoints at least every 0.5 seconds. Instead, this enables to set a
setpoint once and only set it again when it changes. The implementation
will automatically send it at 20 Hz internally.

If the setpoints need to be sent at a higher rate, e.g. for very smooth
control, that is still possible by just setting the setpoints at high
rate. Whenever a setpoint changes, it is immediately sent to the
vehicle.

Tested in SITL using the integration tests as well as the offboard example.

julianoes requested review from hamishwillee and shakthi-prashanth-m

October 25, 2017 18:41

coveralls commented Oct 25, 2017

Coverage increased (+6.5%) to 55.131% when pulling 31a1407 on improve-offboard into e12c969 on develop.

coveralls commented Oct 25, 2017

Coverage increased (+6.5%) to 55.131% when pulling 2c066b6 on improve-offboard into e12c969 on develop.

hamishwillee reviewed

View reviewed changes

plugins/offboard/offboard_impl.cpp Outdated

+                          }
+                      }
+                  }

Collaborator

hamishwillee Oct 25, 2017

Shouldn't this return here so that the following code isn't executed?

Collaborator Author

julianoes Oct 26, 2017

Very good catch, thank you!

Collaborator

hamishwillee commented Oct 25, 2017

In the example

/**
* @name connected device
* @brief Does Offboard control using BODY co-ordinates
* returns true if everything went well in Offboard control, exits with a log otherwise.
*/

Seems odd to have @ docs for examples. Even so, should be correct - what is @name ? should it be @returns ?

Collaborator

hamishwillee commented Oct 25, 2017 •

edited

Loading

Looks awesome. Some thoughts:

Is it possible for the vehicle to decide to switch mode due to RC or whatever while the script thinks it is in RC mode? In this case, what should happen / how should the script respond?
We should add a paragraph up in the class description explaining a) how message resending should be used b)need to call setpoint before calling mode.
E.g. update along the lines of:

/**
 * @brief This class is used to control a drone with velocity commands.
 *
 * The module is called offboard because the velocity commands can be sent from external sources
 * as opposed to onboard control right inside the autopilot "board".
 *
 * Client code must specify a setpoint before starting offboard mode.
 * DroneCore automatically resends setpoints at 20Hz (PX4 Offboard mode requires that setpoints are
 * minimally resent at 2Hz). If more precise control is required, clients can call the
 * setpoint methods at whatever rate is required.
 *
 * **Attention:** this is work in progress, use with caution!
 */

Is there ever a case where someone might want to resend at lower than 20Hz?
I still think it would be nice to automatically put into the mode rather than requiring "start" semantics. This is however fairly easy to understand.

hamishwillee reviewed

View reviewed changes

Collaborator

hamishwillee left a comment •

edited

Loading

I've added some feedback. Will leave for @shakthi-prashanth-m to do technical C++ review.

Collaborator

hamishwillee commented Oct 26, 2017 •

edited

Loading

@julianoes As part of this can I please also get a small docs update. For all the members in VelocityNEDYaw and VelocityBodyYawspeed add "@brief" (easiest way to fix output). So

float north_m_s; /**< Velocity North in metres/second. */

to

float north_m_s; /**< @brief Velocity North in metres/second. */

etc.

And also, in particular can I also get a closing ")" on these two:

float yaw_deg; /**< Yaw in degrees (0 North, positive is clock-wise looking from above. */
float yawspeed_deg_s; /**< Yaw angular rate in degrees/second (positive for clock-wise looking from above. */

hamishwillee reviewed

View reviewed changes

example/offboard_velocity/offboard_velocity.cpp

		@@ -76,12 +78,9 @@ bool offb_ctrl_ned(Device &device)

Collaborator

hamishwillee Oct 26, 2017

@julianoes In line above, why sleep for one second after starting offboard? API should not require artificial timing breaks.

Collaborator Author

julianoes Oct 26, 2017

Removed.

hamishwillee reviewed

View reviewed changes

example/offboard_velocity/offboard_velocity.cpp Outdated

		@@ -76,12 +78,9 @@ bool offb_ctrl_ned(Device &device)

		// Let yaw settle.

Collaborator

hamishwillee Oct 26, 2017

Don't think we need this note - since we show it as log in next line.

Collaborator Author

julianoes Oct 26, 2017

Removed.

hamishwillee reviewed

View reviewed changes

example/offboard_velocity/offboard_velocity.cpp Outdated

		@@ -76,12 +78,9 @@ bool offb_ctrl_ned(Device &device)

		// Let yaw settle.
		offboard_log(offb_mode, " Let yaw settle...");

Collaborator

hamishwillee Oct 26, 2017 •

edited

Loading

This log is confusing - because at this point we haven't told the user we're yawing. We also report done, which I think is redundant since we're not actually checking that we're done and we then say the next step.

Suggest replace with following text - states that we're yawing and gives user 3 seconds to watch that behaviour. We don't need to tell the watcher that yaw is settling, but we add a comment so programmers know why we added that sleep.

    offboard_log(offb_mode, " Turn (yaw) clockwise to face East (90 deg.)");
    device.offboard().set_velocity_ned({0.0f, 0.0f, 0.0f, 90.0f});
    sleep_for(seconds(3));  // Let yaw settle

Contributor

shakthi-prashanth-m Oct 27, 2017

makes sense

hamishwillee reviewed

View reviewed changes

example/offboard_velocity/offboard_velocity.cpp Outdated

		@@ -99,31 +98,24 @@ bool offb_ctrl_ned(Device &device)
		// NOTE: Use sleep_for() after each velocity-ned command to closely monitor their behaviour.

Collaborator

hamishwillee Oct 26, 2017 •

edited

Loading

Section above is confusing to watch and the code is a bit complicated:

Description incorrect
- The vehicle does not "go right". It turns to face east unless it is already facing east.
- It technically does "oscillate", but that could mean almost anything.
Propose change text to "Oscillate along a North-South path. Turn clockwise to face East (90 deg.)"
Propose make this do two cycles to be easier to observe when running the example (ie. const unsigned steps = (unsigned)(one_cycle / step_size); to const unsigned steps = 2 * (unsigned)(one_cycle / step_size);
Maybe "Done" is OK here because the action is quite extended.

Collaborator Author

julianoes Oct 26, 2017

Thanks, changed it along these lines.

hamishwillee reviewed

View reviewed changes

example/offboard_velocity/offboard_velocity.cpp Outdated

		@@ -99,31 +98,24 @@ bool offb_ctrl_ned(Device &device)
		// NOTE: Use sleep_for() after each velocity-ned command to closely monitor their behaviour.

		offboard_log(offb_mode, " Turn clock-wise 270 deg");

Collaborator

hamishwillee Oct 26, 2017 •

edited

Loading

"Turn clockwise to face West (270 deg.)"

hamishwillee reviewed

View reviewed changes

example/offboard_velocity/offboard_velocity.cpp Outdated

-                  }
+                  device.offboard().set_velocity_ned({0.0f, 0.0f, 0.0f, 270.0f});
+                  sleep_for(seconds(2));
                   offboard_log(offb_mode, " Done");
                   offboard_log(offb_mode, " Go UP 2 m/s, Turn clock-wise 180 deg");

Collaborator

hamishwillee Oct 26, 2017

"Go UP 2 m/s. Turn clockwise to face South (180 deg.)"

hamishwillee reviewed

View reviewed changes

example/offboard_velocity/offboard_velocity.cpp Outdated

-                  }
+                  device.offboard().set_velocity_ned({0.0f, 0.0f, 0.0f, 270.0f});
+                  sleep_for(seconds(2));
                   offboard_log(offb_mode, " Done");
                   offboard_log(offb_mode, " Go UP 2 m/s, Turn clock-wise 180 deg");

Collaborator

hamishwillee Oct 26, 2017

@julianoes When this command executes the vehicle should turn clockwise from 270 through to 0 and back round to 180 - because the yaw value is positive. What I'm seeing in SITL is that it goes anticlockwise back to 180 irrespective of the sign on the yaw value. So either a bug in our docs, or dronecore or PX4.

Collaborator Author

julianoes Oct 26, 2017

It just goes the quickest way to the new setpoint. I'll get rid of the "(anti-)clock-wise" indications.

hamishwillee reviewed

View reviewed changes

example/offboard_velocity/offboard_velocity.cpp Outdated

-                      sleep_for(milliseconds(10));
-                  }
+                  device.offboard().set_velocity_ned({0.0f, 0.0f, -2.0f, 180.0f});
+                  sleep_for(seconds(4));
                   offboard_log(offb_mode, " Done...");
                   offboard_log(offb_mode,  " Turn clock-wise 90 deg");

Collaborator

hamishwillee Oct 26, 2017

Turn clockwise to face East (90 deg.)"

Collaborator

hamishwillee Oct 26, 2017

It would also be good if at least one of these cases turned anticlockwise to demonstrate that working. Certainly we need that in the test code.

Collaborator Author

julianoes Oct 26, 2017 •

edited

Loading

As said, it's up to the controller. We can only really check it for body coordinates where we control yaw rate.

hamishwillee reviewed

View reviewed changes

example/offboard_velocity/offboard_velocity.cpp Outdated

-                      sleep_for(milliseconds(10));
-                  }
+                  device.offboard().set_velocity_ned({0.0f, 0.0f, 0.0f, 90.0f});
+                  sleep_for(seconds(4));
                   offboard_log(offb_mode, " Done...");
                   offboard_log(offb_mode,  " Go DOWN 1.0 m/s");

Collaborator

hamishwillee Oct 26, 2017

Go DOWN 1.0 m. Turn clockwise to face North (0 deg.)

hamishwillee reviewed

View reviewed changes

example/offboard_velocity/offboard_velocity.cpp Outdated

		@@ -152,53 +144,39 @@ bool offb_ctrl_body(Device &device)

		// Turn around yaw and climb
		offboard_log(offb_mode, " Turn around yaw & climb");

Collaborator

hamishwillee Oct 26, 2017

Text should be: " Go UP 1 m/s. Turn clockwise at 60 degrees/second."

hamishwillee reviewed

View reviewed changes

example/offboard_velocity/offboard_velocity.cpp Outdated

-                      sleep_for(milliseconds(10));
-                  }
+                  device.offboard().set_velocity_body({0.0f, 0.0f, -1.0f, 60.0f});
+                  sleep_for(seconds(2));

Collaborator

hamishwillee Oct 26, 2017

Shouldn't all these cases print "Done" for consistency? Either all should do or all should not do.

Collaborator Author

julianoes Oct 26, 2017 •

edited

Loading

I've removed all.

hamishwillee reviewed

View reviewed changes

example/offboard_velocity/offboard_velocity.cpp Outdated

-                      sleep_for(milliseconds(10));
-                  }
+                  device.offboard().set_velocity_body({0.0f, 0.0f, -1.0f, 60.0f});
+                  sleep_for(seconds(2));
                   // Turn back
                   offboard_log(offb_mode, " Turn back");

Collaborator

hamishwillee Oct 26, 2017

" Turn the other direction (anticlockwise) at 60 degrees/second."

hamishwillee reviewed

View reviewed changes

example/offboard_velocity/offboard_velocity.cpp Outdated

-                      sleep_for(milliseconds(10));
-                  }
+                  device.offboard().set_velocity_body({0.0f, 0.0f, -1.0f, 60.0f});
+                  sleep_for(seconds(2));

Collaborator

hamishwillee Oct 26, 2017

Maybe sleep 5 seconds - so this is easy to observe for someone viewing the example.

Collaborator Author

julianoes Oct 26, 2017

Ok.

hamishwillee reviewed

View reviewed changes

example/offboard_velocity/offboard_velocity.cpp Outdated

-                      sleep_for(milliseconds(10));
-                  }
+                  device.offboard().set_velocity_body({0.0f, 0.0f, 0.0f, -60.0f});
+                  sleep_for(seconds(2));

Collaborator

hamishwillee Oct 26, 2017

Maybe sleep 5 seconds - so this is easy to observe for someone viewing the example.

Collaborator Author

julianoes Oct 26, 2017

Ok.

hamishwillee reviewed

View reviewed changes

example/offboard_velocity/offboard_velocity.cpp Outdated

-                      sleep_for(milliseconds(10));
-                  }
+                  device.offboard().set_velocity_body({0.0f, 0.0f, 0.0f, 0.0f});
+                  sleep_for(seconds(2));
                   // NOTE: Use sleep_for() after each velocity-ned command to closely monitor their behaviour.

Collaborator

hamishwillee Oct 26, 2017

Duplicate Note. If this is to stay, make sure it changes to "after velocity-body".

Collaborator Author

julianoes Oct 26, 2017

Removed.

hamishwillee reviewed

View reviewed changes

example/offboard_velocity/offboard_velocity.cpp Outdated

-                      device.offboard().set_velocity_body({5.0f, 0.0f, 0.0f, 60.0f});
-                      sleep_for(milliseconds(10));
-                  }
+                  device.offboard().set_velocity_body({5.0f, 0.0f, 0.0f, 60.0f});

Collaborator

hamishwillee Oct 26, 2017 •

edited

Loading

Propose rotate at 30.0f (bigger circle) and wait for 15 seconds. This is much more visible in QGC., and it doesn't hurt to show different rates at work.

Collaborator Author

julianoes Oct 26, 2017

Sure.

hamishwillee reviewed

View reviewed changes

example/offboard_velocity/offboard_velocity.cpp Outdated

-                      sleep_for(milliseconds(10));
-                  }
+                  device.offboard().set_velocity_body({0.0f, 0.0f, 0.0f, 0.0f});
+                  sleep_for(seconds(5));

Collaborator

hamishwillee Oct 26, 2017

Change to at least 8 seconds.

Collaborator Author

julianoes commented Oct 26, 2017 •

edited

Loading

Seems odd to have @ docs for examples. Even so, should be correct - what is @name ? should it be @returns ?

Agreed, I would remove the whole comment, or at least remove the @s.

Collaborator Author

julianoes commented Oct 26, 2017 •

edited

Loading

Is it possible for the vehicle to decide to switch mode due to RC or whatever while the script thinks it is in RC mode? In this case, what should happen / how should the script respond?

The script would keep on sending setpoints but not try to enter offboard mode again. I guess we could check if we're still in the correct mode and complain otherwise. The problem is though that the setter does not return with an error.

What about having a function to check if offboard is still running?

We should add a paragraph up in the class description explaining a) how message resending should be used b)need to call setpoint before calling mode.
E.g. update along the lines of:

Thanks, I copied what you suggested.

Is there ever a case where someone might want to resend at lower than 20Hz?

Maybe on a really slow link. We could default to 10 Hz which should work for all cases I can think of.

I still think it would be nice to automatically put into the mode rather than requiring "start" semantics. This is however fairly easy to understand.

The benefit of having start() is that you can check the error of start. If it is implicit in set(), you'll need to check for an error with every set().

Collaborator Author

julianoes commented Oct 26, 2017

@julianoes As part of this can I please also get a small docs update. For all the members in VelocityNEDYaw and VelocityBodyYawspeed add "@brief" (easiest way to fix output). So

Sure, done.

julianoes force-pushed the improve-offboard branch from 2c066b6 to db67ed5 Compare

October 26, 2017 15:53

shakthi-prashanth-m reviewed

View reviewed changes

plugins/offboard/offboard_impl.cpp Outdated

+                      // reschedule the next call, so we don't send setpoints too often.
+                      _parent->reset_call_every(_call_every_cookie);
+                  }
+                  _velocity_ned_yaw = velocity_ned_yaw;

Contributor

shakthi-prashanth-m Oct 27, 2017

shouldn't we assign this before entering calling send_velocity_ned() at line:191.

Collaborator Author

julianoes Oct 27, 2017

Good spotting! Call every will only schedule it and not run it immediately, however, it is still a race, so you're right!
Changed in 2c0a887.

shakthi-prashanth-m reviewed

View reviewed changes

plugins/offboard/offboard_impl.cpp Outdated

+                      // reschedule the next call, so we don't send setpoints too often.
+                      _parent->reset_call_every(_call_every_cookie);
+                  }
+                  _velocity_body_yawspeed = velocity_body_yawspeed;

Contributor

shakthi-prashanth-m Oct 27, 2017

shouldn't this happen before Line 218 ?

Collaborator Author

julianoes Oct 27, 2017

Yep, same thing.

shakthi-prashanth-m reviewed

View reviewed changes

plugins/offboard/offboard_impl.cpp

                   //const static uint16_t IGNORE_YAW = (1 << 10);
                   const static uint16_t IGNORE_YAW_RATE = (1 << 11);
-                  const float yaw = to_rad_from_deg(velocity_ned_yaw.yaw_deg);
+                  _mutex.lock();

Contributor

shakthi-prashanth-m Oct 27, 2017

Does this lock makes sure _velocity_body_ned_yaw has updated value?

Collaborator Author

julianoes Oct 27, 2017

Right, I guess the lock prevents the race from happening. Still changed it makes more sense.

julianoes added 18 commits

October 27, 2017 09:18


          core: add function to shift steady_time

e8555a0


          core: added a call every functionality

fe39e22

This adds a class to call a callback et a constant rate (constant
interval). Presumably, this should be done using some sort of proper
event loop but this could be good enough for now.


          core: use call every handler in device_impl

06c4d35


          core: add way to reset call_every

179a56a

This means that we can reschedule the next time call_every is triggered,
so the interval time is reset.


          offboard: automatically send setpoins at set rate

aacc5f6

This changes the offboard API so that it is no longer required to send
setpoints at least every 0.5 seconds. Instead, this enables to set a
setpoint once and only set it again when it changes. The implementation
will automatically send it at 20 Hz internally.

If the setpoints need to be sent at a higher rate, e.g. for very smooth
control, that is still possible by just setting the setpoints at high
rate. Whenever a setpoint changes, it is immediately sent to the
vehicle.


          integration_tests: adapt test to new offboard API

2ec264f

The new API does not require the setpoint to be constantly set.


          example: use new offboard API

1613cce


          core: fix float vs. double types

90e513c


          offboard: add missing early return

24afc04


          example: clean up comments

647f3d1


          offboard: comment update

0d455d0


          integration_tests: address review comments

a177a2d


          example: address review comments, improve printfs

4ff7859


          offboard: note about stop putting it in hold mode

00a2ac0


          offboard: locks were commented out by accident


          offboard: remove timeout check for command

92335d8

We don't need to check whether a command times out because that is
reliably handled inside device_impl.


          offboard: add is_active, stop if mode changes

a9c83ec

This adds an interface to check if offboard is still active which means
the vehicle is still in offboard mode and offboard setpoints are still
sent.

If the vehicle drops out of offboard mode, we stop sending setpoints and
it is required to re-initiate sending offboard commands.


          offboard: set cached setpoints before call_every

2c0a887

This prevents a race between the first time `send_velocity_` is called
and setting the cached setpoint that it will send.

julianoes force-pushed the improve-offboard branch from db67ed5 to 2c0a887 Compare

October 27, 2017 14:18

Collaborator Author

julianoes commented Oct 27, 2017

@julianoes One more doc fix please. For stop and stop_async in API it would be good to note that when offboard mode stops we put it the vehicle into Hold mode.

Added, thx.

If I move into Hold mode without calling stop(), will DroneCore realise that it has to stop sending setpoints?

Note yet, but it makes sense to add a check for it and stop sending setpoints... done in a9c83ec.

coveralls commented Oct 27, 2017

Coverage increased (+6.5%) to 55.131% when pulling 2c0a887 on improve-offboard into 97d98b9 on develop.

Collaborator Author

julianoes commented Oct 27, 2017 •

edited

Loading

We could merge this for now. I'll figure out the CI issues in a separate pull request. The problem is that timing sensitive tests fail on slow or busted CI instances. The solution will be to mock time.

hamishwillee approved these changes

View reviewed changes

Collaborator

hamishwillee left a comment

Sounds good to me!

julianoes merged commit 09053c3 into develop

julianoes deleted the improve-offboard branch

October 30, 2017 18:05

julianoes mentioned this pull request

Add fake time #147

Merged

Collaborator Author

julianoes commented Nov 1, 2017

CI troubles addressed in #147.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet