Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 13:55:40 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

Merge pull request #1025 from paparazzi/ardrone2_mag_fix

Browse Source 
Chose this one over mine, if it is not working will add a fix, whatver the fix will be and then merge it again
This commit is contained in:
OpenUAS
2014-12-12 00:52:57 +01:00
parent fbd531b1b6 3d3795116f
commit 33f30d9e22
3 changed files with 13 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/telemetry/default_ardrone.xml
+1
View File
@@ -19,6 +19,7 @@
<message name="I2C_ERRORS" period="4.1"/> <message name="I2C_ERRORS" period="4.1"/>
<message name="UART_ERRORS" period="3.1"/> <message name="UART_ERRORS" period="3.1"/>
<message name="DATALINK_REPORT" period="5.1"/> <message name="DATALINK_REPORT" period="5.1"/>
<message name="STATE_FILTER_STATUS" period="3.2"/>
</mode> </mode>
<mode name="ppm"> <mode name="ppm">
conf/telemetry/default_rotorcraft.xml
+1
View File
@@ -21,6 +21,7 @@
<message name="SUPERBITRF" period="3"/> <message name="SUPERBITRF" period="3"/>
<message name="ENERGY" period="2.5"/> <message name="ENERGY" period="2.5"/>
<message name="DATALINK_REPORT" period="5.1"/> <message name="DATALINK_REPORT" period="5.1"/>
<message name="STATE_FILTER_STATUS" period="3.2"/>
</mode> </mode>
<mode name="ppm"> <mode name="ppm">
sw/airborne/boards/ardrone/navdata.c
+10 -1
View File
@@ -58,6 +58,7 @@ static int nav_fd = 0;
measures_t navdata; measures_t navdata;
static int imu_lost = 0; static int imu_lost = 0;
static int imu_lost_counter = 0;
/** Sonar offset. /** Sonar offset.
* Offset value in ADC * Offset value in ADC
@@ -158,7 +159,7 @@ static void send_filter_status(struct transport_tx *trans, struct link_device *d
uint8_t mde = 3; uint8_t mde = 3;
if (ahrs.status == AHRS_UNINIT) mde = 2; if (ahrs.status == AHRS_UNINIT) mde = 2;
if (imu_lost) mde = 5; if (imu_lost) mde = 5;
uint16_t val = 0; uint16_t val = imu_lost_counter;
pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &mde, &val); pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &mde, &val);
} }
@@ -317,6 +318,7 @@ static void mag_freeze_check(void) {
printf("Mag needs resetting, Values are frozen!!! %d , %d \n", LastMagValue, navdata.mx); printf("Mag needs resetting, Values are frozen!!! %d , %d \n", LastMagValue, navdata.mx);
// set imu_lost flag // set imu_lost flag
imu_lost = 1; imu_lost = 1;
imu_lost_counter++;
if (mag_freeze_retry < MAG_FREEZE_MAX_RETRY) { if (mag_freeze_retry < MAG_FREEZE_MAX_RETRY) {
printf("Setting GPIO 177 to reset PIC Navigation Board \n"); printf("Setting GPIO 177 to reset PIC Navigation Board \n");
@@ -332,6 +334,7 @@ static void mag_freeze_check(void) {
// do the navboard reset via GPIOs // do the navboard reset via GPIOs
gpio_clear(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_NAVDATA); gpio_clear(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_NAVDATA);
usleep(20000); //Otherwise set sometime does not work
gpio_set(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_NAVDATA); gpio_set(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_NAVDATA);
//// wait 20ms to retrieve data //// wait 20ms to retrieve data
@@ -339,8 +342,13 @@ static void mag_freeze_check(void) {
//// restart acquisition //// restart acquisition
cmd = 0x01; cmd = 0x01;
usleep(5000);
for (int i=0;i<10;i++)
{
usleep(1000);
navdata_write(&cmd, 1); navdata_write(&cmd, 1);
} }
}
MagFreezeCounter = 0; // reset counter back to zero MagFreezeCounter = 0; // reset counter back to zero
} }
@@ -499,6 +507,7 @@ void navdata_update()
// Check if initialized // Check if initialized
if (!nav_port.isInitialized) { if (!nav_port.isInitialized) {
navdata_init(); navdata_init();
mag_freeze_check();
return; return;
} }