Skip to content
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.

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

offboard: automatically send setpoins at set rate #134

Merged
merged 18 commits into from Oct 30, 2017

Conversation

Projects
None yet
4 participants
@julianoes
Copy link
Contributor

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.

@coveralls

This comment has been minimized.

Copy link

coveralls commented Oct 25, 2017

Coverage Status

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

@coveralls

This comment has been minimized.

Copy link

coveralls commented Oct 25, 2017

Coverage Status

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

}
}
}

This comment has been minimized.

Copy link
@hamishwillee

hamishwillee Oct 25, 2017

Contributor

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

This comment has been minimized.

Copy link
@julianoes

julianoes Oct 26, 2017

Author Contributor

Very good catch, thank you!

@hamishwillee

This comment has been minimized.

Copy link
Contributor

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 ?

@hamishwillee

This comment has been minimized.

Copy link
Contributor

hamishwillee commented Oct 25, 2017

Looks awesome. Some thoughts:

  1. 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?
  2. 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!
 */
  1. Is there ever a case where someone might want to resend at lower than 20Hz?
  2. 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
Copy link
Contributor

hamishwillee left a comment

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

@hamishwillee

This comment has been minimized.

Copy link
Contributor

hamishwillee 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

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. */
@@ -76,12 +78,9 @@ bool offb_ctrl_ned(Device &device)

This comment has been minimized.

Copy link
@hamishwillee

hamishwillee Oct 26, 2017

Contributor

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

This comment has been minimized.

Copy link
@julianoes

julianoes Oct 26, 2017

Author Contributor

Removed.

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

// Let yaw settle.

This comment has been minimized.

Copy link
@hamishwillee

hamishwillee Oct 26, 2017

Contributor

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

This comment has been minimized.

Copy link
@julianoes

julianoes Oct 26, 2017

Author Contributor

Removed.

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

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

This comment has been minimized.

Copy link
@hamishwillee

hamishwillee Oct 26, 2017

Contributor

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

This comment has been minimized.

Copy link
@shakthi-prashanth-m

shakthi-prashanth-m Oct 27, 2017

Contributor

makes sense

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

This comment has been minimized.

Copy link
@hamishwillee

hamishwillee Oct 26, 2017

Contributor

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.

This comment has been minimized.

Copy link
@julianoes

julianoes Oct 26, 2017

Author Contributor

Thanks, changed it along these lines.

@@ -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");

This comment has been minimized.

Copy link
@hamishwillee

hamishwillee Oct 26, 2017

Contributor

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

}
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");

This comment has been minimized.

Copy link
@hamishwillee

hamishwillee Oct 26, 2017

Contributor

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

}
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");

This comment has been minimized.

Copy link
@hamishwillee

hamishwillee Oct 26, 2017

Contributor

@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.

This comment has been minimized.

Copy link
@julianoes

julianoes Oct 26, 2017

Author Contributor

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

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");

This comment has been minimized.

Copy link
@hamishwillee

hamishwillee Oct 26, 2017

Contributor

Turn clockwise to face East (90 deg.)"

This comment has been minimized.

Copy link
@hamishwillee

hamishwillee Oct 26, 2017

Contributor

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.

This comment has been minimized.

Copy link
@julianoes

julianoes Oct 26, 2017

Author Contributor

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

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");

This comment has been minimized.

Copy link
@hamishwillee

hamishwillee Oct 26, 2017

Contributor

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

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

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

This comment has been minimized.

Copy link
@hamishwillee

hamishwillee Oct 26, 2017

Contributor

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

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

This comment has been minimized.

Copy link
@hamishwillee

hamishwillee Oct 26, 2017

Contributor

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

This comment has been minimized.

Copy link
@julianoes

julianoes Oct 26, 2017

Author Contributor

I've removed all.

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");

This comment has been minimized.

Copy link
@hamishwillee

hamishwillee Oct 26, 2017

Contributor

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

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

This comment has been minimized.

Copy link
@hamishwillee

hamishwillee Oct 26, 2017

Contributor

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

