Crazyflie 2 with GPS

[tobias]

I copied in the gtgps-7 cf2 fw changes into the latest master cf2 branch. Compiled a debug build and flashed with a stlink_v2 so I could debug it however it seems to be running fine. Unfortunately I have to go now so I don't have time to test a cload build.

Any chance you can fork the crazyflie-firmware project only and apply your changes to it and not put it into a new structure. Will be easier for us to collaborate then? Same with the cflient.

[jackemoore]

Hi,

Thanks Tobias for looking at my firmware. You are correct that without calibration, gyro.yaw was getting messed up. I had intended to leave gyro.yaw alone when not calibrated, had even prepared code to do just that, but had failed to include it in my latest firmware update into github.

I made some changes to my github repository. Branches gtgps-5, gtgps-6, gtgps-7 and dkgps-7 were removed. Then branch gtgps-5 and dkgps-5 were created with my latest firmware updates. Both updates included a change in compass.c to prevent altering gyro.yaw prior to calibrating the compass. I have yet to add code to prevent position hold mode from doing anything until calibration is completed and will update the github firmware with this change the next go around.

I forked the repository for latest crazyflie-firrmware and the first change was just the deck driver gtgps.
This worked fine, and 3D Fix positions appeared in cfclient's GpsTab display. So, there is not a uart1 problem per se as originally thought. All my previous attempts included the compassController along with the gtgps driver and related modifications to the source code.

When incorporating all the firmware changes, including both gtgps and compass, the results appear to be a mixed bag. With the watchdog timer enabled, there is a timeout immediately after flashing the firmware.

With the watchdog timer disabled, the red led flashes 2 Hz after bootloading, sometimes crashes when connecting to cfclient, and other times there is no crash. In all cases, the priimary output of the compassController, yawgeo, is incorrectly zero.

As an experiment, I temporarily removed the trigonometric function calls in compass.c, which were two calls to sinf(), two calls to cosf() and one call to atan2f() in a 5Hz update control loop. Then tested this in a crazyflie with the gps receiver. This didn't appear to alter the failure situation.

The baffling aspect is the source code works in updates dated 2016.05.19 and those prior to this. In more recent updates , something has changed where the same code no longer works.

I placed the new gtgps, compass changes and supporting modification to a pull request to the firmware master dated 2016.06.23. I'm new to pushing to GitHub via a fork of the master project, and for some reason the config.mk for the deck gtgps located in tools/make did not get picked up as a commit change. My personal repository contains a copy of this file.

Let me know what additional information may be needed in order to debug the problem that I've been enountering with the latest crazyflie-firmware updates on GitHub.

Thanks again for your help, and I appreciate the fact that you have a lot going on requiring higher priority attention than my project.

Best regards,
Jack

[jackemoore]

Hi,
I removed all of my changes including gtgps except for compass.c (without any references to configblockeeprom) and the problem remains untouched. After a watchdog timeout the crazyflie firmware crashes.

Also, added a fork to crazyflie-clients-python, applied my updates to sync with gps & compass firmware changes, and submitted a pull request.
Best regards,
Jack

[jackemoore]

Hi,

A new Branch gtgps-6 was added and contains the latest firmware updates. This commit contains the latest updates involving changes to files gtgps.c, compass.h, compass.c, and sensors_stock.c. If the compass (magnetometer) has not been calibrated, then (a)the gtgps driver does not pass on lat/lon positions, (b)yawgeo (yawangle) is (are) set to gyro.yaw, and (c)drift compensation is not applied to gyro.yaw. As a result, if position hold mode is activated when uncalibrated, position x, y and z remain zero values. A bug fix was applied to the function sensorsAcquire() that if position.timestamp is non-zero, the faulty statement "sensor->position = position;" was replaced by individual statements "sensor->position.x = position.x;" and correspondingly for y and z, without timestamp. For an unknown reason setting "sensor->position.timestamp" in the point_t structure was clobbering the sensorData_t structure (specifically baro.asl).

Best regards,
Jack

[tobias]

I tested your crazyflie-firmware fork to try and find the "uart" bug but for me it runs without problem. I ran into the watchdog problem in the cload build though but that was a know problem. Could it be a toolchain problem?

Code: Select all

arm-none-eabi-gcc --version
arm-none-eabi-gcc (GNU Tools for ARM Embedded Processors) 4.9.3 20141119 (release) [ARM/embedded-4_9-branch revision 218278]
Copyright (C) 2014 Free Software Foundation, Inc.
This is free software; see the source for copying conditions.  There is NO
warranty; not even for MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.

