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

Add streaming the sensor data out of the serial port

Browse Source
This commit is contained in:
James Cotton 2011-12-25 09:42:57 -06:00
parent e34f618d89
commit bf98630851
6 changed files with 277 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

74
flight/Modules/Sensors/sensors.c
View File

@ -56,6 +56,8 @@
#include "attitudesettings.h"
#include "revocalibration.h"
#include "flightstatus.h"
#include "gpsposition.h"
#include "baroaltitude.h"
#include "CoordinateConversions.h"
// Private constants
@ -68,10 +70,13 @@
// Private variables
static xTaskHandle sensorsTaskHandle;
static bool gps_updated = false;
static bool baro_updated = false;
// Private functions
static void SensorsTask(void *parameters);
static void settingsUpdatedCb(UAVObjEvent * objEv);
static void sensorsUpdatedCb(UAVObjEvent * objEv);
// These values are initialized by settings but can be updated by the attitude algorithm
static bool bias_correct_gyro = true;
@ -103,7 +108,6 @@ int32_t SensorsInitialize(void)
RevoCalibrationInitialize();
RevoCalibrationConnectCallback(&settingsUpdatedCb);
return 0;
}
@ -162,6 +166,12 @@ static void SensorsTask(void *parameters)
}
}
// If debugging connect callback
if(pios_com_aux_id != 0) {
BaroAltitudeConnectCallback(&sensorsUpdatedCb);
GPSPositionConnectCallback(&sensorsUpdatedCb);
}
// Main task loop
lastSysTime = xTaskGetTickCount();
while (1) {
@ -239,22 +249,80 @@ static void SensorsTask(void *parameters)
// Because most crafts wont get enough information from gravity to zero yaw gyro, we try
// and make it average zero (weakly)
MagnetometerData mag;
bool mag_updated = false;
if (PIOS_HMC5883_NewDataAvailable()) {
mag_updated = true;
int16_t values[3];
PIOS_HMC5883_ReadMag(values);
MagnetometerData mag; // Skip get as we set all the fields
mag.x = values[1] * mag_scale[0] - mag_bias[0];
mag.y = values[0] * mag_scale[1] - mag_bias[1];
mag.z = -values[2] * mag_scale[2] - mag_bias[2];
MagnetometerSet(&mag);
}
// For debugging purposes here we can output all of the sensors. Do it as a single transaction
// so the message isn't split if anything else is writing to it
if(pios_com_aux_id != 0) {
uint32_t message_size = 3;
uint8_t message[200] = {0xff, (lastSysTime & 0xff00) >> 8, lastSysTime & 0x00ff};
// Add accel data
memcpy(&message[message_size], &accelsData.x, sizeof(accelsData.x) * 3);
message_size += sizeof(accelsData.x) * 3;
// Add gyro data with temp
memcpy(&message[message_size], &gyrosData, sizeof(gyrosData));
message_size += sizeof(gyrosData);
if(mag_updated) { // Add mag data
message[message_size] = 0x01; // Indicate mag data here
message_size++;
memcpy(&message[message_size], &mag, sizeof(mag));
message_size += sizeof(mag);
}
if(gps_updated) { // Add GPS data
gps_updated = false;
GPSPositionData gps;
GPSPositionGet(&gps);
message[message_size] = 0x02; // Indicate gps data here
message_size++;
memcpy(&message[message_size], &gps, sizeof(gps));
message_size += sizeof(gps);
}
if(baro_updated) { // Add baro data
baro_updated = false;
BaroAltitudeData baro;
BaroAltitudeGet(&baro);
message[message_size] = 0x03; // Indicate mag data here
message_size++;
memcpy(&message[message_size], &baro, sizeof(baro));
message_size += sizeof(baro);
}
PIOS_COM_SendBufferNonBlocking(pios_com_aux_id, message, message_size);
}
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
vTaskDelayUntil(&lastSysTime, 2 / portTICK_RATE_MS);
}
}
/**
* Indicate that these sensors have been updated
*/
static void sensorsUpdatedCb(UAVObjEvent * objEv)
{
if(objEv->obj == GPSPositionHandle())
gps_updated = true;
if(objEv->obj == BaroAltitudeHandle())
baro_updated = true;
}
/**
* Locally cache some variables from the AtttitudeSettings object
*/

1
flight/Revolution/System/inc/pios_config.h
View File

@ -64,6 +64,7 @@
/* Com systems to include */
#define PIOS_INCLUDE_COM
#define PIOS_INCLUDE_COM_TELEM
//#define PIOS_INCLUDE_COM_AUX
#define PIOS_INCLUDE_GPS
/* Supported receiver interfaces */

