diff --git a/flight/Modules/Attitude/revolution/attitude.c b/flight/Modules/Attitude/revolution/attitude.c index 05369479e..7d8d1d15e 100644 --- a/flight/Modules/Attitude/revolution/attitude.c +++ b/flight/Modules/Attitude/revolution/attitude.c @@ -58,6 +58,7 @@ #include "baroaltitude.h" #include "flightstatus.h" #include "gpsposition.h" +#include "gpsvelocity.h" #include "gyros.h" #include "gyrosbias.h" #include "homelocation.h" @@ -86,6 +87,7 @@ static xQueueHandle accelQueue; static xQueueHandle magQueue; static xQueueHandle baroQueue; static xQueueHandle gpsQueue; +static xQueueHandle gpsVelQueue; static AttitudeSettingsData attitudeSettings; static HomeLocationData homeLocation; @@ -165,6 +167,7 @@ int32_t AttitudeStart(void) magQueue = xQueueCreate(1, sizeof(UAVObjEvent)); baroQueue = xQueueCreate(1, sizeof(UAVObjEvent)); gpsQueue = xQueueCreate(1, sizeof(UAVObjEvent)); + gpsVelQueue = xQueueCreate(1, sizeof(UAVObjEvent)); // Start main task xTaskCreate(AttitudeTask, (signed char *)"Attitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &attitudeTaskHandle); @@ -176,7 +179,8 @@ int32_t AttitudeStart(void) MagnetometerConnectQueue(magQueue); BaroAltitudeConnectQueue(baroQueue); GPSPositionConnectQueue(gpsQueue); - + GPSVelocityConnectQueue(gpsVelQueue); + return 0; } @@ -468,12 +472,14 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) MagnetometerData magData; BaroAltitudeData baroData; GPSPositionData gpsData; + GPSVelocityData gpsVelData; GyrosBiasData gyrosBias; HomeLocationData home; static bool mag_updated; static bool baro_updated; static bool gps_updated; + static bool gps_vel_updated; static uint32_t ins_last_time = 0; static bool inited; @@ -508,6 +514,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) mag_updated |= (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && homeLocation.Set == HOMELOCATION_SET_TRUE; baro_updated |= xQueueReceive(baroQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE; gps_updated |= (xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode; + gps_vel_updated |= (xQueueReceive(gpsVelQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode; // Get most recent data GyrosGet(&gyrosData); @@ -515,6 +522,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) MagnetometerGet(&magData); BaroAltitudeGet(&baroData); GPSPositionGet(&gpsData); + GPSVelocityGet(&gpsVelData); HomeLocationGet(&home); // Have a minimum requirement for gps usage @@ -658,10 +666,15 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) GPSPositionData gpsPosition; GPSPositionGet(&gpsPosition); - vel[0] = gpsPosition.Groundspeed * cosf(gpsPosition.Heading * F_PI / 180.0f); - vel[1] = gpsPosition.Groundspeed * sinf(gpsPosition.Heading * F_PI / 180.0f); - vel[2] = 0; - + if (0) { + vel[0] = gpsPosition.Groundspeed * cosf(gpsPosition.Heading * F_PI / 180.0f); + vel[1] = gpsPosition.Groundspeed * sinf(gpsPosition.Heading * F_PI / 180.0f); + vel[2] = 0; + } else { + vel[0] = gpsVelData.North; + vel[1] = gpsVelData.East; + vel[2] = gpsVelData.Down; + } // Transform the GPS position into NED coordinates getNED(&gpsPosition, NED); diff --git a/flight/Modules/GPS/GPS.c b/flight/Modules/GPS/GPS.c index b4fd67ce4..eeb4e1a4a 100644 --- a/flight/Modules/GPS/GPS.c +++ b/flight/Modules/GPS/GPS.c @@ -36,11 +36,14 @@ #include #include "NMEA.h" +#include "UBX.h" + #include "gpsposition.h" #include "homelocation.h" #include "gpstime.h" #include "gpssatellites.h" +#include "gpsvelocity.h" #include "WorldMagModel.h" #include "CoordinateConversions.h" #include "hwsettings.h" @@ -137,6 +140,7 @@ int32_t GPSInitialize(void) if (gpsPort && gpsEnabled) { GPSPositionInitialize(); + GPSVelocityInitialize(); #if !defined(PIOS_GPS_MINIMAL) GPSTimeInitialize(); GPSSatellitesInitialize(); @@ -167,10 +171,14 @@ static void gpsTask(void *parameters) portTickType xDelay = 100 / portTICK_RATE_MS; uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;; GPSPositionData GpsData; + UBXPacket *ubx = (UBXPacket *)gps_rx_buffer; uint8_t rx_count = 0; - bool start_flag = false; +// bool start_flag = false; bool found_cr = false; + enum proto_states {START,NMEA,UBX_SY2,UBX_CLASS,UBX_ID,UBX_LEN1, + UBX_LEN2,UBX_PAYLOAD,UBX_CHK1,UBX_CHK2}; + enum proto_states proto_state = START; int32_t gpsRxOverflow = 0; numUpdates = 0; @@ -184,6 +192,7 @@ static void gpsTask(void *parameters) while (1) { uint8_t c; + // NMEA or SINGLE-SENTENCE GPS mode // This blocks the task until there is something on the buffer @@ -191,22 +200,94 @@ static void gpsTask(void *parameters) { // detect start while acquiring stream - if (!start_flag && (c == '$')) + switch (proto_state) { - start_flag = true; - found_cr = false; - rx_count = 0; + case START: // detect protocol + switch (c) + { + case UBX_SYNC1: // first UBX sync char found + proto_state = UBX_SY2; + continue; + case '$': // NMEA identifier found + proto_state = NMEA; + found_cr = false; + rx_count = 0; + break; + default: + continue; + } + break; + case UBX_SY2: + if (c == UBX_SYNC2) // second UBX sync char found + { + proto_state = UBX_CLASS; + found_cr = false; + rx_count = 0; + } + else + { + proto_state = START; // reset state + } + continue; + case UBX_CLASS: + ubx->header.class = c; + proto_state = UBX_ID; + continue; + case UBX_ID: + ubx->header.id = c; + proto_state = UBX_LEN1; + continue; + case UBX_LEN1: + ubx->header.len = c; + proto_state = UBX_LEN2; + continue; + case UBX_LEN2: + ubx->header.len += (c << 8); + if ((sizeof (UBXHeader)) + ubx->header.len > NMEA_MAX_PACKET_LENGTH) + { + gpsRxOverflow++; + proto_state = START; + found_cr = false; + rx_count = 0; + } + else + { + proto_state = UBX_PAYLOAD; + } + continue; + case UBX_PAYLOAD: + if (rx_count < ubx->header.len) + { + ubx->payload.payload[rx_count] = c; + if (++rx_count == ubx->header.len) + proto_state = UBX_CHK1; + } + else + proto_state = START; + continue; + case UBX_CHK1: + ubx->header.ck_a = c; + proto_state = UBX_CHK2; + continue; + case UBX_CHK2: + ubx->header.ck_b = c; + if (checksum_ubx_message(ubx)) + { + parse_ubx_message(ubx); + } + proto_state = START; + continue; + case NMEA: + break; } - else - if (!start_flag) - continue; - + + if (rx_count >= NMEA_MAX_PACKET_LENGTH) { // The buffer is already full and we haven't found a valid NMEA sentence. // Flush the buffer and note the overflow event. gpsRxOverflow++; - start_flag = false; + proto_state = START; found_cr = false; rx_count = 0; } @@ -230,7 +311,7 @@ static void gpsTask(void *parameters) gps_rx_buffer[rx_count-2] = 0; // prepare to parse next sentence - start_flag = false; + proto_state = START; found_cr = false; rx_count = 0; // Our rxBuffer must look like this now: diff --git a/flight/Modules/GPS/UBX.c b/flight/Modules/GPS/UBX.c new file mode 100644 index 000000000..19f42081b --- /dev/null +++ b/flight/Modules/GPS/UBX.c @@ -0,0 +1,97 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup GSPModule GPS Module + * @brief Process GPS information (UBX binary format) + * @{ + * + * @file UBX.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief GPS module, handles GPS and NMEA stream + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "openpilot.h" +#include "pios.h" +#include "UBX.h" +#include "gpsvelocity.h" + +bool checksum_ubx_message (UBXPacket *ubx) +{ + int i; + uint8_t ck_a, ck_b; + + ck_a = ubx->header.class; + ck_b = ck_a; + + ck_a += ubx->header.id; + ck_b += ck_a; + + ck_a += ubx->header.len & 0xff; + ck_b += ck_a; + + ck_a += ubx->header.len >> 8; + ck_b += ck_a; + + + + for (i = 0; i < ubx->header.len; i++) + { + ck_a += ubx->payload.payload[i]; + ck_b += ck_a; + } + + if (ubx->header.ck_a == ck_a && + ubx->header.ck_b == ck_b) + { + return true; + } + else + return false; + +} + +void parse_ubx_nav_velned (UBXPayload payload) +{ + GPSVelocityData GpsVelocity; + GPSVelocityGet(&GpsVelocity); + + GpsVelocity.North = (float)payload.nav_velned.velN/100.0; + GpsVelocity.East = (float)payload.nav_velned.velE/100.0; + GpsVelocity.Down = (float)payload.nav_velned.velD/100.0; + + GPSVelocitySet(&GpsVelocity); +} + +void parse_ubx_message (UBXPacket *ubx) +{ + switch (ubx->header.class) + { + case UBX_CLASS_NAV: + switch (ubx->header.id) + { + case UBX_ID_VELNED: + parse_ubx_nav_velned (ubx->payload); + } + break; + } +} + + diff --git a/flight/Modules/GPS/inc/UBX.h b/flight/Modules/GPS/inc/UBX.h new file mode 100644 index 000000000..21cd60ce4 --- /dev/null +++ b/flight/Modules/GPS/inc/UBX.h @@ -0,0 +1,80 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup GSPModule GPS Module + * @brief Process GPS information + * @{ + * + * @file UBX.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief GPS module, handles GPS and NMEA stream + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef UBX_H +#define UBX_H +#include "openpilot.h" + +#define UBX_SYNC1 0xb5 // UBX protocol synchronization characters +#define UBX_SYNC2 0x62 + +// From u-blox6 receiver protocol specification + +// Messages classes +#define UBX_CLASS_NAV 0x01 + +// Message IDs +#define UBX_ID_VELNED 0x12 + +// private structures + +typedef struct { + uint32_t iTOW; // ms GPS Millisecond Time of Week + int32_t velN; // cm/s NED north velocity + int32_t velE; // cm/s NED east velocity + int32_t velD; // cm/s NED down velocity + uint32_t speed; // cm/s Speed (3-D) + uint32_t gSpeed; // cm/s Ground Speed (2-D) + int32_t heading; // 1e-5 *deg Heading of motion 2-D + uint32_t sAcc; // cm/s Speed Accuracy Estimate + uint32_t cAcc; // 1e-5 *deg Course / Heading Accuracy Estimate +} UBX_NAV_VELNED; + +typedef union { // add more message types later + uint8_t payload[0]; + UBX_NAV_VELNED nav_velned; +} UBXPayload; + +typedef struct { + uint8_t class; + uint8_t id; + uint16_t len; + uint8_t ck_a; + uint8_t ck_b; +} UBXHeader; + +typedef struct { + UBXHeader header; + UBXPayload payload; +} UBXPacket; + +bool checksum_ubx_message(UBXPacket *); +void parse_ubx_message(UBXPacket *); +#endif /* UBX_H */ diff --git a/flight/Modules/Guidance/guidance.c b/flight/Modules/Guidance/guidance.c index 8cae6a290..214a1f8b1 100644 --- a/flight/Modules/Guidance/guidance.c +++ b/flight/Modules/Guidance/guidance.c @@ -279,7 +279,7 @@ void updateVtolDesiredVelocity() eastPosIntegral); - float total_vel = sqrtf(powf(velocityDesired.North,2) + powf(velocityDesired.East,2)); + float total_vel = sqrtf(powf(northCommand,2) + powf(eastCommand,2)); float scale = 1; if(total_vel > guidanceSettings.HorizontalVelMax) scale = guidanceSettings.HorizontalVelMax / total_vel; diff --git a/flight/Revolution/UAVObjects.inc b/flight/Revolution/UAVObjects.inc index c1601d9f6..b2080bc31 100644 --- a/flight/Revolution/UAVObjects.inc +++ b/flight/Revolution/UAVObjects.inc @@ -47,6 +47,7 @@ UAVOBJSRCFILENAMES += gcstelemetrystats UAVOBJSRCFILENAMES += gpsposition UAVOBJSRCFILENAMES += gpssatellites UAVOBJSRCFILENAMES += gpstime +UAVOBJSRCFILENAMES += gpsvelocity UAVOBJSRCFILENAMES += guidancesettings UAVOBJSRCFILENAMES += homelocation UAVOBJSRCFILENAMES += i2cstats diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 95115b248..75f4e31e7 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -54,6 +54,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/gpsposition.h \ $$UAVOBJECT_SYNTHETICS/gpstime.h \ $$UAVOBJECT_SYNTHETICS/gpssatellites.h \ + $$UAVOBJECT_SYNTHETICS/gpsvelocity.h \ $$UAVOBJECT_SYNTHETICS/positionactual.h \ $$UAVOBJECT_SYNTHETICS/flightbatterystate.h \ $$UAVOBJECT_SYNTHETICS/homelocation.h \ @@ -117,6 +118,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/gpsposition.cpp \ $$UAVOBJECT_SYNTHETICS/gpstime.cpp \ $$UAVOBJECT_SYNTHETICS/gpssatellites.cpp \ + $$UAVOBJECT_SYNTHETICS/gpsvelocity.cpp \ $$UAVOBJECT_SYNTHETICS/positionactual.cpp \ $$UAVOBJECT_SYNTHETICS/flightbatterystate.cpp \ $$UAVOBJECT_SYNTHETICS/homelocation.cpp \ diff --git a/shared/uavobjectdefinition/gpsvelocity.xml b/shared/uavobjectdefinition/gpsvelocity.xml new file mode 100644 index 000000000..7d57ccacc --- /dev/null +++ b/shared/uavobjectdefinition/gpsvelocity.xml @@ -0,0 +1,12 @@ + + + Raw GPS data from @ref GPSModule. Should only be used by @ref AHRSCommsModule. + + + + + + + + +