rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

Merge remote-tracking branch 'origin/rel-15.02'

Browse Source
This commit is contained in:
abeck70 2015-03-12 21:20:25 +11:00
commit 827075c31c
207 changed files with 45220 additions and 13583 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CREDITS.txt
View File

@ -111,6 +111,7 @@ Kevin Vertucio
Richard Von Lehe
Alex Vrubel
Mike Walters
Sam Wang
Brian Webb
Justin Welander
Mat Wellington

117
Makefile
View File

@ -135,6 +135,9 @@ all_clean:
@$(ECHO) " CLEAN $(call toprel, $(BUILD_DIR))"
$(V1) [ ! -d "$(BUILD_DIR)" ] || $(RM) -rf "$(BUILD_DIR)"
.PONY: clean
clean: all_clean
$(DL_DIR):
$(MKDIR) -p $@
@ -144,6 +147,12 @@ $(TOOLS_DIR):
$(BUILD_DIR):
$(MKDIR) -p $@
$(PACKAGE_DIR):
$(MKDIR) -p $@
$(DIST_DIR):
$(MKDIR) -p $@
##############################
#
# UAVObjects
@ -243,8 +252,8 @@ EF_TARGETS := $(addprefix ef_, $(EF_BOARDS))
# When building any of the "all_*" targets, tell all sub makefiles to display
# additional details on each line of output to describe which build and target
# that each line applies to. The same applies also to all, opfw_resource,
# package and clean_package targets
ifneq ($(strip $(filter all_% all opfw_resource package clean_package,$(MAKECMDGOALS))),)
# package targets
ifneq ($(strip $(filter all_% all opfw_resource package,$(MAKECMDGOALS))),)
export ENABLE_MSG_EXTRA := yes
endif
@ -766,51 +775,29 @@ $(OPFW_RESOURCE): $(FW_TARGETS)
$(V1) $(ECHO) $(QUOTE)$(OPFW_CONTENTS)$(QUOTE) > $@
# If opfw_resource or all firmware are requested, GCS should depend on the resource
ifneq ($(strip $(filter opfw_resource all all_fw all_flight,$(MAKECMDGOALS))),)
ifneq ($(strip $(filter opfw_resource all all_fw all_flight package,$(MAKECMDGOALS))),)
$(eval openpilotgcs_qmake: $(OPFW_RESOURCE))
endif
# Packaging targets: package, clean_package
# - removes build directory (clean_package only)
# Packaging targets: package
# - builds all firmware, opfw_resource, gcs
# - copies firmware into a package directory
# - calls paltform-specific packaging script
# Do some checks and define some values if package is requested
ifneq ($(strip $(filter package clean_package,$(MAKECMDGOALS))),)
# Define some variables
export PACKAGE_LBL := $(shell $(VERSION_INFO) --format=\$${LABEL})
export PACKAGE_NAME := OpenPilot
export PACKAGE_SEP := -
# We can only package release builds
ifneq ($(GCS_BUILD_CONF),release)
$(error Packaging is currently supported for release builds only)
endif
# Packaged GCS should depend on opfw_resource
ifneq ($(strip $(filter package clean_package,$(MAKECMDGOALS))),)
$(eval openpilotgcs_qmake: $(OPFW_RESOURCE))
endif
# Clean the build directory if clean_package is requested
ifneq ($(strip $(filter clean_package,$(MAKECMDGOALS))),)
$(info Cleaning build directory before packaging...)
ifneq ($(shell $(MAKE) all_clean >/dev/null 2>&1 && $(ECHO) "clean"), clean)
$(error Cannot clean build directory)
endif
.PHONY: clean_package
clean_package: package
endif
endif
# Define some variables
export PACKAGE_LBL := $(shell $(VERSION_INFO) --format=\$${LABEL})
export PACKAGE_NAME := OpenPilot
export PACKAGE_SEP := -
.PHONY: package
package: all_fw all_ground uavobjects_matlab
@$(ECHO) "Packaging for $(UNAME) $(ARCH) into $(call toprel, $(PACKAGE_DIR)) directory"
$(V1) [ ! -d "$(PACKAGE_DIR)" ] || $(RM) -rf "$(PACKAGE_DIR)"
$(V1) $(MKDIR) -p "$(PACKAGE_DIR)"
$(MAKE) --no-print-directory -C $(ROOT_DIR)/package --file=$(UNAME).mk $@
include $(ROOT_DIR)/package/$(UNAME).mk
package: all_fw all_ground uavobjects_matlab $(PACKAGE_DIR)
ifneq ($(GCS_BUILD_CONF),release)
# We can only package release builds
$(error Packaging is currently supported for release builds only)
endif
##############################
#
@ -893,60 +880,23 @@ build-info:
#
##############################
DIST_VER_INFO := $(DIST_DIR)/version-info.json
.PHONY: $(DIST_VER_INFO) # Because to many deps to list
$(DIST_VER_INFO): $(DIST_DIR)
$(V1) $(VERSION_INFO) --jsonpath="$(DIST_DIR)"
.PHONY: dist
dist:
dist: $(DIST_DIR) $(DIST_VER_INFO)
@$(ECHO) " SOURCE FOR DISTRIBUTION $(call toprel, $(DIST_DIR))"
$(V1) $(MKDIR) -p "$(DIST_DIR)"
$(V1) $(VERSION_INFO) \
--jsonpath="$(DIST_DIR)"
$(eval DIST_NAME := $(call toprel, "$(DIST_DIR)/OpenPilot-$(shell git describe).tar"))
$(V1) git archive --prefix="OpenPilot/" -o "$(DIST_NAME)" HEAD
$(V1) tar --append --file="$(DIST_NAME)" \
--transform='s,.*version-info.json,OpenPilot/version-info.json,' \
$(call toprel, "$(DIST_DIR)/version-info.json")
$(call toprel, "$(DIST_VER_INFO)")
$(V1) gzip -f "$(DIST_NAME)"
##############################
#
# Install OpenPilot
#
##############################
prefix := /usr/local
bindir := $(prefix)/bin
libdir := $(prefix)/lib
datadir := $(prefix)/share
INSTALL = cp -a --no-preserve=ownership
LN = ln
LN_S = ln -s
ifeq ($(MAKECMDGOALS), install)
ifneq ($(UNAME), Linux)
$(error install only supported for Linux)
endif
endif
.PHONY: install
install:
@$(ECHO) " INSTALLING GCS TO $(DESTDIR)/)"
$(V1) $(MKDIR) -p $(DESTDIR)$(bindir)
$(V1) $(MKDIR) -p $(DESTDIR)$(libdir)
$(V1) $(MKDIR) -p $(DESTDIR)$(datadir)
$(V1) $(MKDIR) -p $(DESTDIR)$(datadir)/applications
$(V1) $(MKDIR) -p $(DESTDIR)$(datadir)/pixmaps
$(V1) $(MKDIR) -p $(DESTDIR)$(udevdir)
$(V1) $(INSTALL) $(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF)/bin/openpilotgcs $(DESTDIR)$(bindir)
$(V1) $(INSTALL) $(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF)/bin/udp_test $(DESTDIR)$(bindir)
$(V1) $(INSTALL) $(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF)/lib/openpilotgcs $(DESTDIR)$(libdir)
$(V1) $(INSTALL) $(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF)/share/openpilotgcs $(DESTDIR)$(datadir)
$(V1) $(INSTALL) $(ROOT_DIR)/package/linux/openpilot.desktop $(DESTDIR)$(datadir)/applications
$(V1) $(INSTALL) $(ROOT_DIR)/package/linux/openpilot.png $(DESTDIR)$(datadir)/pixmaps
$(V1) rm $(DESTDIR)/$(datadir)/openpilotgcs/translations/Makefile
##############################
#
# Help message, the default Makefile goal
@ -1079,7 +1029,6 @@ help:
@$(ECHO) " Supported groups are ($(UAVOBJ_TARGETS))"
@$(ECHO)
@$(ECHO) " [Packaging]"
@$(ECHO) " clean_package - Clean, build and package the OpenPilot platform-dependent package"
@$(ECHO) " package - Build and package the OpenPilot platform-dependent package (no clean)"
@$(ECHO) " opfw_resource - Generate resources to embed firmware binaries into the GCS"
@$(ECHO) " dist - Generate source archive for distribution"

71
WHATSNEW.txt
View File

@ -1,3 +1,74 @@
--- RELEASE-15.02 RC7
This release introduces major flight performance improvements, enhancements as well as bug fixes. Many enhancements have been made to reducing dead-time of the communication between the flight controller and ESCs. In our testing, we have found this to be not only the best flight performance so far in the OpenPilot project but the best flight performance of any project we have tested against. This is a recommended upgrade for everyone and the more skilled of a pilot you are, the more you will love this release.
A key improvement that helped achieve this was the addition of the PWMSync code path, this is now enabled by default. Some restrictions applies to CC3D/CC as it needs a compatible input method to enable PWMSync. Compatible input methods are PPM, S.Bus, DSM and OPLink. This release also introduces support for OneShot125 capable ESCs, such as the KISS ESCs and all ESCs supported in BLHeli V13 and above. Note that OneShot125 support has the same restrictions as PWMSync for CC and CC3D.
Note for CC3D/CC: To support PWMSync/OneShot125 PPM input pin has changed from earlier releases to the last pin on the input IO port, please refer to the connection diagram in the setup wizard to see the required new pin layout for PPM.
Other enhancements include key parts of the GCS translated to Chinese and further OPLink reliability additions allowing us to confidently say it can be used as a primary control link as well as for telemetry.
The full list of features, improvements and bugfixes in this release is accessible here:
https://progress.openpilot.org/issues/?filter=12161
Release Notes - OpenPilot - Version RELEASE-15.02
** Bug
* [OP-969] - Input Configuration Wizard has scrollbars showing up and next/previous buttons are pushed down out of sight
* [OP-1034] - CCPM Config Widget crashes GCS if required boxes aren't set i.e. Channel set to None
* [OP-1236] - Icons on Welcome tab - moves to the left
* [OP-1466] - Gcs crashes on Helicopter config tab
* [OP-1522] - Improve Robustness of OPLink radio
* [OP-1601] - Still not enough ram on CC for gps to be usable
* [OP-1644] - Radio Setup Wiz problem with 6 flight modes
* [OP-1670] - cruise control conflict with flight modes (rate & acro +)
* [OP-1680] - OPLink control limited to 1000-1896
* [OP-1682] - Overflow issue with pwm rx and CC3D
* [OP-1686] - Slave OPLM should receive PPM in PPM_only mode
* [OP-1706] - Output Reverse checkboxes tick them selves when min equals max
* [OP-1718] - FW vehicle setup wizard may strip servo gears
* [OP-1722] - libusb include is incorrect.
* [OP-1728] - FW servos ignore set neutral
* [OP-1733] - version-info.py should check for version-info.json not git repo.
* [OP-1735] - Build fails with a tilde (~) in path.
* [OP-1737] - Min/max will be greyed even if motor output test is canceled
* [OP-1741] - Repo path is incorrect in version_info.pro
* [OP-1743] - cc3d fails to connect once "next " is loaded 2-23-15
* [OP-1744] - Vehicle config wizard produces a bad configuration
* [OP-1754] - Vehicle Wizard bad config when setting PPM in and RapidESC out
* [OP-1755] - Add additional path for cloudconfigs
* [OP-1758] - Upgrade hidapi for all OSs (except windows) to solve mac issue:Fix incorrect device list after device removal
* [OP-1761] - Wizard bad config when PPM in, RapidESC out and hexa frame with CC/CC3D/Atom
* [OP-1764] - System should sanity check RC Input Channel value ranges and raise alarm accordingly
* [OP-1768] - PWM Sync and OneShot125 wizard output level and warning are incorrect
** Improvement
* [OP-1519] - Auto Reboot of board when required by Wizard
* [OP-1576] - Remove tx resent accounting from OPlink
* [OP-1635] - Remove Quad H from wizard to avoid confusion with Quad X
* [OP-1650] - Reduce telemetry to improve OPLink
* [OP-1658] - Sensor driver API and overhaul of sensor module
* [OP-1683] - Support synchronous (OneShot) and OneShot125 output mode
* [OP-1685] - Support OneShot/OneShot125 for CC* targets
* [OP-1694] - Make package make rule non-nested
* [OP-1698] - Add easy to set channel passtrough GCS functionality
* [OP-1704] - Add support for sanity check custom hooks
* [OP-1759] - Hide CC3D non-supported options (GPSAssist)
** New Feature
* [OP-1723] - RCCar Forward/reverse support
** Task
* [OP-1721] - C++ enable flight controller and upgrade ARM tools
* [OP-1738] - change default flight modes and thrust settings
* [OP-1747] - 15.02 rc1 motor end points do not reflect oneshot125 / pwmsync
** Sub task
* [OP-1748] - Chinese translation for 15.02
* [OP-1752] - Add Alarm sub status to SystemHealth
--- RELEASE-15.01 --- Look Ma, No hands ---
This release mainly focuses on a new feature, GPSAssist which is a new form of assisted control for multirotors.
Assisted Control provides assistance functions on top of existing flight modes. GPSAssist is the

0
artwork/3D Model/boards/OPGPS/OPGPS.3DS → artwork/3D Model/boards/OPGPSv8/OPGPS.3DS
View File

0
artwork/3D Model/boards/OPGPS/OPGPS.jpg → artwork/3D Model/boards/OPGPSv8/OPGPS.jpg
View File

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 129 KiB

After

Width:  |  Height:  |  Size: 129 KiB

0
artwork/3D Model/boards/OPGPS/TEXTURE.PNG → artwork/3D Model/boards/OPGPSv8/TEXTURE.PNG
View File

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 188 KiB

After

Width:  |  Height:  |  Size: 188 KiB

BIN
artwork/3D Model/boards/OPGPSv9/OPGPSv9.3DS Normal file
View File

Binary file not shown.

BIN
artwork/3D Model/boards/OPGPSv9/OPGPSv9.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 136 KiB

BIN
artwork/3D Model/boards/OPGPSv9/TEXTURE.JPG Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 309 KiB

BIN
artwork/3D Model/multi/blackout/BlackoutMiniHQuad.3DS Normal file
View File

Binary file not shown.

BIN
artwork/3D Model/multi/blackout/BlackoutMiniHQuad.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 114 KiB

BIN
artwork/3D Model/multi/blackout/TEXTURE.PNG Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 1.0 MiB

17
flight/libraries/inc/sanitycheck.h
View File