77
flight/Revolution/System/pios_board.c
View File

@ -460,8 +460,8 @@ static const struct pios_usart_cfg pios_usart_gps_cfg = {
* AUX USART
*/
static const struct pios_usart_cfg pios_usart_aux_cfg = {
.regs = UART4,
.remap = GPIO_AF_UART4,
.regs = USART6,
.remap = GPIO_AF_USART6,
.init = {
.USART_BaudRate = 230400,
.USART_WordLength = USART_WordLength_8b,
@ -473,8 +473,8 @@ static const struct pios_usart_cfg pios_usart_aux_cfg = {
},
.irq = {
.init = {
.NVIC_IRQChannel = UART4_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannel = USART6_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
@ -482,7 +482,7 @@ static const struct pios_usart_cfg pios_usart_aux_cfg = {
.rx = {
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_11,
.GPIO_Pin = GPIO_Pin_7,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
@ -492,7 +492,7 @@ static const struct pios_usart_cfg pios_usart_aux_cfg = {
.tx = {
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Pin = GPIO_Pin_6,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
@ -1567,36 +1567,49 @@ void PIOS_Board_Init(void) {
#endif /* PIOS_INCLUDE_GPS */
#if defined(PIOS_INCLUDE_COM_AUX)
uint32_t pios_usart_aux_id;
if (PIOS_USART_Init(&pios_usart_aux_id, &pios_usart_aux_cfg)) {
PIOS_DEBUG_Assert(0);
{
uint32_t pios_usart_aux_id;
if (PIOS_USART_Init(&pios_usart_aux_id, &pios_usart_aux_cfg)) {
PIOS_DEBUG_Assert(0);
}
const uint32_t BUF_SIZE = 512;
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(BUF_SIZE);
uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(BUF_SIZE);
PIOS_Assert(rx_buffer);
PIOS_Assert(tx_buffer);
if (PIOS_COM_Init(&pios_com_aux_id, &pios_usart_com_driver, pios_usart_aux_id,
rx_buffer, BUF_SIZE,
tx_buffer, BUF_SIZE)) {
PIOS_DEBUG_Assert(0);
}
}
if (PIOS_COM_Init(&pios_com_aux_id, &pios_usart_com_driver, pios_usart_aux_id,
pios_com_aux_rx_buffer, sizeof(pios_com_aux_rx_buffer),
pios_com_aux_tx_buffer, sizeof(pios_com_aux_tx_buffer))) {
PIOS_DEBUG_Assert(0);
}
#endif
#else
pios_com_aux_id = 0;
#endif /* PIOS_INCLUDE_COM_AUX */
#if defined(PIOS_INCLUDE_COM_TELEM)
{ /* Eventually add switch for this port function */
uint32_t pios_usart_telem_rf_id;
if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_main_cfg)) {
PIOS_Assert(0);
{ /* Eventually add switch for this port function */
uint32_t pios_usart_telem_rf_id;
if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_main_cfg)) {
PIOS_Assert(0);
}
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN);
uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN);
PIOS_Assert(rx_buffer);
PIOS_Assert(tx_buffer);
if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id,
rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN,
tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) {
PIOS_Assert(0);
}
}
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN);
uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN);
PIOS_Assert(rx_buffer);
PIOS_Assert(tx_buffer);
if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id,
rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN,
tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_COM_AUX */
#else
pios_com_telem_rf_id = 0;
#endif /* PIOS_INCLUDE_COM_TELEM */
#endif /* PIOS_INCLUDE_COM */

15
matlab/revo/lla2ecef.m Normal file
View File

@ -0,0 +1,15 @@
function ECEF = lla2ecef(latitude,longitude,altitude)
a = 6378137.0;
e = 8.1819190842622e-2;
DEG2RAD = pi / 180;
sinLat = sin(DEG2RAD * latitude);
sinLon = sin(DEG2RAD * longitude);
cosLat = cos(DEG2RAD * latitude);
cosLon = cos(DEG2RAD * longitude);
N = a ./ sqrt(1.0 - e * e .* sinLat .* sinLat);
ECEF(:,1) = (N + altitude) .* cosLat .* cosLon;
ECEF(:,2) = (N + altitude) .* cosLat .* sinLon;
ECEF(:,3) = ((1 - e * e) * N + altitude) .* sinLat;

138
matlab/revo/sensor_log.m Normal file
View File

@ -0,0 +1,138 @@
fn = '~/Desktop/kenn_stationary (2011-12-26 14:51:11 +0000)';
fid = fopen(fn);
dat = uint8(fread(fid,'uint8'));
fclose(fid);
accel = [];
accel_idx = 0;
gyro = [];
gyro_idx = 0;
mag = [];
mag_idx = 0;
latitude = [];
longitude = [];
altitude = [];
heading = [];
groundspeed = [];
gps_satellites = [];
gps_time = [];
gps_idx = 0;
baro = [];
baro_idx = 0;
total = length(dat);
count = 0;
head = 0;
last_time = 0;
while head < (length(dat) - 200)
count = count + 1;
if count >= 100
disp(sprintf('%0.3g%%',(head/total) * 100));
count = 0;
end
head = head + find(dat(head+1:end)==255,1,'first');
% Get the time
time = typecast(flipud(dat(head+(1:2))),'uint16');
if (time - last_time) ~= 2 && (time-last_time) ~= (hex2dec('10000')-2)
last_time = time;
disp('Err');
continue
end
last_time = time;
head = head + 2;
% Get the accels
accel_idx = accel_idx + 1;
if accel_idx > size(accel,1)
accel(accel_idx * 2,:) = 0;
end
accel(accel_idx,1:3) = typecast(dat(head+(1:12)),'single');
head = head + 12;
accel(accel_idx,4) = time;
% Get the gyros
gyro_idx = gyro_idx + 1;
if gyro_idx > size(gyro,1);
gyro(gyro_idx * 2,:) = 0;
end
gyro(gyro_idx,1:4) = typecast(dat(head+(1:16)),'single');
head = head + 16;
gyro(gyro_idx,5) = time;
if dat(head+1) == 1 % Process the mag data
head = head+1;
mag_idx = mag_idx + 1;
if mag_idx > size(mag,1)
mag(mag_idx * 2, :) = 0;
end
mag(mag_idx,1:3) = typecast(dat(head + (1:12)),'single');
head = head+12;
mag(mag_idx,4) = time;
end
if dat(head+1) == 2 % Process the GPS data
% typedef struct {
% int32_t Latitude;
% int32_t Longitude;
% float Altitude;
% float GeoidSeparation;
% float Heading;
% float Groundspeed;
% float PDOP;
% float HDOP;
% float VDOP;
% uint8_t Status;
% int8_t Satellites;
% } __attribute__((packed)) GPSPositionData;
head = head+1;
gps_idx = gps_idx + 1;
if gps_idx > length(latitude)
latitude(gps_idx * 2) = 0;
longitude(gps_idx * 2) = 0;
altitude(gps_idx * 2) = 0;
heading(gps_idx * 2) = 0;
groundspeed(gps_idx * 2) = 0;
gps_satellites(gps_idx * 2) = 0;
gps_time(gps_idx * 2) = 0;
end
latitude(gps_idx) = double(typecast(dat(head+(1:4)),'int32')) / 1e7;
longitude(gps_idx) = double(typecast(dat(head+(5:8)),'int32')) / 1e7;
altitude(gps_idx) = typecast(dat(head+(9:12)),'single');
heading(gps_idx) = typecast(dat(head+(17:20)),'single');
groundspeed(gps_idx) = typecast(dat(head+(21:24)),'single');
gps_satelites(gps_idx) = dat(head+38);
gps_time(gps_idx) = time;
head = head + 9 * 4 + 2;
end
if dat(head+1) == 3 % Process the baro data
head = head + 1;
baro_idx = baro_idx + 1;
if baro_idx > size(baro,1)
baro(baro_idx * 2,:) = 0;
end
baro(baro_idx,1) = typecast(dat(head+(1:4)),'single');
baro(baro_idx,2) = time;
end
end
accel(accel_idx+1:end,:) = [];
gyro(gyro_idx+1:end,:) = [];
mag(mag_idx+1:end) = [];
latitude(gps_idx+1:end) = [];
longitude(gps_idx+1:end) = [];
altitude(gps_idx+1:end) = [];
groundspeed(gps_idx+1:end) = [];
gps_satelites(gps_idx+1:end) = [];
gps_time(gps_idx+1:end) = [];
baro(baro_idx+1:end,:) = [];

7
matlab/revo/unwrap_time.m Normal file
View File

@ -0,0 +1,7 @@
function t = unwrap_time(t)
wrap = find((t(2:end) - t(1:end-1) ) < -64000);
jump = zeros(size(t));
jump(wrap+1) = hex2dec('10000');
t = t + cumsum(jump);