So I think this SIL stuff could be really cool; it'd be good both for people who just want to use the firmware as is but need a simulation option, and it would really open it up for people who want to do things like develop and implement new attitude controllers on the autopilot itself.

One challenge with the current implementation though, is that it requires duplicating a lot of code between ROSflight2 and the fcu_sim plugin in order to change the low-level functionality to work with gazebo instead of BreezySTM32.

A potential solution would be to implement a board support layer (BSL) that abstracts away the serial communication, sensors, actuators, and memory aspects of the firmware. The actual autopilot code that is not board-dependent would then be separated out and sit on top of that.

For our case, this means that we could have BreezySTM32 BSL implementation and a ROS/gazebo BSL implementation that would be interchangeable without having to modify any of the higher level functionality. Depending on how we structure things, we could even have the ROS/gazebo BSL implementation live inside of the gazebo plugin so we don't clutter up our firmware.

In fcu_io, we could also have two different interfaces: one that uses an actual serial port to pass mavlink messages back and forth (the current implementation), and one that uses ROS to pass mavlink messages back and forth. The ROS/gazebo BSL implementation would then use that interface instead of the SerialWrite from BreezySTM32. Then since both versions go through fcu_io, the ROS interface would be absolutely identical between hardware and sim.

In summary, I think it could look something like this:

There are a few ways we could implement it. One example, using C++, is given once again by @simondlevy at https://github.com/simondlevy/hackflight. If we want to stick with plain C, another option would be to just create a board.h header file with function prototypes, then for each case create a board.c/cpp file that implements those functions and then include it in the sources list. There would definitely be some technical issues to iron out, but it could potentially be pretty slick.

Thoughts @superjax?

I think we are ready to start trying control from MAVlink. We just need the hooks in fcu_io2 to send the MAVlink control messages. @dpkoch, do you think you could help me get that set up? I think I'm almost done getting the muxing between MAVlink and RC working, but i've been manually creating the control struct to make it work. The last step is to start streaming offboard control messages.

By the way, I wrote a latex document about the signal muxing.

rc.pdf

We just need to add and integrator anti-windup scheme. (Perhaps Section 6.5 of the UAV book?)

After the asynchronous I2C driver was implemented, I noticed that occasionally, the barometer stops responding. Prior to this, we haven't spent much time really looking into the barometer, so I can't be sure if this is due to the changes in I2C driver, or if it has been happening all along and we didn't know about it.

I borrowed a saelae from work (an awesome logic analyzer), and saw that the barometer sometimes just sends zeros instead of actual data. In the datasheet, it says that this may happen if the barometer is polled too often, but after it sends one set of zeros, it keeps sending them, which the datasheet does not indicate would happen. If I slow down the polling rate, it takes longer to happen, but it does happen eventually.

I think that the way to solve this is to detect a bad packet and reset the barometer. Bad packets are easy to spot, since they are all zeros, and that measurement correlates to somewhere like zero psi, which is likely never going to happen.

I decided to test the RC muxing and offboard control stuff with Gazebo. I figured that it would be pretty easy to just grab the command messages coming off the autopilot and push them to the naze. It was incredibly easy.

I'm wondering if we should make this a supported feature. I suppose I'll have to play around with bandwidth, which is the main concern, but I think that it wouldn't be too terrible to support a HIL simulator for ROSflight and Gazebo, certainly for fixed-wings it shouldn't be too much of a problem, but I'm thinking that it actually wouldn't be too crazy to get it going for the multirotors too.

Stuff we would have to do.

• Forces and Moments would need to subscribe to the servo_output_raw topic, rather than command like it currently does
• IMU would need to be streamed into the naze, rather than out of it.