@ -32,6 +32,9 @@
#include <systemalarms.h>
#include <stdint.h>
#include <utlist.h>
typedef enum {
FRAME_TYPE_MULTIROTOR,
FRAME_TYPE_HELI,
@ -40,6 +43,8 @@ typedef enum {
FRAME_TYPE_CUSTOM,
} FrameType_t;
typedef SystemAlarmsExtendedAlarmStatusOptions (SANITYCHECK_CustomHook_function)();
#define SANITYCHECK_STATUS_ERROR_NONE SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE
#define SANITYCHECK_STATUS_ERROR_FLIGHTMODE SYSTEMALARMS_EXTENDEDALARMSTATUS_FLIGHTMODE
@ -55,4 +60,16 @@ extern int32_t configuration_check();
extern FrameType_t GetCurrentFrameType();
/**
* Attach a custom hook to the sanity check process
* @param hook a custom hook function
*/
extern void SANITYCHECK_AttachHook(SANITYCHECK_CustomHook_function *hook);
/**
* Detach a custom hook to the sanity check process
* @param hook a custom hook function
*/
extern void SANITYCHECK_DetachHook(SANITYCHECK_CustomHook_function *hook);
#endif /* SANITYCHECK_H */

37
flight/libraries/inc/sha1.h Normal file
View File

@ -0,0 +1,37 @@
/* $NetBSD: sha1.h,v 1.14 2009/11/06 20:31:19 joerg Exp $ */
/*
* SHA-1 in C
* By Steve Reid <steve@edmweb.com>
* 100% Public Domain
*/
#ifndef _SYS_SHA1_H_
#define _SYS_SHA1_H_
#include <sys/cdefs.h>
#include <sys/types.h>
#define SHA1_DIGEST_LENGTH 20
#define SHA1_DIGEST_STRING_LENGTH 41
typedef struct {
uint32_t state[5];
uint32_t count[2];
uint8_t buffer[64];
} SHA1_CTX;
__BEGIN_DECLS
void SHA1Transform(uint32_t[5], const uint8_t[64]);
void SHA1Init(SHA1_CTX *);
void SHA1Update(SHA1_CTX *, const uint8_t *, unsigned int);
void SHA1Final(uint8_t[SHA1_DIGEST_LENGTH], SHA1_CTX *);
#ifndef _KERNEL
char *SHA1End(SHA1_CTX *, char *);
char *SHA1FileChunk(const char *, char *, off_t, off_t);
char *SHA1File(const char *, char *);
char *SHA1Data(const uint8_t *, size_t, char *);
#endif /* _KERNEL */
__END_DECLS
#endif /* _SYS_SHA1_H_ */

0
flight/uavobjects/inc/utlist.h → flight/libraries/inc/utlist.h
View File

50
flight/modules/Altitude/revolution/inc/altitude.h → flight/libraries/math/vectors.h
View File

@ -1,14 +1,13 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @addtogroup OpenPilot Math Utilities
* @{
* @addtogroup AltitudeModule Altitude Module
* @brief Communicate with BMP085 and update @ref AltitudeActual "AltitudeActual UAV Object"
* @addtogroup Reuseable vector data type and functions
* @{
*
* @file altitude.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Altitude module, reads temperature and pressure from BMP085
* @file vectors.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Reuseable vector data type and functions
*
* @see The GNU Public License (GPL) Version 3
*
@ -28,14 +27,37 @@
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef ALTITUDE_H
#define ALTITUDE_H
int32_t AltitudeInitialize();
#ifndef VECTORS_H_
#define VECTORS_H_
#endif // ALTITUDE_H
#include <math.h>
#include <stdint.h>
/**
* @}
* @}
*/
#define DECLAREVECTOR3(suffix, datatype) \
typedef struct Vector3##suffix##_t { \
datatype x; \
datatype y; \
datatype z; \
} Vector3##suffix
#define DECLAREVECTOR2(suffix, datatype) \
typedef struct Vector2##suffix##_t { \
datatype x; \
datatype y; \
} Vector2##suffix
DECLAREVECTOR3(i16, int16_t);
DECLAREVECTOR3(i32, int32_t);
DECLAREVECTOR3(u16, uint16_t);
DECLAREVECTOR3(u32, uint32_t);
DECLAREVECTOR3(f, float);
DECLAREVECTOR2(i16, int16_t);
DECLAREVECTOR2(i32, int32_t);
DECLAREVECTOR2(u16, uint16_t);
DECLAREVECTOR2(u32, uint32_t);
DECLAREVECTOR2(f, float);
#endif /* VECTORS_H_ */

89
flight/libraries/mini_cpp.cpp Normal file
View File

@ -0,0 +1,89 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
*
* @file mini_cpp.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief CPP support methods
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include <pios.h>
// _init is called by __libc_init_array during invocation of static constructors
// __libc_init_array calls _init, which is defined in crti.o. _init calls functions that are in .init section.
// If you don't have meaningful stuff in .init section, just define an empty _init function.
extern "C" int _init(void)
{
return 0;
}
// operator new
void *operator new(size_t size) throw()
{
return pios_malloc(size);
}
// operator delete
void operator delete(void *p) throw()
{
pios_free(p);
}
// The function __aeabi_atexit() handles the static destructors. This can be empty
// because we have no operating system to return to, hence the static destructors
// will never be called.
extern "C" int __aeabi_atexit(__attribute__((unused)) void *object, __attribute__((unused)) void (*destructor)(void *), __attribute__((unused)) void *dso_handle)
{
return 0;
}
// see https://answers.launchpad.net/gcc-arm-embedded/+question/221105
// and https://answers.launchpad.net/gcc-arm-embedded/+question/224709
__extension__ typedef int __guard __attribute__((mode(__DI__)));
extern "C" int __cxa_atexit(void (*f)(void *), void *p, void *d);
extern "C" int __cxa_guard_acquire(__guard *);
extern "C" void __cxa_guard_release(__guard *);
extern "C" void __cxa_guard_abort(__guard *);
extern "C" void __cxa_pure_virtual(void);
int __cxa_guard_acquire(__attribute__((unused)) __guard *g)
{
return !*(char *)(g);
};
void __cxa_guard_release(__attribute__((unused)) __guard *g)
{
*(char *)g = 1;
};
void __cxa_guard_abort(__attribute__((unused)) __guard *) {};
void __cxa_pure_virtual(void)
{
while (1) {
;
}
}
int __cxa_atexit(__attribute__((unused)) void (*f)(void *), __attribute__((unused)) void *p, __attribute__((unused)) void *d)
{
return 0;
};
/**
* @}
*/

95
flight/libraries/sanitycheck.c
View File

@ -43,12 +43,21 @@
#include <taskinfo.h>
// a number of useful macros
#define ADDSEVERITY(check) severity = (severity != SYSTEMALARMS_ALARM_OK ? severity : ((check) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_CRITICAL))
#define ADDSEVERITY(check) severity = (severity != SYSTEMALARMS_ALARM_OK ? severity : ((check) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_CRITICAL))
#define ADDEXTENDEDALARMSTATUS(error_code, error_substatus) if ((severity != SYSTEMALARMS_ALARM_OK) && (alarmstatus == SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE)) { alarmstatus = (error_code); alarmsubstatus = (error_substatus); }
// private types
typedef struct SANITYCHECK_CustomHookInstance {
SANITYCHECK_CustomHook_function *hook;
struct SANITYCHECK_CustomHookInstance *next;
bool enabled;
} SANITYCHECK_CustomHookInstance;
// ! Check a stabilization mode switch position for safety
static bool check_stabilization_settings(int index, bool multirotor, bool coptercontrol, bool gpsassisted);
SANITYCHECK_CustomHookInstance *hooks = 0;
/**
* Run a preflight check over the hardware configuration
* and currently active modules
@ -170,6 +179,41 @@ int32_t configuration_check()
}
}
// Check throttle/collective channel range for valid configuration of input for critical control
SystemSettingsThrustControlOptions thrustType;
SystemSettingsThrustControlGet(&thrustType);
ManualControlSettingsChannelMinData channelMin;
ManualControlSettingsChannelMaxData channelMax;
ManualControlSettingsChannelMinGet(&channelMin);
ManualControlSettingsChannelMaxGet(&channelMax);
switch (thrustType) {
case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE:
ADDSEVERITY(fabsf(channelMax.Throttle - channelMin.Throttle) > 300.0f);
ADDEXTENDEDALARMSTATUS(SYSTEMALARMS_EXTENDEDALARMSTATUS_BADTHROTTLEORCOLLECTIVEINPUTRANGE, 0);
break;
case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE:
ADDSEVERITY(fabsf(channelMax.Collective - channelMin.Collective) > 300.0f);
ADDEXTENDEDALARMSTATUS(SYSTEMALARMS_EXTENDEDALARMSTATUS_BADTHROTTLEORCOLLECTIVEINPUTRANGE, 0);
break;
default:
break;
}
// query sanity check hooks
if (severity < SYSTEMALARMS_ALARM_CRITICAL) {
SANITYCHECK_CustomHookInstance *instance = NULL;
LL_FOREACH(hooks, instance) {
if (instance->enabled) {
alarmstatus = instance->hook();
if (alarmstatus != SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE) {
severity = SYSTEMALARMS_ALARM_CRITICAL;
break;
}
}
}
}
uint8_t checks_disabled;
FlightModeSettingsDisableSanityChecksGet(&checks_disabled);
if (checks_disabled == FLIGHTMODESETTINGS_DISABLESANITYCHECKS_TRUE) {
@ -265,11 +309,23 @@ static bool check_stabilization_settings(int index, bool multirotor, bool copter
return false;
}
// if cruise control, ensure rate or acro are not set
if (modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL) {
for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_YAW; i++) {
if ((modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RATE ||
modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ACRO)) {
return false;
}
}
}
// Warning: This assumes that certain conditions in the XML file are met. That
// FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL has the same numeric value for each channel
// and is the same for STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL
// (this is checked at compile time by static constraint manualcontrol.h)
return true;
}
@ -281,7 +337,6 @@ FrameType_t GetCurrentFrameType()
switch ((SystemSettingsAirframeTypeOptions)airframe_type) {
case SYSTEMSETTINGS_AIRFRAMETYPE_QUADX:
case SYSTEMSETTINGS_AIRFRAMETYPE_QUADP:
case SYSTEMSETTINGS_AIRFRAMETYPE_QUADH:
case SYSTEMSETTINGS_AIRFRAMETYPE_HEXA:
case SYSTEMSETTINGS_AIRFRAMETYPE_OCTO:
case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOX:
@ -314,3 +369,39 @@ FrameType_t GetCurrentFrameType()
// anyway it should not reach here
return FRAME_TYPE_CUSTOM;
}
void SANITYCHECK_AttachHook(SANITYCHECK_CustomHook_function *hook)
{
PIOS_Assert(hook);
SANITYCHECK_CustomHookInstance *instance = NULL;
// Check whether there is an existing instance and enable it
LL_FOREACH(hooks, instance) {
if (instance->hook == hook) {
instance->enabled = true;
return;
}
}
// No existing instance found, attach this new one
instance = (SANITYCHECK_CustomHookInstance *)pios_malloc(sizeof(SANITYCHECK_CustomHookInstance));
PIOS_Assert(instance);
instance->hook = hook;
instance->next = NULL;
instance->enabled = true;
LL_APPEND(hooks, instance);
}
void SANITYCHECK_DetachHook(SANITYCHECK_CustomHook_function *hook)
{
if (!hooks) {
return;
}
SANITYCHECK_CustomHookInstance *instance = NULL;
LL_FOREACH(hooks, instance) {
if (instance->hook == hook) {
instance->enabled = false;
return;
}
}
}

279
flight/libraries/sha1.c Normal file
View File

@ -0,0 +1,279 @@
/* $NetBSD: sha1.c,v 1.6 2009/11/06 20:31:18 joerg Exp $ */
/* $OpenBSD: sha1.c,v 1.9 1997/07/23 21:12:32 kstailey Exp $ */
/*
* SHA-1 in C
* By Steve Reid <steve@edmweb.com>
* 100% Public Domain
*
* Test Vectors (from FIPS PUB 180-1)
* "abc"
* A9993E36 4706816A BA3E2571 7850C26C 9CD0D89D
* "abcdbcdecdefdefgefghfghighijhijkijkljklmklmnlmnomnopnopq"
* 84983E44 1C3BD26E BAAE4AA1 F95129E5 E54670F1
* A million repetitions of "a"
* 34AA973C D4C4DAA4 F61EEB2B DBAD2731 6534016F
*/
#define SHA1HANDSOFF /* Copies data before messing with it. */
#include <sys/cdefs.h>
#if defined(_KERNEL) || defined(_STANDALONE)
__KERNEL_RCSID(0, "$NetBSD: sha1.c,v 1.6 2009/11/06 20:31:18 joerg Exp $");
#include <lib/libkern/libkern.h>
#else
#if defined(LIBC_SCCS) && !defined(lint)
__RCSID("$NetBSD: sha1.c,v 1.6 2009/11/06 20:31:18 joerg Exp $");
#endif /* LIBC_SCCS and not lint */
// #include "namespace.h"
#include <assert.h>
#include <string.h>
#endif
#include <stdint.h>
#include <sha1.h>
#if HAVE_NBTOOL_CONFIG_H
#include "nbtool_config.h"
#endif
#if !HAVE_SHA1_H
#define rol(value, bits) (((value) << (bits)) | ((value) >> (32 - (bits))))
/*
* blk0() and blk() perform the initial expand.
* I got the idea of expanding during the round function from SSLeay
*/
#if BYTE_ORDER == LITTLE_ENDIAN
# define blk0(i) \
(block->l[i] = (rol(block->l[i], 24) & 0xFF00FF00) \
| (rol(block->l[i], 8) & 0x00FF00FF))
#else
# define blk0(i) block->l[i]
#endif
#define blk(i) \
(block->l[i & 15] = rol(block->l[(i + 13) & 15] ^ block->l[(i + 8) & 15] \
^ block->l[(i + 2) & 15] ^ block->l[i & 15], 1))
/*
* (R0+R1), R2, R3, R4 are the different operations (rounds) used in SHA1
*/
#define R0(v, w, x, y, z, i) z += ((w & (x ^ y)) ^ y) + blk0(i) + 0x5A827999 + rol(v, 5); w = rol(w, 30);
#define R1(v, w, x, y, z, i) z += ((w & (x ^ y)) ^ y) + blk(i) + 0x5A827999 + rol(v, 5); w = rol(w, 30);
#define R2(v, w, x, y, z, i) z += (w ^ x ^ y) + blk(i) + 0x6ED9EBA1 + rol(v, 5); w = rol(w, 30);
#define R3(v, w, x, y, z, i) z += (((w | x) & y) | (w & x)) + blk(i) + 0x8F1BBCDC + rol(v, 5); w = rol(w, 30);
#define R4(v, w, x, y, z, i) z += (w ^ x ^ y) + blk(i) + 0xCA62C1D6 + rol(v, 5); w = rol(w, 30);
#if !defined(_KERNEL) && !defined(_STANDALONE)
#if defined(__weak_alias)
__weak_alias(SHA1Transform, _SHA1Transform)
__weak_alias(SHA1Init, _SHA1Init)
__weak_alias(SHA1Update, _SHA1Update)
__weak_alias(SHA1Final, _SHA1Final)
#endif
#endif
typedef union {
uint8_t c[64];
uint32_t l[16];
} CHAR64LONG16;
/* old sparc64 gcc could not compile this */
#undef SPARC64_GCC_WORKAROUND
#if defined(__sparc64__) && defined(__GNUC__) && __GNUC__ < 3
#define SPARC64_GCC_WORKAROUND
#endif
#ifdef SPARC64_GCC_WORKAROUND
void do_R01(uint32_t *a, uint32_t *b, uint32_t *c, uint32_t *d, uint32_t *e, CHAR64LONG16 *);
void do_R2(uint32_t *a, uint32_t *b, uint32_t *c, uint32_t *d, uint32_t *e, CHAR64LONG16 *);
void do_R3(uint32_t *a, uint32_t *b, uint32_t *c, uint32_t *d, uint32_t *e, CHAR64LONG16 *);
void do_R4(uint32_t *a, uint32_t *b, uint32_t *c, uint32_t *d, uint32_t *e, CHAR64LONG16 *);
#define nR0(v, w, x, y, z, i) R0(*v, *w, *x, *y, *z, i)
#define nR1(v, w, x, y, z, i) R1(*v, *w, *x, *y, *z, i)
#define nR2(v, w, x, y, z, i) R2(*v, *w, *x, *y, *z, i)
#define nR3(v, w, x, y, z, i) R3(*v, *w, *x, *y, *z, i)
#define nR4(v, w, x, y, z, i) R4(*v, *w, *x, *y, *z, i)
void do_R01(uint32_t *a, uint32_t *b, uint32_t *c, uint32_t *d, uint32_t *e, CHAR64LONG16 *block)
{
nR0(a, b, c, d, e, 0); nR0(e, a, b, c, d, 1); nR0(d, e, a, b, c, 2); nR0(c, d, e, a, b, 3);
nR0(b, c, d, e, a, 4); nR0(a, b, c, d, e, 5); nR0(e, a, b, c, d, 6); nR0(d, e, a, b, c, 7);
nR0(c, d, e, a, b, 8); nR0(b, c, d, e, a, 9); nR0(a, b, c, d, e, 10); nR0(e, a, b, c, d, 11);
nR0(d, e, a, b, c, 12); nR0(c, d, e, a, b, 13); nR0(b, c, d, e, a, 14); nR0(a, b, c, d, e, 15);
nR1(e, a, b, c, d, 16); nR1(d, e, a, b, c, 17); nR1(c, d, e, a, b, 18); nR1(b, c, d, e, a, 19);
}
void do_R2(uint32_t *a, uint32_t *b, uint32_t *c, uint32_t *d, uint32_t *e, CHAR64LONG16 *block)
{
nR2(a, b, c, d, e, 20); nR2(e, a, b, c, d, 21); nR2(d, e, a, b, c, 22); nR2(c, d, e, a, b, 23);
nR2(b, c, d, e, a, 24); nR2(a, b, c, d, e, 25); nR2(e, a, b, c, d, 26); nR2(d, e, a, b, c, 27);
nR2(c, d, e, a, b, 28); nR2(b, c, d, e, a, 29); nR2(a, b, c, d, e, 30); nR2(e, a, b, c, d, 31);
nR2(d, e, a, b, c, 32); nR2(c, d, e, a, b, 33); nR2(b, c, d, e, a, 34); nR2(a, b, c, d, e, 35);
nR2(e, a, b, c, d, 36); nR2(d, e, a, b, c, 37); nR2(c, d, e, a, b, 38); nR2(b, c, d, e, a, 39);
}
void do_R3(uint32_t *a, uint32_t *b, uint32_t *c, uint32_t *d, uint32_t *e, CHAR64LONG16 *block)
{
nR3(a, b, c, d, e, 40); nR3(e, a, b, c, d, 41); nR3(d, e, a, b, c, 42); nR3(c, d, e, a, b, 43);
nR3(b, c, d, e, a, 44); nR3(a, b, c, d, e, 45); nR3(e, a, b, c, d, 46); nR3(d, e, a, b, c, 47);
nR3(c, d, e, a, b, 48); nR3(b, c, d, e, a, 49); nR3(a, b, c, d, e, 50); nR3(e, a, b, c, d, 51);
nR3(d, e, a, b, c, 52); nR3(c, d, e, a, b, 53); nR3(b, c, d, e, a, 54); nR3(a, b, c, d, e, 55);
nR3(e, a, b, c, d, 56); nR3(d, e, a, b, c, 57); nR3(c, d, e, a, b, 58); nR3(b, c, d, e, a, 59);
}
void do_R4(uint32_t *a, uint32_t *b, uint32_t *c, uint32_t *d, uint32_t *e, CHAR64LONG16 *block)
{
nR4(a, b, c, d, e, 60); nR4(e, a, b, c, d, 61); nR4(d, e, a, b, c, 62); nR4(c, d, e, a, b, 63);
nR4(b, c, d, e, a, 64); nR4(a, b, c, d, e, 65); nR4(e, a, b, c, d, 66); nR4(d, e, a, b, c, 67);
nR4(c, d, e, a, b, 68); nR4(b, c, d, e, a, 69); nR4(a, b, c, d, e, 70); nR4(e, a, b, c, d, 71);
nR4(d, e, a, b, c, 72); nR4(c, d, e, a, b, 73); nR4(b, c, d, e, a, 74); nR4(a, b, c, d, e, 75);
nR4(e, a, b, c, d, 76); nR4(d, e, a, b, c, 77); nR4(c, d, e, a, b, 78); nR4(b, c, d, e, a, 79);
}
#endif /* ifdef SPARC64_GCC_WORKAROUND */
/*
* Hash a single 512-bit block. This is the core of the algorithm.
*/
void SHA1Transform(uint32_t state[5], const uint8_t buffer[64])
{
uint32_t a, b, c, d, e;
CHAR64LONG16 *block;
#ifdef SHA1HANDSOFF
CHAR64LONG16 workspace;
#endif
#ifdef SHA1HANDSOFF
block = &workspace;
(void)memcpy(block, buffer, 64);
#else
block = (CHAR64LONG16 *)(void *)buffer;
#endif
/* Copy context->state[] to working vars */
a = state[0];
b = state[1];
c = state[2];
d = state[3];
e = state[4];
#ifdef SPARC64_GCC_WORKAROUND
do_R01(&a, &b, &c, &d, &e, block);
do_R2(&a, &b, &c, &d, &e, block);
do_R3(&a, &b, &c, &d, &e, block);
do_R4(&a, &b, &c, &d, &e, block);
#else
/* 4 rounds of 20 operations each. Loop unrolled. */
R0(a, b, c, d, e, 0); R0(e, a, b, c, d, 1); R0(d, e, a, b, c, 2); R0(c, d, e, a, b, 3);
R0(b, c, d, e, a, 4); R0(a, b, c, d, e, 5); R0(e, a, b, c, d, 6); R0(d, e, a, b, c, 7);
R0(c, d, e, a, b, 8); R0(b, c, d, e, a, 9); R0(a, b, c, d, e, 10); R0(e, a, b, c, d, 11);
R0(d, e, a, b, c, 12); R0(c, d, e, a, b, 13); R0(b, c, d, e, a, 14); R0(a, b, c, d, e, 15);
R1(e, a, b, c, d, 16); R1(d, e, a, b, c, 17); R1(c, d, e, a, b, 18); R1(b, c, d, e, a, 19);
R2(a, b, c, d, e, 20); R2(e, a, b, c, d, 21); R2(d, e, a, b, c, 22); R2(c, d, e, a, b, 23);
R2(b, c, d, e, a, 24); R2(a, b, c, d, e, 25); R2(e, a, b, c, d, 26); R2(d, e, a, b, c, 27);
R2(c, d, e, a, b, 28); R2(b, c, d, e, a, 29); R2(a, b, c, d, e, 30); R2(e, a, b, c, d, 31);
R2(d, e, a, b, c, 32); R2(c, d, e, a, b, 33); R2(b, c, d, e, a, 34); R2(a, b, c, d, e, 35);
R2(e, a, b, c, d, 36); R2(d, e, a, b, c, 37); R2(c, d, e, a, b, 38); R2(b, c, d, e, a, 39);
R3(a, b, c, d, e, 40); R3(e, a, b, c, d, 41); R3(d, e, a, b, c, 42); R3(c, d, e, a, b, 43);
R3(b, c, d, e, a, 44); R3(a, b, c, d, e, 45); R3(e, a, b, c, d, 46); R3(d, e, a, b, c, 47);
R3(c, d, e, a, b, 48); R3(b, c, d, e, a, 49); R3(a, b, c, d, e, 50); R3(e, a, b, c, d, 51);
R3(d, e, a, b, c, 52); R3(c, d, e, a, b, 53); R3(b, c, d, e, a, 54); R3(a, b, c, d, e, 55);
R3(e, a, b, c, d, 56); R3(d, e, a, b, c, 57); R3(c, d, e, a, b, 58); R3(b, c, d, e, a, 59);
R4(a, b, c, d, e, 60); R4(e, a, b, c, d, 61); R4(d, e, a, b, c, 62); R4(c, d, e, a, b, 63);
R4(b, c, d, e, a, 64); R4(a, b, c, d, e, 65); R4(e, a, b, c, d, 66); R4(d, e, a, b, c, 67);
R4(c, d, e, a, b, 68); R4(b, c, d, e, a, 69); R4(a, b, c, d, e, 70); R4(e, a, b, c, d, 71);
R4(d, e, a, b, c, 72); R4(c, d, e, a, b, 73); R4(b, c, d, e, a, 74); R4(a, b, c, d, e, 75);
R4(e, a, b, c, d, 76); R4(d, e, a, b, c, 77); R4(c, d, e, a, b, 78); R4(b, c, d, e, a, 79);
#endif /* ifdef SPARC64_GCC_WORKAROUND */
/* Add the working vars back into context.state[] */
state[0] += a;
state[1] += b;
state[2] += c;
state[3] += d;
state[4] += e;
/* Wipe variables */
a = b = c = d = e = 0;
}
/*
* SHA1Init - Initialize new context
*/
void SHA1Init(SHA1_CTX *context)
{
/* SHA1 initialization constants */
context->state[0] = 0x67452301;
context->state[1] = 0xEFCDAB89;
context->state[2] = 0x98BADCFE;
context->state[3] = 0x10325476;
context->state[4] = 0xC3D2E1F0;
context->count[0] = context->count[1] = 0;
}
/*
* Run your data through this.
*/
void SHA1Update(SHA1_CTX *context, const uint8_t *data, unsigned int len)
{
unsigned int i, j;
j = context->count[0];
if ((context->count[0] += len << 3) < j) {
context->count[1] += (len >> 29) + 1;
}
j = (j >> 3) & 63;
if ((j + len) > 63) {
(void)memcpy(&context->buffer[j], data, (i = 64 - j));
SHA1Transform(context->state, context->buffer);
for (; i + 63 < len; i += 64) {
SHA1Transform(context->state, &data[i]);
}
j = 0;
} else {
i = 0;
}
(void)memcpy(&context->buffer[j], &data[i], len - i);
}
/*
* Add padding and return the message digest.
*/
void SHA1Final(uint8_t digest[20], SHA1_CTX *context)
{
unsigned int i;
uint8_t finalcount[8];
for (i = 0; i < 8; i++) {
finalcount[i] = (uint8_t)((context->count[(i >= 4 ? 0 : 1)]
>> ((3 - (i & 3)) * 8)) & 255); /* Endian independent */
}
SHA1Update(context, (const uint8_t *)"\200", 1);
while ((context->count[0] & 504) != 448) {
SHA1Update(context, (const uint8_t *)"\0", 1);
}
SHA1Update(context, finalcount, 8); /* Should cause a SHA1Transform() */
if (digest) {
for (i = 0; i < 20; i++) {
digest[i] = (uint8_t)
((context->state[i >> 2] >> ((3 - (i & 3)) * 8)) & 255);
}
}
}
#endif /* HAVE_SHA1_H */

108
flight/modules/Actuator/actuator.c
View File

@ -53,20 +53,23 @@ static int8_t counter;
#endif
// Private constants
#define MAX_QUEUE_SIZE 2
#define MAX_QUEUE_SIZE 2
#if defined(PIOS_ACTUATOR_STACK_SIZE)
#define STACK_SIZE_BYTES PIOS_ACTUATOR_STACK_SIZE
#define STACK_SIZE_BYTES PIOS_ACTUATOR_STACK_SIZE
#else
#define STACK_SIZE_BYTES 1312
#define STACK_SIZE_BYTES 1312
#endif
#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) // device driver
#define FAILSAFE_TIMEOUT_MS 100
#define MAX_MIX_ACTUATORS ACTUATORCOMMAND_CHANNEL_NUMELEM
#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) // device driver
#define FAILSAFE_TIMEOUT_MS 100
#define MAX_MIX_ACTUATORS ACTUATORCOMMAND_CHANNEL_NUMELEM
#define CAMERA_BOOT_DELAY_MS 7000
#define CAMERA_BOOT_DELAY_MS 7000
#define ACTUATOR_ONESHOT125_CLOCK 2000000
#define ACTUATOR_ONESHOT125_PULSE_SCALE 4
#define ACTUATOR_PWM_CLOCK 1000000
// Private types
@ -74,8 +77,9 @@ static int8_t counter;
static xQueueHandle queue;
static xTaskHandle taskHandle;
static float lastResult[MAX_MIX_ACTUATORS] = { 0, 0, 0, 0, 0, 0, 0, 0 };
static float filterAccumulator[MAX_MIX_ACTUATORS] = { 0, 0, 0, 0, 0, 0, 0, 0 };
static float lastResult[MAX_MIX_ACTUATORS] = { 0 };
static float filterAccumulator[MAX_MIX_ACTUATORS] = { 0 };
static uint8_t pinsMode[MAX_MIX_ACTUATORS];
// used to inform the actuator thread that actuator update rate is changed
static volatile bool actuator_settings_updated;
// used to inform the actuator thread that mixer settings are changed
@ -436,7 +440,6 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
if (command.UpdateTime > command.MaxUpdateTime) {
command.MaxUpdateTime = command.UpdateTime;
}
// Update output object
ActuatorCommandSet(&command);
// Update in case read only (eg. during servo configuration)
@ -454,6 +457,8 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
success &= set_channel(n, command.Channel[n], &actuatorSettings);
}
PIOS_Servo_Update();
if (!success) {
command.NumFailedUpdates++;
ActuatorCommandSet(&command);
@ -476,11 +481,11 @@ float ProcessMixer(const int index, const float curve1, const float curve2,
const Mixer_t *mixers = (Mixer_t *)&mixerSettings->Mixer1Type; // pointer to array of mixers in UAVObjects
const Mixer_t *mixer = &mixers[index];
float result = (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE1] / 128.0f) * curve1) +
(((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE2] / 128.0f) * curve2) +
(((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_ROLL] / 128.0f) * desired->Roll) +
(((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_PITCH] / 128.0f) * desired->Pitch) +
(((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_YAW] / 128.0f) * desired->Yaw);
float result = ((((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE1]) * curve1) +
(((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE2]) * curve2) +
(((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_ROLL]) * desired->Roll) +
(((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_PITCH]) * desired->Pitch) +
(((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_YAW]) * desired->Yaw)) / 128.0f;
// note: no feedforward for reversable motors yet for safety reasons
if (mixer->type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
@ -615,6 +620,8 @@ static void setFailsafe(const ActuatorSettingsData *actuatorSettings, const Mixe
for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) {
set_channel(n, Channel[n], actuatorSettings);
}
// Send the updated command
PIOS_Servo_Update();
// Update output object's parts that we changed
ActuatorCommandChannelSet(Channel);
@ -730,8 +737,19 @@ static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSet
return true;
case ACTUATORSETTINGS_CHANNELTYPE_PWM:
PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], value);
{
uint8_t mode = pinsMode[actuatorSettings->ChannelAddr[mixer_channel]];
switch (mode) {
case ACTUATORSETTINGS_BANKMODE_ONESHOT125:
// Remap 1000-2000 range to 125-250
PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], value / ACTUATOR_ONESHOT125_PULSE_SCALE);
break;
default:
PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], value);
break;
}
return true;
}
#if defined(PIOS_INCLUDE_I2C_ESC)
case ACTUATORSETTINGS_CHANNELTYPE_MK:
@ -754,18 +772,56 @@ static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSet
*/
static void actuator_update_rate_if_changed(const ActuatorSettingsData *actuatorSettings, bool force_update)
{
static uint16_t prevChannelUpdateFreq[ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM];
static uint16_t prevBankUpdateFreq[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM];
static uint8_t prevBankMode[ACTUATORSETTINGS_BANKMODE_NUMELEM];
bool updateMode = force_update || (memcmp(prevBankMode, actuatorSettings->BankMode, sizeof(prevBankMode)) != 0);
bool updateFreq = force_update || (memcmp(prevBankUpdateFreq, actuatorSettings->BankUpdateFreq, sizeof(prevBankUpdateFreq)) != 0);
// check if the any rate setting is changed
if (force_update ||
memcmp(prevChannelUpdateFreq,
actuatorSettings->ChannelUpdateFreq,
sizeof(prevChannelUpdateFreq)) != 0) {
// check if any setting is changed
if (updateMode || updateFreq) {
/* Something has changed, apply the settings to HW */
memcpy(prevChannelUpdateFreq,
actuatorSettings->ChannelUpdateFreq,
sizeof(prevChannelUpdateFreq));
PIOS_Servo_SetHz(actuatorSettings->ChannelUpdateFreq, ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM);
uint16_t freq[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM];
uint32_t clock[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM] = { 0 };
for (uint8_t i = 0; i < ACTUATORSETTINGS_BANKMODE_NUMELEM; i++) {
if (force_update || (actuatorSettings->BankMode[i] != prevBankMode[i])) {
PIOS_Servo_SetBankMode(i,
actuatorSettings->BankMode[i] ==
ACTUATORSETTINGS_BANKMODE_PWM ?
PIOS_SERVO_BANK_MODE_PWM :
PIOS_SERVO_BANK_MODE_SINGLE_PULSE
);
}
switch (actuatorSettings->BankMode[i]) {
case ACTUATORSETTINGS_BANKMODE_ONESHOT125:
freq[i] = 100; // Value must be small enough so CCr isn't update until the PIOS_Servo_Update is triggered
clock[i] = ACTUATOR_ONESHOT125_CLOCK; // Setup an 2MHz timer clock
break;
case ACTUATORSETTINGS_BANKMODE_PWMSYNC:
freq[i] = 100;
clock[i] = ACTUATOR_PWM_CLOCK;
break;
default: // PWM
freq[i] = actuatorSettings->BankUpdateFreq[i];
clock[i] = ACTUATOR_PWM_CLOCK;
break;
}
}
memcpy(prevBankMode,
actuatorSettings->BankMode,
sizeof(prevBankMode));
PIOS_Servo_SetHz(freq, clock, ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM);
memcpy(prevBankUpdateFreq,
actuatorSettings->BankUpdateFreq,
sizeof(prevBankUpdateFreq));
// retrieve mode from related bank
for (uint8_t i = 0; i < MAX_MIX_ACTUATORS; i++) {
uint8_t bank = PIOS_Servo_GetPinBank(i);
pinsMode[i] = actuatorSettings->BankMode[bank];
}
}
}

221
flight/modules/Altitude/revolution/altitude.c
View File

@ -1,221 +0,0 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AltitudeModule Altitude Module
* @brief Communicate with BMP085 and update @ref BaroSensor "BaroSensor UAV Object"
* @{
*
* @file altitude.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Altitude module, handles temperature and pressure readings from BMP085
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/**
* Output object: BaroSensor
*
* This module will periodically update the value of the BaroSensor object.
*
*/
#include <openpilot.h>
#include "altitude.h"
#include "barosensor.h" // object that will be updated by the module
#include "revosettings.h"
#include <mathmisc.h>
#if defined(PIOS_INCLUDE_HCSR04)
#include "sonaraltitude.h" // object that will be updated by the module
#endif
#include "taskinfo.h"
// Private constants
#define STACK_SIZE_BYTES 550
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
// Interval in number of sample to recalculate temp bias
#define TEMP_CALIB_INTERVAL 10
// LPF
#define TEMP_DT (1.0f / 120.0f)
#define TEMP_LPF_FC 5.0f
static const float temp_alpha = TEMP_DT / (TEMP_DT + 1.0f / (2.0f * M_PI_F * TEMP_LPF_FC));
// Private types
// Private variables
static xTaskHandle taskHandle;
static RevoSettingsBaroTempCorrectionPolynomialData baroCorrection;
static RevoSettingsBaroTempCorrectionExtentData baroCorrectionExtent;
static volatile bool tempCorrectionEnabled;
static float baro_temp_bias = 0;
static float baro_temperature = NAN;
static uint8_t temp_calibration_count = 0;
// Private functions
static void altitudeTask(void *parameters);
static void SettingsUpdatedCb(UAVObjEvent *ev);
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AltitudeStart()
{
// Start main task
xTaskCreate(altitudeTask, "Altitude", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ALTITUDE, taskHandle);
return 0;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AltitudeInitialize()
{
BaroSensorInitialize();
RevoSettingsInitialize();
RevoSettingsConnectCallback(&SettingsUpdatedCb);
SettingsUpdatedCb(NULL);
#if defined(PIOS_INCLUDE_HCSR04)
SonarAltitudeInitialize();
#endif
return 0;
}
MODULE_INITCALL(AltitudeInitialize, AltitudeStart);
/**
* Module thread, should not return.
*/
static void altitudeTask(__attribute__((unused)) void *parameters)
{
BaroSensorData data;
#if defined(PIOS_INCLUDE_HCSR04)
SonarAltitudeData sonardata;
int32_t value = 0, timeout = 10, sample_rate = 0;
float coeff = 0.25, height_out = 0, height_in = 0;
PIOS_HCSR04_Trigger();
#endif
// TODO: Check the pressure sensor and set a warning if it fails test
// Option to change the interleave between Temp and Pressure conversions
// Undef for normal operation
// #define PIOS_MS5611_SLOW_TEMP_RATE 20
RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection);
#ifdef PIOS_MS5611_SLOW_TEMP_RATE
uint8_t temp_press_interleave_count = 1;
#endif
// Main task loop
while (1) {
#if defined(PIOS_INCLUDE_HCSR04)
// Compute the current altitude
// depends on baro samplerate
if (!(sample_rate--)) {
if (PIOS_HCSR04_Completed()) {
value = PIOS_HCSR04_Get();
// from 3.4cm to 5.1m
if ((value > 100) && (value < 15000)) {
height_in = value * 0.00034f / 2.0f;
height_out = (height_out * (1 - coeff)) + (height_in * coeff);
sonardata.Altitude = height_out; // m/us
}
// Update the SonarAltitude UAVObject
SonarAltitudeSet(&sonardata);
timeout = 10;
PIOS_HCSR04_Trigger();
}
if (!(timeout--)) {
// retrigger
timeout = 10;
PIOS_HCSR04_Trigger();
}
sample_rate = 25;
}
#endif /* if defined(PIOS_INCLUDE_HCSR04) */
float temp, press;
#ifdef PIOS_MS5611_SLOW_TEMP_RATE
temp_press_interleave_count--;
if (temp_press_interleave_count == 0) {
#endif
// Update the temperature data
PIOS_MS5611_StartADC(TemperatureConv);
vTaskDelay(PIOS_MS5611_GetDelay());
PIOS_MS5611_ReadADC();
#ifdef PIOS_MS5611_SLOW_TEMP_RATE
temp_press_interleave_count = PIOS_MS5611_SLOW_TEMP_RATE;
}
#endif
// Update the pressure data
PIOS_MS5611_StartADC(PressureConv);
vTaskDelay(PIOS_MS5611_GetDelay());
PIOS_MS5611_ReadADC();
temp = PIOS_MS5611_GetTemperature();
press = PIOS_MS5611_GetPressure();
if (isnan(baro_temperature)) {
baro_temperature = temp;
}
baro_temperature = temp_alpha * (temp - baro_temperature) + baro_temperature;
if (tempCorrectionEnabled && !temp_calibration_count) {
temp_calibration_count = TEMP_CALIB_INTERVAL;
// pressure bias = A + B*t + C*t^2 + D * t^3
// in case the temperature is outside of the calibrated range, uses the nearest extremes
float ctemp = boundf(baro_temperature, baroCorrectionExtent.max, baroCorrectionExtent.min);
baro_temp_bias = baroCorrection.a + ((baroCorrection.d * ctemp + baroCorrection.c) * ctemp + baroCorrection.b) * ctemp;
}
press -= baro_temp_bias;
float altitude = 44330.0f * (1.0f - powf((press) / MS5611_P0, (1.0f / 5.255f)));
if (!isnan(altitude)) {
data.Altitude = altitude;
data.Temperature = temp;
data.Pressure = press;
// Update the BasoSensor UAVObject
BaroSensorSet(&data);
}
}
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection);
RevoSettingsBaroTempCorrectionExtentGet(&baroCorrectionExtent);
tempCorrectionEnabled = !(baroCorrectionExtent.max - baroCorrectionExtent.min < 0.1f ||
(baroCorrection.a < 1e-9f && baroCorrection.b < 1e-9f && baroCorrection.c < 1e-9f && baroCorrection.d < 1e-9f));
}
/**
* @}
* @}
*/

56
flight/modules/Attitude/attitude.c
View File

@ -62,6 +62,10 @@
#include "manualcontrolcommand.h"
#include "taskinfo.h"
#include <pios_sensors.h>
#include <pios_adxl345.h>
#include <pios_mpu6000.h>
#include "CoordinateConversions.h"
#include <pios_notify.h>
#include <mathmisc.h>
@ -160,9 +164,11 @@ int32_t const MAX_TRIM_FLIGHT_SAMPLES = 65535;
#define STD_CC_ANALOG_GYRO_NEUTRAL 1665
#define STD_CC_ANALOG_GYRO_GAIN 0.42f
static struct PIOS_SENSORS_3Axis_SensorsWithTemp *mpu6000_data = NULL;
// Used to detect CC vs CC3D
static const struct pios_board_info *bdinfo = &pios_board_info_blob;
#define BOARDISCC3D (bdinfo->board_rev == 0x02)
#define BOARDISCC3D (bdinfo->board_rev == 0x02)
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
@ -172,8 +178,9 @@ int32_t AttitudeStart(void)
// Start main task
xTaskCreate(AttitudeTask, "Attitude", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ATTITUDE, taskHandle);
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE);
#endif
return 0;
}
@ -238,14 +245,18 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
if (cc3d) {
#if defined(PIOS_INCLUDE_MPU6000)
gyro_test = PIOS_MPU6000_Test();
gyro_test = PIOS_MPU6000_Driver.test(0);
mpu6000_data = pios_malloc(sizeof(PIOS_SENSORS_3Axis_SensorsWithTemp) + sizeof(Vector3i16) * 2);
#endif
} else {
#if defined(PIOS_INCLUDE_ADXL345)
// Set critical error and wait until the accel is producing data
while (PIOS_ADXL345_FifoElements() == 0) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
#endif
}
accel_test = PIOS_ADXL345_Test();
#endif
@ -302,9 +313,9 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
}
init = 1;
}
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
#endif
AccelStateData accelState;
GyroStateData gyros;
int32_t retval = 0;
@ -455,7 +466,6 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
* @param[in] attitudeRaw Populate the UAVO instead of saving right here
* @return 0 if successfull, -1 if not
*/
static struct pios_mpu6000_data mpu6000_data;
static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *gyrosData)
{
float accels[3] = { 0 };
@ -465,22 +475,22 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
#if defined(PIOS_INCLUDE_MPU6000)
xQueueHandle queue = PIOS_MPU6000_GetQueue();
BaseType_t ret = xQueueReceive(queue, (void *)&mpu6000_data, sensor_period_ms);
xQueueHandle queue = PIOS_MPU6000_Driver.get_queue(0);
BaseType_t ret = xQueueReceive(queue, (void *)mpu6000_data, sensor_period_ms);
while (ret == pdTRUE) {
gyros[0] += mpu6000_data.gyro_x;
gyros[1] += mpu6000_data.gyro_y;
gyros[2] += mpu6000_data.gyro_z;
gyros[0] += mpu6000_data->sample[1].x;
gyros[1] += mpu6000_data->sample[1].y;
gyros[2] += mpu6000_data->sample[1].z;
accels[0] += mpu6000_data.accel_x;
accels[1] += mpu6000_data.accel_y;
accels[2] += mpu6000_data.accel_z;
accels[0] += mpu6000_data->sample[0].x;
accels[1] += mpu6000_data->sample[0].y;
accels[2] += mpu6000_data->sample[0].z;
temp += mpu6000_data.temperature;
temp += mpu6000_data->temperature;
count++;
// check if further samples are already in queue
ret = xQueueReceive(queue, (void *)&mpu6000_data, 0);
ret = xQueueReceive(queue, (void *)mpu6000_data, 0);
}
PERF_TRACK_VALUE(counterAccelSamples, count);
@ -751,17 +761,19 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
temp_calibrated_extent.max = accelGyroSettings.temp_calibrated_extent.max;
if (BOARDISCC3D) {
float scales[2];
PIOS_MPU6000_Driver.get_scale(scales, 2, 0);
accel_bias.X = accelGyroSettings.accel_bias.X;
accel_bias.Y = accelGyroSettings.accel_bias.Y;
accel_bias.Z = accelGyroSettings.accel_bias.Z;
gyro_scale.X = accelGyroSettings.gyro_scale.X * PIOS_MPU6000_GetScale();
gyro_scale.Y = accelGyroSettings.gyro_scale.Y * PIOS_MPU6000_GetScale();
gyro_scale.Z = accelGyroSettings.gyro_scale.Z * PIOS_MPU6000_GetScale();
gyro_scale.X = accelGyroSettings.gyro_scale.X * scales[1];
gyro_scale.Y = accelGyroSettings.gyro_scale.Y * scales[1];
gyro_scale.Z = accelGyroSettings.gyro_scale.Z * scales[1];
accel_scale.X = accelGyroSettings.accel_scale.X * PIOS_MPU6000_GetAccelScale();
accel_scale.Y = accelGyroSettings.accel_scale.Y * PIOS_MPU6000_GetAccelScale();
accel_scale.Z = accelGyroSettings.accel_scale.Z * PIOS_MPU6000_GetAccelScale();
accel_scale.X = accelGyroSettings.accel_scale.X * scales[0];
accel_scale.Y = accelGyroSettings.accel_scale.Y * scales[0];
accel_scale.Z = accelGyroSettings.accel_scale.Z * scales[0];
} else {
// Original CC with analog gyros and ADXL accel
accel_bias.X = accelGyroSettings.accel_bias.X;

14
flight/modules/GPS/GPS.c
View File

@ -91,7 +91,12 @@ void updateGpsSettings(UAVObjEvent *ev);
#else
#if defined(PIOS_GPS_MINIMAL)
#define GPS_READ_BUFFER 32
#define STACK_SIZE_BYTES 500
#ifdef PIOS_INCLUDE_GPS_NMEA_PARSER
#define STACK_SIZE_BYTES 580 // NMEA
#else
#define STACK_SIZE_BYTES 440 // UBX
#endif // PIOS_INCLUDE_GPS_NMEA_PARSER
#else
#define STACK_SIZE_BYTES 650
#endif // PIOS_GPS_MINIMAL
@ -203,9 +208,11 @@ int32_t GPSInitialize(void)
GPSSettingsInitialize();
GPSSettingsDataProtocolGet(&gpsProtocol);
switch (gpsProtocol) {
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
case GPSSETTINGS_DATAPROTOCOL_NMEA:
gps_rx_buffer = pios_malloc(NMEA_MAX_PACKET_LENGTH);
break;
#endif
case GPSSETTINGS_DATAPROTOCOL_UBX:
gps_rx_buffer = pios_malloc(sizeof(struct UBXPacket));
break;
@ -459,12 +466,12 @@ static void updateHwSettings()
}
}
#ifdef PIOS_INCLUDE_GPS_UBX_PARSER
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
load_mag_settings();
}
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev)
{
uint8_t ubxAutoConfig;
@ -537,7 +544,6 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev)
ubx_autoconfig_set(newconfig);
}
#endif /* if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) */
#endif /* ifdef PIOS_INCLUDE_GPS_UBX_PARSER */
/**
* @}
* @}

73
flight/modules/GPS/NMEA.c
View File

@ -40,14 +40,13 @@
#include "GPS.h"
// #define ENABLE_DEBUG_MSG ///< define to enable debug-messages
#define DEBUG_PORT PIOS_COM_TELEM_RF ///< defines which serial port is ued for debug-messages
#define DEBUG_PORT PIOS_COM_TELEM_RF ///< defines which serial port is used for debug-messages
// Debugging
#ifdef ENABLE_DEBUG_MSG
// #define DEBUG_MSG_IN ///< define to display the incoming NMEA messages
// #define DEBUG_PARAMS ///< define to display the incoming NMEA messages split into its parameters
// #define DEBUG_MGSID_IN ///< define to display the the names of the incoming NMEA messages
// #define DEBUG_MSGID_IN ///< define to display the names of the incoming NMEA messages
// #define NMEA_DEBUG_PKT ///< define to enable debug of all NMEA messages
// #define NMEA_DEBUG_GGA ///< define to enable debug of GGA messages
// #define NMEA_DEBUG_VTG ///< define to enable debug of VTG messages
@ -68,40 +67,40 @@ struct nmea_parser {
bool (*handler)(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
};
static bool nmeaProcessGPGGA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
static bool nmeaProcessGPRMC(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
static bool nmeaProcessGPVTG(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
static bool nmeaProcessGPGSA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
static bool nmeaProcessGxGGA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
static bool nmeaProcessGxRMC(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
static bool nmeaProcessGxVTG(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
static bool nmeaProcessGxGSA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
#if !defined(PIOS_GPS_MINIMAL)
static bool nmeaProcessGPZDA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
static bool nmeaProcessGPGSV(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
static bool nmeaProcessGxZDA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
static bool nmeaProcessGxGSV(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
#endif // PIOS_GPS_MINIMAL
static const struct nmea_parser nmea_parsers[] = {
{
.prefix = "GPGGA",
.handler = nmeaProcessGPGGA,
.prefix = "GGA",
.handler = nmeaProcessGxGGA,
},
{
.prefix = "GPVTG",
.handler = nmeaProcessGPVTG,
.prefix = "VTG",
.handler = nmeaProcessGxVTG,
},
{
.prefix = "GPGSA",
.handler = nmeaProcessGPGSA,
.prefix = "GSA",
.handler = nmeaProcessGxGSA,
},
{
.prefix = "GPRMC",
.handler = nmeaProcessGPRMC,
.prefix = "RMC",
.handler = nmeaProcessGxRMC,
},
#if !defined(PIOS_GPS_MINIMAL)
{
.prefix = "GPZDA",
.handler = nmeaProcessGPZDA,
.prefix = "ZDA",
.handler = nmeaProcessGxZDA,
},
{
.prefix = "GPGSV",
.handler = nmeaProcessGPGSV,
.prefix = "GSV",
.handler = nmeaProcessGxGSV,
},
#endif // PIOS_GPS_MINIMAL
};
@ -373,6 +372,8 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionSensorData *GpsData)
// Sample NMEA message: "GPRMC,000131.736,V,,,,,0.00,0.00,060180,,,N*43"
// The first parameter starts at the beginning of the message
// Skip first two character, allow GL, GN, GP...
p += 2;
params[0] = p;
nbParams = 1;
while (*p != 0) {
@ -406,11 +407,13 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionSensorData *GpsData)
parser = NMEA_find_parser_by_prefix(params[0]);
if (!parser) {
// No parser found
#ifdef DEBUG_MSGID_IN
DEBUG_MSG(" NO PARSER (\"%s\")\n", params[0]);
#endif
return false;
}
#ifdef DEBUG_MGSID_IN
#ifdef DEBUG_MSGID_IN
DEBUG_MSG("%s %d ", params[0]);
#endif
// Send the message to the parser and get it update the GpsData
@ -422,7 +425,9 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionSensorData *GpsData)
if (!parser->handler(GpsData, &gpsDataUpdated, params, nbParams)) {
// Parse failed
#ifdef DEBUG_MSGID_IN
DEBUG_MSG("PARSE FAILED (\"%s\")\n", params[0]);
#endif
if (gpsDataUpdated && (GpsData->Status == GPSPOSITIONSENSOR_STATUS_NOFIX)) {
GPSPositionSensorSet(GpsData);
}
@ -432,13 +437,13 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionSensorData *GpsData)
// All is fine :) Update object if data has changed
if (gpsDataUpdated) {
#ifdef DEBUG_MGSID_IN
#ifdef DEBUG_MSGID_IN
DEBUG_MSG("U");
#endif
GPSPositionSensorSet(GpsData);
}
#ifdef DEBUG_MGSID_IN
#ifdef DEBUG_MSGID_IN
DEBUG_MSG("\n");
#endif
return true;
@ -446,11 +451,11 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionSensorData *GpsData)
/**
* Parse an NMEA GPGGA sentence and update the given UAVObject
* Parse an NMEA GxGGA sentence and update the given UAVObject
* \param[in] A pointer to a GPSPositionSensor UAVObject to be updated.
* \param[in] An NMEA sentence with a valid checksum
*/
static bool nmeaProcessGPGGA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
static bool nmeaProcessGxGGA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
{
if (nbParam != 15) {
return false;
@ -499,11 +504,11 @@ static bool nmeaProcessGPGGA(GPSPositionSensorData *GpsData, bool *gpsDataUpdate
}
/**
* Parse an NMEA GPRMC sentence and update the given UAVObject
* Parse an NMEA GxRMC sentence and update the given UAVObject
* \param[in] A pointer to a GPSPositionSensor UAVObject to be updated.
* \param[in] An NMEA sentence with a valid checksum
*/
static bool nmeaProcessGPRMC(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
static bool nmeaProcessGxRMC(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
{
if (nbParam != 13) {
return false;
@ -567,11 +572,11 @@ static bool nmeaProcessGPRMC(GPSPositionSensorData *GpsData, bool *gpsDataUpdate
}
/**
* Parse an NMEA GPVTG sentence and update the given UAVObject
* Parse an NMEA GxVTG sentence and update the given UAVObject
* \param[in] A pointer to a GPSPositionSensor UAVObject to be updated.
* \param[in] An NMEA sentence with a valid checksum
*/
static bool nmeaProcessGPVTG(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
static bool nmeaProcessGxVTG(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
{
if (nbParam != 9 && nbParam != 10 /*GTOP GPS seems to gemnerate an extra parameter...*/) {
return false;
@ -592,11 +597,11 @@ static bool nmeaProcessGPVTG(GPSPositionSensorData *GpsData, bool *gpsDataUpdate
#if !defined(PIOS_GPS_MINIMAL)
/**
* Parse an NMEA GPZDA sentence and update the @ref GPSTime object
* Parse an NMEA GxZDA sentence and update the @ref GPSTime object
* \param[in] A pointer to a GPSPositionSensor UAVObject to be updated (unused).
* \param[in] An NMEA sentence with a valid checksum
*/
static bool nmeaProcessGPZDA(__attribute__((unused)) GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
static bool nmeaProcessGxZDA(__attribute__((unused)) GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
{
if (nbParam != 7) {
return false;
@ -636,7 +641,7 @@ static uint8_t gsv_processed_mask;
static uint16_t gsv_incomplete_error;
static uint16_t gsv_duplicate_error;
static bool nmeaProcessGPGSV(__attribute__((unused)) GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
static bool nmeaProcessGxGSV(__attribute__((unused)) GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
{
if (nbParam < 4) {
return false;
@ -723,7 +728,7 @@ static bool nmeaProcessGPGSV(__attribute__((unused)) GPSPositionSensorData *GpsD
* \param[in] A pointer to a GPSPositionSensor UAVObject to be updated.
* \param[in] An NMEA sentence with a valid checksum
*/
static bool nmeaProcessGPGSA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
static bool nmeaProcessGxGSA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
{
if (nbParam != 18) {
return false;

3
flight/modules/GPS/ubx_autoconfig.c
View File

@ -399,6 +399,7 @@ void ubx_autoconfig_set(ubx_autoconfig_settings_t config)
if (config.autoconfigEnabled) {
if (!status) {
status = (status_t *)pios_malloc(sizeof(status_t));
PIOS_Assert(status);
memset(status, 0, sizeof(status_t));
status->currentStep = INIT_STEP_DISABLED;
}
@ -406,7 +407,7 @@ void ubx_autoconfig_set(ubx_autoconfig_settings_t config)
status->currentStep = INIT_STEP_START;
enabled = true;
} else {
if (!status) {
if (status) {
status->currentStep = INIT_STEP_DISABLED;
}
}

4
flight/modules/ManualControl/manualcontrol.c
View File

@ -43,8 +43,8 @@
#include <systemsettings.h>
#include <stabilizationdesired.h>
#include <callbackinfo.h>
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
#include <stabilizationsettings.h>
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
#include <vtolpathfollowersettings.h>
#endif
@ -160,9 +160,9 @@ int32_t ManualControlInitialize()
ManualControlSettingsInitialize();
FlightModeSettingsInitialize();
SystemSettingsInitialize();
StabilizationSettingsInitialize();
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
VtolSelfTuningStatsInitialize();
StabilizationSettingsInitialize();
VtolPathFollowerSettingsInitialize();
#endif
callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&manualControlTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_MANUALCONTROL, STACK_SIZE_BYTES);

1
flight/modules/OPLink/oplinkmod.c
View File

@ -158,7 +158,6 @@ static void systemTask(__attribute__((unused)) void *parameters)
oplinkStatus.RxMissed = radio_stats.rx_missed;
oplinkStatus.RxFailure = radio_stats.rx_failure;
oplinkStatus.TxDropped = radio_stats.tx_dropped;
oplinkStatus.TxResent = radio_stats.tx_resent;
oplinkStatus.TxFailure = radio_stats.tx_failure;
oplinkStatus.Resets = radio_stats.resets;
oplinkStatus.Timeouts = radio_stats.timeouts;

2
flight/modules/Receiver/receiver.c
View File

@ -135,7 +135,7 @@ int32_t ReceiverInitialize()
{
/* Check the assumptions about uavobject enum's are correct */
PIOS_STATIC_ASSERT(assumptions);
AccessoryDesiredInitialize();
ManualControlCommandInitialize();
ReceiverActivityInitialize();
ManualControlSettingsInitialize();

11
flight/modules/Sensors/inc/sensors.h
View File

@ -2,16 +2,17 @@
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup Sensors Sensors Module
* @addtogroup Sensors
* @brief Acquires sensor data
* @{
*
* @file attitude.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
* @brief Acquires sensor data and fuses it into attitude estimate for CC
* @file sensors.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Module to handle fetch and preprocessing of sensor data
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
******************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by

653
flight/modules/Sensors/sensors.c
View File

@ -4,12 +4,11 @@
* @{
* @addtogroup Sensors
* @brief Acquires sensor data
* Specifically updates the the @ref GyroSensor, @ref AccelSensor, and @ref MagSensor objects
* @{
*
* @file sensors.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Module to handle all comms to the AHRS on a periodic basis.
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Module to handle fetch and preprocessing of sensor data
*
* @see The GNU Public License (GPL) Version 3
*
@ -47,69 +46,118 @@
*/
#include <openpilot.h>
#include <pios_sensors.h>
#include <homelocation.h>
#include <magsensor.h>
#include <accelsensor.h>
#include <gyrosensor.h>
#include <attitudestate.h>
#include <barosensor.h>
#include <flightstatus.h>
#include <attitudesettings.h>
#include <revocalibration.h>
#include <accelgyrosettings.h>
#include <flightstatus.h>
#include <revosettings.h>
#include <mathmisc.h>
#include <taskinfo.h>
#include <pios_math.h>
#include <pios_constants.h>
#include <CoordinateConversions.h>
#include <pios_board_info.h>
#include <string.h>
// Private constants
#define STACK_SIZE_BYTES 1000
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
#define STACK_SIZE_BYTES 1000
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
static const uint32_t sensor_period_ms = ((uint32_t)1000.0f / PIOS_SENSOR_RATE);
#define MAX_SENSORS_PER_INSTANCE 2
#ifdef PIOS_INCLUDE_WDG
#define RELOAD_WDG() PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS)
#define REGISTER_WDG() PIOS_WDG_RegisterFlag(PIOS_WDG_SENSORS)
#else
#define RELOAD_WDG()
#define REGISTER_WDG()
#endif
static const TickType_t sensor_period_ticks = ((uint32_t)1000.0f / PIOS_SENSOR_RATE) / portTICK_RATE_MS;
// Interval in number of sample to recalculate temp bias
#define TEMP_CALIB_INTERVAL 30
#define TEMP_CALIB_INTERVAL 30
// LPF
#define TEMP_DT (1.0f / PIOS_SENSOR_RATE)
#define TEMP_LPF_FC 5.0f
static const float temp_alpha = TEMP_DT / (TEMP_DT + 1.0f / (2.0f * M_PI_F * TEMP_LPF_FC));
#define TEMP_DT_GYRO_ACCEL (1.0f / PIOS_SENSOR_RATE)
#define TEMP_LPF_FC_GYRO_ACCEL 5.0f
static const float temp_alpha_gyro_accel = LPF_ALPHA(TEMP_DT_GYRO_ACCEL, TEMP_LPF_FC_GYRO_ACCEL);
#define ZERO_ROT_ANGLE 0.00001f
// Interval in number of sample to recalculate temp bias
#define BARO_TEMP_CALIB_INTERVAL 10
// LPF
#define TEMP_DT_BARO (1.0f / 120.0f)
#define TEMP_LPF_FC_BARO 5.0f
static const float temp_alpha_baro = TEMP_DT_BARO / (TEMP_DT_BARO + 1.0f / (2.0f * M_PI_F * TEMP_LPF_FC_BARO));
#define ZERO_ROT_ANGLE 0.00001f
// Private types
typedef struct {
// used to accumulate all samples in a task iteration
Vector3i32 accum[2];
int32_t temperature;
uint32_t count;
} sensor_fetch_context;
#define MAX_SENSOR_DATA_SIZE (sizeof(PIOS_SENSORS_3Axis_SensorsWithTemp) + MAX_SENSORS_PER_INSTANCE * sizeof(Vector3i16))
typedef union {
PIOS_SENSORS_3Axis_SensorsWithTemp sensorSample3Axis;
PIOS_SENSORS_1Axis_SensorsWithTemp sensorSample1Axis;
} sensor_data;
#define PIOS_INSTRUMENT_MODULE
#include <pios_instrumentation_helper.h>
PERF_DEFINE_COUNTER(counterGyroSamples);
PERF_DEFINE_COUNTER(counterAccelSamples);
PERF_DEFINE_COUNTER(counterAccelPeriod);
PERF_DEFINE_COUNTER(counterMagPeriod);
PERF_DEFINE_COUNTER(counterBaroPeriod);
PERF_DEFINE_COUNTER(counterSensorPeriod);
// Counters:
// - 0x53000001 Sensor fetch rate(period)
// - 0x53000002 number of gyro samples read for each loop
PERF_DEFINE_COUNTER(counterSensorResets);
// Private functions
static void SensorsTask(void *parameters);
static void settingsUpdatedCb(UAVObjEvent *objEv);
static void accumulateSamples(sensor_fetch_context *sensor_context, sensor_data *sample);
static void processSamples3d(sensor_fetch_context *sensor_context, const PIOS_SENSORS_Instance *sensor);
static void processSamples1d(PIOS_SENSORS_1Axis_SensorsWithTemp *sample, const PIOS_SENSORS_Instance *sensor);
static void clearContext(sensor_fetch_context *sensor_context);
static void handleAccel(float *samples, float temperature);
static void handleGyro(float *samples, float temperature);
static void handleMag(float *samples, float temperature);
static void handleBaro(float sample, float temperature);
static void updateAccelTempBias(float temperature);
static void updateGyroTempBias(float temperature);
static void updateBaroTempBias(float temperature);
// Private variables
static sensor_data *source_data;
static xTaskHandle sensorsTaskHandle;
RevoCalibrationData cal;
AccelGyroSettingsData agcal;
#ifdef PIOS_INCLUDE_HMC5X83
#include <pios_hmc5x83.h>
extern pios_hmc5x83_dev_t onboard_mag;
#endif
// These values are initialized by settings but can be updated by the attitude algorithm
static float mag_bias[3] = { 0, 0, 0 };
static float mag_transform[3][3] = {
{ 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }
};
// temp coefficient to calculate gyro bias
// Variables used to handle accel/gyro temperature bias
static volatile bool gyro_temp_calibrated = false;
static volatile bool accel_temp_calibrated = false;
@ -117,42 +165,45 @@ static float accel_temperature = NAN;
static float gyro_temperature = NAN;
static float accel_temp_bias[3] = { 0 };
static float gyro_temp_bias[3] = { 0 };
static uint8_t temp_calibration_count = 0;
static uint8_t accel_temp_calibration_count = 0;
static uint8_t gyro_temp_calibration_count = 0;
static float R[3][3] = {
{ 0 }
};
// Variables used to handle baro temperature bias
static RevoSettingsBaroTempCorrectionPolynomialData baroCorrection;
static RevoSettingsBaroTempCorrectionExtentData baroCorrectionExtent;
static volatile bool baro_temp_correction_enabled;
static float baro_temp_bias = 0;
static float baro_temperature = NAN;
static uint8_t baro_temp_calibration_count = 0;
static int8_t rotate = 0;
/**
* API for sensor fusion algorithms:
* Configure(xQueueHandle gyro, xQueueHandle accel, xQueueHandle mag, xQueueHandle baro)
* Stores all the queues the algorithm will pull data from
* FinalizeSensors() -- before saving the sensors modifies them based on internal state (gyro bias)
* Update() -- queries queues and updates the attitude estiamte
*/
/**
* Initialise the module. Called before the start function
* \returns 0 on success or -1 if initialisation failed
*/
int32_t SensorsInitialize(void)
{
source_data = (sensor_data *)pios_malloc(MAX_SENSOR_DATA_SIZE);
GyroSensorInitialize();
AccelSensorInitialize();
MagSensorInitialize();
BaroSensorInitialize();
RevoCalibrationInitialize();
RevoSettingsInitialize();
AttitudeSettingsInitialize();
AccelGyroSettingsInitialize();
rotate = 0;
RevoSettingsConnectCallback(&settingsUpdatedCb);
RevoCalibrationConnectCallback(&settingsUpdatedCb);
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
AccelGyroSettingsConnectCallback(&settingsUpdatedCb);
return 0;
}
@ -165,10 +216,7 @@ int32_t SensorsStart(void)
// Start main task
xTaskCreate(SensorsTask, "Sensors", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &sensorsTaskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SENSORS, sensorsTaskHandle);
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_SENSORS);
#endif
REGISTER_WDG();
return 0;
}
@ -184,82 +232,55 @@ int32_t mag_test;
* The sensor task. This polls the gyros at 500 Hz and pumps that data to
* stabilization and to the attitude loop
*
* This function has a lot of if/defs right now to allow these configurations:
* 1. BMA180 accel and MPU6000 gyro
* 2. MPU6000 gyro and accel
* 3. BMA180 accel and L3GD20 gyro
*/
uint32_t sensor_dt_us;
static void SensorsTask(__attribute__((unused)) void *parameters)
{
portTickType lastSysTime;
uint32_t accel_samples = 0;
uint32_t gyro_samples = 0;
int32_t accel_accum[3] = { 0, 0, 0 };
int32_t gyro_accum[3] = { 0, 0, 0 };
float gyro_scaling = 0;
float accel_scaling = 0;
static int32_t timeval;
sensor_fetch_context sensor_context;
bool error = false;
const PIOS_SENSORS_Instance *sensors_list = PIOS_SENSORS_GetList();
PIOS_SENSORS_Instance *sensor;
AlarmsClear(SYSTEMALARMS_ALARM_SENSORS);
settingsUpdatedCb(NULL);
UAVObjEvent ev;
settingsUpdatedCb(&ev);
// Performance counters
PERF_INIT_COUNTER(counterAccelSamples, 0x53000001);
PERF_INIT_COUNTER(counterAccelPeriod, 0x53000002);
PERF_INIT_COUNTER(counterMagPeriod, 0x53000003);
PERF_INIT_COUNTER(counterBaroPeriod, 0x53000004);
PERF_INIT_COUNTER(counterSensorPeriod, 0x53000005);
PERF_INIT_COUNTER(counterSensorResets, 0x53000006);
const struct pios_board_info *bdinfo = &pios_board_info_blob;
switch (bdinfo->board_rev) {
case 0x01:
#if defined(PIOS_INCLUDE_L3GD20)
gyro_test = PIOS_L3GD20_Test();
#endif
#if defined(PIOS_INCLUDE_BMA180)
accel_test = PIOS_BMA180_Test();
#endif
break;
case 0x02:
#if defined(PIOS_INCLUDE_MPU6000)
gyro_test = PIOS_MPU6000_Test();
accel_test = gyro_test;
#endif
break;
default:
PIOS_DEBUG_Assert(0);
// Test sensors
bool sensors_test = true;
uint8_t count = 0;
LL_FOREACH((PIOS_SENSORS_Instance *)sensors_list, sensor) {
sensors_test &= PIOS_SENSORS_Test(sensor);
count++;
}
#if defined(PIOS_INCLUDE_HMC5X83)
mag_test = PIOS_HMC5x83_Test(onboard_mag);
#else
mag_test = 0;
#endif
if (accel_test < 0 || gyro_test < 0 || mag_test < 0) {
PIOS_Assert(count);
RELOAD_WDG();
if (!sensors_test) {
AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL);
while (1) {
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
#endif
vTaskDelay(10);
}
}
PERF_INIT_COUNTER(counterGyroSamples, 0x53000001);
PERF_INIT_COUNTER(counterSensorPeriod, 0x53000002);
// Main task loop
lastSysTime = xTaskGetTickCount();
bool error = false;
uint32_t mag_update_time = PIOS_DELAY_GetRaw();
uint32_t reset_counter = 0;
while (1) {
// TODO: add timeouts to the sensor reads and set an error if the fail
sensor_dt_us = PIOS_DELAY_DiffuS(timeval);
timeval = PIOS_DELAY_GetRaw();
if (error) {
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
#endif
RELOAD_WDG();
lastSysTime = xTaskGetTickCount();
vTaskDelayUntil(&lastSysTime, sensor_period_ms / portTICK_RATE_MS);
vTaskDelayUntil(&lastSysTime, sensor_period_ticks);
AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL);
error = false;
} else {
@ -267,219 +288,250 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
}
for (int i = 0; i < 3; i++) {
accel_accum[i] = 0;
gyro_accum[i] = 0;
}
accel_samples = 0;
gyro_samples = 0;
// reset the fetch context
clearContext(&sensor_context);
LL_FOREACH((PIOS_SENSORS_Instance *)sensors_list, sensor) {
// we will wait on the sensor that's marked as primary( that means the sensor with higher sample rate)
bool is_primary = (sensor->type & PIOS_SENSORS_TYPE_3AXIS_ACCEL);
AccelSensorData accelSensorData;
GyroSensorData gyroSensorData;
switch (bdinfo->board_rev) {
case 0x01: // L3GD20 + BMA180 board
#if defined(PIOS_INCLUDE_BMA180)
{
struct pios_bma180_data accel;
int32_t read_good;
int32_t count;
count = 0;
while ((read_good = PIOS_BMA180_ReadFifo(&accel)) != 0 && !error) {
error = ((xTaskGetTickCount() - lastSysTime) > sensor_period_ms) ? true : error;
if (!sensor->driver->is_polled) {
const QueueHandle_t queue = PIOS_SENSORS_GetQueue(sensor);
while (xQueueReceive(queue,
(void *)source_data,
(is_primary && !sensor_context.count) ? sensor_period_ticks : 0) == pdTRUE) {
accumulateSamples(&sensor_context, source_data);
}
if (error) {
// Unfortunately if the BMA180 ever misses getting read, then it will not
// trigger more interrupts. In this case we must force a read to kickstarts
// it.
struct pios_bma180_data data;
PIOS_BMA180_ReadAccels(&data);
continue;
}
while (read_good == 0) {
count++;
accel_accum[1] += accel.x;
accel_accum[0] += accel.y;
accel_accum[2] -= accel.z;
read_good = PIOS_BMA180_ReadFifo(&accel);
}
accel_samples = count;
accel_scaling = PIOS_BMA180_GetScale();
// Get temp from last reading
accelSensorData.temperature = 25.0f + ((float)accel.temperature - 2.0f) / 2.0f;
}
#endif /* if defined(PIOS_INCLUDE_BMA180) */
#if defined(PIOS_INCLUDE_L3GD20)
{
struct pios_l3gd20_data gyro;
gyro_samples = 0;
xQueueHandle gyro_queue = PIOS_L3GD20_GetQueue();
if (xQueueReceive(gyro_queue, (void *)&gyro, 4) == errQUEUE_EMPTY) {
if (sensor_context.count) {
processSamples3d(&sensor_context, sensor);
clearContext(&sensor_context);
} else if (is_primary) {
PIOS_SENSOR_Reset(sensor);
reset_counter++;
PERF_TRACK_VALUE(counterSensorResets, reset_counter);
error = true;
continue;
}
gyro_samples = 1;
gyro_accum[1] += gyro.gyro_x;
gyro_accum[0] += gyro.gyro_y;
gyro_accum[2] -= gyro.gyro_z;
gyro_scaling = PIOS_L3GD20_GetScale();
// Get temp from last reading
gyroSensorData.temperature = gyro.temperature;
}
#endif /* if defined(PIOS_INCLUDE_L3GD20) */
break;
case 0x02: // MPU6000 board
case 0x03: // MPU6000 board
#if defined(PIOS_INCLUDE_MPU6000)
{
struct pios_mpu6000_data mpu6000_data;
xQueueHandle queue = PIOS_MPU6000_GetQueue();
while (xQueueReceive(queue, (void *)&mpu6000_data, gyro_samples == 0 ? 10 : 0) != errQUEUE_EMPTY) {
gyro_accum[0] += mpu6000_data.gyro_x;
gyro_accum[1] += mpu6000_data.gyro_y;
gyro_accum[2] += mpu6000_data.gyro_z;
accel_accum[0] += mpu6000_data.accel_x;
accel_accum[1] += mpu6000_data.accel_y;
accel_accum[2] += mpu6000_data.accel_z;
gyro_samples++;
accel_samples++;
} else {
if (PIOS_SENSORS_Poll(sensor)) {
PIOS_SENSOR_Fetch(sensor, (void *)source_data, MAX_SENSORS_PER_INSTANCE);
if (sensor->type & PIOS_SENSORS_TYPE_3D) {
accumulateSamples(&sensor_context, source_data);
processSamples3d(&sensor_context, sensor);
} else {
processSamples1d(&source_data->sensorSample1Axis, sensor);
}
clearContext(&sensor_context);
}
PERF_MEASURE_PERIOD(counterSensorPeriod);
PERF_TRACK_VALUE(counterGyroSamples, gyro_samples);
if (gyro_samples == 0) {
PIOS_MPU6000_ReadGyros(&mpu6000_data);
error = true;
continue;
}
gyro_scaling = PIOS_MPU6000_GetScale();
accel_scaling = PIOS_MPU6000_GetAccelScale();
gyroSensorData.temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f;
accelSensorData.temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f;
}
#endif /* PIOS_INCLUDE_MPU6000 */
break;
default:
PIOS_DEBUG_Assert(0);
}
if (isnan(accel_temperature)) {
accel_temperature = accelSensorData.temperature;
gyro_temperature = gyroSensorData.temperature;
}
accel_temperature = temp_alpha * (accelSensorData.temperature - accel_temperature) + accel_temperature;
gyro_temperature = temp_alpha * (gyroSensorData.temperature - gyro_temperature) + gyro_temperature;
if ((accel_temp_calibrated || gyro_temp_calibrated) && !temp_calibration_count) {
temp_calibration_count = TEMP_CALIB_INTERVAL;
if (accel_temp_calibrated) {
float ctemp = boundf(accel_temperature, agcal.temp_calibrated_extent.max, agcal.temp_calibrated_extent.min);
accel_temp_bias[0] = agcal.accel_temp_coeff.X * ctemp;
accel_temp_bias[1] = agcal.accel_temp_coeff.Y * ctemp;
accel_temp_bias[2] = agcal.accel_temp_coeff.Z * ctemp;
}
if (gyro_temp_calibrated) {
float ctemp = boundf(gyro_temperature, agcal.temp_calibrated_extent.max, agcal.temp_calibrated_extent.min);
gyro_temp_bias[0] = (agcal.gyro_temp_coeff.X + agcal.gyro_temp_coeff.X2 * ctemp) * ctemp;
gyro_temp_bias[1] = (agcal.gyro_temp_coeff.Y + agcal.gyro_temp_coeff.Y2 * ctemp) * ctemp;
gyro_temp_bias[2] = (agcal.gyro_temp_coeff.Z + agcal.gyro_temp_coeff.Z2 * ctemp) * ctemp;
}
}
temp_calibration_count--;
// Scale the accels
float accels[3] = { (float)accel_accum[0] / accel_samples,
(float)accel_accum[1] / accel_samples,
(float)accel_accum[2] / accel_samples };
float accels_out[3] = { accels[0] * accel_scaling * agcal.accel_scale.X - agcal.accel_bias.X - accel_temp_bias[0],
accels[1] * accel_scaling * agcal.accel_scale.Y - agcal.accel_bias.Y - accel_temp_bias[1],
accels[2] * accel_scaling * agcal.accel_scale.Z - agcal.accel_bias.Z - accel_temp_bias[2] };
if (rotate) {
rot_mult(R, accels_out, accels);
accelSensorData.x = accels[0];
accelSensorData.y = accels[1];
accelSensorData.z = accels[2];
} else {
accelSensorData.x = accels_out[0];
accelSensorData.y = accels_out[1];
accelSensorData.z = accels_out[2];
}
AccelSensorSet(&accelSensorData);
// Scale the gyros
float gyros[3] = { (float)gyro_accum[0] / gyro_samples,
(float)gyro_accum[1] / gyro_samples,
(float)gyro_accum[2] / gyro_samples };
float gyros_out[3] = { gyros[0] * gyro_scaling * agcal.gyro_scale.X - agcal.gyro_bias.X - gyro_temp_bias[0],
gyros[1] * gyro_scaling * agcal.gyro_scale.Y - agcal.gyro_bias.Y - gyro_temp_bias[1],
gyros[2] * gyro_scaling * agcal.gyro_scale.Z - agcal.gyro_bias.Z - gyro_temp_bias[2] };
if (rotate) {
rot_mult(R, gyros_out, gyros);
gyroSensorData.x = gyros[0];
gyroSensorData.y = gyros[1];
gyroSensorData.z = gyros[2];
} else {
gyroSensorData.x = gyros_out[0];
gyroSensorData.y = gyros_out[1];
gyroSensorData.z = gyros_out[2];
}
GyroSensorSet(&gyroSensorData);
// Because most crafts wont get enough information from gravity to zero yaw gyro, we try
// and make it average zero (weakly)
#if defined(PIOS_INCLUDE_HMC5X83)
MagSensorData mag;
if (PIOS_HMC5x83_NewDataAvailable(onboard_mag) || PIOS_DELAY_DiffuS(mag_update_time) > 150000) {
int16_t values[3];
PIOS_HMC5x83_ReadMag(onboard_mag, values);
float mags[3] = { (float)values[1] - mag_bias[0],
(float)values[0] - mag_bias[1],
-(float)values[2] - mag_bias[2] };
float mag_out[3];
rot_mult(mag_transform, mags, mag_out);
mag.x = mag_out[0];
mag.y = mag_out[1];
mag.z = mag_out[2];
MagSensorSet(&mag);
mag_update_time = PIOS_DELAY_GetRaw();
}
#endif /* if defined(PIOS_INCLUDE_HMC5X83) */
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
#endif
vTaskDelayUntil(&lastSysTime, sensor_period_ms / portTICK_RATE_MS);
PERF_MEASURE_PERIOD(counterSensorPeriod);
RELOAD_WDG();
vTaskDelayUntil(&lastSysTime, sensor_period_ticks);
}
}
static void clearContext(sensor_fetch_context *sensor_context)
{
// clear the context once it has finished
for (uint32_t i = 0; i < MAX_SENSORS_PER_INSTANCE; i++) {
sensor_context->accum[i].x = 0;
sensor_context->accum[i].y = 0;
sensor_context->accum[i].z = 0;
}
sensor_context->temperature = 0;
sensor_context->count = 0;
}
static void accumulateSamples(sensor_fetch_context *sensor_context, sensor_data *sample)
{
for (uint32_t i = 0; (i < MAX_SENSORS_PER_INSTANCE) && (i < sample->sensorSample3Axis.count); i++) {
sensor_context->accum[i].x += sample->sensorSample3Axis.sample[i].x;
sensor_context->accum[i].y += sample->sensorSample3Axis.sample[i].y;
sensor_context->accum[i].z += sample->sensorSample3Axis.sample[i].z;
}
sensor_context->temperature += sample->sensorSample3Axis.temperature;
sensor_context->count++;
}
static void processSamples3d(sensor_fetch_context *sensor_context, const PIOS_SENSORS_Instance *sensor)
{
float samples[3];
float temperature;
float scales[MAX_SENSORS_PER_INSTANCE];
PIOS_SENSORS_GetScales(sensor, scales, MAX_SENSORS_PER_INSTANCE);
float inv_count = 1.0f / (float)sensor_context->count;
if ((sensor->type & PIOS_SENSORS_TYPE_3AXIS_ACCEL) ||
(sensor->type == PIOS_SENSORS_TYPE_3AXIS_MAG)) {
float t = inv_count * scales[0];
samples[0] = ((float)sensor_context->accum[0].x * t);
samples[1] = ((float)sensor_context->accum[0].y * t);
samples[2] = ((float)sensor_context->accum[0].z * t);
temperature = (float)sensor_context->temperature * inv_count * 0.01f;
if (sensor->type == PIOS_SENSORS_TYPE_3AXIS_MAG) {
handleMag(samples, temperature);
PERF_MEASURE_PERIOD(counterMagPeriod);
return;
} else {
PERF_TRACK_VALUE(counterAccelSamples, sensor_context->count);
PERF_MEASURE_PERIOD(counterAccelPeriod);
handleAccel(samples, temperature);
}
}
if (sensor->type & PIOS_SENSORS_TYPE_3AXIS_GYRO) {
uint8_t index = 0;
if (sensor->type == PIOS_SENSORS_TYPE_3AXIS_GYRO_ACCEL) {
index = 1;
}
float t = inv_count * scales[index];
samples[0] = ((float)sensor_context->accum[index].x * t);
samples[1] = ((float)sensor_context->accum[index].y * t);
samples[2] = ((float)sensor_context->accum[index].z * t);
temperature = (float)sensor_context->temperature * inv_count * 0.01f;
handleGyro(samples, temperature);
return;
}
}
static void processSamples1d(PIOS_SENSORS_1Axis_SensorsWithTemp *sample, const PIOS_SENSORS_Instance *sensor)
{
switch (sensor->type) {
case PIOS_SENSORS_TYPE_1AXIS_BARO:
PERF_MEASURE_PERIOD(counterBaroPeriod);
handleBaro(sample->sample, sample->temperature);
return;
default:
PIOS_Assert(0);
}
}
static void handleAccel(float *samples, float temperature)
{
AccelSensorData accelSensorData;
updateAccelTempBias(temperature);
float accels_out[3] = { samples[0] * agcal.accel_scale.X - agcal.accel_bias.X - accel_temp_bias[0],
samples[1] * agcal.accel_scale.Y - agcal.accel_bias.Y - accel_temp_bias[1],
samples[2] * agcal.accel_scale.Z - agcal.accel_bias.Z - accel_temp_bias[2] };
rot_mult(R, accels_out, samples);
accelSensorData.x = samples[0];
accelSensorData.y = samples[1];
accelSensorData.z = samples[2];
accelSensorData.temperature = temperature;
AccelSensorSet(&accelSensorData);
}
static void handleGyro(float *samples, float temperature)
{
GyroSensorData gyroSensorData;
updateGyroTempBias(temperature);
float gyros_out[3] = { samples[0] * agcal.gyro_scale.X - agcal.gyro_bias.X - gyro_temp_bias[0],
samples[1] * agcal.gyro_scale.Y - agcal.gyro_bias.Y - gyro_temp_bias[1],
samples[2] * agcal.gyro_scale.Z - agcal.gyro_bias.Z - gyro_temp_bias[2] };
rot_mult(R, gyros_out, samples);
gyroSensorData.temperature = temperature;
gyroSensorData.x = samples[0];
gyroSensorData.y = samples[1];
gyroSensorData.z = samples[2];
GyroSensorSet(&gyroSensorData);
}
static void handleMag(float *samples, float temperature)
{
MagSensorData mag;
float mags[3] = { (float)samples[1] - mag_bias[0],
(float)samples[0] - mag_bias[1],
(float)samples[2] - mag_bias[2] };
rot_mult(mag_transform, mags, samples);
mag.x = samples[0];
mag.y = samples[1];
mag.z = samples[2];
mag.temperature = temperature;
MagSensorSet(&mag);
}
static void handleBaro(float sample, float temperature)
{
updateBaroTempBias(temperature);
sample -= baro_temp_bias;
float altitude = 44330.0f * (1.0f - powf((sample) / PIOS_CONST_MKS_STD_ATMOSPHERE_F, (1.0f / 5.255f)));
if (!isnan(altitude)) {
BaroSensorData data;
data.Altitude = altitude;
data.Temperature = temperature;
data.Pressure = sample;
// Update the BasoSensor UAVObject
BaroSensorSet(&data);
}
}
static void updateAccelTempBias(float temperature)
{
if (isnan(accel_temperature)) {
accel_temperature = temperature;
}
accel_temperature = temp_alpha_gyro_accel * (temperature - accel_temperature) + accel_temperature;
if ((accel_temp_calibrated) && !accel_temp_calibration_count) {
accel_temp_calibration_count = TEMP_CALIB_INTERVAL;
if (accel_temp_calibrated) {
float ctemp = boundf(accel_temperature, agcal.temp_calibrated_extent.max, agcal.temp_calibrated_extent.min);
accel_temp_bias[0] = agcal.accel_temp_coeff.X * ctemp;
accel_temp_bias[1] = agcal.accel_temp_coeff.Y * ctemp;
accel_temp_bias[2] = agcal.accel_temp_coeff.Z * ctemp;
}
}
accel_temp_calibration_count--;
}
static void updateGyroTempBias(float temperature)
{
if (isnan(gyro_temperature)) {
gyro_temperature = temperature;
}
gyro_temperature = temp_alpha_gyro_accel * (temperature - gyro_temperature) + gyro_temperature;
if (gyro_temp_calibrated && !gyro_temp_calibration_count) {
gyro_temp_calibration_count = TEMP_CALIB_INTERVAL;
if (gyro_temp_calibrated) {
float ctemp = boundf(gyro_temperature, agcal.temp_calibrated_extent.max, agcal.temp_calibrated_extent.min);
gyro_temp_bias[0] = (agcal.gyro_temp_coeff.X + agcal.gyro_temp_coeff.X2 * ctemp) * ctemp;
gyro_temp_bias[1] = (agcal.gyro_temp_coeff.Y + agcal.gyro_temp_coeff.Y2 * ctemp) * ctemp;
gyro_temp_bias[2] = (agcal.gyro_temp_coeff.Z + agcal.gyro_temp_coeff.Z2 * ctemp) * ctemp;
}
}
gyro_temp_calibration_count--;
}
static void updateBaroTempBias(float temperature)
{
if (isnan(baro_temperature)) {
baro_temperature = temperature;
}
baro_temperature = temp_alpha_baro * (temperature - baro_temperature) + baro_temperature;
if (baro_temp_correction_enabled && !baro_temp_calibration_count) {
baro_temp_calibration_count = BARO_TEMP_CALIB_INTERVAL;
// pressure bias = A + B*t + C*t^2 + D * t^3
// in case the temperature is outside of the calibrated range, uses the nearest extremes
float ctemp = boundf(baro_temperature, baroCorrectionExtent.max, baroCorrectionExtent.min);
baro_temp_bias = baroCorrection.a + ((baroCorrection.d * ctemp + baroCorrection.c) * ctemp + baroCorrection.b) * ctemp;
}
baro_temp_calibration_count--;
}
/**
* Locally cache some variables from the AtttitudeSettings object
*/
@ -533,6 +585,11 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
Quaternion2R(rotationQuat, R);
}
matrix_mult_3x3f((float(*)[3])RevoCalibrationmag_transformToArray(cal.mag_transform), R, mag_transform);
RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection);
RevoSettingsBaroTempCorrectionExtentGet(&baroCorrectionExtent);
baro_temp_correction_enabled = !(baroCorrectionExtent.max - baroCorrectionExtent.min < 0.1f ||
(baroCorrection.a < 1e-9f && baroCorrection.b < 1e-9f && baroCorrection.c < 1e-9f && baroCorrection.d < 1e-9f));
}
/**
* @}

1
flight/modules/System/systemmod.c
View File

@ -267,7 +267,6 @@ static void systemTask(__attribute__((unused)) void *parameters)
oplinkStatus.RxMissed = radio_stats.rx_missed;
oplinkStatus.RxFailure = radio_stats.rx_failure;
oplinkStatus.TxDropped = radio_stats.tx_dropped;
oplinkStatus.TxResent = radio_stats.tx_resent;
oplinkStatus.TxFailure = radio_stats.tx_failure;
oplinkStatus.Resets = radio_stats.resets;
oplinkStatus.Timeouts = radio_stats.timeouts;

2
flight/pios/common/pios_adxl345.c
View File

@ -29,7 +29,7 @@
*/
#include "pios.h"
#include <pios_adxl345.h>
#ifdef PIOS_INCLUDE_ADXL345
enum pios_adxl345_dev_magic {

2
flight/pios/common/pios_bma180.c
View File

@ -30,7 +30,7 @@
*/
#include "pios.h"
#include <pios_bma180.h>
#ifdef PIOS_INCLUDE_BMA180
#include "fifo_buffer.h"

2
flight/pios/common/pios_bmp085.c
View File

@ -29,7 +29,7 @@
*/
#include "pios.h"
#include <pios_bmp085.h>
#ifdef PIOS_INCLUDE_BMP085
#ifndef PIOS_INCLUDE_EXTI

52
flight/pios/common/pios_hmc5x83.c
View File

@ -50,6 +50,22 @@ typedef struct {
static int32_t PIOS_HMC5x83_Config(pios_hmc5x83_dev_data_t *dev);
// sensor driver interface
bool PIOS_HMC5x83_driver_Test(uintptr_t context);
void PIOS_HMC5x83_driver_Reset(uintptr_t context);
void PIOS_HMC5x83_driver_get_scale(float *scales, uint8_t size, uintptr_t context);
void PIOS_HMC5x83_driver_fetch(void *, uint8_t size, uintptr_t context);
bool PIOS_HMC5x83_driver_poll(uintptr_t context);
const PIOS_SENSORS_Driver PIOS_HMC5x83_Driver = {
.test = PIOS_HMC5x83_driver_Test,
.poll = PIOS_HMC5x83_driver_poll,
.fetch = PIOS_HMC5x83_driver_fetch,
.reset = PIOS_HMC5x83_driver_Reset,
.get_queue = NULL,
.get_scale = PIOS_HMC5x83_driver_get_scale,
.is_polled = true,
};
/**
* Allocate the device setting structure
* @return pios_hmc5x83_dev_data_t pointer to newly created structure
@ -99,6 +115,11 @@ pios_hmc5x83_dev_t PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_
return (pios_hmc5x83_dev_t)dev;
}
void PIOS_HMC5x83_Register(pios_hmc5x83_dev_t handler)
{
PIOS_SENSORS_Register(&PIOS_HMC5x83_Driver, PIOS_SENSORS_TYPE_3AXIS_MAG, handler);
}
/**
* @brief Initialize the HMC5x83 magnetometer sensor
* \return none
@ -549,6 +570,37 @@ int32_t PIOS_HMC5x83_I2C_Write(pios_hmc5x83_dev_t handler, uint8_t address, uint
}
#endif /* PIOS_INCLUDE_I2C */
/* PIOS sensor driver implementation */
bool PIOS_HMC5x83_driver_Test(uintptr_t context)
{
return !PIOS_HMC5x83_Test((pios_hmc5x83_dev_t)context);
}
void PIOS_HMC5x83_driver_Reset(__attribute__((unused)) uintptr_t context) {}
void PIOS_HMC5x83_driver_get_scale(float *scales, uint8_t size, __attribute__((unused)) uintptr_t context)
{
PIOS_Assert(size > 0);
scales[0] = 1;
}
void PIOS_HMC5x83_driver_fetch(void *data, uint8_t size, uintptr_t context)
{
PIOS_Assert(size > 0);
int16_t mag[3];
PIOS_HMC5x83_ReadMag((pios_hmc5x83_dev_t)context, mag);
PIOS_SENSORS_3Axis_SensorsWithTemp *tmp = data;
tmp->count = 1;
tmp->sample[0].x = mag[0];
tmp->sample[0].y = mag[1];
tmp->sample[0].z = mag[2];
tmp->temperature = 0;
}
bool PIOS_HMC5x83_driver_poll(uintptr_t context)
{
return PIOS_HMC5x83_NewDataAvailable((pios_hmc5x83_dev_t)context);
}
#endif /* PIOS_INCLUDE_HMC5x83 */

4
flight/pios/common/pios_instrumentation.c
View File

@ -52,8 +52,8 @@ pios_counter_t PIOS_Instrumentation_CreateCounter(uint32_t id)
if (!counter_handle) {
pios_perf_counter_t *newcounter = &pios_instrumentation_perf_counters[++pios_instrumentation_last_used_counter];
newcounter->id = id;
newcounter->max = INT32_MIN;
newcounter->min = INT32_MAX;
newcounter->max = INT32_MIN + 1;
newcounter->min = INT32_MAX - 1;
counter_handle = (pios_counter_t)newcounter;
}
return counter_handle;

2
flight/pios/common/pios_l3gd20.c
View File

@ -30,7 +30,7 @@
*/
#include "pios.h"
#include <pios_l3gd20.h>
#ifdef PIOS_INCLUDE_L3GD20
#include "fifo_buffer.h"

317
flight/pios/common/pios_mpu6000.c
View File

@ -1,6 +1,6 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @addtogroup PIOS PIOS Core haoftware; you can rnedtt
* @{
* @addtogroup PIOS_MPU6000 MPU6000 Functions
* @brief Deals with the hardware interface to the 3-axis gyro
@ -13,8 +13,8 @@
*
******************************************************************************
*/
/*
* This program is free software; you can redistribute it and/or modify
/*istribu
* This program is free software; you can rnedtt ad/oe ir modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
@ -30,20 +30,40 @@
*/
#include "pios.h"
#include <pios_mpu6000.h>
#ifdef PIOS_INCLUDE_MPU6000
#include <stdint.h>
#include <pios_constants.h>
#include <pios_sensors.h>
/* Global Variables */
enum pios_mpu6000_dev_magic {
PIOS_MPU6000_DEV_MAGIC = 0x9da9b3ed,
};
// sensor driver interface
bool PIOS_MPU6000_driver_Test(uintptr_t context);
void PIOS_MPU6000_driver_Reset(uintptr_t context);
void PIOS_MPU6000_driver_get_scale(float *scales, uint8_t size, uintptr_t context);
QueueHandle_t PIOS_MPU6000_driver_get_queue(uintptr_t context);
const PIOS_SENSORS_Driver PIOS_MPU6000_Driver = {
.test = PIOS_MPU6000_driver_Test,
.poll = NULL,
.fetch = NULL,
.reset = PIOS_MPU6000_driver_Reset,
.get_queue = PIOS_MPU6000_driver_get_queue,
.get_scale = PIOS_MPU6000_driver_get_scale,
.is_polled = false,
};
//
struct mpu6000_dev {
uint32_t spi_id;
uint32_t slave_num;
xQueueHandle queue;
uint32_t spi_id;
uint32_t slave_num;
QueueHandle_t queue;
const struct pios_mpu6000_cfg *cfg;
enum pios_mpu6000_range gyro_range;
enum pios_mpu6000_accel_range accel_range;
@ -51,26 +71,19 @@ struct mpu6000_dev {
enum pios_mpu6000_dev_magic magic;
};
#ifdef PIOS_MPU6000_ACCEL
#define PIOS_MPU6000_SAMPLES_BYTES 14
#define PIOS_MPU6000_SENSOR_FIRST_REG PIOS_MPU6000_ACCEL_X_OUT_MSB
#else
#define PIOS_MPU6000_SENSOR_FIRST_REG PIOS_MPU6000_TEMP_OUT_MSB
#define PIOS_MPU6000_SAMPLES_BYTES 8
#endif
typedef union {
uint8_t buffer[1 + PIOS_MPU6000_SAMPLES_BYTES];
struct {
uint8_t dummy;
#ifdef PIOS_MPU6000_ACCEL
uint8_t Accel_X_h;
uint8_t Accel_X_l;
uint8_t Accel_Y_h;
uint8_t Accel_Y_l;
uint8_t Accel_Z_h;
uint8_t Accel_Z_l;
#endif
uint8_t Temperature_h;
uint8_t Temperature_l;
uint8_t Gyro_X_h;
@ -88,7 +101,9 @@ typedef union {
static struct mpu6000_dev *dev;
volatile bool mpu6000_configured = false;
static mpu6000_data_t mpu6000_data;
static PIOS_SENSORS_3Axis_SensorsWithTemp *queue_data = 0;
#define SENSOR_COUNT 2
#define SENSOR_DATA_SIZE (sizeof(PIOS_SENSORS_3Axis_SensorsWithTemp) + sizeof(Vector3i16) * SENSOR_COUNT)
// ! Private functions
static struct mpu6000_dev *PIOS_MPU6000_alloc(const struct pios_mpu6000_cfg *cfg);
static int32_t PIOS_MPU6000_Validate(struct mpu6000_dev *dev);
@ -97,8 +112,14 @@ static int32_t PIOS_MPU6000_SetReg(uint8_t address, uint8_t buffer);
static int32_t PIOS_MPU6000_GetReg(uint8_t address);
static void PIOS_MPU6000_SetSpeed(const bool fast);
static bool PIOS_MPU6000_HandleData();
static bool PIOS_MPU6000_ReadFifo(bool *woken);
static bool PIOS_MPU6000_ReadSensor(bool *woken);
static int32_t PIOS_MPU6000_Test(void);
void PIOS_MPU6000_Register()
{
PIOS_SENSORS_Register(&PIOS_MPU6000_Driver, PIOS_SENSORS_TYPE_3AXIS_GYRO_ACCEL, 0);
}
/**
* @brief Allocate a new device
*/
@ -107,18 +128,16 @@ static struct mpu6000_dev *PIOS_MPU6000_alloc(const struct pios_mpu6000_cfg *cfg
struct mpu6000_dev *mpu6000_dev;
mpu6000_dev = (struct mpu6000_dev *)pios_malloc(sizeof(*mpu6000_dev));
if (!mpu6000_dev) {
return NULL;
}
PIOS_Assert(mpu6000_dev);
mpu6000_dev->magic = PIOS_MPU6000_DEV_MAGIC;
mpu6000_dev->queue = xQueueCreate(cfg->max_downsample + 1, sizeof(struct pios_mpu6000_data));
if (mpu6000_dev->queue == NULL) {
vPortFree(mpu6000_dev);
return NULL;
}
mpu6000_dev->queue = xQueueCreate(cfg->max_downsample + 1, SENSOR_DATA_SIZE);
PIOS_Assert(mpu6000_dev->queue);
queue_data = (PIOS_SENSORS_3Axis_SensorsWithTemp *)pios_malloc(SENSOR_DATA_SIZE);
PIOS_Assert(queue_data);
queue_data->count = SENSOR_COUNT;
return mpu6000_dev;
}
@ -211,15 +230,9 @@ static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const *cfg)
}
// FIFO storage
#if defined(PIOS_MPU6000_ACCEL)
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_FIFO_EN_REG, cfg->Fifo_store | PIOS_MPU6000_ACCEL_OUT) != 0) {
;
}
#else
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_FIFO_EN_REG, cfg->Fifo_store) != 0) {
;
}
#endif
PIOS_MPU6000_ConfigureRanges(cfg->gyro_range, cfg->accel_range, cfg->filter);
// Interrupt configuration
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_USER_CTRL_REG, cfg->User_ctl) != 0) {
@ -279,14 +292,12 @@ int32_t PIOS_MPU6000_ConfigureRanges(
}
dev->gyro_range = gyroRange;
#if defined(PIOS_MPU6000_ACCEL)
// Set the accel range
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_ACCEL_CFG_REG, accelRange) != 0) {
;
}
dev->accel_range = accelRange;
#endif
return 0;
}
@ -414,11 +425,10 @@ static int32_t PIOS_MPU6000_SetReg(uint8_t reg, uint8_t data)
}
/**
* @brief Read current X, Z, Y values (in that order)
* \param[out] int16_t array of size 3 to store X, Z, and Y magnetometer readings
* @brief Perform a dummy read in order to restart interrupt generation
* \returns 0 if succesful
*/
int32_t PIOS_MPU6000_ReadGyros(struct pios_mpu6000_data *data)
int32_t PIOS_MPU6000_DummyReadGyros()
{
// THIS FUNCTION IS DEPRECATED AND DOES NOT PERFORM A ROTATION
uint8_t buf[7] = { PIOS_MPU6000_GYRO_X_OUT_MSB | 0x80, 0, 0, 0, 0, 0, 0 };
@ -434,10 +444,6 @@ int32_t PIOS_MPU6000_ReadGyros(struct pios_mpu6000_data *data)
PIOS_MPU6000_ReleaseBus();
data->gyro_x = rec[1] << 8 | rec[2];
data->gyro_y = rec[3] << 8 | rec[4];
data->gyro_z = rec[5] << 8 | rec[6];
return 0;
}
@ -469,7 +475,7 @@ xQueueHandle PIOS_MPU6000_GetQueue()
}
float PIOS_MPU6000_GetScale()
static float PIOS_MPU6000_GetScale()
{
switch (dev->gyro_range) {
case PIOS_MPU6000_SCALE_250_DEG:
@ -487,7 +493,7 @@ float PIOS_MPU6000_GetScale()
return 0;
}
float PIOS_MPU6000_GetAccelScale()
static float PIOS_MPU6000_GetAccelScale()
{
switch (dev->accel_range) {
case PIOS_MPU6000_ACCEL_2G:
@ -510,7 +516,7 @@ float PIOS_MPU6000_GetAccelScale()
* \return 0 if test succeeded
* \return non-zero value if test succeeded
*/
int32_t PIOS_MPU6000_Test(void)
static int32_t PIOS_MPU6000_Test(void)
{
/* Verify that ID matches (MPU6000 ID is 0x69) */
int32_t mpu6000_id = PIOS_MPU6000_ReadID();
@ -526,172 +532,76 @@ int32_t PIOS_MPU6000_Test(void)
return 0;
}
/**
* @brief Reads the contents of the MPU6000 Interrupt Status register from an ISR
* @return The register value or -1 on failure to claim the bus
*/
static int32_t PIOS_MPU6000_GetInterruptStatusRegISR(bool *woken)
{
/* Interrupt Status register can be read at high SPI clock speed */
uint8_t data;
if (PIOS_MPU6000_ClaimBusISR(woken, false) != 0) {
return -1;
}
PIOS_SPI_TransferByte(dev->spi_id, (0x80 | PIOS_MPU6000_INT_STATUS_REG));
data = PIOS_SPI_TransferByte(dev->spi_id, 0);
PIOS_MPU6000_ReleaseBusISR(woken);
return data;
}
/**
* @brief Resets the MPU6000 FIFO from an ISR
* @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
* task has is now eligible to run, else unchanged
* @return 0 if operation was successful
* @return -1 if unable to claim SPI bus
* @return -2 if write to the device failed
*/
static int32_t PIOS_MPU6000_ResetFifoISR(bool *woken)
{
int32_t result = 0;
if (PIOS_MPU6000_ClaimBusISR(woken, false) != 0) {
return -1;
}
/* Reset FIFO. */
if (PIOS_SPI_TransferByte(dev->spi_id, 0x7f & PIOS_MPU6000_USER_CTRL_REG) != 0) {
result = -2;
} else if (PIOS_SPI_TransferByte(dev->spi_id, (dev->cfg->User_ctl | PIOS_MPU6000_USERCTL_FIFO_RST)) != 0) {
result = -2;
}
PIOS_MPU6000_ReleaseBusISR(woken);
return result;
}
/**
* @brief Obtains the number of bytes in the FIFO. Call from ISR only.
* @return the number of bytes in the FIFO
* @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
* task has is now eligible to run, else unchanged
*/
static int32_t PIOS_MPU6000_FifoDepthISR(bool *woken)
{
uint8_t mpu6000_send_buf[3] = { PIOS_MPU6000_FIFO_CNT_MSB | 0x80, 0, 0 };
uint8_t mpu6000_rec_buf[3];
if (PIOS_MPU6000_ClaimBusISR(woken, false) != 0) {
return -1;
}
if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) {
PIOS_MPU6000_ReleaseBusISR(woken);
return -1;
}
PIOS_MPU6000_ReleaseBusISR(woken);
return (mpu6000_rec_buf[1] << 8) | mpu6000_rec_buf[2];
}
/**
* @brief EXTI IRQ Handler. Read all the data from onboard buffer
* @return a boolean to the EXTI IRQ Handler wrapper indicating if a
* @return a boleoan to the EXTI IRQ Handler wrapper indicating if a
* higher priority task is now eligible to run
*/
uint32_t mpu6000_irq = 0;
int32_t mpu6000_count;
uint32_t mpu6000_fifo_backup = 0;
uint8_t mpu6000_last_read_count = 0;
uint32_t mpu6000_fails = 0;
uint32_t mpu6000_interval_us;
uint32_t mpu6000_time_us;
uint32_t mpu6000_transfer_size;
bool PIOS_MPU6000_IRQHandler(void)
{
bool woken = false;
static uint32_t timeval;
mpu6000_interval_us = PIOS_DELAY_DiffuS(timeval);
timeval = PIOS_DELAY_GetRaw();
if (!mpu6000_configured) {
return false;
}
bool read_ok = false;
if (dev->cfg->User_ctl & PIOS_MPU6000_USERCTL_FIFO_EN) {
read_ok = PIOS_MPU6000_ReadFifo(&woken);
} else {
read_ok = PIOS_MPU6000_ReadSensor(&woken);
}
read_ok = PIOS_MPU6000_ReadSensor(&woken);
if (read_ok) {
bool woken2 = PIOS_MPU6000_HandleData();
woken |= woken2;
}
mpu6000_irq++;
mpu6000_time_us = PIOS_DELAY_DiffuS(timeval);
return woken;
}
static bool PIOS_MPU6000_HandleData()
{
if (!queue_data) {
return false;
}
// Rotate the sensor to OP convention. The datasheet defines X as towards the right
// and Y as forward. OP convention transposes this. Also the Z is defined negatively
// to our convention
static struct pios_mpu6000_data data;
// Currently we only support rotations on top so switch X/Y accordingly
switch (dev->cfg->orientation) {
case PIOS_MPU6000_TOP_0DEG:
#ifdef PIOS_MPU6000_ACCEL
data.accel_y = GET_SENSOR_DATA(mpu6000_data, Accel_X); // chip X
data.accel_x = GET_SENSOR_DATA(mpu6000_data, Accel_Y); // chip Y
#endif
data.gyro_y = GET_SENSOR_DATA(mpu6000_data, Gyro_X); // chip X
data.gyro_x = GET_SENSOR_DATA(mpu6000_data, Gyro_Y); // chip Y
queue_data->sample[0].y = GET_SENSOR_DATA(mpu6000_data, Accel_X); // chip X
queue_data->sample[0].x = GET_SENSOR_DATA(mpu6000_data, Accel_Y); // chip Y
queue_data->sample[1].y = GET_SENSOR_DATA(mpu6000_data, Gyro_X); // chip X
queue_data->sample[1].x = GET_SENSOR_DATA(mpu6000_data, Gyro_Y); // chip Y
break;
case PIOS_MPU6000_TOP_90DEG:
// -1 to bring it back to -32768 +32767 range
#ifdef PIOS_MPU6000_ACCEL
data.accel_y = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Y)); // chip Y
data.accel_x = GET_SENSOR_DATA(mpu6000_data, Accel_X); // chip X
#endif
data.gyro_y = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Y)); // chip Y
data.gyro_x = GET_SENSOR_DATA(mpu6000_data, Gyro_X); // chip X
queue_data->sample[0].y = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Y)); // chip Y
queue_data->sample[0].x = GET_SENSOR_DATA(mpu6000_data, Accel_X); // chip X
queue_data->sample[1].y = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Y)); // chip Y
queue_data->sample[1].x = GET_SENSOR_DATA(mpu6000_data, Gyro_X); // chip X
break;
case PIOS_MPU6000_TOP_180DEG:
#ifdef PIOS_MPU6000_ACCEL
data.accel_y = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_X)); // chip X
data.accel_x = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Y)); // chip Y
#endif
data.gyro_y = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_X)); // chip X
data.gyro_x = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Y)); // chip Y
queue_data->sample[0].y = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_X)); // chip X
queue_data->sample[0].x = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Y)); // chip Y
queue_data->sample[1].y = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_X)); // chip X
queue_data->sample[1].x = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Y)); // chip Y
break;
case PIOS_MPU6000_TOP_270DEG:
#ifdef PIOS_MPU6000_ACCEL
data.accel_y = GET_SENSOR_DATA(mpu6000_data, Accel_Y); // chip Y
data.accel_x = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_X)); // chip X
#endif
data.gyro_y = GET_SENSOR_DATA(mpu6000_data, Gyro_Y); // chip Y
data.gyro_x = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_X)); // chip X
queue_data->sample[0].y = GET_SENSOR_DATA(mpu6000_data, Accel_Y); // chip Y
queue_data->sample[0].x = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_X)); // chip X
queue_data->sample[1].y = GET_SENSOR_DATA(mpu6000_data, Gyro_Y); // chip Y
queue_data->sample[1].x = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_X)); // chip X
break;
}
#ifdef PIOS_MPU6000_ACCEL
data.accel_z = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Z));
#endif
data.gyro_z = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Z));
data.temperature = GET_SENSOR_DATA(mpu6000_data, Temperature);
queue_data->sample[0].z = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Z));
queue_data->sample[1].z = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Z));
const int16_t temp = GET_SENSOR_DATA(mpu6000_data, Temperature);
queue_data->temperature = 3500 + ((float)(temp + 512)) * (1.0f / 3.4f);
BaseType_t higherPriorityTaskWoken;
xQueueSendToBackFromISR(dev->queue, (void *)&data, &higherPriorityTaskWoken);
xQueueSendToBackFromISR(dev->queue, (void *)queue_data, &higherPriorityTaskWoken);
return higherPriorityTaskWoken == pdTRUE;
}
@ -704,72 +614,33 @@ static bool PIOS_MPU6000_ReadSensor(bool *woken)
}
if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_data.buffer[0], sizeof(mpu6000_data_t), NULL) < 0) {
PIOS_MPU6000_ReleaseBusISR(woken);
mpu6000_fails++;
return false;
}
PIOS_MPU6000_ReleaseBusISR(woken);
return true;
}
static bool PIOS_MPU6000_ReadFifo(bool *woken)
// Sensor driver implementation
bool PIOS_MPU6000_driver_Test(__attribute__((unused)) uintptr_t context)
{
/* Temporary fix for OP-1049. Expected to be superceded for next major release
* by code changes for OP-1039.
* Read interrupt status register to check for FIFO overflow. Must be the
* first read after interrupt, in case the device is configured so that
* any read clears in the status register (PIOS_MPU6000_INT_CLR_ANYRD set in
* interrupt config register) */
int32_t result;
return !PIOS_MPU6000_Test();
}
if ((result = PIOS_MPU6000_GetInterruptStatusRegISR(woken)) < 0) {
return false;
}
if (result & PIOS_MPU6000_INT_STATUS_FIFO_OVERFLOW) {
/* The FIFO has overflowed, so reset it,
* to enable sample sync to be recovered.
* If the reset fails, we are in trouble, but
* we keep trying on subsequent interrupts. */
PIOS_MPU6000_ResetFifoISR(woken);
/* Return and wait for the next new sample. */
return false;
}
void PIOS_MPU6000_driver_Reset(__attribute__((unused)) uintptr_t context)
{
PIOS_MPU6000_DummyReadGyros();
}
/* Usual case - FIFO has not overflowed. */
mpu6000_count = PIOS_MPU6000_FifoDepthISR(woken);
if (mpu6000_count < PIOS_MPU6000_SAMPLES_BYTES) {
return false;
}
void PIOS_MPU6000_driver_get_scale(float *scales, uint8_t size, __attribute__((unused)) uintptr_t contet)
{
PIOS_Assert(size >= 2);
scales[0] = PIOS_MPU6000_GetAccelScale();
scales[1] = PIOS_MPU6000_GetScale();
}
if (PIOS_MPU6000_ClaimBusISR(woken, true) != 0) {
return false;
}
const uint8_t mpu6000_send_buf[1 + PIOS_MPU6000_SAMPLES_BYTES] = { PIOS_MPU6000_FIFO_REG | 0x80 };
if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_data.buffer[0], sizeof(mpu6000_data_t), NULL) < 0) {
PIOS_MPU6000_ReleaseBusISR(woken);
mpu6000_fails++;
return false;
}
PIOS_MPU6000_ReleaseBusISR(woken);
// In the case where extras samples backed up grabbed an extra
if (mpu6000_count >= PIOS_MPU6000_SAMPLES_BYTES * 2) {
mpu6000_fifo_backup++;
if (PIOS_MPU6000_ClaimBusISR(woken, true) != 0) {
return false;
}
if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_data.buffer[0], sizeof(mpu6000_data_t), NULL) < 0) {
PIOS_MPU6000_ReleaseBusISR(woken);
mpu6000_fails++;
return false;
}
PIOS_MPU6000_ReleaseBusISR(woken);
}
return true;
QueueHandle_t PIOS_MPU6000_driver_get_queue(__attribute__((unused)) uintptr_t context)
{
return dev->queue;
}
#endif /* PIOS_INCLUDE_MPU6000 */

