LibrePilot Forum

Development => Firmware General => Topic started by: sallyc on August 15, 2017, 02:58:24 pm

Title: Low level (Attitude mode) stabilization on the CC3D and navigation in Pi
Post by: sallyc on August 15, 2017, 02:58:24 pm
Hello,

My intetion is to do the low level (Attitude mode) stabilization on the CC3D and navigation in Pi with my own algorithms.

I donot have GPS (in door flying), I will be using Attitude mode.

I integrated the Pi and CC3D using serial. I  use UAVObjects to receive and send data from Pi (thanks for the Python API).

I can read whatever I need from UAVobjects, the quadcopter state, Rc commands etc.

But how can I send my commands to CC3D? Shall I override Rc commands?

What is the best way to use telemetry commands (UAVobjects) and Rc together?

CC3D will not stop processing Rc commands. The values from Pi and Rc will interfare. How to solve it?

Is it possible to change input channel configuration from PWM to GCS on the fly (using UAV Setting Objects) and still having the Rc commands in UAVObjects?

Bests,

Sally.

Title: Re: Low level (Attitude mode) stabilization on the CC3D and navigation in Pi
Post by: TheOtherCliff on August 15, 2017, 07:27:36 pm
The GCS can take over control of the FC and bypass RC input, you would just need to determine how it does it.

In the flight code I recall this has to do with the inline function ManualControlCommandReadOnly()

I haven't researched it...
Title: Re: Low level (Attitude mode) stabilization on the CC3D and navigation in Pi
Post by: wojakzek on August 16, 2017, 08:28:56 am
Thanks TheOtherCliff. I think UAVObject ManualControlSettings enables us to change from PWM to GCS, even during runtime.

In the same UAVObject specification, there is <access gcs="readwrite" flight="readwrite"/> . How can we configure them to read from Rc and overwrite from telemetry?

In ManualControlCommand specification, the description says:

<object name="ManualControlCommand" singleinstance="true" settings="false" category="Control">
        <description>The output from the @ref ManualControlModule which decodes the receiver inputs. Overriden by GCS for fly-by-wire control.</description>

How can we achieve "Overriden by GCS for fly-by-wire control."?


Title: Re: Low level (Attitude mode) stabilization on the CC3D and navigation in Pi
Post by: f5soh on August 16, 2017, 11:22:55 am
You need to take control of ManualControlCommand and AccessoryDesired (if needed), you need to disable the previous RC Input configured and avoid two sides changing the same UAVO.

You can try two solutions:
- temporary set the input type and input channel to None.
- change metadata and set readonly from flight side.

Maybe like in example.py for ActuatorCommand:
Code: [Select]
    def driveServo(self):
        print "Taking control of self.actuatorCmd"
        self.objMan.ActuatorCommand.metadata.access = UAVMetaDataObject.Access.READONLY
        self.objMan.ActuatorCommand.metadata.updated()

        while True:
            self.objMan.ActuatorCommand.Channel.value[0] = 1000
            self.objMan.ActuatorCommand.updated()
            time.sleep(1)

            self.objMan.ActuatorCommand.Channel.value[0] = 2000
            self.objMan.ActuatorCommand.updated()
            time.sleep(1)
Title: Re: Low level (Attitude mode) stabilization on the CC3D and navigation in Pi
Post by: wojakzek on August 16, 2017, 12:49:36 pm
Thanks f5soh. That is a great example. I like the idea to make it read only to the flight side (as in the code) and set it from the UAVtalk. How can be get/read the Rc commands at the same time when ManualControlCommand is readonly from flight side? Is it possible? I could not find any other place (then ManualControlCommand) in UAVO that Rc commands are written. Is there any intermidiate buffer?
Title: Re: Low level (Attitude mode) stabilization on the CC3D and navigation in Pi
Post by: f5soh on August 16, 2017, 01:26:36 pm
Raw input values from receiver are populated into ManualControlCommand > Channels
Next, in ManualControlCommand > Throttle, Pitch, Roll, Yaw the values are translated to -1 > 0 > +1 range according to the settings (min/neutral/max + channel mapping)

If the readonly solution do not work the first one disabling inputs will work, you need to experiment by yourself.
Title: Re: Low level (Attitude mode) stabilization on the CC3D and navigation in Pi
Post by: sallyc on August 19, 2017, 03:05:46 pm
You need to take control of ManualControlCommand and AccessoryDesired (if needed), you need to disable the previous RC Input configured and avoid two sides changing the same UAVO.

You can try two solutions:
- temporary set the input type and input channel to None.
- change metadata and set readonly from flight side.

