forked from Archive/PX4-Autopilot
Merge remote-tracking branch 'upstream/master' into ros
This commit is contained in:
commit
53a40dc986
|
@ -0,0 +1,55 @@
|
|||
import serial, time
|
||||
|
||||
|
||||
port = serial.Serial('/dev/ttyACM0', baudrate=57600, timeout=2)
|
||||
|
||||
data = '01234567890123456789012345678901234567890123456789'
|
||||
#data = 'hellohello'
|
||||
outLine = 'echo %s\n' % data
|
||||
|
||||
port.write('\n\n\n')
|
||||
port.write('free\n')
|
||||
line = port.readline(80)
|
||||
while line != '':
|
||||
print(line)
|
||||
line = port.readline(80)
|
||||
|
||||
|
||||
i = 0
|
||||
bytesOut = 0
|
||||
bytesIn = 0
|
||||
|
||||
startTime = time.time()
|
||||
lastPrint = startTime
|
||||
while True:
|
||||
bytesOut += port.write(outLine)
|
||||
line = port.readline(80)
|
||||
bytesIn += len(line)
|
||||
# check command line echo
|
||||
if (data not in line):
|
||||
print('command error %d: %s' % (i,line))
|
||||
#break
|
||||
# read echo output
|
||||
line = port.readline(80)
|
||||
if (data not in line):
|
||||
print('echo output error %d: %s' % (i,line))
|
||||
#break
|
||||
bytesIn += len(line)
|
||||
#print('%d: %s' % (i,line))
|
||||
#print('%d: bytesOut: %d, bytesIn: %d' % (i, bytesOut, bytesIn))
|
||||
|
||||
elapsedT = time.time() - lastPrint
|
||||
if (time.time() - lastPrint >= 5):
|
||||
outRate = bytesOut / elapsedT
|
||||
inRate = bytesIn / elapsedT
|
||||
usbRate = (bytesOut + bytesIn) / elapsedT
|
||||
lastPrint = time.time()
|
||||
print('elapsed time: %f' % (time.time() - startTime))
|
||||
print('data rates (bytes/sec): out: %f, in: %f, total: %f' % (outRate, inRate, usbRate))
|
||||
|
||||
bytesOut = 0
|
||||
bytesIn = 0
|
||||
|
||||
i += 1
|
||||
#if (i > 2): break
|
||||
|
|
@ -176,6 +176,7 @@ static const int ERROR = -1;
|
|||
#define L3G4200D_DEFAULT_RATE 800
|
||||
#define L3GD20_DEFAULT_RANGE_DPS 2000
|
||||
#define L3GD20_DEFAULT_FILTER_FREQ 30
|
||||
#define L3GD20_TEMP_OFFSET_CELSIUS 40
|
||||
|
||||
#ifndef SENSOR_BOARD_ROTATION_DEFAULT
|
||||
#define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG
|
||||
|
@ -856,7 +857,7 @@ L3GD20::measure()
|
|||
#pragma pack(push, 1)
|
||||
struct {
|
||||
uint8_t cmd;
|
||||
uint8_t temp;
|
||||
int8_t temp;
|
||||
uint8_t status;
|
||||
int16_t x;
|
||||
int16_t y;
|
||||
|
@ -930,6 +931,8 @@ L3GD20::measure()
|
|||
|
||||
report.z_raw = raw_report.z;
|
||||
|
||||
report.temperature_raw = raw_report.temp;
|
||||
|
||||
report.x = ((report.x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
|
||||
report.y = ((report.y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
|
||||
report.z = ((report.z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
|
||||
|
@ -938,6 +941,8 @@ L3GD20::measure()
|
|||
report.y = _gyro_filter_y.apply(report.y);
|
||||
report.z = _gyro_filter_z.apply(report.z);
|
||||
|
||||
report.temperature = L3GD20_TEMP_OFFSET_CELSIUS - raw_report.temp;
|
||||
|
||||
// apply user specified rotation
|
||||
rotate_3f(_rotation, report.x, report.y, report.z);
|
||||
|
||||
|
@ -1091,9 +1096,11 @@ test()
|
|||
warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x);
|
||||
warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y);
|
||||
warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z);
|
||||
warnx("temp: \t%d\tC", (int)g_report.temperature);
|
||||
warnx("gyro x: \t%d\traw", (int)g_report.x_raw);
|
||||
warnx("gyro y: \t%d\traw", (int)g_report.y_raw);
|
||||
warnx("gyro z: \t%d\traw", (int)g_report.z_raw);
|
||||
warnx("temp: \t%d\traw", (int)g_report.temperature_raw);
|
||||
warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
|
||||
(int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f));
|
||||
|
||||
|
|
|
@ -74,6 +74,7 @@ enum Rotation {
|
|||
ROTATION_ROLL_270_YAW_135 = 23,
|
||||
ROTATION_PITCH_90 = 24,
|
||||
ROTATION_PITCH_270 = 25,
|
||||
ROTATION_ROLL_270_YAW_270 = 26,
|
||||
ROTATION_MAX
|
||||
};
|
||||
|
||||
|
@ -109,7 +110,8 @@ const rot_lookup_t rot_lookup[] = {
|
|||
{270, 0, 90 },
|
||||
{270, 0, 135 },
|
||||
{ 0, 90, 0 },
|
||||
{ 0, 270, 0 }
|
||||
{ 0, 270, 0 },
|
||||
{270, 0, 270 }
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
|
@ -1396,7 +1396,7 @@ MavlinkReceiver::receive_start(Mavlink *parent)
|
|||
|
||||
struct sched_param param;
|
||||
(void)pthread_attr_getschedparam(&receiveloop_attr, ¶m);
|
||||
param.sched_priority = SCHED_PRIORITY_MAX - 40;
|
||||
param.sched_priority = SCHED_PRIORITY_MAX - 80;
|
||||
(void)pthread_attr_setschedparam(&receiveloop_attr, ¶m);
|
||||
|
||||
pthread_attr_setstacksize(&receiveloop_attr, 2900);
|
||||
|
|
|
@ -41,3 +41,5 @@ SRCS = nshterm.c
|
|||
MODULE_STACKSIZE = 1600
|
||||
|
||||
MAXOPTIMIZATION = -Os
|
||||
|
||||
MODULE_PRIORITY = "SCHED_PRIORITY_DEFAULT-30"
|
||||
|
|
Loading…
Reference in New Issue