From a837d814ead983d8e58d7e1a37d4be85497f22f6 Mon Sep 17 00:00:00 2001 From: Eric Day Date: Tue, 19 Mar 2013 12:57:39 +0800 Subject: [PATCH] Set home/hold waypoint from serial --- Serial.ino | 38 ++++++++++++++++++++++++++++++++++++-- 1 file changed, 36 insertions(+), 2 deletions(-) diff --git a/Serial.ino b/Serial.ino index 2e47153..28cbac1 100644 --- a/Serial.ino +++ b/Serial.ino @@ -552,6 +552,23 @@ void evaluateCommand() if (wp_no == 0) { WP[HOME].Lat = lat; WP[HOME].Lon = lon; + + #if defined(I2C_GPS) + i2c_rep_start(I2C_GPS_ADDRESS << 1); + i2c_write(I2C_GPS_WP0); + uint8_t* ptr = (uint8_t*)&(WP[HOME].Lat); + i2c_write(*ptr++); + i2c_write(*ptr++); + i2c_write(*ptr++); + i2c_write(*ptr); + + ptr = (uint8_t*)&(WP[HOME].Lon); + i2c_write(*ptr++); + i2c_write(*ptr++); + i2c_write(*ptr++); + i2c_write(*ptr); + #endif + f.GPS_HOME_MODE = 0; // with this flag, GPS_set_next_wp will be called in the next loop -- OK with SERIAL GPS / OK with I2C GPS f.GPS_FIX_HOME = 1; if (alt != 0) @@ -565,8 +582,25 @@ void evaluateCommand() WP[HOLD].Alt = alt; // temporary implementation to test feature with apps WP[HOLD].Vario = WP_VARIO; // vario can be inserted here instead of predefined WP[HOLD].Updated = 1; // HOLD WP is updated - #if !defined(I2C_GPS) - nav_mode = NAV_MODE_WP; + #if defined(I2C_GPS) + i2c_rep_start(I2C_GPS_ADDRESS << 1); + i2c_write(I2C_GPS_WP1); // Shall I use this waypoint? + uint8_t* ptr = (uint8_t*)&(WP[HOLD].Lat); + i2c_write(*ptr++); + i2c_write(*ptr++); + i2c_write(*ptr++); + i2c_write(*ptr); + + ptr = (uint8_t*)&(WP[HOLD].Lon); + i2c_write(*ptr++); + i2c_write(*ptr++); + i2c_write(*ptr++); + i2c_write(*ptr); + + GPS_I2C_command(I2C_GPS_COMMAND_START_NAV, 1); + nav_mode = NAV_MODE_WP; + #else + nav_mode = NAV_MODE_WP; GPS_set_next_wp(&WP[HOLD].Lat,&WP[HOLD].Lon); #endif }