Maybe like in example.py for ActuatorCommand:
Code: [Select]
    def driveServo(self):
        print "Taking control of self.actuatorCmd"
        self.objMan.ActuatorCommand.metadata.access = UAVMetaDataObject.Access.READONLY
        self.objMan.ActuatorCommand.metadata.updated()

        while True:
            self.objMan.ActuatorCommand.Channel.value[0] = 1000
            self.objMan.ActuatorCommand.updated()
            time.sleep(1)

            self.objMan.ActuatorCommand.Channel.value[0] = 2000
            self.objMan.ActuatorCommand.updated()
            time.sleep(1)
Hello, Thanks for your replying. what do you mean - temporary set the input type and input channel to None? If I set the input type and input channel to None then I cannot get data from RC. 
Title: Re: Low level (Attitude mode) stabilization on the CC3D and navigation in Pi
Post by: f5soh on August 19, 2017, 06:35:05 pm
Quote
If I set the input type and input channel to None then I cannot get data from RC.


And if you keep the Rc active you cannot get control from Pi3.
If you want control from Pi3 and python you cannot take control using Rc at same time.
Title: Re: Low level (Attitude mode) stabilization on the CC3D and navigation in Pi
Post by: sallyc on September 05, 2017, 12:20:01 pm
Raw input values from receiver are populated into ManualControlCommand > Channels
Next, in ManualControlCommand > Throttle, Pitch, Roll, Yaw the values are translated to -1 > 0 > +1 range according to the settings (min/neutral/max + channel mapping)

If the readonly solution do not work the first one disabling inputs will work, you need to experiment by yourself.
Hello f5soh,
Does that mean the value of Throttle, Pitch, Roll, Yaw are always in the range of -1 to 1? Because when I read the value on pi now, sometimes it is beyond this range.
I think if i want to control cc3d with pi, this 4 value of ManualControlCommand is enough, right?
Title: Re: Low level (Attitude mode) stabilization on the CC3D and navigation in Pi
Post by: TheOtherCliff on September 05, 2017, 12:46:30 pm
[-1,+1] for ManualControlCommand Roll Pitch Yaw Thrust Throttle

There are other UAVOs where the values are scaled differently though, like in degrees or degrees per second.

In GCS you can go to System page and expand DataObjects then expand ManualControlCommand and watch the values in close to real time.
Title: Re: Low level (Attitude mode) stabilization on the CC3D and navigation in Pi
Post by: sallyc on September 05, 2017, 02:23:41 pm
[-1,+1] for ManualControlCommand Roll Pitch Yaw Thrust Throttle

There are other UAVOs where the values are scaled differently though, like in degrees or degrees per second.

In GCS you can go to System page and expand DataObjects then expand ManualControlCommand and watch the values in close to real time.
Thank you TheOtherCliff,

And what is the difference between thrust and throttle? Do I have to populate the values for Pitch, Yaw, Roll, Thtrottle and Thrust for ManualControlCommand to control it from pi side? Or do I need to take control of more UAVOs?
Title: Re: Low level (Attitude mode) stabilization on the CC3D and navigation in Pi
Post by: TheOtherCliff on September 05, 2017, 04:35:53 pm
Thrust and Throttle are the same value for multicopters and Collective is unused.

I recall that collective pitch helis have Thrust and Collective with the same value.  For CP, the calculation model assumes that the Throttle channel sets the motor RPM and from then on a motor/engine governor maintains that RPM with the varying load.

So you need to populate the Thrust field / member because that is the field the code always uses.  The later code ignores the Throttle and Collective fields.
Title: Re: Low level (Attitude mode) stabilization on the CC3D and navigation in Pi
Post by: sallyc on September 09, 2017, 03:21:27 pm
Thrust and Throttle are the same value for multicopters and Collective is unused.

I recall that collective pitch helis have Thrust and Collective with the same value.  For CP, the calculation model assumes that the Throttle channel sets the motor RPM and from then on a motor/engine governor maintains that RPM with the varying load.

So you need to populate the Thrust field / member because that is the field the code always uses.  The later code ignores the Throttle and Collective fields.
Thank you very much, TheOtherCliff,
Now I can control it, to make sure, so the range of thrust is -1 to 1 too, right? The same as Throttle
Title: Re: Low level (Attitude mode) stabilization on the CC3D and navigation in Pi
Post by: TheOtherCliff on September 09, 2017, 07:14:06 pm
The range is [-1,1], but everything from [-1,0] is throttle off, maps to -1.

Beware of exactly zero, I recall that it is throttle off, but maps to 0 where negative values all map to -1.  This may no longer be true.
Title: Re: Low level (Attitude mode) stabilization on the CC3D and navigation in Pi
Post by: irissmit on December 06, 2017, 07:15:59 pm
Dear all,

We are trying to do the exact same thing.
We also use the example.py and with this example we are able to read the attitude state, and to control the servo directly.
Based on the example to control the servo, we try to populate the ManualControlCommand with Roll, Pitch, Yaw, Throttle.

