rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-30 08:24:11 +01:00
Code Issues Releases Activity

Merge branch 'revolution_ubx' into sim

Browse Source
This commit is contained in:
James Cotton 2012-04-14 12:10:23 -05:00
commit 1a98a46606
8 changed files with 303 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
flight/Modules/Attitude/revolution/attitude.c
View File

@ -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);

103
flight/Modules/GPS/GPS.c
View File

@ -36,11 +36,14 @@
#include <stdbool.h>
#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:

97
flight/Modules/GPS/UBX.c Normal file
View File

@ -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;
}
}

80
flight/Modules/GPS/inc/UBX.h Normal file
View File

@ -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 */

2
flight/Modules/Guidance/guidance.c
View File

@ -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;

1
flight/Revolution/UAVObjects.inc
View File

@ -47,6 +47,7 @@ UAVOBJSRCFILENAMES += gcstelemetrystats
UAVOBJSRCFILENAMES += gpsposition
UAVOBJSRCFILENAMES += gpssatellites
UAVOBJSRCFILENAMES += gpstime
UAVOBJSRCFILENAMES += gpsvelocity
UAVOBJSRCFILENAMES += guidancesettings
UAVOBJSRCFILENAMES += homelocation
UAVOBJSRCFILENAMES += i2cstats

2
ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
View File

@ -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 \

12
shared/uavobjectdefinition/gpsvelocity.xml Normal file
View File

@ -0,0 +1,12 @@
<xml>
<object name="GPSVelocity" singleinstance="true" settings="false">
<description>Raw GPS data from @ref GPSModule. Should only be used by @ref AHRSCommsModule.</description>
<field name="North" units="m/s" type="float" elements="1"/>
<field name="East" units="m/s" type="float" elements="1"/>
<field name="Down" units="m/s" type="float" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
<logging updatemode="periodic" period="1000"/>
</object>
</xml>