[tobias]

I think I found the lockup problem and it has to do with the uart, or combination at least. I seams the gps uart can get an overrun (writing over the shift register because it isn't handled fast enough). The overrun interrupt which is enabled together with the RX not empty interrupt isn't handled in the interrupt service routine and once it gets triggered and not handled it enters an endless loop. This can be fixed by raising the interrupt level for the gps uart (uart1). There might be consequences doing this though. It can also be fixed by handling the overrun case in the interrupt but as data will be missed I don't think it is a viable solution.

Code: Select all

void __attribute__((used)) USART3_IRQHandler(void)
{
  uint8_t rxData;
  portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;

  if (USART_GetITStatus(UART1_TYPE, USART_IT_RXNE))
  {
    rxData = USART_ReceiveData(UART1_TYPE) & 0x00FF;
    xQueueSendFromISR(uart1queue, &rxData, &xHigherPriorityTaskWoken);
  }
  else if (USART_GetITStatus(UART1_TYPE, USART_IT_ORE_RX))
  {
    //Reset overrun interrupt flag by reading data register.
    rxData = USART_ReceiveData(UART1_TYPE) & 0x00FF;
  }
}

[jackemoore]

Hi,
Excellent work to both isolate and diagnose the crazyflie with gps hang up problem. In the current gps implementation, message data from the receiver is updated at a 5 Hz rate. Preventing the hang up would seem far more important and less of an issue of occasionally losing some data. Handling the problem in the interrupt service routine would seem to be an ideal solution and fix. Is this something Bitcraze would consider doing?
Best regards,
Jack

[jackemoore]

Hi,

Branch gtgps-6 was updated and contains the latest firmware updates. This commit contains the latest updates involving changes to function sensorsAcquire() in sensors_stock.c, to function stateEstimator() in estimator_complementary.c, and the more aggressive pid values used in the attitude roll/pitch controller are hard coded in pid.h. Gyro bias updates in imu_cf.c and thrust tilt compensation in controller_pid.c are not included in this build.
A previous statement that the eeprom is cleared of compass calibration values when the bootloader downloads a new flash program into
the crazyflie was incorrect. The last compass cal values remain there until a new cal is performed. The reported problem that the
"sensor->position.timestamp" in the point_t structure was clobbering baro.asl was incorrect. A bettter understanding of the baseline
code in stateEstimator() helped resolve what was actually happenning to baro.asl and new code was added to properly handle gps fix
position updates. Gps position x & y are passed through, and filtered baro.asl continues to be used for position z updates. Gps hMSL
is not used to control position z updates. A future study might be to investigate whether velocity information derived from gps 3D
positions would be of benefit or not.
Branch dkgps-6 will be updated in the near future to be in sync with these latest gtgps-6 changes. Also, the push request to crazyflie-firmware
needs to be put in sync as well.

Best regards,
Jack

[tobias]

Great that you found the issue!

I'm thinking how we can start integrate this into the cf2 repository and at the moment there is a lot of changes at once. Would be better to divide them up in smaller categories such as compass driver, compass calibration, gps driver, position control, etc is pull requests. Would this be viable at all or would it be way to much work?

Another solution would be to open a new branch and move everything there and when it is working well we merge it to master but as branches tend to stay branches an never get merged I would vote for dividing it up in smaller chunks.

As for compass calibration we once got tipped about using this routine. It might be too advanced at the moment but there is c code that shouldn't be to complicated to implement.

[jackemoore]

Hi,

Regarding 3-axis magnetometer calibration. Recording a set of static measurements for the magnetometer by positioning the crazyflie in a large number of orientations covering a whole sphere and processing these recordings using matrix and inverse matrix computations can be quite onerous with respect to floating point math and microprocessor workload (while other real-time critical tasks are ongoing). In addition, the effective benefits for the crazyflie, that nominally is horizontal flight, appears unnecessary and unwarranted.

A seemingly more practical approach has been implemented by holding the crazyflie horizontal and rotate it around the gravitational force line for about 360 degrees. Then, holding the crazyflie vertically (for example its nose downward) and rotate it around the gravitational force line for about 360 degrees. This does not require massive data recording, and extensive floating point math computations The accuracy achieved with this simpler method should be more than sufficient for what's needed by the crazyflie. In addition, the approach implemented includes up to 90 degrees tilt (roll and/or pitch) compensation.

For those familiar with DJI Inspire, Phantom and Naza-M GPS flight controller products, this orthogonal approach is the same method they use.

Best regards,
Jack