That doesn't sound too bad. I am certain that there is enough bandwidth to handle the fixed wing information. I'm not so sure about the multirotor (in fact, I'm not even sure that we can get Gazebo running at a full 1000 Hz to simulate the multirotor) Something to think about.

Not sure what is going on. I did make some changes to the way the z-axis was handled in the estimator, I need to figure out if what I changed broke something.

We just need to implement a sonar.

We need to derive a complementary filter for altitude in order to perform altitude control.

I haven't found any good references for this yet, unfortunately. If I find some, I'll put it up.

We need to be able to stream the magnetometer from onboard the naze to ROS (not for control, but for completeness)

Parameter writing doesn't seem to complete; the light doesn't flash, no command acknowledgement is sent over MAVLink, and parameters aren't saved

Mavlink is having a couple of issues:

1. Sometimes doesn't start streaming on startup; have to send stream rate commands to get it started
2. Parameters don't play nice with QGroundControl anymore; it always misses one or two of them when requesting the parameter list

@superjax So I flashed the latest firmware on both a rev5 and a rev2 Flip32 board. When streaming IMU over mavlink, the accel scale factor in fcu_io2 rev5 board is correct (I get 9.8m/s^2 at rest). The scale factor is off by 2 (I get 4.9m/s^2 at rest) for the rev2 board.

The solution to this problem may depend on fcu_io2, but it's also possible that this could be affecting onboard estimation. On the fcu_io2 side we could imlement some way to retrieve the scale factor from the naze. Another possibly is to send scaled values over mavlink instead, although I'm not sure what the answer is there based on our previous discussion.

Either way, we'll need to ensure that the accel is being scaled correctly on board when necessary (e.g. for altitude estimation). We may already have what we need from the mpu6050_init, but we'll need to check.

There is currently some goofy stuff going on where we calculate values in floats, store them as ints, then convert them back to floats before we use them. There are also several parameters that should be converting to floats, such as the controller gains.

The last thing we need to do to get ready for flight testing is support the airspeed sensor.

• Add airspeed sensor to BreezySTM32
• Add to ROSflight2
• Support MAVlink streaming of airspeed message

Hey @dpkoch,

I've been thinking a lot about trying to get good IMU timestamps. I talked with Dr. McLain, and he pointed out that we can often make do with just knowing the difference between measurements, rather than the absolute timestamp. I really liked that idea, and I'm wondering if you could add a field to the IMU message (could be just int16), for a time stamp. It would be defined as the number of microseconds since the last IMU measurement was taken that this measurement was taken.

Simon pulled my changes to Breezy so we have an interrupt routine to play with to get the IMU timing data, so it should be pretty straight-forward.

I have noticed some discreptancies in timing. I believe that this is coming from a variable amount of random junk being on the UART buffer at any time on both the computer and the flight controller. I believe that if we were to use the onboard oscillator, then we would be able to sync up the time and let it go for a long period without touching it.

Right now, the chip does not use the onboard oscillator. I don't really know why, but I have noticed in the code that it does not use it.

Do the RC-specific stuff first, then do the muxing in another file

We need a way to abstract RC switches, these can be used in rc.c and when arming in mode.c

Hey @superjax, so I was wondering if we wanted to pursue getting a BYU domains website for this project? I emailed and asked if they could set up a group domain for us, and this is the response I got:

In order for us to set up a group BYU domains account, it will have to be sponsored by a full-time faculty or staff member and be "owned" by the department or college. If you find a sponsor for this website and it is "owned" by the the department or college (or the MAGICC lab in this case) send us the following information and we can get this started for you.

1. Purpose of the shared account.
2. Full name of the sponsor of the account:
   • (must be a full time employee/faculty member):
3. Preferred Net ID:
   • (to be created for the group, not a person in the group,)
4. Preferred Domain Name:
5. Preferred Email Alias for the group:
   • (should be an existing @byu.edu address for the group, if not, an alias will be created, this alias will point to the full-time sponsor)

Notes:

1. The Preferred Net ID should not be an existing Net ID. A non-person Net ID will be created for the group, so the name of the Net ID to be created should be relevant and related to the name of the requesting group.
2. The Preferred Email Alias should be a previously created @byu.edu email address associated with the requesting group. If the group does not have an existing @byu.edu email address already, an @byu.edu alias should be created that is relevant and related to the requesting group.

What do you think, should we pursue this? Do you think Dr. McLain would be okay with sponsoring it, and do we want the website to be "owned" by the MAGICC lab?

@superjax I was thinking we might be at the point where we want to start creating releases. That would allow us to define points in the code where we think there's a stable and substantial change for people to upgrade to. In addition, we could attach the compiled firmware binary to the release so people don't have to build it themselves, just flash it.

I propose releasing our current code in the master branch as v0.1.0, since it's kind of the first pre-release where most of the functionality works. What do you think? If you approve I'll give it a try.

I started looking into the I2C routine in Breezy, to figure out why I2C takes so long (Per the research done by @dpkoch, I2C reading of the IMU takes about 500 us, which is more than double any other routine).

It is actually designed to be asychronous, but there are some design decisions that leave it blocking. It appears that whoever wrote i2cWriteBuffer, i2cRead and i2cWrite wanted them to return a flag of whether or not it succeeded, and you can't do that unless you block and wait until the write is finished.

Hence these lines are in both i2cWriteBuffer and i2cRead

    timeout = I2C_DEFAULT_TIMEOUT;
    while (busy && --timeout > 0) {
        ;
    }

busy is modified by the function i2c_ev_handler which is already set up to be the ISR for the i2c

nvic.NVIC_IRQChannel = i2cHardwareMap[index].ev_irq;

If we moved to an asynchronous I2C system (which shouldn't be too hard), the most difficult part would be making sure that we don't try to run the estimator unless we have actually received the new values, and perhaps creating a queue of pending I2C operations.

Just some research

Hey, @dpkoch, I just learned how to protect a branch. Should we protect master?

A few things to try:

• Use the xx_send_buf() function instead of the xx_send() function; it's supposed to be more efficient
• Use the SCALED_IMU message type instead of HIGHRES_IMU; it's 22 bytes as opposed to 62
• If that still doesn't work, create a custom mavlink message (e.g. SMALL_IMU) that strips out the timestamp (4 bytes) and maybe the magnetometer(?) (6 bytes), leaving us with 18 or 12 bytes.

Questions to answer:

• Is SerialWrite blocking or asynchronous? - It is asynchronous
• Is it buffered? - It is also buffered
• What is the limiting factor on message rates? Do they start colliding or overwriting messages halfway through write?
• How to make the serial sending more robust based on the answers to the questions above. May include buffering or checks on whether the serial line is busy, etc.

We need a way to save trims from the RC transmitter into the FCU so that ROS is controlling with respect to trimmed commands

I know this because I tried to fly from ROS, and the trims were off, and it was nearly uncontrollable.

We need to build an altitude controller in the Naze

We need a way to know if the RC has stopped responding. If it has, then we need to enter a failsafe mode. What I think is that we should create a command_struct that is the failsafe command that we mux in if the RC is disabled.

Just an idea. What do you think @dpkoch?

Okay, I forgot about this, but this is another of the reasons I had for working on #38.

I don't know if you see this, but on my rev5 board (I'm not bothering with the rev2 board until #29 is resolved) the z accelerometer bias always gets calibrated incorrectly so that while sitting on my desk the z accelerometer reads ~8.75m/s^2. This happens with both the full temperature calibration done in fcu_io, and with the simple bias calibration done in ROSflight2. I do not see this problem with the floating point version.

@superjax Do you have this issue with yours? Any thoughts?

I have realized that the estimator doesn't do well at high angles of pitch and roll. If I've rolled a ton, then the pitch measurement is super sensitive. I think it's because 98% of the gravity vector is in the y axis, so the arctan between the remaining x and z axes is really small, and the measurement is small, so it jumps around a lot.

I am confident that what I have written so far works really well at low (less than 70 degrees) angles of pitch and roll, but I think we could be a little bit smarter about how we do our estimate.

First thing is we can try to be smarter about calculating acc_phi and acc_theta. Here is the current implementation

acc_phi = turboatan2(_accel_data[1], _accel_data[2]);
acc_theta = turboatan2(_accel_data[0], _accel_data[2]);

I think there is a better way that isn't so senstive. Also, the complementary filter I coded assumes that

Which we know is not true. That works at low angles of pitch and roll, but not if theta is big. What we should do is calculate

and then extract phi and theta from R. What do you think?

This is a pretty big modification

@superjax Okay I could be wrong here, but this looks like it could be a problem.

On line 37 of sensors.c, we check the mpuDataReady flag to see if we have new IMU data available. However, as I was looking through drv_mpu6050.c, I realized that mpuDataReady never gets set to false, so after the first time that check will always return true. Is that correct, or did I miss something?

Two possible fixes:

• in sensors.c, set mpuDataReady = false like you do in your accelgyro example. Simple, but it seems a little weird to be modifying a flag inside a library from outside the library. Might be okay though.
• store a copy of mpuMeasurementTime and do an equality comparison. When a different value from your local copy is detected, you have a new measurement.

Thoughts? Also I wonder what this might have been doing to our timing.

It'd be nice to have some generic messages available we could use for sending debug values over MAVLink and seeing them as ROS topics. Luckily, MAVLink has some messages already built in! The candidates are:

1. MEMORY_VECT
2. DEBUG_VECT
3. NAMED_VALUE_FLOAT
4. NAMED_VALUE_INT
5. DEBUG

I don't think we'd use the vect messages much, so I'm thinking we won't add support for those. The named value messages are 9 bytes bigger than the debug message because they use a char[10] to identify what the value is as opposed to a uint8. However, DEBUG only supports a float, and the named value messages are still only 18 bytes and I don't think we're planning on sending them super fast.

So I propose adding support for the NAMED_VALUE_INT message type. If you think we'd use it, it'd be pretty easy to add the NAMED_VALUE_FLOAT message as well. What do you think @superjax?

@superjax @dpkoch
After zooming in on the IMU data I realized that they are not evenly spaced in time as they should be. The first plot shows the raw data, while the second is a histogram of the inter-message time. The mean is 100Hz but no messages arrive at that rate. This issue is evident on all of the flight tests we have done at the REEF these last couple weeks.

We want to switch the mixer to be in float, rather than integer-based math. This will require re-tuning the PID loops.

Hola! @superjax has created a ZenHub account for the BYU-MAGICC organization. ZenHub is the only project management tool integrated natively in GitHub – created specifically for fast-moving, software-driven teams.

How do I use ZenHub?

To get set up with ZenHub, all you have to do is download the browser extension and log in with your GitHub account. Once you do, you’ll get access to ZenHub’s complete feature-set immediately.

What can ZenHub do?

ZenHub adds a series of enhancements directly inside the GitHub UI:

• Real-time, customizable task boards for GitHub issues;
• Multi-Repository burndown charts, estimates, and velocity tracking based on GitHub Milestones;
• Personal to-do lists and task prioritization;
• Time-saving shortcuts – like a quick repo switcher, a “Move issue” button, and much more.

Add ZenHub to GitHub

Still curious? See more ZenHub features or read user reviews. This issue was written by your friendly ZenHub bot, posted by request from @superjax.

We need to be able to send messages (text) from the fcu to be displayed by fcu_io. MAVLink has the STATUSTEXT message for this purpose, so we just need to implement that.

We may need to implement some slightly fancier buffering onboard so these don't clog up the line. Will look into it.

How do you change the input type from PWM to PPM?

Hey @dpkoch. I'm working on the muxing of RC and offboard control. I have written rc.c and I can fill the following data member

typedef enum
{
  RC_RATE, // Channel is is in rate mode (mrad/s)
  RC_ANGLE, // Channel command is in angle mode (mrad)
  RC_THROTTLE, // Channel is direcly controlling throttle max/1000
  RC_ALTITUDE, // Channel is commanding a specified altitude
  RC_MIN_THROTTLE, // Channel will be compared with the offboard control input and the min throttle command will be taken
  RC_MIN_THROTTLE_ALTITUDE, // Channel will be the min throttle between command to maintain commanded altitude, and offboard throttle command
  RC_PASSTHROUGH, // Channel directly passes PWM input to the mixer
  RC_OFFBOARD // Channel is not being controlled by RC, but instead by offboard computer
} rc_control_type_t;

typedef struct
{
  rc_control_type_t x_type;
  rc_control_type_t y_type;
  rc_control_type_t z_type;
  rc_control_type_t F_type;
  int16_t x;
  int16_t y;
  int16_t z;
  int16_t F;
} rc_control_t;

extern rc_control_t _rc_commands;

This is in rc.h of the override branch.

Anyway, it looks like all the data fields are getting filled correctly. I just need to get the mavlink stuff in so I can do the logic of the muxing. Is there any chance you could help me get the mavlink into a similar state? That is, a global variable with the four control channels, and a flag for each channel of it's type? I know that the MAVlink message right now is a little different, but I'm actually not sure how to access it in the ROSflight code. Is there a callback for offboard commands? I can massage it into the right data fields if I know where to get ahold of it.

What if fcu_io had a service that rebooted us into the bootloader. I think that there are some ways that cleanflight reboots itself into the bootloader. It would be handy for development.

I think ROSFlight sounds like a terrific solution to an easy to get into flight control system! I have gotten hold of a Naze32 Rev 6. One of the changes is that Rev 6. has a upgraded MPU 6500 vs 6050 in Rev 5.

It seems that BreezySTM32 does not contain the drivers of the upgraded MPU. Further the ROSFlight sensors.c specifically calls the 6050 functions.

I guess part of the appeal of the current setup is the simplicity, but being able to use the newest hardware revisions also have some appeal.

To get my own setup working I could fork both projects and do make the required changes. I don't think I'm skilled enough to make a system that could handle both hardware revisions. I have taken a look at how clean and base-flight, handles this, but their solutions are not especially easy to follow.

What's your thought on the issue?

I just wanted to write an update for my progress in building F4 support. I've bought a revo from ready-to-fly-quads link. And I started by trying to strip the relevant drivers from cleanflight, betaflight and raceflight

This was a horrible experience. I can't believe that cleanflight labels itself as ''clean''. There are some absolutely terrible coding practices going on in cleanflight, and after about 20 hours, I gave up, and decided to write my own. In about an hour, I started getting some functional builds.... I feel stupid for even trying to do anything with cleanflight.

The biggest difference, besides the faster processor, is native USB support. I had never worked with a USB peripheral on an embedded controller before, so that was something new. I was hoping that I could bypass learning about how it works by relying on cleanflight, but as I already mentioned, turned out to be a terrible idea. It's not all that complicated, but I need to make sure that it is using asynchronous callback methods or DMA or something like that. I believe that we will be able to have even higher bandwidth on the VCP, (virtual COM port) than we have seen previously.

Anyway, now that I have decided to completely leave the stupid flight XXXXflights behind, I think I'll have a working solution in a couple of hours. I intend on replicating all the BreezySTM32 examples with an F4, but I'll probably leave it as a separate repository just so it doesn't get complicated

1. Update flow diagram pdf to represent what is going on in the code
2. Redesign flow, knowing how it is supposed to work
3. Re-code mux.c

Hey everyone @dpkoch @gellings, I figured we could talk about this here. I am struggling to find a good way to do mixing. So far I have built this mixer_t, which has as many fields as I can think of us needing.

typedef struct {
  uint8_t motor_count;
  uint8_t servo_count;
  output_type_t output_type[8];
  int32_t F[8];
  int32_t x[8];
  int32_t y[8];
  int32_t z[8];
} mixer_t;

where output_type_t is an enumeration like this

typedef enum{
  NONE, // None
  S, // Servo
  M, // Motor
  L // LED
} output_type_t;

Here is an example of two mixers, the quad X and the fixedwing mixer.

static mixer_t quadcopter_x_mixing = {
  4, // Motor Count
  0, // Servo Count
  {M, M, M, M, 0, 0, 0, 0}, // output_type

  {1,   1,  1,  1,  0, 0, 0, 0}, // F Mix
  {-1, -1,  1,  1,  0, 0, 0, 0}, // X Mix
  {-1,  1,  -1, 1,  0, 0, 0, 0}, // Y Mix
  {-1,  1,  1, -1,  0, 0, 0, 0}  // Z Mix
};

static mixer_t fixedwing_mixing = {
  1, // Motor Count
  3, // Servo Count
  {M, S, S, S, 0, 0, 0, 0},

  {1, 0, 0, 0, 0, 0, 0, 0}, // F Mix
  {0, 1, 0, 0, 0, 0, 0, 0}, // X Mix
  {0, 0, 1, 0, 0, 0, 0, 0}, // Y Mix
  {0, 0, 0, 1, 0, 0, 0, 0}  // Z Mix
};

@dpkoch, I guess my main question is how does the mavlink side work. I'm not really sure how to set it up so that it is easy for mavlink to switch between mixers. My idea right now is to have an array of all the pre-programmed mixers, and choose out of that array based on an enum. Alternatively, we could set up a method by which mavlink carries the mixer configuration over the line and it is saved in the EEPROM somehow.

What do you think?

Currently if ROSflight2 modifies a parameter value onboard (like in the IMU bias calibration), fcu_io is not notified of the change and so becomes out of date.

We need a good way to make the IMU calibration better.

Ideas:
Temperature Compensation?
http://www.i2cdevlib.com/forums/topic/53-mpu-6050-temperature-compensation/

Using the DMP?
motion_driver_6.12.zip

Hey @superjax

So I'm just wondering if we want to take another step down the dark path you started us on by implementing your estimator with floating point arithmetic.

My question is this: should we just go for it and do all of our sensors (especially IMU) in floating point too? Here are some reasons why I think it may be a good ideal:

• Doing the estimator in floating point didn't seem to be too much of a performance hit, so this might not either
• Whatever performance we do lose, we could regain by going to the F3. Maybe loops run slower on the lesser boards, but if anybody wants serious performance they can upgrade
• It would make the code much more readable
• I think it would simplify a lot of the issues we're seeing with getting this calibration stuff working. Right now we're trying to stream and calibrate in terms of raw values, but something seems to be getting lost in the translation. If we were just streaming everything over in SI units, figuring out some calibration parameters, then applying those on the Naze as SI units, we'd be much less likely to screw something up.

Obviously, the downside would be a speed hit.

At this point I'm just asking the question. What are your thoughts?

Per my discussion with @dpkoch in #60, Maybe it'd be possible to have a pid_t struct or something for each loop, then a generic PID function that runs an arbitrary loop on the values in that struct.

Something seems to be wonky with the gyro scale factor. The function mpu6050_init() in drv_mpu6050.h returns a scale factor for the gyro that claims to put it into units of urad/s. However, we find ourselves having to multiply the resulting value by a factor of 1e6 to get the value in rad/s, when we would expect to divide by 1e6 to get that value. The likely suspect is line 220 of drv_mpu6050.c:

// 16.4 dps/lsb scalefactor for all Invensense devices
*gyroScale = (4.0f / 16.4f) * (M_PI / 180.0f) * 0.000001f;

The multiply by 0.000001f doesn't make sense; it would seem like it puts the result in units of Mrad/s instead of urad/s. The origin of the factor of 4.0f is also mysterious.

We should re-code mode.c to be a state machine, rather than a series of if and else statements