179
flight/pios/common/pios_ms5611.c
View File

@ -29,17 +29,40 @@
*/
#include "pios.h"
#ifdef PIOS_INCLUDE_MS5611
#include <pios_ms5611.h>
#define POW2(x) (1 << x)
// TODO: Clean this up. Getting around old constant.
#define PIOS_MS5611_OVERSAMPLING oversampling
#define PIOS_MS5611_OVERSAMPLING oversampling
// Option to change the interleave between Temp and Pressure conversions
// Undef for normal operation
#define PIOS_MS5611_SLOW_TEMP_RATE 20
#ifndef PIOS_MS5611_SLOW_TEMP_RATE
#define PIOS_MS5611_SLOW_TEMP_RATE 1
#endif
/* Local Types */
typedef struct {
uint16_t C[6];
} MS5611CalibDataTypeDef;
typedef enum {
MS5611_CONVERSION_TYPE_None = 0,
MS5611_CONVERSION_TYPE_PressureConv,
MS5611_CONVERSION_TYPE_TemperatureConv
} ConversionTypeTypeDef;
typedef enum {
MS5611_FSM_INIT = 0,
MS5611_FSM_TEMPERATURE,
MS5611_FSM_PRESSURE,
MS5611_FSM_CALCULATE,
} MS5611_FSM_State;
/* Glocal Variables */
ConversionTypeTypeDef CurrentRead;
ConversionTypeTypeDef CurrentRead = MS5611_CONVERSION_TYPE_None;
/* Local Variables */
MS5611CalibDataTypeDef CalibData;
@ -50,8 +73,14 @@ static uint32_t RawPressure;
static int64_t Pressure;
static int64_t Temperature;
static int32_t lastConversionStart;
static uint32_t conversionDelayMs;
static uint32_t conversionDelayUs;
static int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len);
static int32_t PIOS_MS5611_WriteCommand(uint8_t command);
static uint32_t PIOS_MS5611_GetDelay();
static uint32_t PIOS_MS5611_GetDelayUs();
// Second order temperature compensation. Temperature offset
static int64_t compensation_t2;
@ -60,17 +89,36 @@ static int64_t compensation_t2;
static uint32_t oversampling;
static const struct pios_ms5611_cfg *dev_cfg;
static int32_t i2c_id;
static PIOS_SENSORS_1Axis_SensorsWithTemp results;
// sensor driver interface
bool PIOS_MS5611_driver_Test(uintptr_t context);
void PIOS_MS5611_driver_Reset(uintptr_t context);
void PIOS_MS5611_driver_get_scale(float *scales, uint8_t size, uintptr_t context);
void PIOS_MS5611_driver_fetch(void *, uint8_t size, uintptr_t context);
bool PIOS_MS5611_driver_poll(uintptr_t context);
const PIOS_SENSORS_Driver PIOS_MS5611_Driver = {
.test = PIOS_MS5611_driver_Test,
.poll = PIOS_MS5611_driver_poll,
.fetch = PIOS_MS5611_driver_fetch,
.reset = PIOS_MS5611_driver_Reset,
.get_queue = NULL,
.get_scale = PIOS_MS5611_driver_get_scale,
.is_polled = true,
};
/**
* Initialise the MS5611 sensor
*/
int32_t ms5611_read_flag;
void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device)
{
i2c_id = i2c_device;
i2c_id = i2c_device;
oversampling = cfg->oversampling;
conversionDelayMs = PIOS_MS5611_GetDelay();
conversionDelayUs = PIOS_MS5611_GetDelayUs();
dev_cfg = cfg; // Store cfg before enabling interrupt
PIOS_MS5611_WriteCommand(MS5611_RESET);
@ -80,10 +128,10 @@ void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device)
// reset temperature compensation values
compensation_t2 = 0;
/* Calibration parameters */
for (int i = 0; i < 6; i++) {
PIOS_MS5611_Read(MS5611_CALIB_ADDR + i * 2, data, 2);
while (PIOS_MS5611_Read(MS5611_CALIB_ADDR + i * 2, data, 2)) {}
;
CalibData.C[i] = (data[0] << 8) | data[1];
}
}
@ -96,11 +144,11 @@ void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device)
int32_t PIOS_MS5611_StartADC(ConversionTypeTypeDef Type)
{
/* Start the conversion */
if (Type == TemperatureConv) {
if (Type == MS5611_CONVERSION_TYPE_TemperatureConv) {
while (PIOS_MS5611_WriteCommand(MS5611_TEMP_ADDR + oversampling) != 0) {
continue;
}
} else if (Type == PressureConv) {
} else if (Type == MS5611_CONVERSION_TYPE_PressureConv) {
while (PIOS_MS5611_WriteCommand(MS5611_PRES_ADDR + oversampling) != 0) {
continue;
}
@ -114,7 +162,7 @@ int32_t PIOS_MS5611_StartADC(ConversionTypeTypeDef Type)
/**
* @brief Return the delay for the current osr
*/
int32_t PIOS_MS5611_GetDelay()
static uint32_t PIOS_MS5611_GetDelay()
{
switch (oversampling) {
case MS5611_OSR_256:
@ -141,7 +189,7 @@ int32_t PIOS_MS5611_GetDelay()
/**
* @brief Return the delay for the current osr in uS
*/
uint32_t PIOS_MS5611_GetDelayUs()
static uint32_t PIOS_MS5611_GetDelayUs()
{
switch (oversampling) {
case MS5611_OSR_256:
@ -167,7 +215,7 @@ uint32_t PIOS_MS5611_GetDelayUs()
/**
* Read the ADC conversion value (once ADC conversion has completed)
* \return 0 if successfully read the ADC, -1 if failed
* \return 0 if successfully read the ADC, -1 if conversion time has not elapsed, -2 if failure occurred
*/
int32_t PIOS_MS5611_ReadADC(void)
{
@ -177,16 +225,19 @@ int32_t PIOS_MS5611_ReadADC(void)
Data[1] = 0;
Data[2] = 0;
while (PIOS_MS5611_GetDelayUs() > PIOS_DELAY_DiffuS(lastConversionStart)) {
vTaskDelay(0);
if (CurrentRead == MS5611_CONVERSION_TYPE_None) {
return -2;
}
if (conversionDelayUs > PIOS_DELAY_DiffuS(lastConversionStart)) {
return -1;
}
static int64_t deltaTemp;
/* Read and store the 16bit result */
if (CurrentRead == TemperatureConv) {
if (CurrentRead == MS5611_CONVERSION_TYPE_TemperatureConv) {
/* Read the temperature conversion */
if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) {
return -1;
return -2;
}
RawTemperature = (Data[0] << 16) | (Data[1] << 8) | Data[2];
@ -205,7 +256,7 @@ int32_t PIOS_MS5611_ReadADC(void)
/* Read the pressure conversion */
if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) {
return -1;
return -2;
}
// check if temperature is less than 20°C
if (Temperature < 2000) {
@ -248,18 +299,18 @@ int32_t PIOS_MS5611_ReadADC(void)
/**
* Return the most recently computed temperature in kPa
*/
float PIOS_MS5611_GetTemperature(void)
static float PIOS_MS5611_GetTemperature(void)
{
// Apply the second order low and very low temperature compensation offset
return ((float)(Temperature - compensation_t2)) / 100.0f;
}
/**
* Return the most recently computed pressure in kPa
* Return the most recently computed pressure in Pa
*/
float PIOS_MS5611_GetPressure(void)
static float PIOS_MS5611_GetPressure(void)
{
return ((float)Pressure) / 1000.0f;
return (float)Pressure;
}
/**
@ -270,7 +321,7 @@ float PIOS_MS5611_GetPressure(void)
* \return 0 if operation was successful
* \return -1 if error during I2C transfer
*/
int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len)
static int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len)
{
const struct pios_i2c_txn txn_list[] = {
{
@ -300,7 +351,7 @@ int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len)
* \return 0 if operation was successful
* \return -1 if error during I2C transfer
*/
int32_t PIOS_MS5611_WriteCommand(uint8_t command)
static int32_t PIOS_MS5611_WriteCommand(uint8_t command)
{
const struct pios_i2c_txn txn_list[] = {
{
@ -326,16 +377,16 @@ int32_t PIOS_MS5611_Test()
int32_t cur_value = 0;
cur_value = Temperature;
PIOS_MS5611_StartADC(TemperatureConv);
PIOS_DELAY_WaitmS(5);
PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_TemperatureConv);
PIOS_DELAY_WaitmS(10);
PIOS_MS5611_ReadADC();
if (cur_value == Temperature) {
return -1;
}
cur_value = Pressure;
PIOS_MS5611_StartADC(PressureConv);
PIOS_DELAY_WaitmS(26);
PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_PressureConv);
PIOS_DELAY_WaitmS(10);
PIOS_MS5611_ReadADC();
if (cur_value == Pressure) {
return -1;
@ -344,6 +395,80 @@ int32_t PIOS_MS5611_Test()
return 0;
}
/* PIOS sensor driver implementation */
void PIOS_MS5611_Register()
{
PIOS_SENSORS_Register(&PIOS_MS5611_Driver, PIOS_SENSORS_TYPE_1AXIS_BARO, 0);
}
bool PIOS_MS5611_driver_Test(__attribute__((unused)) uintptr_t context)
{
return true; // !PIOS_MS5611_Test();
}
void PIOS_MS5611_driver_Reset(__attribute__((unused)) uintptr_t context) {}
void PIOS_MS5611_driver_get_scale(float *scales, uint8_t size, __attribute__((unused)) uintptr_t context)
{
PIOS_Assert(size > 0);
scales[0] = 1;
}
void PIOS_MS5611_driver_fetch(void *data, __attribute__((unused)) uint8_t size, __attribute__((unused)) uintptr_t context)
{
PIOS_Assert(data);
memcpy(data, (void *)&results, sizeof(PIOS_SENSORS_1Axis_SensorsWithTemp));
}
bool PIOS_MS5611_driver_poll(__attribute__((unused)) uintptr_t context)
{
static uint8_t temp_press_interleave_count = 1;
static MS5611_FSM_State next_state = MS5611_FSM_INIT;
int32_t conversionResult = PIOS_MS5611_ReadADC();
if (__builtin_expect(conversionResult == -1, 1)) {
return false; // wait for conversion to complete
} else if (__builtin_expect(conversionResult == -2, 0)) {
temp_press_interleave_count = 1;
next_state = MS5611_FSM_INIT;
}
switch (next_state) {
case MS5611_FSM_INIT:
case MS5611_FSM_TEMPERATURE:
PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_TemperatureConv);
next_state = MS5611_FSM_PRESSURE;
return false;
case MS5611_FSM_PRESSURE:
PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_PressureConv);
next_state = MS5611_FSM_CALCULATE;
return false;
case MS5611_FSM_CALCULATE:
temp_press_interleave_count--;
if (!temp_press_interleave_count) {
temp_press_interleave_count = PIOS_MS5611_SLOW_TEMP_RATE;
PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_TemperatureConv);
next_state = MS5611_FSM_PRESSURE;
} else {
PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_PressureConv);
next_state = MS5611_FSM_CALCULATE;
}
results.temperature = PIOS_MS5611_GetTemperature();
results.sample = PIOS_MS5611_GetPressure();
return true;
default:
// it should not be there
PIOS_Assert(0);
}
return false;
}
#endif /* PIOS_INCLUDE_MS5611 */
/**

227
flight/pios/common/pios_rfm22b.c
View File

@ -47,8 +47,8 @@
// 6-byte (32-bit) preamble .. alternating 0's & 1's
// 4-byte (32-bit) sync
// 1-byte packet length (number of data bytes to follow)
// 1 byte valid bitmask
// 8 PPM values (0-255)
// 1 byte PPM values LSB (bit 0)
// 8 bytes PPM values MSBs (bit 8:1)
// 1 byte CRC
//
// *****************************************************************
@ -61,6 +61,7 @@
#include <pios_rfm22b_priv.h>
#include <pios_ppm_out.h>
#include <ecc.h>
#include <sha1.h>
/* Local Defines */
#define STACK_SIZE_BYTES 200
@ -73,9 +74,16 @@
#define RFM22B_LINK_QUALITY_THRESHOLD 20
#define RFM22B_DEFAULT_MIN_CHANNEL 0
#define RFM22B_DEFAULT_MAX_CHANNEL 250
#define RFM22B_DEFAULT_CHANNEL_SET 24
#define RFM22B_PPM_ONLY_DATARATE RFM22_datarate_9600
// PPM encoding limits
#define RFM22B_PPM_MIN 1
#define RFM22B_PPM_MAX 511
#define RFM22B_PPM_INVALID 0
#define RFM22B_PPM_SCALE 2
#define RFM22B_PPM_MIN_US 990
#define RFM22B_PPM_MAX_US (RFM22B_PPM_MIN_US + (RFM22B_PPM_MAX - RFM22B_PPM_MIN) * RFM22B_PPM_SCALE)
// The maximum amount of time without activity before initiating a reset.
#define PIOS_RFM22B_SUPERVISOR_TIMEOUT 150 // ms
@ -116,6 +124,9 @@
#define USB_LED_OFF
#endif
#define CONNECTED_TIMEOUT (250 / portTICK_RATE_MS) /* ms */
#define MAX_CHANNELS 32
/* Local type definitions */
struct pios_rfm22b_transition {
@ -155,21 +166,6 @@ static const uint8_t OUT_FF[64] = {
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF
};
// The randomized channel list.
static const uint8_t channel_list[] = {
68, 34, 2, 184, 166, 94, 204, 18, 47, 118, 239, 176, 5, 213, 218, 186, 104, 160, 199, 209, 231, 197, 92,
191, 88, 129, 40, 19, 93, 200, 156, 14, 247, 182, 193, 194, 208, 210, 248, 76, 244, 48, 179, 105, 25, 74,
155, 203, 39, 97, 195, 81, 83, 180, 134, 172, 235, 132, 198, 119, 207, 154, 0, 61, 140, 171, 245, 26, 95,
3, 22, 62, 169, 55, 127, 144, 45, 33, 170, 91, 158, 167, 63, 201, 41, 21, 190, 51, 103, 49, 189, 205,
240, 89, 181, 149, 6, 157, 249, 230, 115, 72, 163, 17, 29, 99, 28, 117, 219, 73, 78, 53, 69, 216, 161,
124, 110, 242, 214, 145, 13, 11, 220, 113, 138, 58, 54, 162, 237, 37, 152, 187, 232, 77, 126, 85, 38, 238,
173, 23, 188, 100, 131, 226, 31, 9, 114, 106, 221, 42, 233, 139, 4, 241, 96, 211, 8, 98, 121, 147, 24,
217, 27, 87, 122, 125, 135, 148, 178, 71, 206, 57, 141, 35, 30, 246, 159, 16, 32, 15, 229, 20, 12, 223,
150, 101, 79, 56, 102, 111, 174, 236, 137, 143, 52, 225, 64, 224, 112, 168, 243, 130, 108, 202, 123, 146, 228,
75, 46, 153, 7, 192, 175, 151, 222, 59, 82, 90, 1, 65, 109, 44, 165, 84, 43, 36, 128, 196, 67, 80,
136, 86, 70, 234, 66, 185, 10, 164, 177, 116, 50, 107, 183, 215, 212, 60, 227, 133, 120, 14
};
/* Local function forwared declarations */
static void pios_rfm22_task(void *parameters);
static bool pios_rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev);
@ -207,6 +203,10 @@ static void rfm22_clearLEDs();
// Utility functions.
static uint32_t pios_rfm22_time_difference_ms(portTickType start_time, portTickType end_time);
static struct pios_rfm22b_dev *pios_rfm22_alloc(void);
static void rfm22_hmac_sha1(const uint8_t *data, size_t len, uint8_t key[SHA1_DIGEST_LENGTH],
uint8_t digest[SHA1_DIGEST_LENGTH]);
static bool rfm22_gen_channels(uint32_t coordid, enum rfm22b_datarate datarate, uint8_t min,
uint8_t max, uint8_t channels[MAX_CHANNELS], uint8_t *clen);
// SPI read/write functions
static void rfm22_assertCs(struct pios_rfm22b_dev *rfm22b_dev);
@ -331,6 +331,18 @@ static const uint32_t data_rate[] = {
256000, // 256 kbps, 433 MHz, 150 khz freq dev
};
static const uint8_t channel_spacing[] = {
1, /* 9.6kbps */
2, /* 19.2kps */
2, /* 32kps */
2, /* 57.6kps */
2, /* 64kps */
3, /* 100kps */
4, /* 128kps */
4, /* 192kps */
4 /* 256kps */
};
static const uint8_t reg_1C[] = { 0x01, 0x05, 0x06, 0x95, 0x95, 0x81, 0x88, 0x8B, 0x8D }; // rfm22_if_filter_bandwidth
static const uint8_t reg_1D[] = { 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40 }; // rfm22_afc_loop_gearshift_override
@ -358,7 +370,7 @@ static const uint8_t reg_72[] = { 0x30, 0x48, 0x48, 0x48, 0x48, 0x60, 0x90, 0xCD
static const uint8_t packet_time[] = { 80, 40, 25, 15, 13, 10, 8, 6, 5 };
static const uint8_t packet_time_ppm[] = { 26, 25, 25, 15, 13, 10, 8, 6, 5 };
static const uint8_t num_channels[] = { 4, 4, 4, 6, 8, 8, 10, 12, 16 };
static const uint8_t num_channels[] = { 32, 32, 32, 32, 32, 32, 32, 32, 32 };
static struct pios_rfm22b_dev *g_rfm22b_dev = NULL;
@ -412,7 +424,6 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu
rfm22b_dev->stats.rx_error = 0;
rfm22b_dev->stats.rx_missed = 0;
rfm22b_dev->stats.tx_dropped = 0;
rfm22b_dev->stats.tx_resent = 0;
rfm22b_dev->stats.resets = 0;
rfm22b_dev->stats.timeouts = 0;
rfm22b_dev->stats.link_quality = 0;
@ -423,7 +434,7 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu
// Initialize the channels.
PIOS_RFM22B_SetChannelConfig(*rfm22b_id, RFM22B_DEFAULT_RX_DATARATE, RFM22B_DEFAULT_MIN_CHANNEL,
RFM22B_DEFAULT_MAX_CHANNEL, RFM22B_DEFAULT_CHANNEL_SET, false, false, false, false);
RFM22B_DEFAULT_MAX_CHANNEL, false, false, false, false);
// Create the event queue
rfm22b_dev->eventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(enum pios_radio_event));
@ -566,12 +577,12 @@ void PIOS_RFM22B_SetTxPower(uint32_t rfm22b_id, enum rfm22b_tx_power tx_pwr)
* @param[in] datarate The desired datarate.
* @param[in] min_chan The minimum channel.
* @param[in] max_chan The maximum channel.
* @param[in] chan_set The "seed" for selecting a channel sequence.
* @param[in] coordinator Is this modem an coordinator.
* @param[in] ppm_mode Should this modem send/receive ppm packets?
* @param[in] oneway Only the coordinator can send packets if true.
* @param[in] ppm_only Should this modem run in ppm only mode?
*/
void PIOS_RFM22B_SetChannelConfig(uint32_t rfm22b_id, enum rfm22b_datarate datarate, uint8_t min_chan, uint8_t max_chan, uint8_t chan_set, bool coordinator, bool oneway, bool ppm_mode, bool ppm_only)
void PIOS_RFM22B_SetChannelConfig(uint32_t rfm22b_id, enum rfm22b_datarate datarate, uint8_t min_chan, uint8_t max_chan, bool coordinator, bool oneway, bool ppm_mode, bool ppm_only)
{
struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
@ -596,15 +607,11 @@ void PIOS_RFM22B_SetChannelConfig(uint32_t rfm22b_id, enum rfm22b_datarate datar
}
rfm22b_dev->packet_time = (ppm_mode ? packet_time_ppm[datarate] : packet_time[datarate]);
// Find the first N channels that meet the min/max criteria out of the random channel list.
uint8_t num_found = 0;
for (uint16_t i = 0; (i < RFM22B_NUM_CHANNELS) && (num_found < num_channels[datarate]); ++i) {
uint8_t idx = (i + chan_set) % RFM22B_NUM_CHANNELS;
uint8_t chan = channel_list[idx];
if ((chan >= min_chan) && (chan <= max_chan)) {
rfm22b_dev->channels[num_found++] = chan;
}
}
rfm22_gen_channels(rfm22_destinationID(rfm22b_dev), datarate, min_chan, max_chan,
rfm22b_dev->channels, &num_found);
rfm22b_dev->num_channels = num_found;
// Calculate the maximum packet length from the datarate.
float bytes_per_period = (float)data_rate[datarate] * (float)(rfm22b_dev->packet_time - 2) / 9000;
@ -1354,7 +1361,7 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
rfm22b_dev->packet_start_ticks = 0;
rfm22b_dev->tx_complete_ticks = 0;
rfm22b_dev->rfm22b_state = RFM22B_STATE_INITIALIZING;
rfm22b_dev->on_sync_channel = false;
rfm22b_dev->last_contact = 0;
// software reset the RF chip .. following procedure according to Si4x3x Errata (rev. B)
rfm22_write_claim(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_swres);
@ -1725,7 +1732,7 @@ static bool pios_rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev)
*/
static void rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev)
{
rfm22b_dev->stats.rx_failure++;
rfm22b_add_rx_status(rfm22b_dev, RADIO_FAILURE_RX_PACKET);
rfm22b_dev->rx_buffer_wr = 0;
rfm22b_dev->packet_start_ticks = 0;
rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION;
@ -1767,18 +1774,31 @@ static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *radio_dev)
return RADIO_EVENT_RX_MODE;
}
// The first byte is a bitmask of valid channels.
// The first byte stores the LSB of each channel
p[0] = 0;
// Read the PPM input.
for (uint8_t i = 0; i < RFM22B_PPM_NUM_CHANNELS; ++i) {
int32_t val = radio_dev->ppm[i];
// Clamp and translate value, or transmit as reserved "invalid" constant
if ((val == PIOS_RCVR_INVALID) || (val == PIOS_RCVR_TIMEOUT)) {
p[i + 1] = 0;
val = RFM22B_PPM_INVALID;
} else if (val > RFM22B_PPM_MAX_US) {
val = RFM22B_PPM_MAX;
} else if (val < RFM22B_PPM_MIN_US) {
val = RFM22B_PPM_MIN;
} else {
p[0] |= 1 << i;
p[i + 1] = (val < 1000) ? 0 : ((val >= 1900) ? 255 : (uint8_t)(256 * (val - 1000) / 900));
val = (val - RFM22B_PPM_MIN_US) / RFM22B_PPM_SCALE + RFM22B_PPM_MIN;
}
// Store LSB
if (val & 1) {
p[0] |= (1 << i);
}
// Store upper 8 bits in array
p[i + 1] = val >> 1;
}
// The last byte is a CRC.
@ -1798,8 +1818,8 @@ static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *radio_dev)
len += (radio_dev->tx_out_cb)(radio_dev->tx_out_context, p + len, max_data_len - len, NULL, &need_yield);
}
// Always send a packet on the sync channel if this modem is a coordinator.
if ((len == 0) && ((radio_dev->channel_index != 0) || !rfm22_isCoordinator(radio_dev))) {
// Always send a packet if this modem is a coordinator.
if ((len == 0) && !rfm22_isCoordinator(radio_dev)) {
return RADIO_EVENT_RX_MODE;
}
@ -1921,10 +1941,11 @@ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_d
if (good_packet || corrected_packet) {
for (uint8_t i = 0; i < RFM22B_PPM_NUM_CHANNELS; ++i) {
// Calculate 9-bit value taking the LSB from byte 0
uint32_t val = (p[i + 1] << 1) + ((p[0] >> i) & 1);
// Is this a valid channel?
if (p[0] & (1 << i)) {
uint32_t val = p[i + 1];
radio_dev->ppm[i] = (uint16_t)(1000 + val * 900 / 256);
if (val != RFM22B_PPM_INVALID) {
radio_dev->ppm[i] = (uint16_t)(RFM22B_PPM_MIN_US + (val - RFM22B_PPM_MIN) * RFM22B_PPM_SCALE);
} else {
radio_dev->ppm[i] = PIOS_RCVR_INVALID;
}
@ -1959,12 +1980,16 @@ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_d
(radio_dev->rx_in_cb)(radio_dev->rx_in_context, p, data_len, NULL, &rx_need_yield);
}
// We only synchronize the clock on packets from our coordinator on the sync channel.
if (!rfm22_isCoordinator(radio_dev) && (radio_dev->rx_destination_id == rfm22_destinationID(radio_dev)) && (radio_dev->channel_index == 0)) {
/*
* If the packet is valid and destined for us we synchronize the clock.
*/
if (!rfm22_isCoordinator(radio_dev) &&
radio_dev->rx_destination_id == rfm22_destinationID(radio_dev)) {
rfm22_synchronizeClock(radio_dev);
radio_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTED;
radio_dev->on_sync_channel = false;
}
radio_dev->last_contact = xTaskGetTickCount();
} else {
ret_event = RADIO_EVENT_RX_COMPLETE;
}
@ -2069,7 +2094,7 @@ static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev)
rfm22b_dev->stats.rx_good = 0;
rfm22b_dev->stats.rx_corrected = 0;
rfm22b_dev->stats.rx_error = 0;
rfm22b_dev->stats.tx_resent = 0;
rfm22b_dev->stats.rx_failure = 0;
for (uint8_t i = 0; i < RFM22B_RX_PACKET_STATS_LEN; ++i) {
uint32_t val = rfm22b_dev->rx_packet_stats[i];
for (uint8_t j = 0; j < 16; ++j) {
@ -2083,8 +2108,8 @@ static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev)
case RADIO_ERROR_RX_PACKET:
rfm22b_dev->stats.rx_error++;
break;
case RADIO_RESENT_TX_PACKET:
rfm22b_dev->stats.tx_resent++;
case RADIO_FAILURE_RX_PACKET:
rfm22b_dev->stats.rx_failure++;
break;
}
}
@ -2094,7 +2119,7 @@ static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev)
// Note: This assumes that the number of packets sampled for the stats is 64.
// Using this equation, error and resent packets are counted as -2, and corrected packets are counted as -1.
// The range is 0 (all error or resent packets) to 128 (all good packets).
rfm22b_dev->stats.link_quality = 64 + rfm22b_dev->stats.rx_good - rfm22b_dev->stats.rx_error - rfm22b_dev->stats.tx_resent;
rfm22b_dev->stats.link_quality = 64 + rfm22b_dev->stats.rx_good - rfm22b_dev->stats.rx_error - rfm22b_dev->stats.rx_failure;
}
/**
@ -2160,14 +2185,14 @@ static void rfm22_synchronizeClock(struct pios_rfm22b_dev *rfm22b_dev)
portTickType start_time = rfm22b_dev->packet_start_ticks;
// This packet was transmitted on channel 0, calculate the time delta that will force us to transmit on channel 0 at the time this packet started.
uint8_t num_chan = num_channels[rfm22b_dev->datarate];
uint16_t frequency_hop_cycle_time = rfm22b_dev->packet_time * num_chan;
uint16_t frequency_hop_cycle_time = rfm22b_dev->packet_time * rfm22b_dev->num_channels;
uint16_t time_delta = start_time % frequency_hop_cycle_time;
// Calculate the adjustment for the preamble
uint8_t offset = (uint8_t)ceil(35000.0F / data_rate[rfm22b_dev->datarate]);
rfm22b_dev->time_delta = frequency_hop_cycle_time - time_delta + offset;
rfm22b_dev->time_delta = frequency_hop_cycle_time - time_delta + offset +
rfm22b_dev->packet_time * rfm22b_dev->channel_index;
}
/**
@ -2222,15 +2247,13 @@ static bool rfm22_timeToSend(struct pios_rfm22b_dev *rfm22b_dev)
static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t index)
{
// Make sure we don't index outside of the range.
uint8_t num_chan = num_channels[rfm22b_dev->datarate];
uint8_t idx = index % num_chan;
uint8_t idx = index % rfm22b_dev->num_channels;
// Are we switching to a new channel?
if (idx != rfm22b_dev->channel_index) {
// If the on_sync_channel flag is set, it means that we were on the sync channel, but no packet was received, so transition to a non-connected state.
if (!rfm22_isCoordinator(rfm22b_dev) && (rfm22b_dev->channel_index == 0) && rfm22b_dev->on_sync_channel) {
rfm22b_dev->on_sync_channel = false;
if (!rfm22_isCoordinator(rfm22b_dev) &&
pios_rfm22_time_difference_ms(rfm22b_dev->last_contact, xTaskGetTickCount()) >=
CONNECTED_TIMEOUT) {
// Set the link state to disconnected.
if (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) {
rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_DISCONNECTED;
@ -2240,11 +2263,8 @@ static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t ind
}
}
// Stay on the sync channel.
// Stay on first channel.
idx = 0;
} else if (idx == 0) {
// If we're switching to the sync channel, set a flag that can be used to detect if a packet was received.
rfm22b_dev->on_sync_channel = true;
}
rfm22b_dev->channel_index = idx;
@ -2263,8 +2283,7 @@ static uint8_t rfm22_calcChannelFromClock(struct pios_rfm22b_dev *rfm22b_dev)
portTickType time = rfm22_coordinatorTime(rfm22b_dev, xTaskGetTickCount());
// Divide time into 8ms blocks. Coordinator sends in first 2 ms, and remote send in 5th and 6th ms.
// Channel changes occur in the last 2 ms.
uint8_t num_chan = num_channels[rfm22b_dev->datarate];
uint8_t n = (time / rfm22b_dev->packet_time) % num_chan;
uint8_t n = (time / rfm22b_dev->packet_time) % rfm22b_dev->num_channels;
return rfm22_calcChannel(rfm22b_dev, n);
}
@ -2553,6 +2572,86 @@ static uint8_t rfm22_read(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr)
return in[1];
}
static void rfm22_hmac_sha1(const uint8_t *data, size_t len,
uint8_t key[SHA1_DIGEST_LENGTH],
uint8_t digest[SHA1_DIGEST_LENGTH])
{
uint8_t ipad[64] = { 0 };
uint8_t opad[64] = { 0 };
static SHA1_CTX *ctx;
ctx = pios_malloc(sizeof(SHA1_CTX));
memcpy(ipad, key, SHA1_DIGEST_LENGTH);
memcpy(opad, key, SHA1_DIGEST_LENGTH);
for (int i = 0; i < 64; i++) {
ipad[i] ^= 0x36;
opad[i] ^= 0x5c;
}
SHA1Init(ctx);
SHA1Update(ctx, ipad, sizeof(ipad));
SHA1Update(ctx, data, len);
SHA1Final(digest, ctx);
SHA1Init(ctx);
SHA1Update(ctx, opad, sizeof(opad));
SHA1Update(ctx, digest, SHA1_DIGEST_LENGTH);
SHA1Final(digest, ctx);
pios_free(ctx);
}
static bool rfm22_gen_channels(uint32_t coordid, enum rfm22b_datarate rate, uint8_t min,
uint8_t max, uint8_t channels[MAX_CHANNELS], uint8_t *clen)
{
uint32_t data = 0;
uint8_t cpos = 0;
uint8_t chan_range = (max / channel_spacing[rate] - min / channel_spacing[rate]) + 1;
uint8_t key[SHA1_DIGEST_LENGTH] = { 0 };
uint8_t digest[SHA1_DIGEST_LENGTH];
uint8_t *all_channels;
all_channels = pios_malloc(RFM22B_NUM_CHANNELS);
memcpy(key, &coordid, sizeof(coordid));
for (int i = 0; i < chan_range; i++) {
all_channels[i] = min / channel_spacing[rate] + i;
}
int j = SHA1_DIGEST_LENGTH;
for (int i = 0; i < chan_range && i < MAX_CHANNELS; i++) {
uint8_t rnd;
uint8_t r;
uint8_t tmp;
if (j == SHA1_DIGEST_LENGTH) {
rfm22_hmac_sha1((uint8_t *)&data, sizeof(data), key, digest);
j = 0;
data++;
}
rnd = digest[j];
j++;
r = rnd % (chan_range - i) + i;
tmp = all_channels[i];
all_channels[i] = all_channels[r];
all_channels[r] = tmp;
}
for (int i = 0; i < chan_range && cpos < MAX_CHANNELS; i++, cpos++) {
channels[cpos] = all_channels[i] * channel_spacing[rate];
}
*clen = cpos & 0xfe;
pios_free(all_channels);
return *clen > 0;
}
#endif /* PIOS_INCLUDE_RFM22B */
/**

65
flight/pios/common/pios_sensors.c Normal file
View File

@ -0,0 +1,65 @@
/**
******************************************************************************
*
* @file pios_sensors.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief PiOS sensors handling
* --
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include <pios_mem.h>
#include <pios_sensors.h>
#include <string.h>
// private variables
static PIOS_SENSORS_Instance *sensor_list = 0;
PIOS_SENSORS_Instance *PIOS_SENSORS_Register(const PIOS_SENSORS_Driver *driver, PIOS_SENSORS_TYPE type, uintptr_t context)
{
PIOS_SENSORS_Instance *instance = (PIOS_SENSORS_Instance *)pios_malloc(sizeof(PIOS_SENSORS_Instance));
instance->driver = driver;
instance->type = type;
instance->context = context;
instance->next = NULL;
LL_APPEND(sensor_list, instance);
return instance;
}
PIOS_SENSORS_Instance *PIOS_SENSORS_GetList()
{
return sensor_list;
}
PIOS_SENSORS_Instance *PIOS_SENSORS_GetInstanceByType(const PIOS_SENSORS_Instance *previous_instance, PIOS_SENSORS_TYPE type)
{
if (!previous_instance) {
previous_instance = sensor_list;
}
PIOS_SENSORS_Instance *sensor;
LL_FOREACH((PIOS_SENSORS_Instance *)previous_instance, sensor) {
if (sensor->type && type) {
return sensor;
}
}
return NULL;
}

5
flight/pios/inc/pios_hmc5x83.h
View File

@ -31,6 +31,7 @@
#ifndef PIOS_HMC5x83_H
#define PIOS_HMC5x83_H
#include <stdint.h>
#include <pios_sensors.h>
/* HMC5x83 Addresses */
#define PIOS_HMC5x83_I2C_ADDR 0x1E
#define PIOS_HMC5x83_I2C_READ_ADDR 0x3D
@ -123,12 +124,16 @@ struct pios_hmc5x83_cfg {
/* Public Functions */
extern pios_hmc5x83_dev_t PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_t port_id, uint8_t device_num);
extern void PIOS_HMC5x83_Register(pios_hmc5x83_dev_t handler);
extern bool PIOS_HMC5x83_NewDataAvailable(pios_hmc5x83_dev_t handler);
extern int32_t PIOS_HMC5x83_ReadMag(pios_hmc5x83_dev_t handler, int16_t out[3]);
extern uint8_t PIOS_HMC5x83_ReadID(pios_hmc5x83_dev_t handler, uint8_t out[4]);
extern int32_t PIOS_HMC5x83_Test(pios_hmc5x83_dev_t handler);
extern bool PIOS_HMC5x83_IRQHandler(pios_hmc5x83_dev_t handler);
extern const PIOS_SENSORS_Driver PIOS_HMC5x83_Driver;
#endif /* PIOS_HMC5x83_H */
/**

6
flight/pios/inc/pios_instrumentation.h
View File

@ -53,9 +53,11 @@ inline void PIOS_Instrumentation_updateCounter(pios_counter_t counter_handle, in
vPortEnterCritical();
pios_perf_counter_t *counter = (pios_perf_counter_t *)counter_handle;
counter->value = newValue;
counter->max--;
if (counter->value > counter->max) {
counter->max = counter->value;
}
counter->min++;
if (counter->value < counter->min) {
counter->min = counter->value;
}
@ -88,9 +90,11 @@ inline void PIOS_Instrumentation_TimeEnd(pios_counter_t counter_handle)
pios_perf_counter_t *counter = (pios_perf_counter_t *)counter_handle;
counter->value = PIOS_DELAY_DiffuS(counter->lastUpdateTS);
counter->max--;
if (counter->value > counter->max) {
counter->max = counter->value;
}
counter->min++;
if (counter->value < counter->min) {
counter->min = counter->value;
}
@ -110,9 +114,11 @@ inline void PIOS_Instrumentation_TrackPeriod(pios_counter_t counter_handle)
vPortEnterCritical();
uint32_t period = PIOS_DELAY_DiffuS(counter->lastUpdateTS);
counter->value = (counter->value * 15 + period) / 16;
counter->max--;
if ((int32_t)period > counter->max) {
counter->max = period;
}
counter->min++;
if ((int32_t)period < counter->min) {
counter->min = period;
}

3
flight/pios/inc/pios_math.h
View File

@ -72,6 +72,9 @@
#define RAD2DEG_D(rad) ((rad) * (180.0d / M_PI_D))
#define DEG2RAD_D(deg) ((deg) * (M_PI_D / 180.0d))
// helper macros for LPFs
#define LPF_ALPHA(dt, fc) (dt / (dt + 1.0f / (2.0f * M_PI_F * fc)))
// Useful math macros
#define MAX(a, b) ((a) > (b) ? (a) : (b))
#define MIN(a, b) ((a) < (b) ? (a) : (b))

1
flight/pios/inc/pios_mem.h
View File

@ -26,6 +26,7 @@
*/
#ifndef PIOS_MEM_H
#define PIOS_MEM_H
#include <strings.h>
void *pios_fastheapmalloc(size_t size);

20
flight/pios/inc/pios_mpu6000.h
View File

@ -31,6 +31,7 @@
#ifndef PIOS_MPU6000_H
#define PIOS_MPU6000_H
#include <pios_sensors.h>
/* MPU6000 Addresses */
#define PIOS_MPU6000_SMPLRT_DIV_REG 0X19
@ -131,18 +132,6 @@ enum pios_mpu6000_orientation { // clockwise rotation from board forward
PIOS_MPU6000_TOP_270DEG = 0x03
};
struct pios_mpu6000_data {
int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;
#if defined(PIOS_MPU6000_ACCEL)
int16_t accel_x;
int16_t accel_y;
int16_t accel_z;
#endif /* PIOS_MPU6000_ACCEL */
int16_t temperature;
};
struct pios_mpu6000_cfg {
const struct pios_exti_cfg *exti_cfg; /* Pointer to the EXTI configuration */
@ -167,14 +156,11 @@ struct pios_mpu6000_cfg {
/* Public Functions */
extern int32_t PIOS_MPU6000_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_mpu6000_cfg *new_cfg);
extern int32_t PIOS_MPU6000_ConfigureRanges(enum pios_mpu6000_range gyroRange, enum pios_mpu6000_accel_range accelRange, enum pios_mpu6000_filter filterSetting);
extern xQueueHandle PIOS_MPU6000_GetQueue();
extern int32_t PIOS_MPU6000_ReadGyros(struct pios_mpu6000_data *buffer);
extern int32_t PIOS_MPU6000_ReadID();
extern int32_t PIOS_MPU6000_Test();
extern float PIOS_MPU6000_GetScale();
extern float PIOS_MPU6000_GetAccelScale();
extern void PIOS_MPU6000_Register();
extern bool PIOS_MPU6000_IRQHandler(void);
extern const PIOS_SENSORS_Driver PIOS_MPU6000_Driver;
#endif /* PIOS_MPU6000_H */
/**

21
flight/pios/inc/pios_ms5611.h
View File

@ -30,7 +30,7 @@
#ifndef PIOS_MS5611_H
#define PIOS_MS5611_H
#include <pios_sensors.h>
/* BMP085 Addresses */
#define MS5611_I2C_ADDR 0x77
#define MS5611_RESET 0x1E
@ -40,17 +40,6 @@
#define MS5611_PRES_ADDR 0x40
#define MS5611_TEMP_ADDR 0x50
#define MS5611_ADC_MSB 0xF6
#define MS5611_P0 101.3250f
/* Local Types */
typedef struct {
uint16_t C[6];
} MS5611CalibDataTypeDef;
typedef enum {
PressureConv,
TemperatureConv
} ConversionTypeTypeDef;
struct pios_ms5611_cfg {
uint32_t oversampling;
@ -66,12 +55,8 @@ enum pios_ms5611_osr {
/* Public Functions */
extern void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device);
extern int32_t PIOS_MS5611_StartADC(ConversionTypeTypeDef Type);
extern int32_t PIOS_MS5611_ReadADC(void);
extern float PIOS_MS5611_GetTemperature(void);
extern float PIOS_MS5611_GetPressure(void);
extern int32_t PIOS_MS5611_Test();
extern int32_t PIOS_MS5611_GetDelay();
extern const PIOS_SENSORS_Driver PIOS_MS5611_Driver;
void PIOS_MS5611_Register();
#endif /* PIOS_MS5611_H */

2
flight/pios/inc/pios_pwm.h
View File

@ -30,4 +30,6 @@
#ifndef PIOS_PWM_H
#define PIOS_PWM_H
#define PIOS_PWM_VALID_RANGE_MAX 2250
#define PIOS_PWM_VALID_RANGE_MIN 750
#endif /* PIOS_PWM_H */

3
flight/pios/inc/pios_rfm22b.h
View File

@ -90,7 +90,6 @@ struct rfm22b_stats {
uint8_t rx_missed;
uint8_t rx_failure;
uint8_t tx_dropped;
uint8_t tx_resent;
uint8_t tx_failure;
uint8_t resets;
uint8_t timeouts;
@ -104,7 +103,7 @@ struct rfm22b_stats {
extern int32_t PIOS_RFM22B_Init(uint32_t *rfb22b_id, uint32_t spi_id, uint32_t slave_num, const struct pios_rfm22b_cfg *cfg);
extern void PIOS_RFM22B_Reinit(uint32_t rfb22b_id);
extern void PIOS_RFM22B_SetTxPower(uint32_t rfm22b_id, enum rfm22b_tx_power tx_pwr);
extern void PIOS_RFM22B_SetChannelConfig(uint32_t rfm22b_id, enum rfm22b_datarate datarate, uint8_t min_chan, uint8_t max_chan, uint8_t chan_set, bool coordinator, bool oneway, bool ppm_mode, bool ppm_only);
extern void PIOS_RFM22B_SetChannelConfig(uint32_t rfm22b_id, enum rfm22b_datarate datarate, uint8_t min_chan, uint8_t max_chan, bool coordinator, bool oneway, bool ppm_mode, bool ppm_only);
extern void PIOS_RFM22B_SetCoordinatorID(uint32_t rfm22b_id, uint32_t coord_id);
extern uint32_t PIOS_RFM22B_DeviceID(uint32_t rfb22b_id);
extern void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats);

6
flight/pios/inc/pios_rfm22b_priv.h
View File

@ -40,7 +40,7 @@
// ************************************
#define RFM22B_MAX_PACKET_LEN 64
#define RFM22B_NUM_CHANNELS 250
#define RFM22B_NUM_CHANNELS 251
// ************************************
@ -578,7 +578,7 @@ enum pios_rfm22b_rx_packet_status {
RADIO_GOOD_RX_PACKET = 0x00,
RADIO_CORRECTED_RX_PACKET = 0x01,
RADIO_ERROR_RX_PACKET = 0x2,
RADIO_RESENT_TX_PACKET = 0x3
RADIO_FAILURE_RX_PACKET = 0x3
};
typedef struct {
@ -780,7 +780,7 @@ struct pios_rfm22b_dev {
portTickType packet_start_ticks;
portTickType tx_complete_ticks;
portTickType time_delta;
bool on_sync_channel;
portTickType last_contact;
};

190
flight/pios/inc/pios_sensors.h Normal file
View File

@ -0,0 +1,190 @@
/**
******************************************************************************
*
* @file pios_sensors.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief PiOS sensors handling
* --
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_SENSORS_H
#define PIOS_SENSORS_H
#include <pios.h>
#include <utlist.h>
#include <stdint.h>
#include <vectors.h>
// needed for debug APIs.
typedef bool (*PIOS_SENSORS_test_function)(uintptr_t context);
typedef void (*PIOS_SENSORS_reset_function)(uintptr_t context);
typedef bool (*PIOS_SENSORS_poll_function)(uintptr_t context);
typedef void (*PIOS_SENSORS_fetch_function)(void *samples, uint8_t size, uintptr_t context);
/**
* return an array with current scale for the instance.
* Instances with multiples sensors returns several value in the same
* order as they appear in PIOS_SENSORS_TYPE enums.
*/
typedef void (*PIOS_SENSORS_get_scale_function)(float *, uint8_t size, uintptr_t context);
typedef QueueHandle_t (*PIOS_SENSORS_get_queue_function)(uintptr_t context);
typedef struct PIOS_SENSORS_Driver {
PIOS_SENSORS_test_function test; // called at startup to test the sensor
PIOS_SENSORS_poll_function poll; // called to check whether data are available for polled sensors
PIOS_SENSORS_fetch_function fetch; // called to fetch data for polled sensors
PIOS_SENSORS_reset_function reset; // reset sensor. for example if data are not received in the allotted time
PIOS_SENSORS_get_queue_function get_queue; // get the queue reference
PIOS_SENSORS_get_scale_function get_scale; // return scales for the sensors
bool is_polled;
} PIOS_SENSORS_Driver;
typedef enum PIOS_SENSORS_TYPE {
PIOS_SENSORS_TYPE_3AXIS_ACCEL = 0x01,
PIOS_SENSORS_TYPE_3AXIS_GYRO = 0x02,
PIOS_SENSORS_TYPE_3AXIS_GYRO_ACCEL = 0x03,
PIOS_SENSORS_TYPE_3AXIS_MAG = 0x04,
PIOS_SENSORS_TYPE_1AXIS_BARO = 0x08,
} PIOS_SENSORS_TYPE;
#define PIOS_SENSORS_TYPE_1D (PIOS_SENSORS_TYPE_1AXIS_BARO)
#define PIOS_SENSORS_TYPE_3D (PIOS_SENSORS_TYPE_3AXIS_ACCEL | PIOS_SENSORS_TYPE_3AXIS_GYRO | PIOS_SENSORS_TYPE_3AXIS_MAG)
typedef struct PIOS_SENSORS_Instance {
const PIOS_SENSORS_Driver *driver;
uintptr_t context;
struct PIOS_SENSORS_Instance *next;
uint8_t type;
} PIOS_SENSORS_Instance;
/**
* A 3d Accel sample with temperature
*/
typedef struct PIOS_SENSORS_3Axis_SensorsWithTemp {
uint16_t count; // number of sensor instances
int16_t temperature; // Degrees Celsius * 100
Vector3i16 sample[];
} PIOS_SENSORS_3Axis_SensorsWithTemp;
typedef struct PIOS_SENSORS_1Axis_SensorsWithTemp {
float temperature; // Degrees Celsius
float sample; // sample
} PIOS_SENSORS_1Axis_SensorsWithTemp;
/**
* Register a new sensor instance with sensor subsystem
* @param driver sensor driver
* @param type sensor type @ref PIOS_SENSORS_TYPE
* @param context context to be passed to sensor driver
* @return the new sensor instance
*/
PIOS_SENSORS_Instance *PIOS_SENSORS_Register(const PIOS_SENSORS_Driver *driver, PIOS_SENSORS_TYPE type, uintptr_t context);
/**
* return the list of registered sensors.
* @return the first sensor instance in the list.
*/
PIOS_SENSORS_Instance *PIOS_SENSORS_GetList();
/**
* Perform sensor test and return true if passed
* @param sensor instance to test
* @return true if test passes
*/
static inline bool PIOS_SENSORS_Test(const PIOS_SENSORS_Instance *sensor)
{
PIOS_Assert(sensor);
if (!sensor->driver->test) {
return true;
} else {
return sensor->driver->test(sensor->context);
}
}
/**
* Poll sensor for new values
* @param sensor instance to poll
* @return true if sensor has samples available
*/
static inline bool PIOS_SENSORS_Poll(const PIOS_SENSORS_Instance *sensor)
{
PIOS_Assert(sensor);
if (!sensor->driver->poll) {
return true;
} else {
return sensor->driver->poll(sensor->context);
}
}
/**
*
* @param sensor
* @param samples
* @param size
*/
static inline void PIOS_SENSOR_Fetch(const PIOS_SENSORS_Instance *sensor, void *samples, uint8_t size)
{
PIOS_Assert(sensor);
sensor->driver->fetch(samples, size, sensor->context);
}
static inline void PIOS_SENSOR_Reset(const PIOS_SENSORS_Instance *sensor)
{
PIOS_Assert(sensor);
sensor->driver->reset(sensor->context);
}
/**
* retrieve the sensor queue
* @param sensor
* @return sensor queue or null if not supported
*/
static inline QueueHandle_t PIOS_SENSORS_GetQueue(const PIOS_SENSORS_Instance *sensor)
{
PIOS_Assert(sensor);
if (!sensor->driver->get_queue) {
return NULL;
}
return sensor->driver->get_queue(sensor->context);
}
/**
* Get the sensor scales.
* @param sensor sensor instance
* @param scales float array that will contains scales
* @param size number of floats within the array
*/
static inline void PIOS_SENSORS_GetScales(const PIOS_SENSORS_Instance *sensor, float *scales, uint8_t size)
{
PIOS_Assert(sensor);
sensor->driver->get_scale(scales, size, sensor->context);
}
/**
* return head of sensor list
* @return head of sensor list
*/
PIOS_SENSORS_Instance *PIOS_SENSORS_GetList();
/**
* Return the first occurrence of specified sensor type
* @param previous_instance last instance found or 0
* @param type type of sensor to find
* @return the first occurence found or NULL
*/
PIOS_SENSORS_Instance *PIOS_SENSORS_GetInstanceByType(const PIOS_SENSORS_Instance *previous_instance, PIOS_SENSORS_TYPE type);
#endif /* PIOS_SENSORS_H */

10
flight/pios/inc/pios_servo.h
View File

@ -30,9 +30,17 @@
#ifndef PIOS_SERVO_H
#define PIOS_SERVO_H
/* Global types */
enum pios_servo_bank_mode {
PIOS_SERVO_BANK_MODE_PWM = 0,
PIOS_SERVO_BANK_MODE_SINGLE_PULSE = 1
};
/* Public Functions */
extern void PIOS_Servo_SetHz(const uint16_t *update_rates, uint8_t banks);
extern void PIOS_Servo_SetHz(const uint16_t *speeds, const uint32_t *clock, uint8_t banks);
extern void PIOS_Servo_Set(uint8_t Servo, uint16_t Position);
extern void PIOS_Servo_Update();
extern void PIOS_Servo_SetBankMode(uint8_t bank, uint8_t mode);
extern uint8_t PIOS_Servo_GetPinBank(uint8_t pin);
#endif /* PIOS_SERVO_H */

53
flight/pios/pios.h
View File

@ -34,6 +34,10 @@
#ifndef PIOS_H
#define PIOS_H
#ifdef __cplusplus
extern "C" {
#endif
#include <pios_helpers.h>
#include <pios_math.h>
#include <pios_constants.h>
@ -182,49 +186,6 @@
#endif
#endif
/* PIOS sensor interfaces */
#ifdef PIOS_INCLUDE_ADXL345
/* ADXL345 3-Axis Accelerometer */
#include <pios_adxl345.h>
#endif
#ifdef PIOS_INCLUDE_BMA180
/* BMA180 3-Axis Accelerometer */
#include <pios_bma180.h>
#endif
#ifdef PIOS_INCLUDE_L3GD20
/* L3GD20 3-Axis Gyro */
#include <pios_l3gd20.h>
#endif
#ifdef PIOS_INCLUDE_MPU6000
/* MPU6000 3-Axis Gyro/Accelerometer */
/* #define PIOS_MPU6000_ACCEL */
#include <pios_mpu6000.h>
#endif
#ifdef PIOS_INCLUDE_HMC5843
/* HMC5843 3-Axis Digital Compass */
#include <pios_hmc5843.h>
#endif
#ifdef PIOS_INCLUDE_HMC5X83
/* HMC5883/HMC5983 3-Axis Digital Compass */
/* #define PIOS_HMC5x83_HAS_GPIOS */
#include <pios_hmc5x83.h>
#endif
#ifdef PIOS_INCLUDE_BMP085
/* BMP085 Barometric Pressure Sensor */
#include <pios_bmp085.h>
#endif
#ifdef PIOS_INCLUDE_MS5611
/* MS5611 Barometric Pressure Sensor */
#include <pios_ms5611.h>
#endif
#ifdef PIOS_INCLUDE_MPXV
/* MPXV5004, MPXV7002 based Airspeed Sensor */
#include <pios_mpxv.h>
@ -360,4 +321,10 @@
/* #define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 995998 */
#endif /* USE_SIM_POSIX */
#ifdef __cplusplus
} // closing brace for extern "C"
#endif
#endif /* PIOS_H */

8
flight/pios/posix/pios_servo.c
View File

@ -43,13 +43,13 @@ static volatile uint16_t ServoPosition[PIOS_SERVO_NUM_TIMERS];
*/
void PIOS_Servo_Init(void)
{}
/**
* Set the servo update rate (Max 500Hz)
* \param[in] onetofour Rate for outputs 1 to 4 (Hz)
* \param[in] fivetoeight Rate for outputs 5 to 8 (Hz)
* \param[in] array of rates in Hz
* \param[in] array of timer clocks in Hz
* \param[in] maximum number of banks
*/
void PIOS_Servo_SetHz(const uint16_t *banks, uint8_t num_banks)
void PIOS_Servo_SetHz(const uint16_t *speeds, const uint32_t *clock, uint8_t banks)
{}
/**

21
flight/pios/stm32f10x/pios_pwm.c
View File

@ -209,7 +209,9 @@ static void PIOS_PWM_tim_overflow_cb(__attribute__((unused)) uint32_t tim_id, ui
/* Channel out of range */
return;
}
if (!pwm_dev->CaptureState[channel]) {
return;
}
pwm_dev->us_since_update[channel] += count;
if (pwm_dev->us_since_update[channel] >= PWM_SUPERVISOR_TIMEOUT) {
pwm_dev->CaptureState[channel] = 0;
@ -256,16 +258,19 @@ static void PIOS_PWM_tim_edge_cb(__attribute__((unused)) uint32_t tim_id, uint32
TIM_ICInitStructure.TIM_Channel = chan->timer_chan;
TIM_ICInit(chan->timer, &TIM_ICInitStructure);
} else {
uint32_t value = 0;
/* Capture computation */
if (pwm_dev->FallValue[chan_idx] > pwm_dev->RiseValue[chan_idx]) {
pwm_dev->CaptureValue[chan_idx] = (pwm_dev->FallValue[chan_idx] - pwm_dev->RiseValue[chan_idx]);
} else {
pwm_dev->CaptureValue[chan_idx] = ((chan->timer->ARR - pwm_dev->RiseValue[chan_idx]) + pwm_dev->FallValue[chan_idx]);
value = pwm_dev->us_since_update[chan_idx] + (pwm_dev->FallValue[chan_idx] - pwm_dev->RiseValue[chan_idx]);
// From time to time glitches happens. Most of the observed case are related to missing or spurious overflows
// happening when they are triggered very close each other.
// The following workaround prevents them to cause troubles by filtering unacceptable values
if (PIOS_PWM_VALID_RANGE_MAX > value && PIOS_PWM_VALID_RANGE_MIN < value) {
pwm_dev->CaptureValue[chan_idx] = value;
}
/* Switch states */
pwm_dev->CaptureState[chan_idx] = 0;
pwm_dev->CaptureState[chan_idx] = 0;
pwm_dev->us_since_update[chan_idx] = 0;
/* Increase supervisor counter */
pwm_dev->CapCounter[chan_idx]++;

180
flight/pios/stm32f10x/pios_servo.c
View File

@ -39,6 +39,19 @@
static const struct pios_servo_cfg *servo_cfg;
// determine if the related timer will work in synchronous (or OneShot/OneShot125) One Pulse mode.
static uint8_t pios_servo_bank_mode[PIOS_SERVO_BANKS] = { 0 };
// used to skip updates when pulse length is higher than update cycle
static uint16_t pios_servo_bank_next_update[PIOS_SERVO_BANKS] = { 0 };
static uint16_t pios_servo_bank_max_pulse[PIOS_SERVO_BANKS] = { 0 };
// timer associated to each bank
static TIM_TypeDef *pios_servo_bank_timer[PIOS_SERVO_BANKS] = { 0 };
// index of bank used for each pin
static uint8_t *pios_servo_pin_bank;
#define PIOS_SERVO_TIMER_CLOCK 1000000
#define PIOS_SERVO_SAFE_MARGIN 50
/**
* Initialise Servos
*/
@ -52,10 +65,42 @@ int32_t PIOS_Servo_Init(const struct pios_servo_cfg *cfg)
/* Store away the requested configuration */
servo_cfg = cfg;
pios_servo_pin_bank = pios_malloc(sizeof(uint8_t) * cfg->num_channels);
uint8_t bank = 0;
/* Configure the channels to be in output compare mode */
for (uint8_t i = 0; i < cfg->num_channels; i++) {
const struct pios_tim_channel *chan = &cfg->channels[i];
const struct pios_tim_channel *chan = &servo_cfg->channels[i];
bool new = true;
/* See if any previous channels use that same timer */
for (uint8_t j = 0; (j < i) && new; j++) {
new &= chan->timer != servo_cfg->channels[j].timer;
}
if (new) {
PIOS_Assert(bank < PIOS_SERVO_BANKS);
for (uint8_t j = i; j < servo_cfg->num_channels; j++) {
if (servo_cfg->channels[j].timer == chan->timer) {
pios_servo_pin_bank[j] = bank;
}
}
pios_servo_bank_timer[bank] = chan->timer;
PIOS_Assert(bank < PIOS_SERVO_BANKS);
for (uint8_t j = i; j < servo_cfg->num_channels; j++) {
if (servo_cfg->channels[j].timer == chan->timer) {
pios_servo_pin_bank[j] = bank;
}
}
pios_servo_bank_timer[bank] = chan->timer;
TIM_ARRPreloadConfig(chan->timer, ENABLE);
TIM_CtrlPWMOutputs(chan->timer, ENABLE);
TIM_Cmd(chan->timer, ENABLE);
bank++;
}
/* Set up for output compare function */
switch (chan->timer_chan) {
@ -76,10 +121,6 @@ int32_t PIOS_Servo_Init(const struct pios_servo_cfg *cfg)
TIM_OC4PreloadConfig(chan->timer, TIM_OCPreload_Enable);
break;
}
TIM_ARRPreloadConfig(chan->timer, ENABLE);
TIM_CtrlPWMOutputs(chan->timer, ENABLE);
TIM_Cmd(chan->timer, ENABLE);
}
return 0;
@ -88,10 +129,12 @@ int32_t PIOS_Servo_Init(const struct pios_servo_cfg *cfg)
/**
* Set the servo update rate (Max 500Hz)
* \param[in] array of rates in Hz
* \param[in] array of timer clocks in Hz
* \param[in] maximum number of banks
*/
void PIOS_Servo_SetHz(const uint16_t *speeds, uint8_t banks)
void PIOS_Servo_SetHz(const uint16_t *speeds, const uint32_t *clock, uint8_t banks)
{
PIOS_Assert(banks <= PIOS_SERVO_BANKS);
if (!servo_cfg) {
return;
}
@ -99,23 +142,18 @@ void PIOS_Servo_SetHz(const uint16_t *speeds, uint8_t banks)
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = servo_cfg->tim_base_init;
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1;
uint8_t set = 0;
for (uint8_t i = 0; i < banks && i < PIOS_SERVO_BANKS; i++) {
const TIM_TypeDef *timer = pios_servo_bank_timer[i];
if (timer) {
uint32_t new_clock = PIOS_SERVO_TIMER_CLOCK;
if (clock[i]) {
new_clock = clock[i];
}
TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_MASTER_CLOCK / new_clock) - 1;
TIM_TimeBaseStructure.TIM_Period = ((new_clock / speeds[i]) - 1);
for (uint8_t i = 0; (i < servo_cfg->num_channels) && (set < banks); i++) {
bool new = true;
const struct pios_tim_channel *chan = &servo_cfg->channels[i];
/* See if any previous channels use that same timer */
for (uint8_t j = 0; (j < i) && new; j++) {
new &= chan->timer != servo_cfg->channels[j].timer;
}
if (new) {
TIM_TimeBaseStructure.TIM_Period = ((1000000 / speeds[set]) - 1);
TIM_TimeBaseInit(chan->timer, &TIM_TimeBaseStructure);
set++;
TIM_TimeBaseInit((TIM_TypeDef *)timer, &TIM_TimeBaseStructure);
}
}
}
@ -132,22 +170,114 @@ void PIOS_Servo_Set(uint8_t servo, uint16_t position)
return;
}
/* Update the position */
const struct pios_tim_channel *chan = &servo_cfg->channels[servo];
uint16_t val = position;
uint16_t margin = chan->timer->ARR / 50; // Leave 2% of period as margin to prevent overlaps
if (val > (chan->timer->ARR - margin)) {
val = chan->timer->ARR - margin;
}
uint8_t bank = pios_servo_pin_bank[servo];
if (pios_servo_bank_max_pulse[bank] < val) {
pios_servo_bank_max_pulse[bank] = val;
}
switch (chan->timer_chan) {
case TIM_Channel_1:
TIM_SetCompare1(chan->timer, position);
TIM_SetCompare1(chan->timer, val);
break;
case TIM_Channel_2:
TIM_SetCompare2(chan->timer, position);
TIM_SetCompare2(chan->timer, val);
break;
case TIM_Channel_3:
TIM_SetCompare3(chan->timer, position);
TIM_SetCompare3(chan->timer, val);
break;
case TIM_Channel_4:
TIM_SetCompare4(chan->timer, position);
TIM_SetCompare4(chan->timer, val);
break;
}
}
void PIOS_Servo_Update()
{
for (uint8_t i = 0; (i < PIOS_SERVO_BANKS); i++) {
const TIM_TypeDef *timer = pios_servo_bank_timer[i];
if (timer && pios_servo_bank_mode[i] == PIOS_SERVO_BANK_MODE_SINGLE_PULSE) {
// a pulse to be generated is longer than cycle period. skip this update.
if (TIM_GetCounter((TIM_TypeDef *)timer) > (uint32_t)(pios_servo_bank_next_update[i] + PIOS_SERVO_SAFE_MARGIN)) {
TIM_GenerateEvent((TIM_TypeDef *)timer, TIM_EventSource_Update);
pios_servo_bank_next_update[i] = pios_servo_bank_max_pulse[i];
}
}
pios_servo_bank_max_pulse[i] = 0;
}
for (uint8_t i = 0; (i < servo_cfg->num_channels); i++) {
uint8_t bank = pios_servo_pin_bank[i];
uint8_t mode = pios_servo_bank_mode[bank];
if (mode == PIOS_SERVO_BANK_MODE_SINGLE_PULSE) {
/* Update the position */
const struct pios_tim_channel *chan = &servo_cfg->channels[i];
switch (chan->timer_chan) {
case TIM_Channel_1:
TIM_SetCompare1(chan->timer, 0);
break;
case TIM_Channel_2:
TIM_SetCompare2(chan->timer, 0);
break;
case TIM_Channel_3:
TIM_SetCompare3(chan->timer, 0);
break;
case TIM_Channel_4:
TIM_SetCompare4(chan->timer, 0);
break;
}
}
}
}
void PIOS_Servo_SetBankMode(uint8_t bank, uint8_t mode)
{
PIOS_Assert(bank < PIOS_SERVO_BANKS);
pios_servo_bank_mode[bank] = mode;
if (pios_servo_bank_timer[bank]) {
for (uint8_t i = 0; (i < servo_cfg->num_channels); i++) {
if (pios_servo_pin_bank[i] == bank) {
const struct pios_tim_channel *chan = &servo_cfg->channels[i];
/* Set up for output compare function */
switch (chan->timer_chan) {
case TIM_Channel_1:
TIM_OC1PolarityConfig(chan->timer, TIM_OCPolarity_High);
break;
case TIM_Channel_2:
TIM_OC2PolarityConfig(chan->timer, TIM_OCPolarity_High);
break;
case TIM_Channel_3:
TIM_OC3PolarityConfig(chan->timer, TIM_OCPolarity_High);
break;
case TIM_Channel_4:
TIM_OC4PolarityConfig(chan->timer, TIM_OCPolarity_High);
break;
}
}
}
// Setup the timer accordingly
TIM_SelectOnePulseMode(pios_servo_bank_timer[bank], TIM_OPMode_Repetitive);
TIM_Cmd(pios_servo_bank_timer[bank], ENABLE);
}
}
uint8_t PIOS_Servo_GetPinBank(uint8_t pin)
{
if (pin < servo_cfg->num_channels) {
return pios_servo_pin_bank[pin];
} else {
return 0;
}
}
#endif /* PIOS_INCLUDE_SERVO */

87
flight/pios/stm32f10x/pios_tim.c
View File

@ -159,6 +159,16 @@ out_fail:
static void PIOS_TIM_generic_irq_handler(TIM_TypeDef *timer)
{
/* Check for an overflow event on this timer */
bool overflow_event = 0;
uint16_t overflow_count = false;
if (TIM_GetITStatus(timer, TIM_IT_Update) == SET) {
TIM_ClearITPendingBit(timer, TIM_IT_Update);
overflow_count = timer->ARR;
overflow_event = true;
}
/* Iterate over all registered clients of the TIM layer to find channels on this timer */
for (uint8_t i = 0; i < pios_tim_num_devs; i++) {
const struct pios_tim_dev *tim_dev = &pios_tim_devs[i];
@ -168,18 +178,6 @@ static void PIOS_TIM_generic_irq_handler(TIM_TypeDef *timer)
continue;
}
/* Check for an overflow event on this timer */
bool overflow_event;
uint16_t overflow_count;
if (TIM_GetITStatus(timer, TIM_IT_Update) == SET) {
TIM_ClearITPendingBit(timer, TIM_IT_Update);
overflow_count = timer->ARR;
overflow_event = true;
} else {
overflow_count = 0;
overflow_event = false;
}
for (uint8_t j = 0; j < tim_dev->num_channels; j++) {
const struct pios_tim_channel *chan = &tim_dev->channels[j];
@ -251,11 +249,11 @@ static void PIOS_TIM_generic_irq_handler(TIM_TypeDef *timer)
* get the order wrong, our pulse width calculations could be off by up
* to ARR ticks. That could be bad.
*
* Heuristic: If the edge_count is < 16 ticks above zero then we assume the
* Heuristic: If the edge_count is < 32 ticks above zero then we assume the
* edge happened just after the overflow.
*/
if (edge_count < 16) {
if (edge_count < 32) {
/* Call the overflow callback first */
if (tim_dev->callbacks->overflow) {
(*tim_dev->callbacks->overflow)((uint32_t)tim_dev,
@ -300,67 +298,6 @@ static void PIOS_TIM_generic_irq_handler(TIM_TypeDef *timer)
}
}
}
#if 0
uint16_t val = 0;
for (uint8_t i = 0; i < pios_pwm_cfg.num_channels; i++) {
struct pios_pwm_channel channel = pios_pwm_cfg.channels[i];
if ((channel.timer == timer) && (TIM_GetITStatus(channel.timer, channel.ccr) == SET)) {
TIM_ClearITPendingBit(channel.timer, channel.ccr);
switch (channel.channel) {
case TIM_Channel_1:
val = TIM_GetCapture1(channel.timer);
break;
case TIM_Channel_2:
val = TIM_GetCapture2(channel.timer);
break;
case TIM_Channel_3:
val = TIM_GetCapture3(channel.timer);
break;
case TIM_Channel_4:
val = TIM_GetCapture4(channel.timer);
break;
}
if (CaptureState[i] == 0) {
RiseValue[i] = val;
} else {
FallValue[i] = val;
}
// flip state machine and capture value here
/* Simple rise or fall state machine */
TIM_ICInitTypeDef TIM_ICInitStructure = pios_pwm_cfg.tim_ic_init;
if (CaptureState[i] == 0) {
/* Switch states */
CaptureState[i] = 1;
/* Switch polarity of input capture */
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling;
TIM_ICInitStructure.TIM_Channel = channel.channel;
TIM_ICInit(channel.timer, &TIM_ICInitStructure);
} else {
/* Capture computation */
if (FallValue[i] > RiseValue[i]) {
CaptureValue[i] = (FallValue[i] - RiseValue[i]);
} else {
CaptureValue[i] = ((channel.timer->ARR - RiseValue[i]) + FallValue[i]);
}
/* Switch states */
CaptureState[i] = 0;
/* Increase supervisor counter */
CapCounter[i]++;
/* Switch polarity of input capture */
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
TIM_ICInitStructure.TIM_Channel = channel.channel;
TIM_ICInit(channel.timer, &TIM_ICInitStructure);
}
}
}
#endif /* if 0 */
/* Bind Interrupt Handlers
*

192
flight/pios/stm32f4xx/pios_servo.c
View File

@ -39,6 +39,19 @@
static const struct pios_servo_cfg *servo_cfg;
// determine if the related timer will work in synchronous (or OneShot/OneShot125) One Pulse mode.
static uint8_t pios_servo_bank_mode[PIOS_SERVO_BANKS] = { 0 };
// used to skip updates when pulse length is higher than update cycle
static uint16_t pios_servo_bank_next_update[PIOS_SERVO_BANKS] = { 0 };
static uint16_t pios_servo_bank_max_pulse[PIOS_SERVO_BANKS] = { 0 };
// timer associated to each bank
static TIM_TypeDef *pios_servo_bank_timer[PIOS_SERVO_BANKS] = { 0 };
// index of bank used for each pin
static uint8_t *pios_servo_pin_bank;
#define PIOS_SERVO_TIMER_CLOCK 1000000
#define PIOS_SERVO_SAFE_MARGIN 50
/**
* Initialise Servos
*/
@ -52,46 +65,137 @@ int32_t PIOS_Servo_Init(const struct pios_servo_cfg *cfg)
/* Store away the requested configuration */
servo_cfg = cfg;
pios_servo_pin_bank = pios_malloc(sizeof(uint8_t) * cfg->num_channels);
/* Configure the channels to be in output compare mode */
for (uint8_t i = 0; i < cfg->num_channels; i++) {
const struct pios_tim_channel *chan = &cfg->channels[i];
uint8_t bank = 0;
for (uint8_t i = 0; (i < servo_cfg->num_channels); i++) {
const struct pios_tim_channel *chan = &servo_cfg->channels[i];
bool new = true;
/* See if any previous channels use that same timer */
for (uint8_t j = 0; (j < i) && new; j++) {
new &= chan->timer != servo_cfg->channels[j].timer;
}
if (new) {
PIOS_Assert(bank < PIOS_SERVO_BANKS);
for (uint8_t j = i; j < servo_cfg->num_channels; j++) {
if (servo_cfg->channels[j].timer == chan->timer) {
pios_servo_pin_bank[j] = bank;
}
}
pios_servo_bank_timer[bank] = chan->timer;
TIM_ARRPreloadConfig(chan->timer, ENABLE);
TIM_CtrlPWMOutputs(chan->timer, ENABLE);
TIM_Cmd(chan->timer, DISABLE);
bank++;
}
/* Set up for output compare function */
switch (chan->timer_chan) {
case TIM_Channel_1:
TIM_OC1Init(chan->timer, &cfg->tim_oc_init);
TIM_OC1Init(chan->timer, &servo_cfg->tim_oc_init);
TIM_OC1PreloadConfig(chan->timer, TIM_OCPreload_Enable);
break;
case TIM_Channel_2:
TIM_OC2Init(chan->timer, &cfg->tim_oc_init);
TIM_OC2Init(chan->timer, &servo_cfg->tim_oc_init);
TIM_OC2PreloadConfig(chan->timer, TIM_OCPreload_Enable);
break;
case TIM_Channel_3:
TIM_OC3Init(chan->timer, &cfg->tim_oc_init);
TIM_OC3Init(chan->timer, &servo_cfg->tim_oc_init);
TIM_OC3PreloadConfig(chan->timer, TIM_OCPreload_Enable);
break;
case TIM_Channel_4:
TIM_OC4Init(chan->timer, &cfg->tim_oc_init);
TIM_OC4Init(chan->timer, &servo_cfg->tim_oc_init);
TIM_OC4PreloadConfig(chan->timer, TIM_OCPreload_Enable);
break;
}
TIM_ARRPreloadConfig(chan->timer, ENABLE);
TIM_CtrlPWMOutputs(chan->timer, ENABLE);
TIM_Cmd(chan->timer, ENABLE);
}
return 0;
}
void PIOS_Servo_SetBankMode(uint8_t bank, uint8_t mode)
{
PIOS_Assert(bank < PIOS_SERVO_BANKS);
pios_servo_bank_mode[bank] = mode;
if (pios_servo_bank_timer[bank]) {
for (uint8_t i = 0; (i < servo_cfg->num_channels); i++) {
if (pios_servo_pin_bank[i] == bank) {
const struct pios_tim_channel *chan = &servo_cfg->channels[i];
/* Set up for output compare function */
switch (chan->timer_chan) {
case TIM_Channel_1:
TIM_OC1PolarityConfig(chan->timer, TIM_OCPolarity_High);
break;
case TIM_Channel_2:
TIM_OC2PolarityConfig(chan->timer, TIM_OCPolarity_High);
break;
case TIM_Channel_3:
TIM_OC3PolarityConfig(chan->timer, TIM_OCPolarity_High);
break;
case TIM_Channel_4:
TIM_OC4PolarityConfig(chan->timer, TIM_OCPolarity_High);
break;
}
}
}
// Setup the timer accordingly
TIM_SelectOnePulseMode(pios_servo_bank_timer[bank], TIM_OPMode_Repetitive);
TIM_Cmd(pios_servo_bank_timer[bank], ENABLE);
}
}
void PIOS_Servo_Update()
{
for (uint8_t i = 0; (i < PIOS_SERVO_BANKS); i++) {
const TIM_TypeDef *timer = pios_servo_bank_timer[i];
if (timer && pios_servo_bank_mode[i] == PIOS_SERVO_BANK_MODE_SINGLE_PULSE) {
// a pulse to be generated is longer than cycle period. skip this update.
if (TIM_GetCounter((TIM_TypeDef *)timer) > (uint32_t)(pios_servo_bank_next_update[i] + PIOS_SERVO_SAFE_MARGIN)) {
TIM_GenerateEvent((TIM_TypeDef *)timer, TIM_EventSource_Update);
pios_servo_bank_next_update[i] = pios_servo_bank_max_pulse[i];
}
}
pios_servo_bank_max_pulse[i] = 0;
}
for (uint8_t i = 0; (i < servo_cfg->num_channels); i++) {
uint8_t bank = pios_servo_pin_bank[i];
uint8_t mode = pios_servo_bank_mode[bank];
if (mode == PIOS_SERVO_BANK_MODE_SINGLE_PULSE) {
/* Update the position */
const struct pios_tim_channel *chan = &servo_cfg->channels[i];
switch (chan->timer_chan) {
case TIM_Channel_1:
TIM_SetCompare1(chan->timer, 0);
break;
case TIM_Channel_2:
TIM_SetCompare2(chan->timer, 0);
break;
case TIM_Channel_3:
TIM_SetCompare3(chan->timer, 0);
break;
case TIM_Channel_4:
TIM_SetCompare4(chan->timer, 0);
break;
}
}
}
}
/**
* Set the servo update rate (Max 500Hz)
* \param[in] array of rates in Hz
* \param[in] array of timer clocks in Hz
* \param[in] maximum number of banks
*/
void PIOS_Servo_SetHz(const uint16_t *speeds, uint8_t banks)
void PIOS_Servo_SetHz(const uint16_t *speeds, const uint32_t *clock, uint8_t banks)
{
PIOS_Assert(banks <= PIOS_SERVO_BANKS);
if (!servo_cfg) {
return;
}
@ -99,30 +203,22 @@ void PIOS_Servo_SetHz(const uint16_t *speeds, uint8_t banks)
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = servo_cfg->tim_base_init;
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
//
uint8_t set = 0;
for (uint8_t i = 0; (i < servo_cfg->num_channels) && (set < banks); i++) {
bool new = true;
const struct pios_tim_channel *chan = &servo_cfg->channels[i];
/* See if any previous channels use that same timer */
for (uint8_t j = 0; (j < i) && new; j++) {
new &= chan->timer != servo_cfg->channels[j].timer;
}
if (new) {
// Choose the correct prescaler value for the APB the timer is attached
if (chan->timer == TIM1 || chan->timer == TIM8 || chan->timer == TIM9 || chan->timer == TIM10 || chan->timer == TIM11) {
TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1;
} else {
TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / 1000000) - 1;
for (uint8_t i = 0; i < banks && i < PIOS_SERVO_BANKS; i++) {
const TIM_TypeDef *timer = pios_servo_bank_timer[i];
if (timer) {
uint32_t new_clock = PIOS_SERVO_TIMER_CLOCK;
if (clock[i]) {
new_clock = clock[i];
}
TIM_TimeBaseStructure.TIM_Period = ((1000000 / speeds[set]) - 1);
TIM_TimeBaseInit(chan->timer, &TIM_TimeBaseStructure);
set++;
// Choose the correct prescaler value for the APB the timer is attached
if (timer == TIM1 || timer == TIM8 || timer == TIM9 || timer == TIM10 || timer == TIM11) {
TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / new_clock) - 1;
} else {
TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / new_clock) - 1;
}
TIM_TimeBaseStructure.TIM_Period = ((new_clock / speeds[i]) - 1);
TIM_TimeBaseInit((TIM_TypeDef *)timer, &TIM_TimeBaseStructure);
}
}
}
@ -139,22 +235,42 @@ void PIOS_Servo_Set(uint8_t servo, uint16_t position)
return;
}
/* Update the position */
const struct pios_tim_channel *chan = &servo_cfg->channels[servo];
uint16_t val = position;
uint16_t margin = chan->timer->ARR / 50; // Leave 2% of period as margin to prevent overlaps
if (val > (chan->timer->ARR - margin)) {
val = chan->timer->ARR - margin;
}
uint8_t bank = pios_servo_pin_bank[servo];
if (pios_servo_bank_max_pulse[bank] < val) {
pios_servo_bank_max_pulse[bank] = val;
}
switch (chan->timer_chan) {
case TIM_Channel_1:
TIM_SetCompare1(chan->timer, position);
TIM_SetCompare1(chan->timer, val);
break;
case TIM_Channel_2:
TIM_SetCompare2(chan->timer, position);
TIM_SetCompare2(chan->timer, val);
break;
case TIM_Channel_3:
TIM_SetCompare3(chan->timer, position);
TIM_SetCompare3(chan->timer, val);
break;
case TIM_Channel_4:
TIM_SetCompare4(chan->timer, position);
TIM_SetCompare4(chan->timer, val);
break;
}
}
uint8_t PIOS_Servo_GetPinBank(uint8_t pin)
{
if (pin < servo_cfg->num_channels) {
return pios_servo_pin_bank[pin];
} else {
return 0;
}
}
#endif /* PIOS_INCLUDE_SERVO */

6
flight/pios/stm32f4xx/startup.c
View File

@ -29,6 +29,8 @@
#include <string.h>
#include <stm32f4xx.h>
extern int __libc_init_array(void);
/* prototype for main() that tells us not to worry about it possibly returning */
extern int main(void) __attribute__((noreturn));
@ -91,6 +93,10 @@ void _main(void)
/* leave a little space at the top in case memset() isn't a leaf with no locals */
memset(&irq_stack, 0xa5, sizeof(irq_stack) - 64);
#ifdef PIOS_ENABLE_CXX
__libc_init_array();
#endif
/* call main */
(void)main();
}

12
flight/targets/boards/coptercontrol/board_hw_defs.c
View File

@ -1247,6 +1247,18 @@ const struct pios_ppm_cfg pios_ppm_cfg = {
.num_channels = 1,
};
const struct pios_ppm_cfg pios_ppm_pin8_cfg = {
.tim_ic_init = {
.TIM_ICPolarity = TIM_ICPolarity_Rising,
.TIM_ICSelection = TIM_ICSelection_DirectTI,
.TIM_ICPrescaler = TIM_ICPSC_DIV1,
.TIM_ICFilter = 0x0,
},
/* Use only the first channel for ppm */
.channels = &pios_tim_rcvrport_all_channels[5],
.num_channels = 1,
};
#endif /* PIOS_INCLUDE_PPM */
#if defined(PIOS_INCLUDE_PPM_FLEXI)

23
flight/targets/boards/coptercontrol/firmware/Makefile
View File

@ -30,6 +30,12 @@ USE_DSP_LIB ?= NO
# Set to YES to build a FW version that will erase data flash memory
ERASE_FLASH ?= NO
# Set to yes to include Gcsreceiver module
GCSRECEIVER ?= NO
# Enable Diag tasks ?
DIAG_TASKS ?= NO
# List of mandatory modules to include
MODULES += Attitude
MODULES += Stabilization
@ -103,7 +109,6 @@ ifndef TESTAPP
SRC += $(OPUAVSYNTHDIR)/gyrostate.c
SRC += $(OPUAVSYNTHDIR)/attitudestate.c
SRC += $(OPUAVSYNTHDIR)/manualcontrolcommand.c
SRC += $(OPUAVSYNTHDIR)/i2cstats.c
SRC += $(OPUAVSYNTHDIR)/watchdogstatus.c
SRC += $(OPUAVSYNTHDIR)/manualcontrolsettings.c
SRC += $(OPUAVSYNTHDIR)/flightmodesettings.c
@ -116,15 +121,23 @@ ifndef TESTAPP
SRC += $(OPUAVSYNTHDIR)/gpsvelocitysensor.c
SRC += $(OPUAVSYNTHDIR)/gpssettings.c
SRC += $(OPUAVSYNTHDIR)/hwsettings.c
SRC += $(OPUAVSYNTHDIR)/gcsreceiver.c
SRC += $(OPUAVSYNTHDIR)/receiveractivity.c
SRC += $(OPUAVSYNTHDIR)/taskinfo.c
SRC += $(OPUAVSYNTHDIR)/callbackinfo.c
SRC += $(OPUAVSYNTHDIR)/mixerstatus.c
SRC += $(OPUAVSYNTHDIR)/ratedesired.c
SRC += $(OPUAVSYNTHDIR)/txpidsettings.c
SRC += $(OPUAVSYNTHDIR)/mpu6000settings.c
SRC += $(OPUAVSYNTHDIR)/perfcounter.c
# Command line option for Gcsreceiver module
ifeq ($(GCSRECEIVER), YES)
SRC += $(OPUAVSYNTHDIR)/gcsreceiver.c
endif
# Enable Diag tasks and UAVOs needed
ifeq ($(DIAG_TASKS), YES)
CDEFS += -DDIAG_TASKS
SRC += $(OPUAVSYNTHDIR)/taskinfo.c
SRC += $(OPUAVSYNTHDIR)/callbackinfo.c
SRC += $(OPUAVSYNTHDIR)/perfcounter.c
SRC += $(OPUAVSYNTHDIR)/i2cstats.c
endif
else
## Test Code
SRC += $(OPTESTS)/test_common.c

8
flight/targets/boards/coptercontrol/firmware/inc/pios_config.h
View File

@ -97,10 +97,10 @@
/* PIOS receiver drivers */
#define PIOS_INCLUDE_PWM
#define PIOS_INCLUDE_PPM
#define PIOS_INCLUDE_PPM_FLEXI
/* #define PIOS_INCLUDE_PPM_FLEXI */
#define PIOS_INCLUDE_DSM
#define PIOS_INCLUDE_SBUS
#define PIOS_INCLUDE_GCSRCVR
/* #define PIOS_INCLUDE_GCSRCVR */
/* #define PIOS_INCLUDE_OPLINKRCVR */
/* PIOS abstract receiver interface */
@ -142,7 +142,7 @@
/* #define PIOS_TELEM_PRIORITY_QUEUE */
#define PIOS_INCLUDE_GPS
#define PIOS_GPS_MINIMAL
#define PIOS_INCLUDE_GPS_NMEA_PARSER
/* #define PIOS_INCLUDE_GPS_NMEA_PARSER */
#define PIOS_INCLUDE_GPS_UBX_PARSER
/* #define PIOS_GPS_SETS_HOMELOCATION */
@ -162,7 +162,7 @@
/* Task stack sizes */
#define PIOS_ACTUATOR_STACK_SIZE 820
#define PIOS_MANUAL_STACK_SIZE 635
#define PIOS_MANUAL_STACK_SIZE 735
#define PIOS_RECEIVER_STACK_SIZE 620
#define PIOS_STABILIZATION_STACK_SIZE 400

102
flight/targets/boards/coptercontrol/firmware/pios_board.c
View File

@ -32,10 +32,16 @@
#include <manualcontrolsettings.h>
#include <gcsreceiver.h>
#include <taskinfo.h>
#include <sanitycheck.h>
#include <actuatorsettings.h>
#ifdef PIOS_INCLUDE_INSTRUMENTATION
#include <pios_instrumentation.h>
#endif
#if defined(PIOS_INCLUDE_ADXL345)
#include <pios_adxl345.h>
#endif
/*
* Pull in the board-specific static HW definitions.
* Including .c files is a bit ugly but this allows all of
@ -52,6 +58,9 @@
*/
uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE];
static SystemAlarmsExtendedAlarmStatusOptions CopterControlConfigHook();
static void ActuatorSettingsUpdatedCb(UAVObjEvent *ev);
#define PIOS_COM_TELEM_RF_RX_BUF_LEN 32
#define PIOS_COM_TELEM_RF_TX_BUF_LEN 12
@ -232,8 +241,10 @@ void PIOS_Board_Init(void)
HwSettingsInitialize();
#ifndef ERASE_FLASH
#ifdef PIOS_INCLUDE_WDG
/* Initialize watchdog as early as possible to catch faults during init */
PIOS_WDG_Init();
#endif
#endif
/* Initialize the alarms library */
@ -712,8 +723,8 @@ void PIOS_Board_Init(void)
uint8_t hwsettings_rcvrport;
HwSettingsCC_RcvrPortGet(&hwsettings_rcvrport);
switch (hwsettings_rcvrport) {
case HWSETTINGS_CC_RCVRPORT_DISABLED:
switch ((HwSettingsCC_RcvrPortOptions)hwsettings_rcvrport) {
case HWSETTINGS_CC_RCVRPORT_DISABLEDONESHOT:
#if defined(PIOS_INCLUDE_HCSR04)
{
uint32_t pios_hcsr04_id;
@ -721,7 +732,7 @@ void PIOS_Board_Init(void)
}
#endif
break;
case HWSETTINGS_CC_RCVRPORT_PWM:
case HWSETTINGS_CC_RCVRPORT_PWMNOONESHOT:
#if defined(PIOS_INCLUDE_PWM)
{
uint32_t pios_pwm_id;
@ -735,12 +746,17 @@ void PIOS_Board_Init(void)
}
#endif /* PIOS_INCLUDE_PWM */
break;
case HWSETTINGS_CC_RCVRPORT_PPM:
case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTS:
case HWSETTINGS_CC_RCVRPORT_PPMNOONESHOT:
case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTSNOONESHOT:
case HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT:
#if defined(PIOS_INCLUDE_PPM)
{
uint32_t pios_ppm_id;
PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg);
if (hwsettings_rcvrport == HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT) {
PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_pin8_cfg);
} else {
PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg);
}
uint32_t pios_ppm_rcvr_id;
if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) {
@ -750,7 +766,7 @@ void PIOS_Board_Init(void)
}
#endif /* PIOS_INCLUDE_PPM */
break;
case HWSETTINGS_CC_RCVRPORT_PPMPWM:
case HWSETTINGS_CC_RCVRPORT_PPMPWMNOONESHOT:
/* This is a combination of PPM and PWM inputs */
#if defined(PIOS_INCLUDE_PPM)
{
@ -777,6 +793,8 @@ void PIOS_Board_Init(void)
}
#endif /* PIOS_INCLUDE_PWM */
break;
case HWSETTINGS_CC_RCVRPORT_OUTPUTSONESHOT:
break;
}
#if defined(PIOS_INCLUDE_GCSRCVR)
@ -794,15 +812,16 @@ void PIOS_Board_Init(void)
GPIO_PinRemapConfig(GPIO_Remap_SWJ_NoJTRST, ENABLE);
#ifndef PIOS_ENABLE_DEBUG_PINS
switch (hwsettings_rcvrport) {
case HWSETTINGS_CC_RCVRPORT_DISABLED:
case HWSETTINGS_CC_RCVRPORT_PWM:
case HWSETTINGS_CC_RCVRPORT_PPM:
case HWSETTINGS_CC_RCVRPORT_PPMPWM:
switch ((HwSettingsCC_RcvrPortOptions)hwsettings_rcvrport) {
case HWSETTINGS_CC_RCVRPORT_DISABLEDONESHOT:
case HWSETTINGS_CC_RCVRPORT_PWMNOONESHOT:
case HWSETTINGS_CC_RCVRPORT_PPMNOONESHOT:
case HWSETTINGS_CC_RCVRPORT_PPMPWMNOONESHOT:
case HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT:
PIOS_Servo_Init(&pios_servo_cfg);
break;
case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTS:
case HWSETTINGS_CC_RCVRPORT_OUTPUTS:
case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTSNOONESHOT:
case HWSETTINGS_CC_RCVRPORT_OUTPUTSONESHOT:
PIOS_Servo_Init(&pios_servo_rcvr_cfg);
break;
}
@ -831,7 +850,7 @@ void PIOS_Board_Init(void)
}
PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg);
PIOS_MPU6000_CONFIG_Configure();
init_test = PIOS_MPU6000_Test();
init_test = !PIOS_MPU6000_Driver.test(0);
#endif /* PIOS_INCLUDE_MPU6000 */
break;
@ -841,6 +860,59 @@ void PIOS_Board_Init(void)
/* Make sure we have at least one telemetry link configured or else fail initialization */
PIOS_Assert(pios_com_telem_rf_id || pios_com_telem_usb_id);
// Attach the board config check hook
SANITYCHECK_AttachHook(&CopterControlConfigHook);
// trigger a config check if actuatorsettings are updated
ActuatorSettingsInitialize();
ActuatorSettingsConnectCallback(ActuatorSettingsUpdatedCb);
}
SystemAlarmsExtendedAlarmStatusOptions CopterControlConfigHook()
{
// inhibit usage of oneshot for non supported RECEIVER port modes
uint8_t recmode;
HwSettingsCC_RcvrPortGet(&recmode);
uint8_t flexiMode;
uint8_t modes[ACTUATORSETTINGS_BANKMODE_NUMELEM];
ActuatorSettingsBankModeGet(modes);
HwSettingsCC_FlexiPortGet(&flexiMode);
switch ((HwSettingsCC_RcvrPortOptions)recmode) {
// Those modes allows oneshot usage
case HWSETTINGS_CC_RCVRPORT_DISABLEDONESHOT:
case HWSETTINGS_CC_RCVRPORT_OUTPUTSONESHOT:
case HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT:
if ((recmode == HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT ||
flexiMode == HWSETTINGS_CC_FLEXIPORT_PPM) &&
(modes[3] == ACTUATORSETTINGS_BANKMODE_PWMSYNC ||
modes[3] == ACTUATORSETTINGS_BANKMODE_ONESHOT125)) {
return SYSTEMALARMS_EXTENDEDALARMSTATUS_UNSUPPORTEDCONFIG_ONESHOT;
} else {
return SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE;
}
// inhibit oneshot for the following modes
case HWSETTINGS_CC_RCVRPORT_PPMNOONESHOT:
case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTSNOONESHOT:
case HWSETTINGS_CC_RCVRPORT_PPMPWMNOONESHOT:
case HWSETTINGS_CC_RCVRPORT_PWMNOONESHOT:
for (uint8_t i = 0; i < ACTUATORSETTINGS_BANKMODE_NUMELEM; i++) {
if (modes[i] == ACTUATORSETTINGS_BANKMODE_PWMSYNC ||
modes[i] == ACTUATORSETTINGS_BANKMODE_ONESHOT125) {
return SYSTEMALARMS_EXTENDEDALARMSTATUS_UNSUPPORTEDCONFIG_ONESHOT;;
}
return SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE;
}
}
return SYSTEMALARMS_EXTENDEDALARMSTATUS_UNSUPPORTEDCONFIG_ONESHOT;;
}
// trigger a configuration check if ActuatorSettings are changed.
void ActuatorSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
configuration_check();
}
/**

1
flight/targets/boards/coptercontrol/pios_board.h
View File

@ -251,6 +251,7 @@ extern uint32_t pios_com_hkosd_id;
// -------------------------
#define PIOS_SERVO_UPDATE_HZ 50
#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */
#define PIOS_SERVO_BANKS 6
// --------------------------
// Timer controller settings