Code: [Select]
        self.objMan.ManualControlCommand.metadata.access = UAVMetaDataObject.Access.READONLY
        self.objMan.ManualControlCommand.metadata.updated()

        while True:
            print "."
            self.objMan.ManualControlCommand.Throttle.value = 0.9
            self.objMan.ManualControlCommand.Roll.value = 0.9
            self.objMan.ManualControlCommand.Pitch.value = 0.9
            self.objMan.ManualControlCommand.Yaw.value = 0.9
            self.objMan.ManualControlCommand.Thrust.value = 0.9
            self.objMan.ManualControlCommand.updated()
            time.sleep(1)

            self.objMan.ManualControlCommand.Throttle.value = 0.1
            self.objMan.ManualControlCommand.Roll.value = 0.1
            self.objMan.ManualControlCommand.Pitch.value = 0.1
            self.objMan.ManualControlCommand.Yaw.value = 0.1
            self.objMan.ManualControlCommand.Thrust.value = 0.1
            self.objMan.ManualControlCommand.updated()
            time.sleep(1)

But I don't see anything happen on the drone.
I read in this forum that it is possible to set the input type and input channel to None, but I don't understand how to do it. Can someone tell me how to do that.

Kind regards,
Iris
Title: Re: Low level (Attitude mode) stabilization on the CC3D and navigation in Pi
Post by: TheOtherCliff on December 06, 2017, 09:04:02 pm
I am not an expert in this area but there are several places where ManualControlCommand gets set, and if you don't disable those in the firmware or with some setup, your values may be overwritten.

I would suggest you search the firmware source code for places where ManualControlCommand gets set and examine the statements.

The code tries to be safe, and that means that things have to be configured correctly for flight for ManualControlCommand values to be used to actually run the motors.  Being configured correctly means that ManualControlCommand is already getting values from somewhere else.

My initial thought about this is that I would have the Pi pretend it is an RC receiver and send some simple protocol (PPM, SBUS, etc.) to the FC receiver input.  Each protocol has pluses and minuses.  PPM is simple, but requires precise timing (is there a driver for PPM for RPi?)  SBus is a serial protocol, but needs to be inverted in hardware (next has an option to skip the inversion).  Maybe send telemetry packets like you are a GCS with a joystick.  Maybe someone else can suggest other ways.
Title: Re: Low level (Attitude mode) stabilization on the CC3D and navigation in Pi
Post by: irissmit on December 09, 2017, 09:56:39 am
Hi theOtherCliff,

Thank you for your reply. I am going to investigate where the ManuelControlCommnad is used and how it is configured. If there is someone else who knows how the settup the input it would be great.

About your suggestion to pretend it is an RC receiver. We have already tried that. We have had the PPM working, but we didn't get the timing stable with a software only. We didn't know about the Sbus option, can you tell me more about that option?
Title: Re: Low level (Attitude mode) stabilization on the CC3D and navigation in Pi
Post by: TheOtherCliff on December 09, 2017, 08:13:19 pm
SBus is a Futaba protocol that can be implemented with simple serial transmit functions.  The problem is that the signal polarity is actually backwards at the hardware level.  That needs a simple hardware inverter, or disable the hardware inverter on the CC3D (hardware mod or simple change to firmware), or use the unreleased development version called "next" (you usually want to build it yourself) because it has an option to do the non-inverted SBus.

You could look at the LP code to see how SBus is decoded and make a matching encoder for your project, or find an SBus (S.Bus) protocol description on the internet somewhere.

To make a hardware change to skip inverting the SBus, look at the schematic, find the external inverter chip connected to MainPort, note that a signal line tells it whether to invert it or not, cut the line, permanently tie the line high or low, I don't know which without doing this research.

I would guess that someone has written an RPi driver to do PPM accurately, but it would require driver privileges and interrupts/DMA, not application privileges and bit banging.
Title: Re: Low level (Attitude mode) stabilization on the CC3D and navigation in Pi
Post by: f5soh on December 09, 2017, 08:49:08 pm
Here is a example where the Python script take control over RC inputs.
At the beginning and end the sticks move according to Rc radio.

https://www.youtube.com/watch?v=n_uDj-zJRjE

