Altitude Hold

Is altitude hold still supported in ROSFlight? There does not seem to be a flight mode for doing altitude hold anymore.

If it is no longer supported, what methods are others using for achieving altitude hold with rosflight? I am working on a vertical velocity observer that fuses a downward facing lidar sensor and accel measurements, but I am interested in seeing if anyone has anything working currently.

Comments

  • 9 Comments sorted by Votes Date Added
  • Hi @mohrad.

    Altitude hold is not supported in ROSflight; it's a feature we thought about adding but it seemed outside the scope of the project so we've removed it.

    The way to achieve altitude hold is with a controller running in ROS on the onboard computer, which could close a PID loop or something around the estimates coming out of your observer. You would want to publish a rosflight_msgs/Command message to the /command topic, but tell it ignore everything except the F field, which is where you'd set your throttle command. This is accomplished by setting ignore field, which is a bitmask, to the value of IGNORE_X | IGNORE_Y | IGNORE_Z. (In C++ that would be rosflight_msgs::Command::IGNORE_X, etc.). The result is that the flight controller will follow the throttle commands from the computer (unless they're overriden) but follow RC for roll, pitch, and yaw rate.

    Your project sounds interesting, hopefully that helps!

  • Thanks for the response! This was my suspicion and I appreciate the added instructions on creating the command message.

  • edited August 9 Vote Up0Vote Down

    Follow up question:

    I am sending the command message out (60hz publish rate), but I get rapidly alternating ROS warnings alternating between "RC overide active" and "returned to computer control". I checked my RC output and everything is trimmed such that the roll/pitch/yaw channels are nominally at 1500, and the rest are at 1000. This is with the exception of the ARM_CHANNEL which goes from 1000 (unarmed) to 2000 (armed). I am not sure why it won't stay in computer control mode.

    I have the RC_ATT_OVRD_CHN and RC_THR_OVRD_CHN mapped to channels 5 and 6, which are nominally 1000. The last channel, 7, which is unmapped, is nominally at 1000 as well.

    If you have any thoughts on why this might be happening, please let me know!

    Thanks!!

  • One possibility has to do with the throttle override behavior. The firmware will take the minimum of the throttle commands coming from RC and offboard control. So if your throttle stick is low and your offboard command is moving around, it could switch to RC override every time the offboard command goes above the RC value. (You can turn this behavior off with the MIN_THROTTLE parameter, but we've found it to be a nice safety feature.) Are you keeping your throttle stick all the way up? If not, does that eliminate the switching?

    The other usual suspects are trims on the RC that cause it to go into override mode, which you've already eliminated as a potential cause, and offboard control timeout. The offboard control setpoint times out after 100ms, but the 60Hz you're publishing at is plenty fast to avoid that.

  • Thanks @dpkoch !! That did the trick. I was unaware that the controller accepts only the minimum value for throttle.

    I tested my altitude controller with the props off, and I can confirm that above the setpoint altitude the commanded throttle decreases, and conversely, increases when the vehicle is below the setpoint altitude. I am trying to determine how to seamlessly switch from pilot control to my altitude control mode. If I takeoff while publishing the control command from my altitude controller, it is more likely that the commanded throttle is much higher than the RC throttle because I would still be below the setpoint altitude. But once I get near the setpoint, it might be unclear which throttle command is lower. Going over the setpoint will definitely result in the commanded throttle to take over. I hope this makes sense, but without sending the RC throttle to the max value near the setpoint altitude, I am unsure of how the switching between offboard control and RC control on the throttle channel will take place in flight.

    An example: If the vehicle is over the setpoint altitude, the min throttle command will be coming from the controller, which will be trying to bring the vehicle back down to the setpoint. But if it overshoots the setpoint altitude going down, then the pilot RC command will become the min value as the controller commanded throttle will increase to bring the vehicle back to the setpoint. I suppose this might not be a problem, as the vehicle will still remain in flight, and one can continuously increase the RC throttle until the controller completely takes over.

    Alternatively, would it make sense to disable the min throttle command, and have the publishing of the controller commands handled by an auxiliary switch on the transmitter? You mentioned that offboard control will time out after 100ms of not receiving a command. So if I switch off the publishing of control commands, I would regain pilot control, correct? Conversely, switching on the publishing of control commands would disable pilot control? I am hesitant to test my control scheme with the props on without fully understanding how ROSFlight is going to handle the incoming control signals.

    I really appreciate your quick responses, and your input has been super helpful thus far!

  • Glad to hear @mohrad, sound like you're making great progress!

    Here's how we typically fly: On the ground, we'll start our altitude (and whatever else) controller running, then we'll arm and throttle up all the way to allow the controller to do its thing without interference from the RC. We just keep the throttle stick maxed out the whole time. This can be a little unnerving (especially with an untested controller), but we do it because it allows us to simply drop the throttle stick at any time to land the vehicle if something goes wonky. To make it a little more comfortable, I typically ease the throttle up until I can tell the controller has engaged and is working, then I throttle up the rest of the way until I'm ready to land it.

    We typically also have a switch mapped to override both attitude and throttle to completely lock out the computer, but in practice we never really use it. One caveat with using an override switch in combination with the min throttle behavior is that if you switch to manual control with the throttle stick still all the way up, excitement will ensue. That's why we typically only rely on the min throttle and stick deviation methods for our safety piloting. This works well for us, but we typically fly only a meter or two off the ground so if you're flying higher you may prefer other setups.

    One other setup is to disable the min throttle behavior, but still have the controller running before takeoff. Take off in manual override mode (using a switch on your transmitter) and get it hovering, then switch to offboard control mode using your transmitter switch. As long as you don't touch the throttle stick after that, then if something goes weird you can switch back to manual mode and you should be at least close to hover throttle immediately. You do however lose the ability to simply drop the throttle stick to kill the props, which is really helpful in panic mode when your vehicle is trying to fly into a wall or something, so it's a tradeoff and a bit of a personal preference choice. I prefer the first method, but this could work.

    I would recommend one of those two approaches. It could potentially work to turn the publishing on/off with a transmitter switch, but I would probably just have the controller running the whole time and rely on the flight controller safety pilot functionality to do the switching (that approached has been much better tested). If you need to add some integrator anti-windup functionality in to your controller when it's being overriden, you could have your controller subscribe to the /status topic and use the armed and rc_override fields to understand whether your commands are being ignored or not.

    Does that make sense and/or help? Let me know if you need clarification or have further questions.

  • This is perfect, thanks @dpkoch ! I agree that having the min throttle option is useful if something goes wrong. I will give the first option a shot and see how it feels.

    Thanks again for your detailed responses!

  • So I tried it my way first, where I was changing the publish status of the onboard command, and the quad shot off forcing me to cut all control and have it drop down. I didn't have time to correct the sideways velocity which was induced from just flicking straight into offboard control from the ground.

    I learned very quickly the benefit of being able to ease into takeoff manually with MIN_THROTTLE on. It was very easy to see the controller take over when it should, allowing me to seamlessly push the throttle to max and have the controller take over. It was also very easy to regain control and land by dropping the throttle.

    Thanks for the guidance @dpkoch !

  • Glad that's working out for you @mohrad. Thanks for reporting back, and best of luck on the rest of your project!

Sign In or Register to comment.