4
flight/targets/boards/discoveryf4bare/firmware/pios_board.c
View File

@ -747,11 +747,11 @@ void PIOS_Board_Init(void)
}
/* Set the radio configuration parameters. */
PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, datarate, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, oplinkSettings.ChannelSet, is_coordinator, is_oneway, ppm_mode, ppm_only);
PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, datarate, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, is_coordinator, is_oneway, ppm_mode, ppm_only);
PIOS_RFM22B_SetCoordinatorID(pios_rfm22b_id, oplinkSettings.CoordID);
/* Set the PPM callback if we should be receiving PPM. */
if (ppm_mode) {
if (ppm_mode || (ppm_only && !is_coordinator)) {
PIOS_RFM22B_SetPPMCallback(pios_rfm22b_id, PIOS_Board_PPM_callback);
}

1
flight/targets/boards/discoveryf4bare/pios_board.h
View File

@ -248,6 +248,7 @@ extern uint32_t pios_packet_handler;
// -------------------------
#define PIOS_SERVO_UPDATE_HZ 50
#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */
#define PIOS_SERVO_BANKS 6
// --------------------------
// Timer controller settings

3
flight/targets/boards/gpsplatinum/board_hw_defs.c
View File

@ -205,8 +205,9 @@ void PIOS_SPI_mag_flash_irq_handler(void)
#endif /* PIOS_INCLUDE_FLASH */
#if defined(PIOS_INCLUDE_HMC5X83)
pios_hmc5x83_dev_t onboard_mag;
#include "pios_hmc5x83.h"
pios_hmc5x83_dev_t onboard_mag;
#ifdef PIOS_HMC5X83_HAS_GPIOS
bool pios_board_mag_handler()
{

3
flight/targets/boards/oplinkmini/firmware/Makefile
View File

@ -48,6 +48,9 @@ ifndef TESTAPP
SRC += $(OPUAVOBJ)/uavobjectpersistence.c
SRC += $(OPUAVOBJ)/eventdispatcher.c
## Misc library functions
SRC += $(FLIGHTLIB)/sha1.c
## UAVObjects
SRC += $(OPUAVSYNTHDIR)/oplinkstatus.c
SRC += $(OPUAVSYNTHDIR)/oplinksettings.c

4
flight/targets/boards/oplinkmini/firmware/pios_board.c
View File

@ -438,11 +438,11 @@ void PIOS_Board_Init(void)
}
// Set the radio configuration parameters.
PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, datarate, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, oplinkSettings.ChannelSet, is_coordinator, is_oneway, ppm_mode, ppm_only);
PIOS_RFM22B_SetCoordinatorID(pios_rfm22b_id, oplinkSettings.CoordID);
PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, datarate, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, is_coordinator, is_oneway, ppm_mode, ppm_only);
/* Set the PPM callback if we should be receiving PPM. */
if (ppm_mode) {
if (ppm_mode || (ppm_only && !is_coordinator)) {
PIOS_RFM22B_SetPPMCallback(pios_rfm22b_id, PIOS_Board_PPM_callback);
}

1
flight/targets/boards/oplinkmini/pios_board.h
View File

@ -260,6 +260,7 @@ extern uint32_t pios_ppm_out_id;
// -------------------------
#define PIOS_SERVO_UPDATE_HZ 50
#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */
#define PIOS_SERVO_BANKS 6
// --------------------------
// Timer controller settings

2
flight/targets/boards/osd/firmware/Makefile
View File

@ -31,7 +31,7 @@ MODULES += Osd/osdgen
MODULES += Osd/osdinput
MODULES += Osd/WavPlayer
MODULES += GPS
MODULES += Extensions/MagBaro
#MODULES += Extensions/MagBaro
MODULES += FirmwareIAP
MODULES += Telemetry

2
flight/targets/boards/osd/pios_board.h
View File

@ -247,6 +247,8 @@ extern uint32_t pios_com_telem_usb_id;
// --------------------------
#define PIOS_TIM_MAX_DEVS 6
#define PIOS_SERVO_BANKS 6
// -------------------------
// USB
// -------------------------

12
flight/targets/boards/revolution/firmware/Makefile
View File

@ -24,16 +24,16 @@ endif
include ../board-info.mk
include $(ROOT_DIR)/make/firmware-defs.mk
# REVO C++ support
USE_CXX = YES
# ARM DSP library
USE_DSP_LIB ?= NO
# List of mandatory modules to include
MODULES += Sensors
#MODULES += Attitude/revolution
MODULES += StateEstimation # use instead of Attitude
MODULES += Altitude/revolution
MODULES += StateEstimation
MODULES += Airspeed
#MODULES += AltitudeHold # now integrated in Stabilization
MODULES += Stabilization
MODULES += ManualControl
MODULES += Receiver
@ -70,7 +70,7 @@ ifndef TESTAPP
## Application Core
SRC += ../pios_usb_board_data.c
SRC += $(OPMODULEDIR)/System/systemmod.c
SRC += $(OPSYSTEM)/revolution.c
CPPSRC += $(OPSYSTEM)/revolution.cpp
SRC += $(OPSYSTEM)/pios_board.c
SRC += $(FLIGHTLIB)/alarms.c
SRC += $(FLIGHTLIB)/instrumentation.c
@ -91,6 +91,8 @@ ifndef TESTAPP
SRC += $(FLIGHTLIB)/insgps13state.c
SRC += $(FLIGHTLIB)/auxmagsupport.c
SRC += $(FLIGHTLIB)/lednotification.c
SRC += $(FLIGHTLIB)/sha1.c
CPPSRC += $(FLIGHTLIB)/mini_cpp.cpp
## UAVObjects
include ./UAVObjects.inc

21
flight/targets/boards/revolution/firmware/pios_board.c
View File

@ -206,7 +206,7 @@ static const struct pios_mpu6000_cfg pios_mpu6000_cfg = {
.orientation = PIOS_MPU6000_TOP_180DEG,
.fast_prescaler = PIOS_SPI_PRESCALER_4,
.std_prescaler = PIOS_SPI_PRESCALER_64,
.max_downsample = 16,
.max_downsample = 20,
};
#endif /* PIOS_INCLUDE_MPU6000 */
@ -770,11 +770,11 @@ void PIOS_Board_Init(void)
}
/* Set the radio configuration parameters. */
PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, datarate, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, oplinkSettings.ChannelSet, is_coordinator, is_oneway, ppm_mode, ppm_only);
PIOS_RFM22B_SetCoordinatorID(pios_rfm22b_id, oplinkSettings.CoordID);
PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, datarate, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, is_coordinator, is_oneway, ppm_mode, ppm_only);
/* Set the PPM callback if we should be receiving PPM. */
if (ppm_mode) {
if (ppm_mode || (ppm_only && !is_coordinator)) {
PIOS_RFM22B_SetPPMCallback(pios_rfm22b_id, PIOS_Board_PPM_callback);
}
@ -922,20 +922,23 @@ void PIOS_Board_Init(void)
PIOS_ADC_Init(&pios_adc_cfg);
#endif
#if defined(PIOS_INCLUDE_MPU6000)
PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg);
PIOS_MPU6000_CONFIG_Configure();
PIOS_MPU6000_Register();
#endif
#if defined(PIOS_INCLUDE_HMC5X83)
onboard_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_mag_pressure_adapter_id, 0);
PIOS_HMC5x83_Register(onboard_mag);
#endif
#if defined(PIOS_INCLUDE_MS5611)
PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_mag_pressure_adapter_id);
PIOS_MS5611_Register();
#endif
#if defined(PIOS_INCLUDE_MPU6000)
PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg);
PIOS_MPU6000_CONFIG_Configure();
#endif
#ifdef PIOS_INCLUDE_WS2811
#ifdef PIOS_INCLUDE_WS2811
#include <pios_ws2811.h>
HwSettingsWS2811LED_OutOptions ws2811_pin_settings;
HwSettingsWS2811LED_OutGet(&ws2811_pin_settings);