Code is here (https://bitbucket.org/librepilot/librepilot/pull-requests/481/lp-564-add-manualcontrolcommand-example-to/diff)
Hope this helps.
Title: Re: Low level (Attitude mode) stabilization on the CC3D and navigation in Pi
Post by: sallyc on December 14, 2017, 12:26:32 pm
The range is [-1,1], but everything from [-1,0] is throttle off, maps to -1.

Beware of exactly zero, I recall that it is throttle off, but maps to 0 where negative values all map to -1.  This may no longer be true.
Hello TheOtherCliff,
I see in ManualControlCommand, the range is [-1,1], but in AttitudeState, the unit is degree. How can I map them?
Title: Re: Low level (Attitude mode) stabilization on the CC3D and navigation in Pi
Post by: sallyc on December 14, 2017, 03:54:30 pm
Here is a example where the Python script take control over RC inputs.
At the beginning and end the sticks move according to Rc radio.

https://www.youtube.com/watch?v=n_uDj-zJRjE

Code is here (https://bitbucket.org/librepilot/librepilot/pull-requests/481/lp-564-add-manualcontrolcommand-example-to/diff)
Hope this helps.
How can I do this while code running on pi and GCS running on computer?
Title: Re: Low level (Attitude mode) stabilization on the CC3D and navigation in Pi
Post by: TheOtherCliff on December 14, 2017, 04:43:20 pm
Stabilization->Basic->Bank#->Attitude is the maximum bank angle in degrees in Attitude mode.

By default it is 55, so [-1.0f,1.0f] corresponds to [-55.0f,55.0f].

You could go through the hassle of reading the StabilizationSettingsBank# .RollMax .PitchMax (.YawMax) or set these to what you need and assume they are the value you set.

A note here.  See the .YawMax?  It is almost never used, but you can actually set yaw to Attitude mode and set this to maybe 180.

If I recall correctly, ManualControlCommand feeds StabilizationDesired and StabilizationDesired is scaled in degrees for Attitude mode or degrees per second for Rate mode.
Title: Re: Low level (Attitude mode) stabilization on the CC3D and navigation in Pi
Post by: f5soh on December 14, 2017, 09:05:00 pm
Quote
How can I do this while code running on pi and GCS running on computer?
This is done connecting GCS to USB and python using a USBSerial connected to MainPort.
Not sure a CC3D can handle this at same time.
Title: Re: Low level (Attitude mode) stabilization on the CC3D and navigation in Pi
Post by: sallyc on December 14, 2017, 10:20:59 pm
Stabilization->Basic->Bank#->Attitude is the maximum bank angle in degrees in Attitude mode.

By default it is 55, so [-1.0f,1.0f] corresponds to [-55.0f,55.0f].

You could go through the hassle of reading the StabilizationSettingsBank# .RollMax .PitchMax (.YawMax) or set these to what you need and assume they are the value you set.

A note here.  See the .YawMax?  It is almost never used, but you can actually set yaw to Attitude mode and set this to maybe 180.

If I recall correctly, ManualControlCommand feeds StabilizationDesired and StabilizationDesired is scaled in degrees for Attitude mode or degrees per second for Rate mode.
Sorry i don't understand, i wanna to map the attitudeState to [-1,1], so that i can give ManualControlCommand correct to the controller.
Title: Re: Low level (Attitude mode) stabilization on the CC3D and navigation in Pi
Post by: TheOtherCliff on December 15, 2017, 01:26:42 am
AttitudeState .Roll .Pitch .Yaw are in degrees

I recall that roll is +-180, pitch is +-90, and yaw is +-180

Or use the quaternion q1:q2:q3:q4 which will be in radians
Title: Re: Low level (Attitude mode) stabilization on the CC3D and navigation in Pi
Post by: NXTNiklas on December 18, 2017, 03:28:56 pm
Here is a example where the Python script take control over RC inputs.
At the beginning and end the sticks move according to Rc radio.

https://www.youtube.com/watch?v=n_uDj-zJRjE

Code is here (https://bitbucket.org/librepilot/librepilot/pull-requests/481/lp-564-add-manualcontrolcommand-example-to/diff)
Hope this helps.
Thanks for the example! I tried it and it runs without errors, but the motors doesn't move.... I don't understand why it doesn't, the board gets armed through your program..

EDIT: Sorry my fault! I just didn't set the thrust value high enough! Thanks it works!
Title: Re: Low level (Attitude mode) stabilization on the CC3D and navigation in Pi
Post by: sallyc on January 09, 2018, 08:22:51 pm
Here is a example where the Python script take control over RC inputs.
At the beginning and end the sticks move according to Rc radio.

https://www.youtube.com/watch?v=n_uDj-zJRjE

Code is here (https://bitbucket.org/librepilot/librepilot/pull-requests/481/lp-564-add-manualcontrolcommand-example-to/diff)
Hope this helps.

Hi,
here is my code, i only chance something in driveServo.
I get AttitudeState and AccelState and sensor data(I added myself), and use then to be the inputs of a function, getting the outputs of this function and give to ManualControlCommand.
I want it to run in a rate of 50HZ, but i dont know how to achieve that. can anyone help me?
also, the sensor stops sensor after a while here but in the example code it continue sensing.