englebert
/
inav-simplefc
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

[PATCH] GPS: Signal Strength for u-Blox only 

git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@334 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
master
timecop@gmail.com 12 years ago
parent
61caceb861
commit
ef9de70161
4 changed files with 105 additions and 50 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 85
      src/gps.c
  2. 17
      src/mw.c
  3. 17
      src/mw.h
  4. 36
      src/serial.c

85
src/gps.c
View File

@ -928,24 +928,46 @@ typedef struct {
uint32_t speed_2d;
int32_t heading_2d;
uint32_t speed_accuracy;
uint32_t heading_accuracy;
} ubx_nav_velned;
enum {
PREAMBLE1 = 0xb5,
PREAMBLE2 = 0x62,
uint32_t heading_accuracy;
} ubx_nav_velned;
typedef struct
{
uint8_t chn; // Channel number, 255 for SVx not assigned to channel
uint8_t svid; // Satellite ID
uint8_t flags; // Bitmask
uint8_t quality; // Bitfield
uint8_t cno; // Carrier to Noise Ratio (Signal Strength)
uint8_t elev; // Elevation in integer degrees
int16_t azim; // Azimuth in integer degrees
int32_t prRes; // Pseudo range residual in centimetres
} ubx_nav_svinfo_channel;
typedef struct
{
uint32_t time; // GPS Millisecond time of week
uint8_t numCh; // Number of channels
uint8_t globalFlags; // Bitmask, Chip hardware generation 0:Antaris, 1:u-blox 5, 2:u-blox 6
uint16_t reserved2; // Reserved
ubx_nav_svinfo_channel channel[16]; // 16 satellites * 12 byte
} ubx_nav_svinfo;
enum {
PREAMBLE1 = 0xb5,
PREAMBLE2 = 0x62,
CLASS_NAV = 0x01,
CLASS_ACK = 0x05,
CLASS_CFG = 0x06,
MSG_ACK_NACK = 0x00,
MSG_ACK_ACK = 0x01,
MSG_POSLLH = 0x2,
MSG_STATUS = 0x3,
MSG_SOL = 0x6,
MSG_VELNED = 0x12,
MSG_CFG_PRT = 0x00,
MSG_CFG_RATE = 0x08,
MSG_CFG_SET_RATE = 0x01,
MSG_STATUS = 0x3,
MSG_SOL = 0x6,
MSG_VELNED = 0x12,
MSG_SVINFO = 0x30,
MSG_CFG_PRT = 0x00,
MSG_CFG_RATE = 0x08,
MSG_CFG_SET_RATE = 0x01,
MSG_CFG_NAV_SETTINGS = 0x24
} ubs_protocol_bytes;
@ -986,13 +1008,14 @@ static uint8_t _disable_counter;
// Receive buffer
static union {
ubx_nav_posllh posllh;
ubx_nav_status status;
ubx_nav_solution solution;
ubx_nav_velned velned;
uint8_t bytes[64];
} _buffer;
void _update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b)
ubx_nav_status status;
ubx_nav_solution solution;
ubx_nav_velned velned;
ubx_nav_svinfo svinfo;
uint8_t bytes[200];
} _buffer;
void _update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b)
{
while (len--) {
*ck_a += *data;
@ -1068,6 +1091,7 @@ static bool GPS_UBLOX_newFrame(uint8_t data)
static bool UBLOX_parse_gps(void)
{
int i;
switch (_msg_id) {
case MSG_POSLLH:
//i2c_dataset.time = _buffer.posllh.time;
@ -1093,12 +1117,23 @@ static bool UBLOX_parse_gps(void)
case MSG_VELNED:
// speed_3d = _buffer.velned.speed_3d; // cm/s
GPS_speed = _buffer.velned.speed_2d; // cm/s
GPS_ground_course = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10
_new_speed = true;
break;
default:
return false;
}
GPS_ground_course = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10
_new_speed = true;
break;
case MSG_SVINFO:
GPS_numCh = _buffer.svinfo.numCh;
if (GPS_numCh > 16)
GPS_numCh = 16;
for (i = 0; i < GPS_numCh; i++){
GPS_svinfo_chn[i]= _buffer.svinfo.channel[i].chn;
GPS_svinfo_svid[i]= _buffer.svinfo.channel[i].svid;
GPS_svinfo_quality[i]=_buffer.svinfo.channel[i].quality;
GPS_svinfo_cno[i]= _buffer.svinfo.channel[i].cno;
}
break;
default:
return false;
}
// we only return true when we get new position and speed data
// this ensures we don't use stale data

17
src/mw.c
View File

@ -44,12 +44,17 @@ int16_t GPS_angle[2] = { 0, 0 }; // it's the angles that must be applied for
uint16_t GPS_ground_course = 0; // degrees * 10
uint8_t GPS_Present = 0; // Checksum from Gps serial
uint8_t GPS_Enable = 0;
int16_t nav[2];
int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother
int8_t nav_mode = NAV_MODE_NONE; // Navigation mode
// Automatic ACC Offset Calibration
uint16_t InflightcalibratingA = 0;
int16_t nav[2];
int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother
int8_t nav_mode = NAV_MODE_NONE; // Navigation mode
uint8_t GPS_numCh; // Number of channels
uint8_t GPS_svinfo_chn[16]; // Channel number
uint8_t GPS_svinfo_svid[16]; // Satellite ID
uint8_t GPS_svinfo_quality[16]; // Bitfield Qualtity
uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Strength)
// Automatic ACC Offset Calibration
uint16_t InflightcalibratingA = 0;
int16_t AccInflightCalibrationArmed;
uint16_t AccInflightCalibrationMeasurementDone = 0;
uint16_t AccInflightCalibrationSavetoEEProm = 0;

17
src/mw.h
View File

@ -354,12 +354,17 @@ extern int16_t GPS_angle[2]; // it's the angles
extern uint16_t GPS_ground_course; // degrees*10
extern uint8_t GPS_Present; // Checksum from Gps serial
extern uint8_t GPS_Enable;
extern int16_t nav[2];
extern int8_t nav_mode; // Navigation mode
extern int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother
extern master_t mcfg;
extern config_t cfg;
extern int16_t nav[2];
extern int8_t nav_mode; // Navigation mode
extern int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother
extern uint8_t GPS_numCh; // Number of channels
extern uint8_t GPS_svinfo_chn[16]; // Channel number
extern uint8_t GPS_svinfo_svid[16]; // Satellite ID
extern uint8_t GPS_svinfo_quality[16]; // Bitfield Qualtity
extern uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Strength)
extern master_t mcfg;
extern config_t cfg;
extern flags_t f;
extern sensor_t acc;
extern sensor_t gyro;

