Skip to content

Commit

Permalink
Merge pull request #40 from jgoppert/i2c-flow-simpl
Browse files Browse the repository at this point in the history 
Removed i2c struct packing due to alignment warning.
  • Loading branch information
@LorenzMeier
LorenzMeier committed Jan 24, 2015
2 parents 0ad5fd6 + ac9fca9 commit baf60dc
Show file tree
Hide file tree
Showing 3 changed files with 98 additions and 80 deletions.
14 changes: 12 additions & 2 deletions inc/i2c.h
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 Fortiss An-Institut TU Munchen All rights reserved.
* Author: Thomas Boehm <thomas.boehm@fortiss.org>
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
Expand All @@ -13,6 +12,9 @@
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
Expand All @@ -28,6 +30,14 @@
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/

/**
* @file i2c.h
* I2C communication functions.
* @author Thomas Boehm <thomas.boehm@fortiss.org>
*/


#ifndef I2C_H_
#define I2C_H_
#include <inttypes.h>
Expand Down
21 changes: 14 additions & 7 deletions inc/i2c_frame.h
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 Fortiss An-Institut TU Munchen All rights reserved.
* Author: Thomas Boehm <thomas.boehm@fortiss.org>
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
Expand All @@ -13,6 +12,9 @@
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
Expand All @@ -28,13 +30,18 @@
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/

/**
* @file i2c_frame.h
* Definition of i2c frames.
* @author Thomas Boehm <thomas.boehm@fortiss.org>
* @author James Goppert <james.goppert@gmail.com>
*/

#ifndef I2C_FRAME_H_
#define I2C_FRAME_H_
#include <inttypes.h>

// ***Take care***: struct alignment isn't necessarily what you expect,
// so unaligned (i.e., single uint8_t values) should go at the end.
// Otherwise nothing will work.

typedef struct i2c_frame
{
Expand All @@ -50,7 +57,7 @@ typedef struct i2c_frame
uint8_t gyro_range;
uint8_t sonar_timestamp;
int16_t ground_distance;
} __attribute__((packed)) i2c_frame;
} i2c_frame;

#define I2C_FRAME_SIZE (sizeof(i2c_frame))

Expand All @@ -68,7 +75,7 @@ typedef struct i2c_integral_frame
uint16_t ground_distance;
int16_t gyro_temperature;
uint8_t qual;
} __attribute__((packed)) i2c_integral_frame;
} i2c_integral_frame;

#define I2C_INTEGRAL_FRAME_SIZE (sizeof(i2c_integral_frame))

Expand Down
143 changes: 72 additions & 71 deletions src/i2c.c
View file
Original file line number Diff line number Diff line change
@@ -1,33 +1,42 @@
///****************************************************************************
// *
// * Copyright (C) 2013 Fortiss An-Institut TU Munchen All rights reserved.
// * Author: Thomas Boehm <thomas.boehm@fortiss.org>
// *
// * Redistribution and use in source and binary forms, with or without
// * modification, are permitted provided that the following conditions
// * are met:
// *
// * 1. Redistributions of source code must retain the above copyright
// * notice, this list of conditions and the following disclaimer.
// * 2. Redistributions in binary form must reproduce the above copyright
// * notice, this list of conditions and the following disclaimer in
// * the documentation and/or other materials provided with the
// * distribution.
// *
// * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
// * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
// * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
// * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
// * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
// * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
// * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// * POSSIBILITY OF SUCH DAMAGE.
// *
// ****************************************************************************/
/****************************************************************************
*
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/

/**
* @file i2c.c
* Definition of i2c frames.
* @author Thomas Boehm <thomas.boehm@fortiss.org>
* @author James Goppert <james.goppert@gmail.com>
*/