This comment has been minimized.

Copy link
@julianoes

julianoes Oct 26, 2017

Author Contributor

Ok.

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

This comment has been minimized.

Copy link
@hamishwillee

hamishwillee Oct 26, 2017

Contributor

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

This comment has been minimized.

Copy link
@julianoes

julianoes Oct 26, 2017

Author Contributor

Ok.

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.

This comment has been minimized.

Copy link
@hamishwillee

hamishwillee Oct 26, 2017

Contributor

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

This comment has been minimized.

Copy link
@julianoes

julianoes Oct 26, 2017

Author Contributor

Removed.

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});

This comment has been minimized.

Copy link
@hamishwillee

hamishwillee Oct 26, 2017

Contributor

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.

This comment has been minimized.

Copy link
@julianoes

julianoes Oct 26, 2017

Author Contributor

Sure.

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

This comment has been minimized.

Copy link
@hamishwillee

hamishwillee Oct 26, 2017

Contributor

Change to at least 8 seconds.

@julianoes

This comment has been minimized.

Copy link
Contributor Author

julianoes commented Oct 26, 2017

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.

@julianoes

This comment has been minimized.

Copy link
Contributor Author

julianoes commented Oct 26, 2017

  1. 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?

  1. 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.

  1. 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.

  1. 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().

@julianoes

This comment has been minimized.

Copy link
Contributor 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 julianoes force-pushed the improve-offboard branch from 2c066b6 to db67ed5 Oct 26, 2017

@@ -164,17 +247,19 @@ void OffboardImpl::set_velocity_ned(Offboard::VelocityNEDYaw velocity_ned_yaw)
//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();

This comment has been minimized.

Copy link
@shakthi-prashanth-m

shakthi-prashanth-m Oct 27, 2017

Contributor

Does this lock makes sure _velocity_body_ned_yaw has updated value?

This comment has been minimized.

Copy link
@julianoes

julianoes Oct 27, 2017

Author Contributor

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

julianoes added some commits Oct 25, 2017

core: added a call every functionality
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: add way to reset call_every
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
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
The new API does not require the setpoint to be constantly set.
offboard: remove timeout check for command
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
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
This prevents a race between the first time `send_velocity_` is called
and setting the cached setpoint that it will send.

@julianoes julianoes force-pushed the improve-offboard branch from db67ed5 to 2c0a887 Oct 27, 2017

@julianoes

This comment has been minimized.

Copy link
Contributor 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

This comment has been minimized.

Copy link

coveralls commented Oct 27, 2017

Coverage Status

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

@julianoes

This comment has been minimized.

Copy link
Contributor Author

julianoes commented Oct 27, 2017

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
Copy link
Contributor

hamishwillee left a comment

Sounds good to me!

@hamishwillee

This comment has been minimized.

Copy link
Contributor

hamishwillee commented on plugins/offboard/offboard.h in a9c83ec Oct 27, 2017

So presumably an app in offboard could be monitoring flight mode, and warn if flight mode is "dropped out of" after checking this.

This comment has been minimized.

Copy link
Contributor Author

julianoes replied Oct 30, 2017

Yes, correct.

This comment has been minimized.

Copy link
Contributor

hamishwillee replied Oct 31, 2017

Thanks. I guess this makes sense, though I tend to like getting feedback from invalid function calls - less thinking and work for the programmer.

@julianoes julianoes merged commit 09053c3 into develop Oct 30, 2017

2 of 4 checks passed

continuous-integration/travis-ci/push The Travis CI build could not complete due to an error
Details
continuous-integration/appveyor/pr AppVeyor build failed
Details
continuous-integration/travis-ci/pr The Travis CI build passed
Details
coverage/coveralls Coverage increased (+6.5%) to 55.131%
Details

@julianoes julianoes deleted the improve-offboard branch Oct 30, 2017

@julianoes julianoes referenced this pull request Nov 1, 2017

Merged

Add fake time #147

@julianoes

This comment has been minimized.

Copy link
Contributor 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
You can’t perform that action at this time.