36
src/serial.c
View File

@ -47,12 +47,13 @@
#define MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4
// Additional commands that are not compatible with MultiWii
#define MSP_UID 160 //out message Unique device ID
#define MSP_ACC_TRIM 240 //out message get acc angle trim values
#define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values
#define INBUF_SIZE 64
#define MSP_UID 160 //out message Unique device ID
#define MSP_ACC_TRIM 240 //out message get acc angle trim values
#define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values
#define MSP_GPSSVINFO 164 //out message get Signal Strength (only U-Blox)
#define INBUF_SIZE 64
struct box_t {
const uint8_t boxIndex; // this is from boxnames enum
const char *boxName; // GUI-readable box name
@ -565,13 +566,22 @@ static void evaluateCommand(void)
case MSP_UID:
headSerialReply(12);
serialize32(U_ID_0);
serialize32(U_ID_1);
serialize32(U_ID_2);
break;
default: // we do not know how to handle the (valid) message, indicate error MSP $M!
headSerialError(0);
break;
serialize32(U_ID_1);
serialize32(U_ID_2);
break;
case MSP_GPSSVINFO:
headSerialReply(1 + (GPS_numCh * 4));
serialize8(GPS_numCh);
for (i = 0; i < GPS_numCh; i++){
serialize8(GPS_svinfo_chn[i]);
serialize8(GPS_svinfo_svid[i]);
serialize8(GPS_svinfo_quality[i]);
serialize8(GPS_svinfo_cno[i]);
}
break;
default: // we do not know how to handle the (valid) message, indicate error MSP $M!
headSerialError(0);
break;
}
tailSerialReply();
}

Write Preview
Loading…
Cancel
Save