From bf98630851461608b240108c84bf685d00cbb8f5 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 25 Dec 2011 09:42:57 -0600 Subject: [PATCH] Add streaming the sensor data out of the serial port --- flight/Modules/Sensors/sensors.c | 74 ++++++++++- flight/Revolution/System/inc/pios_config.h | 1 + flight/Revolution/System/pios_board.c | 77 +++++++----- matlab/revo/lla2ecef.m | 15 +++ matlab/revo/sensor_log.m | 138 +++++++++++++++++++++ matlab/revo/unwrap_time.m | 7 ++ 6 files changed, 277 insertions(+), 35 deletions(-) create mode 100644 matlab/revo/lla2ecef.m create mode 100644 matlab/revo/sensor_log.m create mode 100644 matlab/revo/unwrap_time.m diff --git a/flight/Modules/Sensors/sensors.c b/flight/Modules/Sensors/sensors.c index 5b0c73599..8151f697f 100644 --- a/flight/Modules/Sensors/sensors.c +++ b/flight/Modules/Sensors/sensors.c @@ -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 */ diff --git a/flight/Revolution/System/inc/pios_config.h b/flight/Revolution/System/inc/pios_config.h index 5bcf04354..70f1c902c 100644 --- a/flight/Revolution/System/inc/pios_config.h +++ b/flight/Revolution/System/inc/pios_config.h @@ -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 */ diff --git a/flight/Revolution/System/pios_board.c b/flight/Revolution/System/pios_board.c index 0ab028e66..701a90887 100644 --- a/flight/Revolution/System/pios_board.c +++ b/flight/Revolution/System/pios_board.c @@ -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 */ diff --git a/matlab/revo/lla2ecef.m b/matlab/revo/lla2ecef.m new file mode 100644 index 000000000..e82cdc9eb --- /dev/null +++ b/matlab/revo/lla2ecef.m @@ -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; diff --git a/matlab/revo/sensor_log.m b/matlab/revo/sensor_log.m new file mode 100644 index 000000000..b17ddbad9 --- /dev/null +++ b/matlab/revo/sensor_log.m @@ -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,:) = []; diff --git a/matlab/revo/unwrap_time.m b/matlab/revo/unwrap_time.m new file mode 100644 index 000000000..44961bb19 --- /dev/null +++ b/matlab/revo/unwrap_time.m @@ -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); \ No newline at end of file