3
flight/targets/boards/revolution/firmware/revolution.c → flight/targets/boards/revolution/firmware/revolution.cpp
View File

@ -31,7 +31,7 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
extern "C" {
#include "inc/openpilot.h"
#include <uavobjectsinit.h>
@ -74,6 +74,7 @@ static void initTask(void *parameters);
/* Prototype of generated InitModules() function */
extern void InitModules(void);
}
/**
* OpenPilot Main function:

2
flight/targets/boards/revolution/pios_board.h
View File

@ -268,7 +268,7 @@ extern uint32_t pios_packet_handler;
// -------------------------
#define PIOS_SERVO_UPDATE_HZ 50
#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */
#define PIOS_SERVO_BANKS 6
// --------------------------
// Timer controller settings
// --------------------------

5
flight/targets/boards/revoproto/firmware/Makefile
View File

@ -29,11 +29,8 @@ USE_DSP_LIB ?= NO
# List of mandatory modules to include
MODULES += Sensors
#MODULES += Attitude/revolution
MODULES += StateEstimation # use instead of Attitude
MODULES += Altitude/revolution
MODULES += StateEstimation
MODULES += Airspeed
#MODULES += AltitudeHold # now integrated in Stabilization
MODULES += Stabilization
MODULES += ManualControl
MODULES += Receiver

