forked from Archive/PX4-Autopilot
Merge remote-tracking branch 'upstream/master' into mtecs
This commit is contained in:
Thomas Gubler
commit
f20a9c4873
|
|
@ -37,3 +37,4 @@ mavlink/include/mavlink/v0.9/
|
|||
tags
|
||||
.tags_sorted_by_file
|
||||
.pydevproject
|
||||
.ropeproject
|
||||
|
|
|
|||
|
|
@ -5,4 +5,4 @@
|
|||
|
||||
sh /etc/init.d/rc.fw_defaults
|
||||
|
||||
set MIXER FMU_RET
|
||||
set MIXER easystar.mix
|
||||
|
|
|
|||
|
|
@ -11,7 +11,7 @@ px4io recovery
|
|||
# Adjust PX4IO update rate limit
|
||||
#
|
||||
set PX4IO_LIMIT 400
|
||||
if hw_ver compare PX4FMU_V1
|
||||
if ver hwcmp PX4FMU_V1
|
||||
then
|
||||
set PX4IO_LIMIT 200
|
||||
fi
|
||||
|
|
|
|||
|
|
@ -5,7 +5,7 @@
|
|||
|
||||
if [ -d /fs/microsd ]
|
||||
then
|
||||
if hw_ver compare PX4FMU_V1
|
||||
if ver hwcmp PX4FMU_V1
|
||||
then
|
||||
echo "Start sdlog2 at 50Hz"
|
||||
sdlog2 start -r 50 -a -b 8 -t
|
||||
|
|
|
|||
|
|
@ -22,7 +22,7 @@ then
|
|||
echo "[init] Using L3GD20(H)"
|
||||
fi
|
||||
|
||||
if hw_ver compare PX4FMU_V2
|
||||
if ver hwcmp PX4FMU_V2
|
||||
then
|
||||
if lsm303d start
|
||||
then
|
||||
|
|
|
|||
|
|
@ -248,7 +248,7 @@ then
|
|||
echo "[init] ERROR: PX4IO not found, disabling output"
|
||||
|
||||
# Avoid using ttyS0 for MAVLink on FMUv1
|
||||
if hw_ver compare PX4FMU_V1
|
||||
if ver hwcmp PX4FMU_V1
|
||||
then
|
||||
set FMU_MODE serial
|
||||
fi
|
||||
|
|
@ -262,7 +262,7 @@ then
|
|||
if [ $HIL == yes ]
|
||||
then
|
||||
set OUTPUT_MODE hil
|
||||
if hw_ver compare PX4FMU_V1
|
||||
if ver hwcmp PX4FMU_V1
|
||||
then
|
||||
set FMU_MODE serial
|
||||
fi
|
||||
|
|
@ -308,7 +308,7 @@ then
|
|||
tone_alarm $TUNE_OUT_ERROR
|
||||
fi
|
||||
|
||||
if hw_ver compare PX4FMU_V1
|
||||
if ver hwcmp PX4FMU_V1
|
||||
then
|
||||
if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ]
|
||||
then
|
||||
|
|
@ -383,7 +383,7 @@ then
|
|||
tone_alarm $TUNE_OUT_ERROR
|
||||
fi
|
||||
|
||||
if hw_ver compare PX4FMU_V1
|
||||
if ver hwcmp PX4FMU_V1
|
||||
then
|
||||
if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ]
|
||||
then
|
||||
|
|
|
|||
|
|
@ -0,0 +1,31 @@
|
|||
EASYSTAR / EASYSTAR II MIXER
|
||||
============================
|
||||
|
||||
Aileron mixer
|
||||
-------------
|
||||
One output - would be easy to add support for 2 servos
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 0 10000 10000 0 -10000 10000
|
||||
|
||||
Elevator mixer
|
||||
------------
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 1 -10000 -10000 0 -10000 10000
|
||||
|
||||
Rudder mixer
|
||||
------------
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 2 -10000 -10000 0 -10000 10000
|
||||
|
||||
Motor speed mixer
|
||||
-----------------
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 3 0 20000 -10000 -10000 10000
|
||||
|
|
@ -43,6 +43,7 @@ from __future__ import print_function
|
|||
import argparse
|
||||
import os
|
||||
|
||||
|
||||
def main():
|
||||
|
||||
# Parse commandline arguments
|
||||
|
|
@ -56,7 +57,7 @@ def main():
|
|||
for (root, dirs, files) in os.walk(args.folder):
|
||||
for file in files:
|
||||
# only prune text files
|
||||
if ".zip" in file or ".bin" in file:
|
||||
if ".zip" in file or ".bin" in file or ".swp" in file:
|
||||
continue
|
||||
|
||||
file_path = os.path.join(root, file)
|
||||
|
|
|
|||
|
|
@ -1,2 +1,4 @@
|
|||
./obj/*
|
||||
mixer_test
|
||||
sbus2_test
|
||||
autodeclination_test
|
||||
|
|
|
|||
|
|
@ -1,47 +1,39 @@
|
|||
|
||||
CC=g++
|
||||
CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers -I../../src -D__EXPORT="" -Dnullptr="0"
|
||||
CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers \
|
||||
-I../../src -I../../src/lib -D__EXPORT="" -Dnullptr="0" -lm
|
||||
|
||||
ODIR=obj
|
||||
LDIR =../lib
|
||||
all: mixer_test sbus2_test autodeclination_test
|
||||
|
||||
LIBS=-lm
|
||||
MIXER_FILES=../../src/systemcmds/tests/test_mixer.cpp \
|
||||
../../src/systemcmds/tests/test_conv.cpp \
|
||||
../../src/modules/systemlib/mixer/mixer_simple.cpp \
|
||||
../../src/modules/systemlib/mixer/mixer_multirotor.cpp \
|
||||
../../src/modules/systemlib/mixer/mixer.cpp \
|
||||
../../src/modules/systemlib/mixer/mixer_group.cpp \
|
||||
../../src/modules/systemlib/mixer/mixer_load.c \
|
||||
../../src/modules/systemlib/pwm_limit/pwm_limit.c \
|
||||
hrt.cpp \
|
||||
mixer_test.cpp
|
||||
|
||||
#_DEPS = test.h
|
||||
#DEPS = $(patsubst %,$(IDIR)/%,$(_DEPS))
|
||||
SBUS2_FILES=../../src/modules/px4iofirmware/sbus.c \
|
||||
hrt.cpp \
|
||||
sbus2_test.cpp
|
||||
|
||||
_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o \
|
||||
mixer.o mixer_group.o mixer_load.o test_conv.o pwm_limit.o hrt.o
|
||||
OBJ = $(patsubst %,$(ODIR)/%,$(_OBJ))
|
||||
AUTODECLINATION_FILES= ../../src/lib/geo/geo_mag_declination.c \
|
||||
hrt.cpp \
|
||||
autodeclination_test.cpp
|
||||
|
||||
#$(DEPS)
|
||||
$(ODIR)/%.o: %.cpp
|
||||
mkdir -p obj
|
||||
$(CC) -c -o $@ $< $(CFLAGS)
|
||||
mixer_test: $(MIXER_FILES)
|
||||
$(CC) -o mixer_test $(MIXER_FILES) $(CFLAGS)
|
||||
|
||||
$(ODIR)/%.o: ../../src/systemcmds/tests/%.cpp
|
||||
$(CC) -c -o $@ $< $(CFLAGS)
|
||||
sbus2_test: $(SBUS2_FILES)
|
||||
$(CC) -o sbus2_test $(SBUS2_FILES) $(CFLAGS)
|
||||
|
||||
$(ODIR)/%.o: ../../src/modules/systemlib/%.cpp
|
||||
$(CC) -c -o $@ $< $(CFLAGS)
|
||||
|
||||
$(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.cpp
|
||||
$(CC) -c -o $@ $< $(CFLAGS)
|
||||
|
||||
$(ODIR)/%.o: ../../src/modules/systemlib/pwm_limit/%.cpp
|
||||
$(CC) -c -o $@ $< $(CFLAGS)
|
||||
|
||||
$(ODIR)/%.o: ../../src/modules/systemlib/pwm_limit/%.c
|
||||
$(CC) -c -o $@ $< $(CFLAGS)
|
||||
|
||||
$(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.c
|
||||
$(CC) -c -o $@ $< $(CFLAGS)
|
||||
|
||||
#
|
||||
mixer_test: $(OBJ)
|
||||
g++ -o $@ $^ $(CFLAGS) $(LIBS)
|
||||
autodeclination_test: $(SBUS2_FILES)
|
||||
$(CC) -o autodeclination_test $(AUTODECLINATION_FILES) $(CFLAGS)
|
||||
|
||||
.PHONY: clean
|
||||
|
||||
clean:
|
||||
rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~
|
||||
rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test
|
||||
|
|
@ -0,0 +1,28 @@
|
|||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
#include <string.h>
|
||||
#include <systemlib/mixer/mixer.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <px4iofirmware/px4io.h>
|
||||
#include "../../src/systemcmds/tests/tests.h"
|
||||
#include <geo/geo.h>
|
||||
|
||||
int main(int argc, char *argv[]) {
|
||||
warnx("autodeclination test started");
|
||||
|
||||
if (argc < 3)
|
||||
errx(1, "Need lat/lon!");
|
||||
|
||||
char* p_end;
|
||||
|
||||
float lat = strtod(argv[1], &p_end);
|
||||
float lon = strtod(argv[2], &p_end);
|
||||
|
||||
float declination = get_mag_declination(lat, lon);
|
||||
|
||||
printf("lat: %f lon: %f, dec: %f\n", lat, lon, declination);
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,5 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <systemlib/err.h>
|
||||
#define lowsyslog warnx
|
||||
|
|
@ -0,0 +1,6 @@
|
|||
#!/bin/sh
|
||||
|
||||
make clean
|
||||
make all
|
||||
./mixer_test
|
||||
./sbus2_test ../../../../data/sbus2/sbus2_r7008SB_gps_baro_tx_off.txt
|
||||
|
|
@ -0,0 +1,75 @@
|
|||
|
||||
#include <stdio.h>
|
||||
#include <unistd.h>
|
||||
#include <string.h>
|
||||
#include <systemlib/mixer/mixer.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <px4iofirmware/px4io.h>
|
||||
#include "../../src/systemcmds/tests/tests.h"
|
||||
|
||||
int main(int argc, char *argv[]) {
|
||||
warnx("SBUS2 test started");
|
||||
|
||||
if (argc < 2)
|
||||
errx(1, "Need a filename for the input file");
|
||||
|
||||
warnx("loading data from: %s", argv[1]);
|
||||
|
||||
FILE *fp;
|
||||
|
||||
fp = fopen(argv[1],"rt");
|
||||
|
||||
if (!fp)
|
||||
errx(1, "failed opening file");
|
||||
|
||||
float f;
|
||||
unsigned x;
|
||||
int ret;
|
||||
|
||||
// Trash the first 20 lines
|
||||
for (unsigned i = 0; i < 20; i++) {
|
||||
(void)fscanf(fp, "%f,%x,,", &f, &x);
|
||||
}
|
||||
|
||||
// Init the parser
|
||||
uint8_t frame[30];
|
||||
unsigned partial_frame_count = 0;
|
||||
uint16_t rc_values[18];
|
||||
uint16_t num_values;
|
||||
bool sbus_failsafe;
|
||||
bool sbus_frame_drop;
|
||||
uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]);
|
||||
|
||||
float last_time = 0;
|
||||
|
||||
while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
|
||||
if (((f - last_time) * 1000 * 1000) > 3000) {
|
||||
partial_frame_count = 0;
|
||||
warnx("FRAME RESET\n\n");
|
||||
}
|
||||
|
||||
frame[partial_frame_count] = x;
|
||||
partial_frame_count++;
|
||||
|
||||
//warnx("%f: 0x%02x, first: 0x%02x, last: 0x%02x, pcount: %u", (double)f, x, frame[0], frame[24], partial_frame_count);
|
||||
|
||||
if (partial_frame_count == sizeof(frame))
|
||||
partial_frame_count = 0;
|
||||
|
||||
last_time = f;
|
||||
|
||||
// Pipe the data into the parser
|
||||
hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
//if (partial_frame_count % 25 == 0)
|
||||
//sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels);
|
||||
}
|
||||
|
||||
if (ret == EOF) {
|
||||
warnx("Test finished, reached end of file");
|
||||
} else {
|
||||
warnx("Test aborted, errno: %d", ret);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -55,8 +55,8 @@ MODULES += systemcmds/top
|
|||
MODULES += systemcmds/tests
|
||||
MODULES += systemcmds/config
|
||||
MODULES += systemcmds/nshterm
|
||||
MODULES += systemcmds/hw_ver
|
||||
MODULES += systemcmds/dumpfile
|
||||
MODULES += systemcmds/ver
|
||||
|
||||
#
|
||||
# General system control
|
||||
|
|
|
|||
|
|
@ -63,8 +63,8 @@ MODULES += systemcmds/tests
|
|||
MODULES += systemcmds/config
|
||||
MODULES += systemcmds/nshterm
|
||||
MODULES += systemcmds/mtd
|
||||
MODULES += systemcmds/hw_ver
|
||||
MODULES += systemcmds/dumpfile
|
||||
MODULES += systemcmds/ver
|
||||
|
||||
#
|
||||
# General system control
|
||||
|
|
|
|||
|
|
@ -29,7 +29,7 @@ MODULES += systemcmds/reboot
|
|||
MODULES += systemcmds/tests
|
||||
MODULES += systemcmds/nshterm
|
||||
MODULES += systemcmds/mtd
|
||||
MODULES += systemcmds/hw_ver
|
||||
MODULES += systemcmds/ver
|
||||
|
||||
#
|
||||
# Library modules
|
||||
|
|
|
|||
|
|
@ -371,6 +371,8 @@ $(ROMFS_IMG): $(ROMFS_SCRATCH) $(ROMFS_DEPS) $(GLOBAL_DEPS)
|
|||
$(ROMFS_SCRATCH): $(ROMFS_DEPS) $(GLOBAL_DEPS)
|
||||
$(Q) $(MKDIR) -p $(ROMFS_SCRATCH)
|
||||
$(Q) $(COPYDIR) $(ROMFS_ROOT)/* $(ROMFS_SCRATCH)
|
||||
# delete all files in ROMFS_SCRATCH which start with a . or end with a ~
|
||||
$(Q) $(RM) $(ROMFS_SCRATCH)/*/.[!.]* $(ROMFS_SCRATCH)/*/*~
|
||||
ifneq ($(ROMFS_EXTRA_FILES),)
|
||||
$(Q) $(MKDIR) -p $(ROMFS_SCRATCH)/extras
|
||||
$(Q) $(COPY) $(ROMFS_EXTRA_FILES) $(ROMFS_SCRATCH)/extras
|
||||
|
|
|
|||
|
|
@ -41,6 +41,7 @@
|
|||
|
||||
#include <sys/types.h>
|
||||
#include <stdbool.h>
|
||||
#include <inttypes.h>
|
||||
|
||||
#include <time.h>
|
||||
#include <queue.h>
|
||||
|
|
|
|||
|
|
@ -50,6 +50,8 @@
|
|||
|
||||
__BEGIN_DECLS
|
||||
|
||||
#include "geo/geo_mag_declination.h"
|
||||
|
||||
#include <stdbool.h>
|
||||
|
||||
#define CONSTANTS_ONE_G 9.80665f /* m/s^2 */
|
||||
|
|
|
|||
|
|
@ -0,0 +1,136 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2014 MAV GEO Library (MAVGEO). 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 MAVGEO 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 geo_mag_declination.c
|
||||
*
|
||||
* Calculation / lookup table for earth magnetic field declination.
|
||||
*
|
||||
* Lookup table from Scott Ferguson <scottfromscott@gmail.com>
|
||||
*
|
||||
* XXX Lookup table currently too coarse in resolution (only full degrees)
|
||||
* and lat/lon res - needs extension medium term.
|
||||
*
|
||||
*/
|
||||
|
||||
#include <geo/geo.h>
|
||||
|
||||
/** set this always to the sampling in degrees for the table below */
|
||||
#define SAMPLING_RES 10.0f
|
||||
#define SAMPLING_MIN_LAT -60.0f
|
||||
#define SAMPLING_MAX_LAT 60.0f
|
||||
#define SAMPLING_MIN_LON -180.0f
|
||||
#define SAMPLING_MAX_LON 180.0f
|
||||
|
||||
static const int8_t declination_table[13][37] = \
|
||||
{
|
||||
46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, \
|
||||
-66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46, 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, \
|
||||
-3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, \
|
||||
29, 30, 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, \
|
||||
-40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21, 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, \
|
||||
8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, \
|
||||
10, 13, 15, 16, 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, \
|
||||
-10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12, 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, \
|
||||
-14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10, 9, 9, 9, \
|
||||
9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, \
|
||||
7, 8, 9, 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, \
|
||||
0, 0, 0, 1, 3, 5, 7, 8, 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, \
|
||||
0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8, 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, \
|
||||
0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6, 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, \
|
||||
-16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5, 4, 8, 12, 15, 17, 18, 16, \
|
||||
12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, \
|
||||
0, 4, 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, \
|
||||
13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3
|
||||
};
|
||||
|
||||
static float get_lookup_table_val(unsigned lat, unsigned lon);
|
||||
|
||||
__EXPORT float get_mag_declination(float lat, float lon)
|
||||
{
|
||||
/*
|
||||
* If the values exceed valid ranges, return zero as default
|
||||
* as we have no way of knowing what the closest real value
|
||||
* would be.
|
||||
*/
|
||||
if (lat < -90.0f || lat > 90.0f ||
|
||||
lon < -180.0f || lon > 180.0f) {
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
/* round down to nearest sampling resolution */
|
||||
int min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES;
|
||||
int min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES;
|
||||
|
||||
/* for the rare case of hitting the bounds exactly
|
||||
* the rounding logic wouldn't fit, so enforce it.
|
||||
*/
|
||||
|
||||
/* limit to table bounds - required for maxima even when table spans full globe range */
|
||||
if (lat <= SAMPLING_MIN_LAT) {
|
||||
min_lat = SAMPLING_MIN_LAT;
|
||||
}
|
||||
|
||||
if (lat >= SAMPLING_MAX_LAT) {
|
||||
min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES;
|
||||
}
|
||||
|
||||
if (lon <= SAMPLING_MIN_LON) {
|
||||
min_lon = SAMPLING_MIN_LON;
|
||||
}
|
||||
|
||||
if (lon >= SAMPLING_MAX_LON) {
|
||||
min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES;
|
||||
}
|
||||
|
||||
/* find index of nearest low sampling point */
|
||||
unsigned min_lat_index = (-(SAMPLING_MIN_LAT) + min_lat) / SAMPLING_RES;
|
||||
unsigned min_lon_index = (-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES;
|
||||
|
||||
float declination_sw = get_lookup_table_val(min_lat_index, min_lon_index);
|
||||
float declination_se = get_lookup_table_val(min_lat_index, min_lon_index + 1);
|
||||
float declination_ne = get_lookup_table_val(min_lat_index + 1, min_lon_index + 1);
|
||||
float declination_nw = get_lookup_table_val(min_lat_index + 1, min_lon_index);
|
||||
|
||||
/* perform bilinear interpolation on the four grid corners */
|
||||
|
||||
float declination_min = ((lon - min_lon) / SAMPLING_RES) * (declination_se - declination_sw) + declination_sw;
|
||||
float declination_max = ((lon - min_lon) / SAMPLING_RES) * (declination_ne - declination_nw) + declination_nw;
|
||||
|
||||
return ((lat - min_lat) / SAMPLING_RES) * (declination_max - declination_min) + declination_min;
|
||||
}
|
||||
|
||||
float get_lookup_table_val(unsigned lat_index, unsigned lon_index)
|
||||
{
|
||||
return declination_table[lat_index][lon_index];
|
||||
}
|
||||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2014 MAV GEO Library (MAVGEO). All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -12,7 +12,7 @@
|
|||
* 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
|
||||
* 3. Neither the name MAVGEO nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
|
|
@ -32,42 +32,16 @@
|
|||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file hw_ver.c
|
||||
* @file geo_mag_declination.h
|
||||
*
|
||||
* Calculation / lookup table for earth magnetic field declination.
|
||||
*
|
||||
* Show and test hardware version.
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#pragma once
|
||||
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <errno.h>
|
||||
#include <version/version.h>
|
||||
__BEGIN_DECLS
|
||||
|
||||
__EXPORT int hw_ver_main(int argc, char *argv[]);
|
||||
__EXPORT float get_mag_declination(float lat, float lon);
|
||||
|
||||
int
|
||||
hw_ver_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc >= 2) {
|
||||
if (!strcmp(argv[1], "show")) {
|
||||
printf(HW_ARCH "\n");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "compare")) {
|
||||
if (argc >= 3) {
|
||||
int ret = strcmp(HW_ARCH, argv[2]) != 0;
|
||||
if (ret == 0) {
|
||||
printf("hw_ver match: %s\n", HW_ARCH);
|
||||
}
|
||||
exit(ret);
|
||||
|
||||
} else {
|
||||
errx(1, "not enough arguments, try 'compare PX4FMU_1'");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
errx(1, "expected a command, try 'show' or 'compare'");
|
||||
}
|
||||
__END_DECLS
|
||||
|
|
@ -35,4 +35,5 @@
|
|||
# Geo library
|
||||
#
|
||||
|
||||
SRCS = geo.c
|
||||
SRCS = geo.c \
|
||||
geo_mag_declination.c
|
||||
|
|
|
|||
|
|
@ -5193,7 +5193,7 @@ void arm_rfft_fast_f32(
|
|||
*pIa = Ialpha;
|
||||
|
||||
/* Calculating pIb from Ialpha and Ibeta by equation pIb = -(1/2) * Ialpha + (sqrt(3)/2) * Ibeta */
|
||||
*pIb = -0.5 * Ialpha + (float32_t) 0.8660254039 *Ibeta;
|
||||
*pIb = (float32_t)-0.5 * Ialpha + (float32_t) 0.8660254039 *Ibeta;
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -432,8 +432,8 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
|
|||
memset(&manual, 0, sizeof(manual));
|
||||
|
||||
manual.timestamp = hrt_absolute_time();
|
||||
manual.roll = man.x / 1000.0f;
|
||||
manual.pitch = man.y / 1000.0f;
|
||||
manual.pitch = man.x / 1000.0f;
|
||||
manual.roll = man.y / 1000.0f;
|
||||
manual.yaw = man.r / 1000.0f;
|
||||
manual.throttle = man.z / 1000.0f;
|
||||
|
||||
|
|
|
|||
|
|
@ -41,6 +41,7 @@
|
|||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include <ctype.h>
|
||||
#include <systemlib/err.h>
|
||||
|
||||
#include "mixer_load.h"
|
||||
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2013, 2014 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
|
||||
|
|
@ -443,6 +443,110 @@ pwm_main(int argc, char *argv[])
|
|||
exit(0);
|
||||
}
|
||||
}
|
||||
usleep(2000);
|
||||
}
|
||||
exit(0);
|
||||
|
||||
|
||||
} else if (!strcmp(argv[1], "steps")) {
|
||||
|
||||
if (set_mask == 0) {
|
||||
usage("no channels set");
|
||||
}
|
||||
|
||||
/* get current servo values */
|
||||
struct pwm_output_values last_spos;
|
||||
|
||||
for (unsigned i = 0; i < servo_count; i++) {
|
||||
|
||||
ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&last_spos.values[i]);
|
||||
if (ret != OK)
|
||||
err(1, "PWM_SERVO_GET(%d)", i);
|
||||
}
|
||||
|
||||
/* perform PWM output */
|
||||
|
||||
/* Open console directly to grab CTRL-C signal */
|
||||
struct pollfd fds;
|
||||
fds.fd = 0; /* stdin */
|
||||
fds.events = POLLIN;
|
||||
|
||||
warnx("Running 5 steps. WARNING! Motors will be live in 5 seconds\nPress any key to abort now.");
|
||||
sleep(5);
|
||||
|
||||
unsigned off = 900;
|
||||
unsigned idle = 1300;
|
||||
unsigned full = 2000;
|
||||
unsigned steps_timings_us[] = {2000, 5000, 20000, 50000};
|
||||
|
||||
unsigned phase = 0;
|
||||
unsigned phase_counter = 0;
|
||||
unsigned const phase_maxcount = 20;
|
||||
|
||||
for ( unsigned steps_timing_index = 0;
|
||||
steps_timing_index < sizeof(steps_timings_us) / sizeof(steps_timings_us[0]);
|
||||
steps_timing_index++ ) {
|
||||
|
||||
warnx("Sending step input with 0 to 100%% over a %u microseconds ramp", steps_timings_us[steps_timing_index]);
|
||||
|
||||
while (1) {
|
||||
for (unsigned i = 0; i < servo_count; i++) {
|
||||
if (set_mask & 1<<i) {
|
||||
|
||||
unsigned val;
|
||||
|
||||
if (phase == 0) {
|
||||
val = idle;
|
||||
} else if (phase == 1) {
|
||||
/* ramp - depending how steep it is this ramp will look instantaneous on the output */
|
||||
val = idle + (full - idle) * (phase_maxcount / (float)phase_counter);
|
||||
} else {
|
||||
val = off;
|
||||
}
|
||||
|
||||
ret = ioctl(fd, PWM_SERVO_SET(i), val);
|
||||
if (ret != OK)
|
||||
err(1, "PWM_SERVO_SET(%d)", i);
|
||||
}
|
||||
}
|
||||
|
||||
/* abort on user request */
|
||||
char c;
|
||||
ret = poll(&fds, 1, 0);
|
||||
if (ret > 0) {
|
||||
|
||||
int ret = read(0, &c, 1);
|
||||
if (ret > 0) {
|
||||
/* reset output to the last value */
|
||||
for (unsigned i = 0; i < servo_count; i++) {
|
||||
if (set_mask & 1<<i) {
|
||||
ret = ioctl(fd, PWM_SERVO_SET(i), last_spos.values[i]);
|
||||
if (ret != OK)
|
||||
err(1, "PWM_SERVO_SET(%d)", i);
|
||||
}
|
||||
}
|
||||
warnx("Key pressed, user abort\n");
|
||||
exit(0);
|
||||
}
|
||||
}
|
||||
if (phase == 1) {
|
||||
usleep(steps_timings_us[steps_timing_index] / phase_maxcount);
|
||||
|
||||
} else if (phase == 0) {
|
||||
usleep(50000);
|
||||
} else if (phase == 2) {
|
||||
usleep(50000);
|
||||
} else {
|
||||
break;
|
||||
}
|
||||
|
||||
phase_counter++;
|
||||
|
||||
if (phase_counter > phase_maxcount) {
|
||||
phase++;
|
||||
phase_counter = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
exit(0);
|
||||
|
||||
|
|
|
|||
|
|
@ -32,11 +32,12 @@
|
|||
############################################################################
|
||||
|
||||
#
|
||||
# Show and test hardware version
|
||||
# "version" nsh-command displays version infromation for hw,sw, gcc,build etc
|
||||
# can be also included and used in own code via "ver.h"
|
||||
#
|
||||
|
||||
MODULE_COMMAND = hw_ver
|
||||
SRCS = hw_ver.c
|
||||
MODULE_COMMAND = ver
|
||||
SRCS = ver.c
|
||||
|
||||
MODULE_STACKSIZE = 1024
|
||||
|
||||
|
|
@ -0,0 +1,123 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2014 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 ver.c
|
||||
*
|
||||
* Version command, unifies way of showing versions of HW, SW, Build, GCC
|
||||
* In case you want to add new version just extend version_main function
|
||||
*
|
||||
* @author Vladimir Kulla <ufon@kullaonline.net>
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <version/version.h>
|
||||
#include <systemlib/err.h>
|
||||
|
||||
// string constants for version commands
|
||||
static const char sz_ver_hw_str[] = "hw";
|
||||
static const char sz_ver_hwcmp_str[] = "hwcmp";
|
||||
static const char sz_ver_git_str[] = "git";
|
||||
static const char sz_ver_bdate_str[] = "bdate";
|
||||
static const char sz_ver_gcc_str[] = "gcc";
|
||||
static const char sz_ver_all_str[] = "all";
|
||||
|
||||
static void usage(const char *reason)
|
||||
{
|
||||
if (reason != NULL) {
|
||||
printf("%s\n", reason);
|
||||
}
|
||||
|
||||
printf("usage: ver {hw|hwcmp|git|bdate|gcc|all}\n\n");
|
||||
}
|
||||
|
||||
__EXPORT int ver_main(int argc, char *argv[]);
|
||||
|
||||
int ver_main(int argc, char *argv[])
|
||||
{
|
||||
int ret = 1; //defaults to an error
|
||||
|
||||
// first check if there are at least 2 params
|
||||
if (argc >= 2) {
|
||||
if (argv[1] != NULL) {
|
||||
if (!strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) {
|
||||
printf("%s\n", HW_ARCH);
|
||||
ret = 0;
|
||||
|
||||
} else if (!strncmp(argv[1], sz_ver_hwcmp_str, sizeof(sz_ver_hwcmp_str))) {
|
||||
if (argc >= 3 && argv[2] != NULL) {
|
||||
// compare 3rd parameter with HW_ARCH string, in case of match, return 0
|
||||
ret = strncmp(HW_ARCH, argv[2], strlen(HW_ARCH));
|
||||
|
||||
if (ret == 0) {
|
||||
printf("ver hwcmp match: %s\n", HW_ARCH);
|
||||
}
|
||||
|
||||
} else {
|
||||
errx(1, "Not enough arguments, try 'ver hwcmp PX4FMU_V2'");
|
||||
}
|
||||
|
||||
} else if (!strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) {
|
||||
printf("%s\n", FW_GIT);
|
||||
ret = 0;
|
||||
|
||||
} else if (!strncmp(argv[1], sz_ver_bdate_str, sizeof(sz_ver_bdate_str))) {
|
||||
printf("%s %s\n", __DATE__, __TIME__);
|
||||
ret = 0;
|
||||
|
||||
} else if (!strncmp(argv[1], sz_ver_gcc_str, sizeof(sz_ver_gcc_str))) {
|
||||
printf("%s\n", __VERSION__);
|
||||
ret = 0;
|
||||
|
||||
} else if (!strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str))) {
|
||||
printf("HW arch: %s\n", HW_ARCH);
|
||||
printf("Build datetime: %s %s\n", __DATE__, __TIME__);
|
||||
printf("FW git-hash: %s\n", FW_GIT);
|
||||
printf("GCC toolchain: %s\n", __VERSION__);
|
||||
ret = 0;
|
||||
|
||||
} else {
|
||||
errx(1, "unknown command.\n");
|
||||
}
|
||||
|
||||
} else {
|
||||
usage("Error, input parameter NULL.\n");
|
||||
}
|
||||
|
||||
} else {
|
||||
usage("Error, not enough parameters.");
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
Loading…
Reference in New Issue