fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-16 20:27:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'autodeclination' into ekf_params

Browse Source
This commit is contained in:
Lorenz Meier
2014-04-28 17:48:27 +02:00
parent df8fb7d2dc e134537ae8
commit 2c37ec16dc
13 changed files with 222 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Debug/NuttX
+5 -5
View File
@@ -34,10 +34,10 @@ define _showheap
else
set $MM_ALLOC_BIT = 0x80000000
end
printf "HEAP %d %p - %p\n", $index, g_heapstart[$index], g_heapend[$index]
printf "HEAP %d %p - %p\n", $index, g_mmheap.mm_heapstart[$index], g_mmheap.mm_heapend[$index]
printf "ptr size\n"
set $node = (char *)g_heapstart[$index] + sizeof(struct mm_allocnode_s)
while $node < g_heapend[$index]
set $node = (char *)g_mmheap.mm_heapstart[$index] + sizeof(struct mm_allocnode_s)
while $node < g_mmheap.mm_heapend[$index]
printf " %p", $node
set $nodestruct = (struct mm_allocnode_s *)$node
printf " %u", $nodestruct->size
@@ -47,7 +47,7 @@ define _showheap
else
set $used = $used + $nodestruct->size
end
if ($nodestruct->size > g_heapsize) || (($node + $nodestruct->size) > g_heapend[$index])
if ($nodestruct->size > g_mmheap.mm_heapsize) || (($node + $nodestruct->size) > g_mmheap.mm_heapend[$index])
printf " (BAD SIZE)"
end
printf "\n"
@@ -59,7 +59,7 @@ define _showheap
end
define showheap
set $nheaps = sizeof(g_heapstart) / sizeof(g_heapstart[0])
set $nheaps = sizeof(g_mmheap.mm_heapstart) / sizeof(g_mmheap.mm_heapstart[0])
printf "Printing %d heaps\n", $nheaps
set $heapindex = (int)0
while $heapindex < $nheaps
Debug/Nuttx.py
+10 -6
View File
@@ -306,12 +306,15 @@ class NX_show_heap (gdb.Command):
def __init__(self):
super(NX_show_heap, self).__init__('show heap', gdb.COMMAND_USER)
if gdb.lookup_type('struct mm_allocnode_s').sizeof == 8:
self._allocflag = 0x80000000
self._allocnodesize = 8
else:
struct_mm_allocnode_s = gdb.lookup_type('struct mm_allocnode_s')
preceding_size = struct_mm_allocnode_s['preceding'].type.sizeof
if preceding_size == 2:
self._allocflag = 0x8000
self._allocnodesize = 4
elif preceding_size == 4:
self._allocflag = 0x80000000
else:
raise gdb.GdbError('invalid mm_allocnode_s.preceding size %u' % preceding_size)
self._allocnodesize = struct_mm_allocnode_s.sizeof
def _node_allocated(self, allocnode):
if allocnode['preceding'] & self._allocflag:
@@ -333,7 +336,8 @@ class NX_show_heap (gdb.Command):
state = ''
else:
state = '(free)'
print ' {} {} {}'.format(allocnode.address + 8, self._node_size(allocnode), state)
print ' {} {} {}'.format(allocnode.address + self._allocnodesize,
self._node_size(allocnode), state)
cursor += self._node_size(allocnode) / self._allocnodesize
def invoke(self, args, from_tty):
src/drivers/drv_pwm_output.h
+3
View File
@@ -199,6 +199,9 @@ ORB_DECLARE(output_pwm);
/** get the lockdown override flag to enable outputs in HIL */
#define PWM_SERVO_GET_DISABLE_LOCKDOWN _IOC(_PWM_SERVO_BASE, 22)
/** force safety switch off (to disable use of safety switch) */
#define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23)
/*
*
*
src/drivers/drv_tone_alarm.h
+1
View File
@@ -148,6 +148,7 @@ enum {
TONE_BATTERY_WARNING_FAST_TUNE,
TONE_GPS_WARNING_TUNE,
TONE_ARMING_FAILURE_TUNE,
TONE_PARACHUTE_RELEASE_TUNE,
TONE_NUMBER_OF_TUNES
};
src/drivers/px4fmu/fmu.cpp
+1
View File
@@ -736,6 +736,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
case PWM_SERVO_SET_ARM_OK:
case PWM_SERVO_CLEAR_ARM_OK:
case PWM_SERVO_SET_FORCE_SAFETY_OFF:
// these are no-ops, as no safety switch
break;
src/drivers/px4io/px4io.cpp
+4
View File
@@ -2129,6 +2129,10 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
case PWM_SERVO_GET_DISABLE_LOCKDOWN:
*(unsigned *)arg = _lockdown_override;
case PWM_SERVO_SET_FORCE_SAFETY_OFF:
/* force safety swith off */
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC);
break;
case DSM_BIND_START:
src/drivers/stm32/tone_alarm/tone_alarm.cpp
+2
View File
@@ -335,6 +335,7 @@ ToneAlarm::ToneAlarm() :
_default_tunes[TONE_BATTERY_WARNING_FAST_TUNE] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"; //battery warning fast
_default_tunes[TONE_GPS_WARNING_TUNE] = "MFT255L4AAAL1F#"; //gps warning slow
_default_tunes[TONE_ARMING_FAILURE_TUNE] = "MFT255L4<<<BAP";
_default_tunes[TONE_PARACHUTE_RELEASE_TUNE] = "MFT255L16agagagag"; // parachute release
_tune_names[TONE_STARTUP_TUNE] = "startup"; // startup tune
_tune_names[TONE_ERROR_TUNE] = "error"; // ERROR tone
@@ -346,6 +347,7 @@ ToneAlarm::ToneAlarm() :
_tune_names[TONE_BATTERY_WARNING_FAST_TUNE] = "fast_bat"; // battery warning fast
_tune_names[TONE_GPS_WARNING_TUNE] = "gps_warning"; // gps warning
_tune_names[TONE_ARMING_FAILURE_TUNE] = "arming_failure"; //fail to arm
_tune_names[TONE_PARACHUTE_RELEASE_TUNE] = "parachute_release"; // parachute release
}
ToneAlarm::~ToneAlarm()
src/lib/geo/geo.h
+2
View File
@@ -48,6 +48,8 @@
#include "uORB/topics/fence.h"
#include "uORB/topics/vehicle_global_position.h"
#include "geo/geo_mag_declination.h"
__BEGIN_DECLS
#include <stdbool.h>
src/lib/geo/geo_mag_declination.c
+135
View File
@@ -0,0 +1,135 @@
/****************************************************************************
*
* 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.
*
*/
/** set this always to the sampling in degrees for the table below */
#define SAMPLING_RES 10
#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 char 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 = (90 + min_lat) / SAMPLING_RES;
unsigned min_lon_index = (180 + min_lat) / 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];
}
src/lib/geo/geo_mag_declination.h
+47
View File
@@ -0,0 +1,47 @@
/****************************************************************************
*
* 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.h
*
* Calculation / lookup table for earth magnetic field declination.
*
*/
#pragma once
__BEGIN_DECLS
__EXPORT float get_mag_declination(float lat, float lon);
__END_DECLS
src/lib/geo/module.mk
+2 -1
View File
@@ -35,4 +35,5 @@
# Geo library
#
SRCS = geo.c
SRCS = geo.c \
geo_mag_declination.c
src/modules/px4iofirmware/protocol.h
+4
View File
@@ -211,6 +211,10 @@ enum { /* DSM bind states */
/* 12 occupied by CRC */
#define PX4IO_P_SETUP_RC_THR_FAILSAFE_US 13 /**< the throttle failsafe pulse length in microseconds */
#define PX4IO_P_SETUP_FORCE_SAFETY_OFF 12 /* force safety switch into
'armed' (PWM enabled) state */
#define PX4IO_FORCE_SAFETY_MAGIC 22027 /* required argument for force safety (random) */
/* autopilot control values, -10000..10000 */
#define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */
#define PX4IO_P_CONTROLS_GROUP_0 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 0) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
src/modules/px4iofirmware/registers.c
+6
View File
@@ -570,6 +570,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
dsm_bind(value & 0x0f, (value >> 4) & 0xF);
break;
case PX4IO_P_SETUP_FORCE_SAFETY_OFF:
if (value == PX4IO_FORCE_SAFETY_MAGIC) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
}
break;
default:
return -1;
}