2
flight/targets/boards/revoproto/firmware/inc/pios_config.h
View File

@ -108,8 +108,6 @@
#define PIOS_INCLUDE_IAP
#define PIOS_INCLUDE_SERVO
/* #define PIOS_INCLUDE_I2C_ESC */
#define PIOS_INCLUDE_OVERO
#define PIOS_OVERO_SPI
/* #define PIOS_INCLUDE_SDCARD */
/* #define LOG_FILENAME "startup.log" */
#define PIOS_INCLUDE_FLASH

8
flight/targets/boards/revoproto/firmware/pios_board.c
View File

@ -82,13 +82,14 @@ void PIOS_ADC_DMC_irq_handler(void)
#endif /* if defined(PIOS_INCLUDE_ADC) */
#if defined(PIOS_INCLUDE_HMC5X83)
#include "pios_hmc5x83.h"
pios_hmc5x83_dev_t onboard_mag = 0;
bool pios_board_internal_mag_handler()
{
return PIOS_HMC5x83_IRQHandler(onboard_mag);
}
#include "pios_hmc5x83.h"
static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = {
.vector = pios_board_internal_mag_handler,
.line = EXTI_Line5,
@ -893,19 +894,23 @@ void PIOS_Board_Init(void)
#if defined(PIOS_INCLUDE_HMC5X83)
onboard_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_mag_adapter_id, 0);
PIOS_HMC5x83_Register(onboard_mag);
#endif
#if defined(PIOS_INCLUDE_MS5611)
PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_pressure_adapter_id);
PIOS_MS5611_Register();
#endif
switch (bdinfo->board_rev) {
case 0x01:
#if defined(PIOS_INCLUDE_L3GD20)
PIOS_Assert(0); // L3gd20 has not been ported to Sensor framework!!!
PIOS_L3GD20_Init(pios_spi_gyro_id, 0, &pios_l3gd20_cfg);
PIOS_Assert(PIOS_L3GD20_Test() == 0);
#endif
#if defined(PIOS_INCLUDE_BMA180)
PIOS_Assert(0); // BMA180 has not been ported to Sensor framework!!!
PIOS_BMA180_Init(pios_spi_accel_id, 0, &pios_bma180_cfg);
PIOS_Assert(PIOS_BMA180_Test() == 0);
#endif
@ -914,6 +919,7 @@ void PIOS_Board_Init(void)
#if defined(PIOS_INCLUDE_MPU6000)
PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg);
PIOS_MPU6000_CONFIG_Configure();
PIOS_MPU6000_Register();
#endif
break;
default:

1
flight/targets/boards/revoproto/pios_board.h
View File

@ -225,6 +225,7 @@ extern uint32_t pios_com_hkosd_id;
// -------------------------
#define PIOS_SERVO_UPDATE_HZ 50
#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */
#define PIOS_SERVO_BANKS 6
// --------------------------
// Timer controller settings

4
ground/openpilotgcs/openpilotgcs.pri
View File

@ -66,6 +66,8 @@ equals(TEST, 1) {
win32:!isEmpty(QMAKE_SH):QMAKE_COPY_DIR = cp -r -f
GCS_SOURCE_TREE = $$PWD
ROOT_DIR = $$GCS_SOURCE_TREE/../..
isEmpty(GCS_BUILD_TREE) {
sub_dir = $$_PRO_FILE_PWD_
sub_dir ~= s,^$$re_escape($$PWD),,
@ -80,7 +82,7 @@ isEmpty(TOOLS_DIR) {
# check for custom enviroment variable,
TOOLS_DIR = $$(OPENPILOT_TOOLS_DIR)
# fallback to default location.
isEmpty(TOOLS_DIR):TOOLS_DIR = $$clean_path($$GCS_SOURCE_TREE/../../tools)
isEmpty(TOOLS_DIR):TOOLS_DIR = $$clean_path($$ROOT_DIR/tools)
}
GCS_APP_PATH = $$GCS_BUILD_TREE/bin

2186
ground/openpilotgcs/share/openpilotgcs/cloudconfig/multirotor/(RACE)FPV260HobbyKin-Multirotor-QuadrocopterX-0482b7a25f7.optmpl Normal file
View File

File diff suppressed because one or more lines are too long

2186
ground/openpilotgcs/share/openpilotgcs/cloudconfig/multirotor/(STOCK)HobbyKingFPV2-Multirotor-QuadrocopterX-0bb44b68883.optmpl Normal file
View File

File diff suppressed because one or more lines are too long

2185
ground/openpilotgcs/share/openpilotgcs/cloudconfig/multirotor/BlackOutB330-Multirotor-QuadrocopterX-e9980c55302.optmpl Normal file
View File

@ -0,0 +1,2185 @@
{
"battery": "2200 4s 60c",
"comment": "Bank 1 Acro + \nBank 2 Attitude mode\npids set for Blheli 13.xx and oneshot enabled",
"controller": "CC3D",
"esc": "Afro 30 Blheli 13.01",
"motor": "Blackout 2208 2000kv",
"name": "BlackOut B330 ",
"nick": "Failsafe",
"objects": [
{
"fields": [
{
"name": "VbarSensitivity",
"type": "float32",
"unit": "frac",
"values": [
{
"name": "Roll",
"value": 0.5
},
{
"name": "Pitch",
"value": 0.5
},
{
"name": "Yaw",
"value": 0.5
}
]
},
{
"name": "VbarRollPI",
"type": "float32",
"unit": "1/(deg/s)",
"values": [
{
"name": "Kp",
"value": 0.004999999888241291
},
{
"name": "Ki",
"value": 0.0020000000949949026
}
]
},
{
"name": "VbarPitchPI",
"type": "float32",
"unit": "1/(deg/s)",
"values": [
{
"name": "Kp",
"value": 0.004999999888241291
},
{
"name": "Ki",
"value": 0.0020000000949949026
}
]
},
{
"name": "VbarYawPI",
"type": "float32",
"unit": "1/(deg/s)",
"values": [
{
"name": "Kp",
"value": 0.004999999888241291
},
{
"name": "Ki",
"value": 0.0020000000949949026
}
]
},
{
"name": "VbarTau",
"type": "float32",
"unit": "sec",
"values": [
{
"name": "0",
"value": 0.5
}
]
},
{
"name": "GyroTau",
"type": "float32",
"unit": "",
"values": [
{
"name": "0",
"value": 0.004999999888241291
}
]
},
{
"name": "DerivativeGamma",
"type": "float32",
"unit": "",
"values": [
{
"name": "0",
"value": 1
}
]
},
{
"name": "AxisLockKp",
"type": "float32",
"unit": "",
"values": [
{
"name": "0",
"value": 2.5
}
]
},
{
"name": "WeakLevelingKp",
"type": "float32",
"unit": "(deg/s)/deg",
"values": [
{
"name": "0",
"value": 0.10000000149011612
}
]
},
{
"name": "CruiseControlMaxPowerFactor",
"type": "float32",
"unit": "x",
"values": [
{
"name": "0",
"value": 3
}
]
},
{
"name": "CruiseControlPowerTrim",
"type": "float32",
"unit": "%",
"values": [
{
"name": "0",
"value": 100
}
]
},
{
"name": "CruiseControlPowerDelayComp",
"type": "float32",
"unit": "sec",
"values": [
{
"name": "0",
"value": 0.25
}
]
},
{
"name": "ScaleToAirspeed",
"type": "float32",
"unit": "m/s",
"values": [
{
"name": "0",
"value": 0
}
]
},
{
"name": "ScaleToAirspeedLimits",
"type": "float32",
"unit": "",
"values": [
{
"name": "Min",
"value": 0.05000000074505806
},
{
"name": "Max",
"value": 3
}
]
},
{
"name": "FlightModeMap",
"type": "enum",
"unit": "",
"values": [
{
"name": "0",
"value": "Bank1"
},
{
"name": "1",
"value": "Bank2"
},
{
"name": "2",
"value": "Bank1"
},
{
"name": "3",
"value": "Bank1"
},
{
"name": "4",
"value": "Bank1"
},
{
"name": "5",
"value": "Bank1"
}
]
},
{
"name": "VbarGyroSuppress",
"type": "int8",
"unit": "%",
"values": [
{
"name": "0",
"value": 30
}
]
},
{
"name": "VbarPiroComp",
"type": "enum",
"unit": "",
"values": [
{
"name": "0",
"value": "FALSE"
}
]
},
{
"name": "VbarMaxAngle",
"type": "uint8",
"unit": "deg",
"values": [
{
"name": "0",
"value": 10
}
]
},
{
"name": "DerivativeCutoff",
"type": "uint8",
"unit": "Hz",
"values": [
{
"name": "0",
"value": 20
}
]
},
{
"name": "MaxAxisLock",
"type": "uint8",
"unit": "deg",
"values": [
{
"name": "0",
"value": 30
}
]
},
{
"name": "MaxAxisLockRate",
"type": "uint8",
"unit": "deg/s",
"values": [
{
"name": "0",
"value": 2
}
]
},
{
"name": "MaxWeakLevelingRate",
"type": "uint8",
"unit": "deg/s",
"values": [
{
"name": "0",
"value": 5
}
]
},
{
"name": "RattitudeModeTransition",
"type": "uint8",
"unit": "%",
"values": [
{
"name": "0",
"value": 80
}
]
},
{
"name": "CruiseControlMinThrust",
"type": "int8",
"unit": "%",
"values": [
{
"name": "0",
"value": 5
}
]
},
{
"name": "CruiseControlMaxThrust",
"type": "uint8",
"unit": "%",
"values": [
{
"name": "0",
"value": 90
}
]
},
{
"name": "CruiseControlMaxAngle",
"type": "uint8",
"unit": "deg",
"values": [
{
"name": "0",
"value": 105
}
]
},
{
"name": "CruiseControlFlightModeSwitchPosEnable",
"type": "enum",
"unit": "",
"values": [
{
"name": "0",
"value": "FALSE"
},
{
"name": "1",
"value": "FALSE"
},
{
"name": "2",
"value": "FALSE"
},
{
"name": "3",
"value": "FALSE"
},
{
"name": "4",
"value": "FALSE"
},
{
"name": "5",
"value": "FALSE"
}
]
},
{
"name": "CruiseControlInvertedThrustReversing",
"type": "enum",
"unit": "",
"values": [
{
"name": "0",
"value": "Unreversed"
}
]
},
{
"name": "CruiseControlInvertedPowerOutput",
"type": "enum",
"unit": "",
"values": [
{
"name": "0",
"value": "Zero"
}
]
},
{
"name": "LowThrottleZeroIntegral",
"type": "enum",
"unit": "",
"values": [
{
"name": "0",
"value": "TRUE"
}
]
},
{
"name": "FlightModeAssistMap",
"type": "enum",
"unit": "",
"values": [
{
"name": "0",
"value": "None"
},
{
"name": "1",
"value": "None"
},
{
"name": "2",
"value": "None"
},
{
"name": "3",
"value": "None"
},
{
"name": "4",
"value": "None"
},
{
"name": "5",
"value": "None"
}
]
}
],
"id": "73603180",
"instance": 0,
"name": "StabilizationSettings",
"setting": true
},
{
"fields": [
{
"name": "ManualRate",
"type": "float32",
"unit": "degrees/sec",
"values": [
{
"name": "Roll",
"value": 200
},
{
"name": "Pitch",
"value": 200
},
{
"name": "Yaw",
"value": 220
}
]
},
{
"name": "MaximumRate",
"type": "float32",
"unit": "degrees/sec",
"values": [
{
"name": "Roll",
"value": 500
},
{
"name": "Pitch",
"value": 500
},
{
"name": "Yaw",
"value": 300
}
]
},
{
"name": "RollRatePID",
"type": "float32",
"unit": "",
"values": [
{
"name": "Kp",
"value": 0.002899999963119626
},
{
"name": "Ki",
"value": 0.0065000001341104507
},
{
"name": "Kd",
"value": 3.4000000596279278e-05
},
{
"name": "ILimit",
"value": 0.30000001192092896
}
]
},
{
"name": "PitchRatePID",
"type": "float32",
"unit": "",
"values": [
{
"name": "Kp",
"value": 0.004120000172406435
},
{
"name": "Ki",
"value": 0.0082999998703598976
},
{
"name": "Kd",
"value": 4.1999999666586518e-05
},
{
"name": "ILimit",
"value": 0.30000001192092896
}
]
},
{
"name": "YawRatePID",
"type": "float32",
"unit": "",
"values": [
{
"name": "Kp",
"value": 0.0086000002920627594
},
{
"name": "Ki",
"value": 0.014299999922513962
},
{
"name": "Kd",
"value": 4.9999998736893758e-05
},
{
"name": "ILimit",
"value": 0.30000001192092896
}
]
},
{
"name": "RollPI",
"type": "float32",
"unit": "",
"values": [
{
"name": "Kp",
"value": 2.9000000953674316
},
{
"name": "Ki",
"value": 0
},
{
"name": "ILimit",
"value": 50
}
]
},
{
"name": "PitchPI",
"type": "float32",
"unit": "",
"values": [
{
"name": "Kp",
"value": 3.2999999523162842
},
{
"name": "Ki",
"value": 0
},
{
"name": "ILimit",
"value": 50
}
]
},
{
"name": "YawPI",
"type": "float32",
"unit": "",
"values": [
{
"name": "Kp",
"value": 2.5
},
{
"name": "Ki",
"value": 0
},
{
"name": "ILimit",
"value": 50
}
]
},
{
"name": "AcroInsanityFactor",
"type": "float32",
"unit": "percent",
"values": [
{
"name": "0",
"value": 0.4699999988079071
}
]
},
{
"name": "ThrustPIDScaleCurve",
"type": "float32",
"unit": "percent",
"values": [
{
"name": "0",
"value": 0.0042716437019407749
},
{
"name": "25",
"value": -0.021436728537082672
},
{
"name": "50",
"value": -0.051431018859148026
},
{
"name": "75",
"value": -0.15000000596046448
},
{
"name": "100",
"value": -0.34714511036872864
}
]
},
{
"name": "RollMax",
"type": "uint8",
"unit": "degrees",
"values": [
{
"name": "0",
"value": 55
}
]
},
{
"name": "PitchMax",
"type": "uint8",
"unit": "degrees",
"values": [
{
"name": "0",
"value": 55
}
]
},
{
"name": "YawMax",
"type": "uint8",
"unit": "degrees",
"values": [
{
"name": "0",
"value": 35
}
]
},
{
"name": "StickExpo",
"type": "int8",
"unit": "percent",
"values": [
{
"name": "Roll",
"value": 12
},
{
"name": "Pitch",
"value": 12
},
{
"name": "Yaw",
"value": -5
}
]
},
{
"name": "EnablePiroComp",
"type": "enum",
"unit": "",
"values": [
{
"name": "0",
"value": "FALSE"
}
]
},
{
"name": "EnableThrustPIDScaling",
"type": "enum",
"unit": "",
"values": [
{
"name": "0",
"value": "TRUE"
}
]
},
{
"name": "ThrustPIDScaleSource",
"type": "enum",
"unit": "",
"values": [
{
"name": "0",
"value": "ActuatorDesiredThrust"
}
]
},
{
"name": "ThrustPIDScaleTarget",
"type": "enum",
"unit": "",
"values": [
{
"name": "0",
"value": "PD"
}
]
},
{
"name": "ThrustPIDScaleAxes",
"type": "enum",
"unit": "",
"values": [
{
"name": "0",
"value": "Roll Pitch"
}
]
}
],
"id": "E8EBBD48",
"instance": 0,
"name": "StabilizationSettingsBank1",
"setting": true
},
{
"fields": [
{
"name": "ManualRate",
"type": "float32",
"unit": "degrees/sec",
"values": [
{
"name": "Roll",
"value": 200
},
{
"name": "Pitch",
"value": 200
},
{
"name": "Yaw",
"value": 220
}
]
},
{
"name": "MaximumRate",
"type": "float32",
"unit": "degrees/sec",
"values": [
{
"name": "Roll",
"value": 500
},
{
"name": "Pitch",
"value": 500
},
{
"name": "Yaw",
"value": 300
}
]
},
{
"name": "RollRatePID",
"type": "float32",
"unit": "",
"values": [
{
"name": "Kp",
"value": 0.00279999990016222
},
{
"name": "Ki",
"value": 0.0054999999701976776
},
{
"name": "Kd",
"value": 2.9000000722589903e-05
},
{
"name": "ILimit",
"value": 0.30000001192092896
}
]
},
{
"name": "PitchRatePID",
"type": "float32",
"unit": "",
"values": [
{
"name": "Kp",
"value": 0.0036200000904500484
},
{
"name": "Ki",
"value": 0.0086000002920627594
},
{
"name": "Kd",
"value": 3.600000127335079e-05
},
{
"name": "ILimit",
"value": 0.30000001192092896
}
]
},
{
"name": "YawRatePID",
"type": "float32",
"unit": "",
"values": [
{
"name": "Kp",
"value": 0.0086000002920627594
},
{
"name": "Ki",
"value": 0.014299999922513962
},
{
"name": "Kd",
"value": 4.9999998736893758e-05
},
{
"name": "ILimit",
"value": 0.30000001192092896
}
]
},
{
"name": "RollPI",
"type": "float32",
"unit": "",
"values": [
{
"name": "Kp",
"value": 3
},
{
"name": "Ki",
"value": 0
},
{
"name": "ILimit",
"value": 50
}
]
},
{
"name": "PitchPI",
"type": "float32",
"unit": "",
"values": [
{
"name": "Kp",
"value": 3.4000000953674316
},
{
"name": "Ki",
"value": 0
},
{
"name": "ILimit",
"value": 50
}
]
},
{
"name": "YawPI",
"type": "float32",
"unit": "",
"values": [
{
"name": "Kp",
"value": 2.5
},
{
"name": "Ki",
"value": 0
},
{
"name": "ILimit",
"value": 50
}
]
},
{
"name": "AcroInsanityFactor",
"type": "float32",
"unit": "percent",
"values": [
{
"name": "0",
"value": 0.40000000596046448
}
]
},
{
"name": "ThrustPIDScaleCurve",
"type": "float32",
"unit": "percent",
"values": [
{
"name": "0",
"value": 0.012843480333685875
},
{
"name": "25",
"value": -7.1359904723067302e-06
},
{
"name": "50",
"value": -0.12000571191310883
},
{
"name": "75",
"value": -0.18857327103614807
},
{
"name": "100",
"value": -0.25714081525802612
}
]
},
{
"name": "RollMax",
"type": "uint8",
"unit": "degrees",
"values": [
{
"name": "0",
"value": 55
}
]
},
{
"name": "PitchMax",
"type": "uint8",
"unit": "degrees",
"values": [
{
"name": "0",
"value": 55
}
]
},
{
"name": "YawMax",
"type": "uint8",
"unit": "degrees",
"values": [
{
"name": "0",
"value": 35
}
]
},
{
"name": "StickExpo",
"type": "int8",
"unit": "percent",
"values": [
{
"name": "Roll",
"value": 15
},
{
"name": "Pitch",
"value": 15
},
{
"name": "Yaw",
"value": -5
}
]
},
{
"name": "EnablePiroComp",
"type": "enum",
"unit": "",
"values": [
{
"name": "0",
"value": "FALSE"
}
]
},
{
"name": "EnableThrustPIDScaling",
"type": "enum",
"unit": "",
"values": [
{
"name": "0",
"value": "TRUE"
}
]
},
{
"name": "ThrustPIDScaleSource",
"type": "enum",
"unit": "",
"values": [
{
"name": "0",
"value": "ActuatorDesiredThrust"
}
]
},
{
"name": "ThrustPIDScaleTarget",
"type": "enum",
"unit": "",
"values": [
{
"name": "0",
"value": "PD"
}
]
},
{
"name": "ThrustPIDScaleAxes",
"type": "enum",
"unit": "",
"values": [
{
"name": "0",
"value": "Roll Pitch"
}
]
}
],
"id": "70E9539A",
"instance": 0,
"name": "StabilizationSettingsBank2",
"setting": true
},
{
"fields": [
{
"name": "ManualRate",
"type": "float32",
"unit": "degrees/sec",
"values": [
{
"name": "Roll",
"value": 220
},
{
"name": "Pitch",
"value": 220
},
{
"name": "Yaw",
"value": 220
}
]
},
{
"name": "MaximumRate",
"type": "float32",
"unit": "degrees/sec",
"values": [
{
"name": "Roll",
"value": 300
},
{
"name": "Pitch",
"value": 300
},
{
"name": "Yaw",
"value": 300
}
]
},
{
"name": "RollRatePID",
"type": "float32",
"unit": "",
"values": [
{
"name": "Kp",
"value": 0.0024999999441206455
},
{
"name": "Ki",
"value": 0.0040000001899898052
},
{
"name": "Kd",
"value": 1.9999999494757503e-05
},
{
"name": "ILimit",
"value": 0.30000001192092896
}
]
},
{
"name": "PitchRatePID",
"type": "float32",
"unit": "",
"values": [
{
"name": "Kp",
"value": 0.0024999999441206455
},
{
"name": "Ki",
"value": 0.0040000001899898052
},
{
"name": "Kd",
"value": 1.9999999494757503e-05
},
{
"name": "ILimit",
"value": 0.30000001192092896
}
]
},
{
"name": "YawRatePID",
"type": "float32",
"unit": "",
"values": [
{
"name": "Kp",
"value": 0.0062000001780688763
},
{
"name": "Ki",
"value": 0.0099999997764825821
},
{
"name": "Kd",
"value": 4.9999998736893758e-05
},
{
"name": "ILimit",
"value": 0.30000001192092896
}
]
},
{
"name": "RollPI",
"type": "float32",
"unit": "",
"values": [
{
"name": "Kp",
"value": 2.5
},
{
"name": "Ki",
"value": 0
},
{
"name": "ILimit",
"value": 50
}
]
},
{
"name": "PitchPI",
"type": "float32",
"unit": "",
"values": [
{
"name": "Kp",
"value": 2.5
},
{
"name": "Ki",
"value": 0
},
{
"name": "ILimit",
"value": 50
}
]
},
{
"name": "YawPI",
"type": "float32",
"unit": "",
"values": [
{
"name": "Kp",
"value": 2.5
},
{
"name": "Ki",
"value": 0
},
{
"name": "ILimit",
"value": 50
}
]
},
{
"name": "AcroInsanityFactor",
"type": "float32",
"unit": "percent",
"values": [
{
"name": "0",
"value": 0.5
}
]
},
{
"name": "ThrustPIDScaleCurve",
"type": "float32",
"unit": "percent",
"values": [
{
"name": "0",
"value": 0.30000001192092896
},
{
"name": "25",
"value": 0.15000000596046448
},
{
"name": "50",
"value": 0
},
{
"name": "75",
"value": -0.15000000596046448
},
{
"name": "100",
"value": -0.30000001192092896
}
]
},
{
"name": "RollMax",
"type": "uint8",
"unit": "degrees",
"values": [
{
"name": "0",
"value": 55
}
]
},
{
"name": "PitchMax",
"type": "uint8",
"unit": "degrees",
"values": [
{
"name": "0",
"value": 55
}
]
},
{
"name": "YawMax",
"type": "uint8",
"unit": "degrees",
"values": [
{
"name": "0",
"value": 35
}
]
},
{
"name": "StickExpo",
"type": "int8",
"unit": "percent",
"values": [
{
"name": "Roll",
"value": 0
},
{
"name": "Pitch",
"value": 0
},
{
"name": "Yaw",
"value": 0
}
]
},
{
"name": "EnablePiroComp",
"type": "enum",
"unit": "",
"values": [
{
"name": "0",
"value": "TRUE"
}
]
},
{
"name": "EnableThrustPIDScaling",
"type": "enum",
"unit": "",
"values": [
{
"name": "0",
"value": "FALSE"
}
]
},
{
"name": "ThrustPIDScaleSource",
"type": "enum",
"unit": "",
"values": [
{
"name": "0",
"value": "ActuatorDesiredThrust"
}
]
},
{
"name": "ThrustPIDScaleTarget",
"type": "enum",
"unit": "",
"values": [
{
"name": "0",
"value": "PID"
}
]
},
{
"name": "ThrustPIDScaleAxes",
"type": "enum",
"unit": "",
"values": [
{
"name": "0",
"value": "Roll Pitch"
}
]
}
],
"id": "C02DAA6A",
"instance": 0,
"name": "StabilizationSettingsBank3",
"setting": true
},
{
"fields": [
{
"name": "MaxAccel",
"type": "float32",
"unit": "units/sec",
"values": [
{
"name": "0",
"value": 1000
}
]
},
{
"name": "FeedForward",
"type": "float32",
"unit": "",
"values": [
{
"name": "0",
"value": 0
}
]
},
{
"name": "AccelTime",
"type": "float32",
"unit": "ms",
"values": [
{
"name": "0",
"value": 0
}
]
},
{
"name": "DecelTime",
"type": "float32",
"unit": "ms",
"values": [
{
"name": "0",
"value": 0
}
]
},
{
"name": "ThrottleCurve1",
"type": "float32",
"unit": "percent",
"values": [
{
"name": "0",
"value": 0
},
{
"name": "25",
"value": 0.22499999403953552
},
{
"name": "50",
"value": 0.44999998807907104
},
{
"name": "75",
"value": 0.67499995231628418
},
{
"name": "100",
"value": 0.89999997615814209
}
]
},
{
"name": "ThrottleCurve2",
"type": "float32",
"unit": "percent",
"values": [
{
"name": "0",
"value": 0
},
{
"name": "25",
"value": 0.22499999403953552
},
{
"name": "50",
"value": 0.44999998807907104
},
{
"name": "75",
"value": 0.67499995231628418
},
{
"name": "100",
"value": 0.89999997615814209
}
]
},
{
"name": "MixerValueRoll",
"type": "int8",
"unit": "percent",
"values": [
{
"name": "0",
"value": 50
}
]
},
{
"name": "MixerValuePitch",
"type": "int8",
"unit": "percent",
"values": [
{
"name": "0",
"value": 50
}
]
},
{
"name": "MixerValueYaw",
"type": "int8",
"unit": "percent",
"values": [
{
"name": "0",
"value": 50
}
]
},
{
"name": "Curve2Source",
"type": "enum",
"unit": "",
"values": [
{
"name": "0",
"value": "Throttle"
}
]
},
{
"name": "Mixer1Type",
"type": "enum",
"unit": "",
"values": [
{
"name": "0",
"value": "Motor"
}
]
},
{
"name": "Mixer1Vector",
"type": "int8",
"unit": "",
"values": [
{
"name": "ThrottleCurve1",
"value": 127
},
{
"name": "ThrottleCurve2",
"value": 0
},
{
"name": "Roll",
"value": 64
},
{
"name": "Pitch",
"value": 64
},
{
"name": "Yaw",
"value": -64
}
]
},
{
"name": "Mixer2Type",
"type": "enum",
"unit": "",
"values": [
{
"name": "0",
"value": "Motor"
}
]
},
{
"name": "Mixer2Vector",
"type": "int8",
"unit": "",
"values": [
{
"name": "ThrottleCurve1",
"value": 127
},
{
"name": "ThrottleCurve2",
"value": 0
},
{
"name": "Roll",
"value": -64
},
{
"name": "Pitch",
"value": 64
},
{
"name": "Yaw",
"value": 64
}
]
},
{
"name": "Mixer3Type",
"type": "enum",
"unit": "",
"values": [
{
"name": "0",
"value": "Motor"
}
]
},
{
"name": "Mixer3Vector",
"type": "int8",
"unit": "",
"values": [
{
"name": "ThrottleCurve1",
"value": 127
},
{
"name": "ThrottleCurve2",
"value": 0
},
{
"name": "Roll",
"value": -64
},
{
"name": "Pitch",
"value": -64
},
{
"name": "Yaw",
"value": -64
}
]
},
{
"name": "Mixer4Type",
"type": "enum",
"unit": "",
"values": [
{
"name": "0",
"value": "Motor"
}
]
},
{
"name": "Mixer4Vector",
"type": "int8",
"unit": "",
"values": [
{
"name": "ThrottleCurve1",
"value": 127
},
{
"name": "ThrottleCurve2",
"value": 0
},
{
"name": "Roll",
"value": 64
},
{
"name": "Pitch",
"value": -64
},
{
"name": "Yaw",
"value": 64
}
]
},
{
"name": "Mixer5Type",
"type": "enum",
"unit": "",
"values": [
{
"name": "0",
"value": "Disabled"
}
]
},
{
"name": "Mixer5Vector",
"type": "int8",
"unit": "",
"values": [
{
"name": "ThrottleCurve1",
"value": 0
},
{
"name": "ThrottleCurve2",
"value": 0
},
{
"name": "Roll",
"value": 0
},
{
"name": "Pitch",
"value": 0
},
{
"name": "Yaw",
"value": 0
}
]
},
{
"name": "Mixer6Type",
"type": "enum",
"unit": "",
"values": [
{
"name": "0",
"value": "Disabled"
}
]
},
{
"name": "Mixer6Vector",
"type": "int8",
"unit": "",
"values": [
{
"name": "ThrottleCurve1",
"value": 0
},
{
"name": "ThrottleCurve2",
"value": 0
},
{
"name": "Roll",
"value": 0
},
{
"name": "Pitch",
"value": 0
},
{
"name": "Yaw",
"value": 0
}
]
},
{
"name": "Mixer7Type",
"type": "enum",
"unit": "",
"values": [
{
"name": "0",
"value": "Disabled"
}
]
},
{
"name": "Mixer7Vector",
"type": "int8",
"unit": "",
"values": [
{
"name": "ThrottleCurve1",
"value": 0
},
{
"name": "ThrottleCurve2",
"value": 0
},
{
"name": "Roll",
"value": 0
},
{
"name": "Pitch",
"value": 0
},
{
"name": "Yaw",
"value": 0
}
]
},
{
"name": "Mixer8Type",
"type": "enum",
"unit": "",
"values": [
{
"name": "0",
"value": "Disabled"
}
]
},
{
"name": "Mixer8Vector",
"type": "int8",
"unit": "",
"values": [
{
"name": "ThrottleCurve1",
"value": 0
},
{
"name": "ThrottleCurve2",
"value": 0
},
{
"name": "Roll",
"value": 0
},
{
"name": "Pitch",
"value": 0
},
{
"name": "Yaw",
"value": 0
}
]
},
{
"name": "Mixer9Type",
"type": "enum",
"unit": "",
"values": [
{
"name": "0",
"value": "Disabled"
}
]
},
{
"name": "Mixer9Vector",
"type": "int8",
"unit": "",
"values": [
{
"name": "ThrottleCurve1",
"value": 0
},
{
"name": "ThrottleCurve2",
"value": 0
},
{
"name": "Roll",
"value": 0
},
{
"name": "Pitch",
"value": 0
},
{
"name": "Yaw",
"value": 0
}
]
},
{
"name": "Mixer10Type",
"type": "enum",
"unit": "",
"values": [
{
"name": "0",
"value": "Disabled"
}
]
},
{
"name": "Mixer10Vector",
"type": "int8",
"unit": "",
"values": [
{
"name": "ThrottleCurve1",
"value": 0
},
{
"name": "ThrottleCurve2",
"value": 0
},
{
"name": "Roll",
"value": 0
},
{
"name": "Pitch",
"value": 0
},
{
"name": "Yaw",
"value": 0
}
]
},
{
"name": "Mixer11Type",
"type": "enum",
"unit": "",
"values": [
{
"name": "0",
"value": "Disabled"
}
]
},
{
"name": "Mixer11Vector",
"type": "int8",
"unit": "",
"values": [
{
"name": "ThrottleCurve1",
"value": 0
},
{
"name": "ThrottleCurve2",
"value": 0
},
{
"name": "Roll",
"value": 0
},
{
"name": "Pitch",
"value": 0
},
{
"name": "Yaw",
"value": 0
}
]
},
{
"name": "Mixer12Type",
"type": "enum",
"unit": "",
"values": [
{
"name": "0",
"value": "Disabled"
}
]
},
{
"name": "Mixer12Vector",
"type": "int8",
"unit": "",
"values": [
{
"name": "ThrottleCurve1",
"value": 0
},
{
"name": "ThrottleCurve2",
"value": 0
},
{
"name": "Roll",
"value": 0
},
{
"name": "Pitch",
"value": 0
},
{
"name": "Yaw",
"value": 0
}
]
}
],
"id": "7BF2CFA8",
"instance": 0,
"name": "MixerSettings",
"setting": true
},
{
"fields": [
{
"name": "P",
"type": "float32",
"unit": "1^2",
"values": [
{
"name": "PositionNorth",
"value": 10
},
{
"name": "PositionEast",
"value": 10
},
{
"name": "PositionDown",
"value": 10
},
{
"name": "VelocityNorth",
"value": 1
},
{
"name": "VelocityEast",
"value": 1
},
{
"name": "VelocityDown",
"value": 1
},
{
"name": "AttitudeQ1",
"value": 0.0070000002160668373
},
{
"name": "AttitudeQ2",
"value": 0.0070000002160668373
},
{
"name": "AttitudeQ3",
"value": 0.0070000002160668373
},
{
"name": "AttitudeQ4",
"value": 0.0070000002160668373
},
{
"name": "GyroDriftX",
"value": 9.9999999747524271e-07
},
{
"name": "GyroDriftY",
"value": 9.9999999747524271e-07
},
{
"name": "GyroDriftZ",
"value": 9.9999999747524271e-07
}
]
},
{
"name": "Q",
"type": "float32",
"unit": "1^2",
"values": [
{
"name": "GyroX",
"value": 0.0099999997764825821
},
{
"name": "GyroY",
"value": 0.0099999997764825821
},
{
"name": "GyroZ",
"value": 0.0099999997764825821
},
{
"name": "AccelX",
"value": 0.0099999997764825821
},
{
"name": "AccelY",
"value": 0.0099999997764825821
},
{
"name": "AccelZ",
"value": 0.0099999997764825821
},
{
"name": "GyroDriftX",
"value": 9.9999999747524271e-07
},
{
"name": "GyroDriftY",
"value": 9.9999999747524271e-07
},
{
"name": "GyroDriftZ",
"value": 9.9999999747524271e-07
}
]
},
{
"name": "R",
"type": "float32",
"unit": "1^2",
"values": [
{
"name": "GPSPosNorth",
"value": 1
},
{
"name": "GPSPosEast",
"value": 1
},
{
"name": "GPSPosDown",
"value": 1000000
},
{
"name": "GPSVelNorth",
"value": 0.0010000000474974513
},
{
"name": "GPSVelEast",
"value": 0.0010000000474974513
},
{
"name": "GPSVelDown",
"value": 0.0010000000474974513
},
{
"name": "MagX",
"value": 10
},
{
"name": "MagY",
"value": 10
},
{
"name": "MagZ",
"value": 10
},
{
"name": "BaroZ",
"value": 0.0099999997764825821
}
]
},
{
"name": "FakeR",
"type": "float32",
"unit": "1^2",
"values": [
{
"name": "FakeGPSPosIndoor",
"value": 10
},
{
"name": "FakeGPSVelIndoor",
"value": 1
},
{
"name": "FakeGPSVelAirspeed",
"value": 1000
}
]
}
],
"id": "5E91213C",
"instance": 0,
"name": "EKFConfiguration",
"setting": true
}
],
"owner": "Jim ",
"propeller": "HQ DD 6 X4.5",
"servo": "",
"size": "330",
"subtype": 2,
"type": 1,
"uuid": "{5bf30e57-f44b-427d-bfd6-9e9980c55302}",
"weight": "1003"
}

95
ground/openpilotgcs/share/openpilotgcs/cloudconfig/multirotor/BlackoutMinih-Multirotor-QuadrocopterX-32d80b0fda3.optmpl → ground/openpilotgcs/share/openpilotgcs/cloudconfig/multirotor/BlackOutMiniH-Multirotor-QuadrocopterX-d415c242154.optmpl
View File

File diff suppressed because one or more lines are too long

2155
ground/openpilotgcs/share/openpilotgcs/cloudconfig/multirotor/BlackoutB330-Multirotor-QuadrocopterX-4f08bcc7d4f.optmpl
View File

File diff suppressed because one or more lines are too long

113
ground/openpilotgcs/share/openpilotgcs/cloudconfig/multirotor/DJIF450-Multirotor-QuadrocopterX-1e92d4aadcd.optmpl → ground/openpilotgcs/share/openpilotgcs/cloudconfig/multirotor/DJIF450-Multirotor-QuadrocopterX-cfa8ec67993.optmpl
View File

File diff suppressed because one or more lines are too long

2186
ground/openpilotgcs/share/openpilotgcs/cloudconfig/multirotor/FlipFpvPro-Multirotor-QuadrocopterX-c117d151f95.optmpl Normal file
View File

File diff suppressed because one or more lines are too long

78
ground/openpilotgcs/share/openpilotgcs/cloudconfig/multirotor/FlyingCinemaCinetank-Multirotor-QuadrocopterX-5b17bc9cfc0.optmpl → ground/openpilotgcs/share/openpilotgcs/cloudconfig/multirotor/FlyingCinemaCinetan-Multirotor-QuadrocopterX-2c3f9181872.optmpl
View File

File diff suppressed because one or more lines are too long

2186
ground/openpilotgcs/share/openpilotgcs/cloudconfig/multirotor/FlyingCinemaTankitoM-Multirotor-QuadrocopterX-385d9ac9ae1.optmpl Normal file
View File

File diff suppressed because one or more lines are too long

2186
ground/openpilotgcs/share/openpilotgcs/cloudconfig/multirotor/GuerrillaDronesToadQ-Multirotor-QuadrocopterX-8f838e43432.optmpl Normal file
View File

File diff suppressed because one or more lines are too long

2186
ground/openpilotgcs/share/openpilotgcs/cloudconfig/multirotor/H-King320mmCarbonARF-Multirotor-QuadrocopterX-3fe218baaba.optmpl Normal file
View File

File diff suppressed because one or more lines are too long

2186
ground/openpilotgcs/share/openpilotgcs/cloudconfig/multirotor/H-KingCarbon250ARF-Multirotor-QuadrocopterX-b91ae8ac277.optmpl Normal file
View File

File diff suppressed because one or more lines are too long

2155
ground/openpilotgcs/share/openpilotgcs/cloudconfig/multirotor/HoverThingsFLIPFPV-Multirotor-QuadrocopterX-32bf14a10fc.optmpl Normal file
View File

File diff suppressed because one or more lines are too long

2186
ground/openpilotgcs/share/openpilotgcs/cloudconfig/multirotor/QAV400-Multirotor-QuadrocopterX-90a90b9fb50.optmpl Normal file
View File

File diff suppressed because one or more lines are too long

2186
ground/openpilotgcs/share/openpilotgcs/cloudconfig/multirotor/QAV500-Multirotor-QuadrocopterX-6da440c4d7c.optmpl Normal file
View File

File diff suppressed because one or more lines are too long

10
ground/openpilotgcs/share/openpilotgcs/cloudconfig/multirotor/QUADI250-Multirotor-QuadrocopterX-c6c6cfc4cf9.optmpl
View File

File diff suppressed because one or more lines are too long

2155
ground/openpilotgcs/share/openpilotgcs/cloudconfig/multirotor/Qav400-Multirotor-QuadrocopterX-0137e789c29.optmpl
View File

File diff suppressed because one or more lines are too long

2186
ground/openpilotgcs/share/openpilotgcs/cloudconfig/multirotor/RCExplorerV3Tricopte-Multirotor-Tricopter-dcbce3f069c.optmpl Normal file
View File

File diff suppressed because one or more lines are too long

2155
ground/openpilotgcs/share/openpilotgcs/cloudconfig/multirotor/ZMR250( Attitude mode)-ChineseQAV250-Multirotor-QuadrocopterX-5d318c0526c (1).optmpl Normal file
View File

File diff suppressed because one or more lines are too long

Some files were not shown because too many files have changed in this diff Show More