#include "i2c.h"
#include "stm32f4xx.h"
Expand Down Expand Up @@ -217,41 +226,34 @@ void update_TX_buffer(float pixel_flow_x, float pixel_flow_y,
float ground_distance, float gyro_x_rate, float gyro_y_rate,
float gyro_z_rate, int16_t gyro_temp) {
static uint16_t frame_count = 0;
int i;
union {
i2c_frame f;
char c[I2C_FRAME_SIZE];
} u;

u.f.frame_count = frame_count;
u.f.pixel_flow_x_sum = pixel_flow_x * 10.0f;
u.f.pixel_flow_y_sum = pixel_flow_y * 10.0f;
u.f.flow_comp_m_x = flow_comp_m_x * 1000;
u.f.flow_comp_m_y = flow_comp_m_y * 1000;
u.f.qual = qual;
u.f.ground_distance = ground_distance * 1000;

u.f.gyro_x_rate = gyro_x_rate * getGyroScalingFactor();
u.f.gyro_y_rate = gyro_y_rate * getGyroScalingFactor();
u.f.gyro_z_rate = gyro_z_rate * getGyroScalingFactor();
u.f.gyro_range = getGyroRange();

i2c_frame f;
i2c_integral_frame f_integral;

f.frame_count = frame_count;
f.pixel_flow_x_sum = pixel_flow_x * 10.0f;
f.pixel_flow_y_sum = pixel_flow_y * 10.0f;
f.flow_comp_m_x = flow_comp_m_x * 1000;
f.flow_comp_m_y = flow_comp_m_y * 1000;
f.qual = qual;
f.ground_distance = ground_distance * 1000;

f.gyro_x_rate = gyro_x_rate * getGyroScalingFactor();
f.gyro_y_rate = gyro_y_rate * getGyroScalingFactor();
f.gyro_z_rate = gyro_z_rate * getGyroScalingFactor();
f.gyro_range = getGyroRange();

uint32_t time_since_last_sonar_update;

time_since_last_sonar_update = (get_boot_time_us()
- get_sonar_measure_time());

if (time_since_last_sonar_update < 255 * 1000) {
u.f.sonar_timestamp = time_since_last_sonar_update / 1000; //convert to ms
f.sonar_timestamp = time_since_last_sonar_update / 1000; //convert to ms
} else {
u.f.sonar_timestamp = 255;
f.sonar_timestamp = 255;
}

union {
i2c_integral_frame f;
char c_integral[I2C_INTEGRAL_FRAME_SIZE];
} u_integral;

static float accumulated_flow_x = 0;
static float accumulated_flow_y = 0;
static uint16_t accumulated_framecount = 0;
Expand Down Expand Up @@ -303,30 +305,29 @@ void update_TX_buffer(float pixel_flow_x, float pixel_flow_y,
//update lasttime
lasttime = get_boot_time_us();

u_integral.f.frame_count_since_last_readout = accumulated_framecount;
u_integral.f.gyro_x_rate_integral = accumulated_gyro_x * 10.0f; //mrad*10
u_integral.f.gyro_y_rate_integral = accumulated_gyro_y * 10.0f; //mrad*10
u_integral.f.gyro_z_rate_integral = accumulated_gyro_z * 10.0f; //mrad*10
u_integral.f.pixel_flow_x_integral = accumulated_flow_x * 10.0f; //mrad*10
u_integral.f.pixel_flow_y_integral = accumulated_flow_y * 10.0f; //mrad*10
u_integral.f.integration_timespan = integration_timespan; //microseconds
u_integral.f.ground_distance = ground_distance * 1000; //mmeters
u_integral.f.sonar_timestamp = time_since_last_sonar_update; //microseconds
u_integral.f.qual =
f_integral.frame_count_since_last_readout = accumulated_framecount;
f_integral.gyro_x_rate_integral = accumulated_gyro_x * 10.0f; //mrad*10
f_integral.gyro_y_rate_integral = accumulated_gyro_y * 10.0f; //mrad*10
f_integral.gyro_z_rate_integral = accumulated_gyro_z * 10.0f; //mrad*10
f_integral.pixel_flow_x_integral = accumulated_flow_x * 10.0f; //mrad*10
f_integral.pixel_flow_y_integral = accumulated_flow_y * 10.0f; //mrad*10
f_integral.integration_timespan = integration_timespan; //microseconds
f_integral.ground_distance = ground_distance * 1000; //mmeters
f_integral.sonar_timestamp = time_since_last_sonar_update; //microseconds
f_integral.qual =
(uint8_t) (accumulated_quality / accumulated_framecount); //0-255 linear quality measurement 0=bad, 255=best
u_integral.f.gyro_temperature = gyro_temp;//Temperature * 100 in centi-degrees Celsius
f_integral.gyro_temperature = gyro_temp;//Temperature * 100 in centi-degrees Celsius

notpublishedIndexFrame1 = 1 - publishedIndexFrame1; // choose not the current published 1 buffer
notpublishedIndexFrame2 = 1 - publishedIndexFrame2; // choose not the current published 2 buffer

// fill I2C transmitbuffer1 with frame1 values
for (i = 0; i < I2C_FRAME_SIZE; i++) {
txDataFrame1[notpublishedIndexFrame1][i] = u.c[i];
}
memcpy(&(txDataFrame1[notpublishedIndexFrame1]),
&f, I2C_FRAME_SIZE);

// fill I2C transmitbuffer2 with frame2 values
for (i = 0; i < I2C_INTEGRAL_FRAME_SIZE; i++)
txDataFrame2[notpublishedIndexFrame2][i] = u_integral.c_integral[i];
memcpy(&(txDataFrame2[notpublishedIndexFrame2]),
&f_integral, I2C_INTEGRAL_FRAME_SIZE);

//swap buffers frame1 if I2C bus is idle
if (readout_done_frame1) {
Expand Down

0 comments on commit baf60dc

Please sign in to comment.