rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-18 08:54:15 +01:00
Code Issues Releases Activity

Merge remote-tracking branch 'origin/next' into rel-14.10

Browse Source
This commit is contained in:
Alessio Morale 2014-10-18 17:58:11 +02:00
commit 3482df62f9
1190 changed files with 239825 additions and 39243 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -34,6 +34,7 @@ ground/openpilotgcs/.settings
/ground/uavobjgenerator/uavobjgenerator.pro.user
/ground/uavobjects/uavobjects.pro.user
/ground/ground.pro.user
/ground/openpilotgcs/src/libs/sdlgamepad.pro.user
# Misc artifacts
/ground/openpilotgcs/share/openpilotgcs/sounds/normalize.exe

11
CREDITS.txt
View File

@ -1,23 +1,27 @@
Connor Abbott
Connor Abbott
Sergiy Anikeyev
David Ankers
Fredrik Arvidsson
Pedro Assuncao
Werner Backes
Jose Barros
Alex Beck
Andre Bernet
Mikael Blomqvist
Pete Boehl
Berkely Brown
Joel Brueziere
Thierry Bugeat
Samuel Brugger
Glenn Campigli
David Carlson
Mike Carr
Stefan Cenkov
Andrés Chavarría Krauser
Cosimo Corrado
James Cotton
Steve Doll
James Duley
Piotr Esden-Tempski
Peter Farnworth
Ed Faulkner
@ -25,6 +29,7 @@ Andrew Finegan
Kevin Finisterre
Richard Flay
Darren Furniss
Oliver Gaste
Cliff Geerdes
Frederic Goddeeris
Daniel Godin
@ -41,8 +46,10 @@ Patrick Huebner
Ryan Hunt
Mark James
Michael Johnston
Stefan Karlsson
Ricky King
Thorsten Klose
Karl Knutsson
Sami Korhonen
Hallvard Kristiansen
Alan Krum
@ -63,6 +70,7 @@ Gary Mortimer
Cathy Moss
Les Newell
Ken Northup
Craig Nuttall
Bertrand Oresve
Angus Peart
John Pike
@ -93,6 +101,7 @@ Alex Sowa
Pete Stapley
Vova Starikh
Rowan Taubitz
Jim Allen Thibodaux
Andrew Thoms
Vladimir Timofeev
Jasper Van Loenen

96
MILESTONES.txt
View File

@ -169,32 +169,6 @@ C: Kavin Gustafson (k_g)
D: October 2012
V: http://www.youtube.com/watch?v=MGO68TqIwKk
M: First successful flight using just a mobile phone
C: Jose (please complete details), demoed in Portugal
D:
V:
M: First CopterControl over 10km FixedWing navigation flight
C:
D:
V:
M: First successful flight using the GCS only and no RC TX
C:
D:
V:
M: First CopterControl Navigation on RC Ground Vechicle
C:
D:
V:
M: First CopterControl Navigation on RC Water Vechicle
C:
D:
V:
M: First Revo Navigated flight on a FixedWing
C: It got done somewhere along the line, likely Corvus.
@ -203,15 +177,56 @@ C: Eric Price (Corvus Corax)
D: March 2012
V:
M: First Revo 20km Navigated flight on a FixedWing
C: Eric Price / Team OP
D: Greece 2013
V: https://vimeo.com/71956880
M: First Auto spot landing on a fixed Wing using Revo
C: Eric Price (Corvus Corax)
D: Greece 2013
V:
M: First Revo Navigated flight on a MultiRotor
C: It got done somewhere along the line, James or Sami
M: First Revo 1km Navigated flight on a MultiRotor
C: Jackson Russell
D: Greece 2013
V:
M: First Revo 5km Navigated flight on a MultiRotor
C: Rodney Grainger (roddersNZ)
D: September 2014
V: https://www.youtube.com/watch?v=DYawRGz5KYM
M: First Revo 6km Navigated flight on a MultiRotor
C:
D:
V:
M: First Revo 5km Navigated flight on a MultiRotor
M: First Revo 7km Navigated flight on a MultiRotor
C:
D:
V:
M: First Revo 8km Navigated flight on a MultiRotor
C: Jim Allen Thibodaux
D: September 2014
V: http://www.youtube.com/watch?v=oeJF8U2k9FA
M: First Revo 9km Navigated flight on a MultiRotor
C:
D:
V:
M: First Revo 10km Navigated flight on a MultiRotor
C:
D:
V:
M: First Revo Position Hold on a Heli
C:
D:
V:
@ -231,33 +246,8 @@ C:
D:
V:
M: First Revo Altitude Hold using Sonar
C:
D:
V:
M: First use of the Magic Waypoint feature
C:
D:
V:
M: First Auto spot landing on a fixed Wing using Revo
C:
D:
V:
M: First Auto take-off on a MultiRotor using Revo
C:
D:
V:
M: First Auto landing on a MultiRotor using Revo
C: Sami (please complete details)
D:
V:
M: First Auto take-off on a Heli using Revo
C:
C: Sami
D:
V:

57
Makefile
View File

@ -197,7 +197,7 @@ export OPUAVSYNTHDIR := $(BUILD_DIR)/uavobject-synthetics/flight
export OPGCSSYNTHDIR := $(BUILD_DIR)/openpilotgcs-synthetics
# Define supported board lists
ALL_BOARDS := coptercontrol oplinkmini revolution osd revoproto simposix discoveryf4bare
ALL_BOARDS := coptercontrol oplinkmini revolution osd revoproto simposix discoveryf4bare gpsplatinum
# Short names of each board (used to display board name in parallel builds)
coptercontrol_short := 'cc '
@ -207,6 +207,7 @@ osd_short := 'osd '
revoproto_short := 'revp'
simposix_short := 'posx'
discoveryf4bare_short := 'df4b'
gpsplatinum_short := 'gps9 '
# SimPosix only builds on Linux so drop it from the list for
# all other platforms.
@ -223,7 +224,7 @@ EF_BOARDS := $(ALL_BOARDS)
# SimPosix doesn't have a BL, BU or EF target so we need to
# filter them out to prevent errors on the all_flight target.
BL_BOARDS := $(filter-out simposix, $(BL_BOARDS))
BU_BOARDS := $(filter-out simposix, $(BU_BOARDS))
BU_BOARDS := $(filter-out simposix gpsplatinum, $(BU_BOARDS))
EF_BOARDS := $(filter-out simposix, $(EF_BOARDS))
# Generate the targets for whatever boards are left in each list
@ -438,7 +439,7 @@ sim_osx_%: uavobjects_flight
##############################
.PHONY: all_ground
all_ground: openpilotgcs
all_ground: openpilotgcs uploader
# Convenience target for the GCS
.PHONY: gcs gcs_clean
@ -478,6 +479,40 @@ openpilotgcs_clean:
@$(ECHO) " CLEAN $(call toprel, $(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF))"
$(V1) [ ! -d "$(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF)" ] || $(RM) -r "$(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF)"
################################
#
# Serial Uploader tool
#
################################
.NOTPARALLEL:
.PHONY: uploader
uploader: uploader_qmake uploader_make
.PHONY: uploader_qmake
uploader_qmake:
ifeq ($(QMAKE_SKIP),)
$(V1) $(MKDIR) -p $(BUILD_DIR)/uploader_$(GCS_BUILD_CONF)
$(V1) ( cd $(BUILD_DIR)/uploader_$(GCS_BUILD_CONF) && \
$(QMAKE) $(ROOT_DIR)/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/upload.pro -spec $(QT_SPEC) -r CONFIG+="$(GCS_BUILD_CONF) $(GCS_SILENT)" $(GCS_QMAKE_OPTS) \
)
else
@$(ECHO) "skipping qmake"
endif
.PHONY: uploader_make
uploader_make:
$(V1) $(MKDIR) -p $(BUILD_DIR)/uploader_$(GCS_BUILD_CONF)
$(V1) ( cd $(BUILD_DIR)/uploader_$(GCS_BUILD_CONF)/$(MAKE_DIR) && \
$(MAKE) -w ; \
)
.PHONY: uploader_clean
uploader_clean:
@$(ECHO) " CLEAN $(call toprel, $(BUILD_DIR)/uploader_$(GCS_BUILD_CONF))"
$(V1) [ ! -d "$(BUILD_DIR)/uploader_$(GCS_BUILD_CONF)" ] || $(RM) -r "$(BUILD_DIR)/uploader_$(GCS_BUILD_CONF)"
################################
#
# Android GCS related components
@ -638,7 +673,7 @@ uavo-collections_clean:
#
##############################
ALL_UNITTESTS := logfs
ALL_UNITTESTS := logfs math lednotification
# Build the directory for the unit tests
UT_OUT_DIR := $(BUILD_DIR)/unit_tests
@ -698,7 +733,8 @@ endif
##############################
# Firmware files to package
PACKAGE_FW_TARGETS := $(filter-out fw_simposix fw_discoveryf4bare, $(FW_TARGETS))
PACKAGE_FW_EXCLUDE := fw_simposix $(if $(PACKAGE_FW_INCLUDE_DISCOVERYF4BARE),,fw_discoveryf4bare)
PACKAGE_FW_TARGETS := $(filter-out $(PACKAGE_FW_EXCLUDE), $(FW_TARGETS))
PACKAGE_ELF_TARGETS := $(filter fw_simposix, $(FW_TARGETS))
# Rules to generate GCS resources used to embed firmware binaries into the GCS.
@ -740,6 +776,9 @@ ifneq ($(strip $(filter package clean_package,$(MAKECMDGOALS))),)
export PACKAGE_NAME := OpenPilot
export PACKAGE_SEP := -
# Copy the Qt libraries regardless whether the building machine needs them to run GCS
export FORCE_COPY_QT := true
# We can only package release builds
ifneq ($(GCS_BUILD_CONF),release)
$(error Packaging is currently supported for release builds only)
@ -968,6 +1007,14 @@ help:
@$(ECHO) " gcs_clean - Remove the Ground Control System (GCS) application (debug|release)"
@$(ECHO) " Supported build configurations: GCS_BUILD_CONF=debug|release (default is $(GCS_BUILD_CONF))"
@$(ECHO)
@$(ECHO) " [Uploader Tool]"
@$(ECHO) " uploader - Build the serial uploader tool (debug|release)"
@$(ECHO) " Skip qmake: QMAKE_SKIP=1"
@$(ECHO) " Example: make uploader QMAKE_SKIP=1"
@$(ECHO) " uploader_clean - Remove the serial uploader tool (debug|release)"
@$(ECHO) " Supported build configurations: GCS_BUILD_CONF=debug|release (default is $(GCS_BUILD_CONF))"
@$(ECHO)
@$(ECHO)
@$(ECHO) " [AndroidGCS]"
@$(ECHO) " androidgcs - Build the Android Ground Control System (GCS) application"
@$(ECHO) " androidgcs_install - Use ADB to install the Android GCS application"

57
flight/libraries/CoordinateConversions.c
View File

@ -180,7 +180,7 @@ void RPY2Quaternion(const float rpy[3], float q[4])
// ** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion **
void Quaternion2R(float q[4], float Rbe[3][3])
{
float q0s = q[0] * q[0], q1s = q[1] * q[1], q2s = q[2] * q[2], q3s = q[3] * q[3];
const float q0s = q[0] * q[0], q1s = q[1] * q[1], q2s = q[2] * q[2], q3s = q[3] * q[3];
Rbe[0][0] = q0s + q1s - q2s - q3s;
Rbe[0][1] = 2 * (q[1] * q[2] + q[0] * q[3]);
@ -193,6 +193,61 @@ void Quaternion2R(float q[4], float Rbe[3][3])
Rbe[2][2] = q0s - q1s - q2s + q3s;
}
// ** Find first row of Rbe, that rotates a vector from earth fixed to body frame, from quaternion **
// ** This vector corresponds to the fuselage/roll vector xB **
void QuaternionC2xB(const float q0, const float q1, const float q2, const float q3, float x[3])
{
const float q0s = q0 * q0, q1s = q1 * q1, q2s = q2 * q2, q3s = q3 * q3;
x[0] = q0s + q1s - q2s - q3s;
x[1] = 2 * (q1 * q2 + q0 * q3);
x[2] = 2 * (q1 * q3 - q0 * q2);
}
void Quaternion2xB(const float q[4], float x[3])
{
QuaternionC2xB(q[0], q[1], q[2], q[3], x);
}
// ** Find second row of Rbe, that rotates a vector from earth fixed to body frame, from quaternion **
// ** This vector corresponds to the spanwise/pitch vector yB **
void QuaternionC2yB(const float q0, const float q1, const float q2, const float q3, float y[3])
{
const float q0s = q0 * q0, q1s = q1 * q1, q2s = q2 * q2, q3s = q3 * q3;
y[0] = 2 * (q1 * q2 - q0 * q3);
y[1] = q0s - q1s + q2s - q3s;
y[2] = 2 * (q2 * q3 + q0 * q1);
}
void Quaternion2yB(const float q[4], float y[3])
{
QuaternionC2yB(q[0], q[1], q[2], q[3], y);
}
// ** Find third row of Rbe, that rotates a vector from earth fixed to body frame, from quaternion **
// ** This vector corresponds to the vertical/yaw vector zB **
void QuaternionC2zB(const float q0, const float q1, const float q2, const float q3, float z[3])
{
const float q0s = q0 * q0, q1s = q1 * q1, q2s = q2 * q2, q3s = q3 * q3;
z[0] = 2 * (q1 * q3 + q0 * q2);
z[1] = 2 * (q2 * q3 - q0 * q1);
z[2] = q0s - q1s - q2s + q3s;
}
void Quaternion2zB(const float q[4], float z[3])
{
QuaternionC2zB(q[0], q[1], q[2], q[3], z);
}
// ****** Express LLA in a local NED Base Frame ********
void LLA2Base(int32_t LLAi[3], double BaseECEF[3], float Rne[3][3], float NED[3])
{

25
flight/libraries/alarms.c
View File

@ -28,7 +28,6 @@
*/
#include <openpilot.h>
#include <pios_struct_helper.h>
#include "inc/alarms.h"
// Private constants
@ -83,9 +82,9 @@ int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity
SystemAlarmsAlarmGet(&alarms);
uint16_t flightTime = (uint16_t)xTaskGetTickCount() * (uint16_t)portTICK_RATE_MS; // this deliberately overflows every 2^16 milliseconds to save memory
if (((uint16_t)(flightTime - lastAlarmChange[alarm]) > PIOS_ALARM_GRACETIME &&
cast_struct_to_array(alarms, alarms.Actuator)[alarm] != severity)
|| cast_struct_to_array(alarms, alarms.Actuator)[alarm] < severity) {
cast_struct_to_array(alarms, alarms.Actuator)[alarm] = severity;
SystemAlarmsAlarmToArray(alarms)[alarm] != severity)
|| SystemAlarmsAlarmToArray(alarms)[alarm] < severity) {
SystemAlarmsAlarmToArray(alarms)[alarm] = severity;
lastAlarmChange[alarm] = flightTime;
SystemAlarmsAlarmSet(&alarms);
}
@ -122,11 +121,11 @@ int32_t ExtendedAlarmsSet(SystemAlarmsAlarmElem alarm,
SystemAlarmsGet(&alarms);
uint16_t flightTime = (uint16_t)xTaskGetTickCount() * (uint16_t)portTICK_RATE_MS; // this deliberately overflows every 2^16 milliseconds to save memory
if (((uint16_t)(flightTime - lastAlarmChange[alarm]) > PIOS_ALARM_GRACETIME &&
cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] != severity)
|| cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] < severity) {
cast_struct_to_array(alarms.ExtendedAlarmStatus, alarms.ExtendedAlarmStatus.BootFault)[alarm] = status;
cast_struct_to_array(alarms.ExtendedAlarmSubStatus, alarms.ExtendedAlarmStatus.BootFault)[alarm] = subStatus;
cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] = severity;
SystemAlarmsAlarmToArray(alarms.Alarm)[alarm] != severity)
|| SystemAlarmsAlarmToArray(alarms.Alarm)[alarm] < severity) {
SystemAlarmsExtendedAlarmStatusToArray(alarms.ExtendedAlarmStatus)[alarm] = status;
SystemAlarmsExtendedAlarmSubStatusToArray(alarms.ExtendedAlarmSubStatus)[alarm] = subStatus;
SystemAlarmsAlarmToArray(alarms.Alarm)[alarm] = severity;
lastAlarmChange[alarm] = flightTime;
SystemAlarmsSet(&alarms);
}
@ -152,7 +151,7 @@ SystemAlarmsAlarmOptions AlarmsGet(SystemAlarmsAlarmElem alarm)
// Read alarm
SystemAlarmsAlarmGet(&alarms);
return cast_struct_to_array(alarms, alarms.Actuator)[alarm];
return SystemAlarmsAlarmToArray(alarms)[alarm];
}
/**
@ -244,7 +243,7 @@ static int32_t hasSeverity(SystemAlarmsAlarmOptions severity)
// Go through alarms and check if any are of the given severity or higher
for (uint32_t n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n) {
if (cast_struct_to_array(alarms, alarms.Actuator)[n] >= severity) {
if (SystemAlarmsAlarmToArray(alarms)[n] >= severity) {
xSemaphoreGiveRecursive(lock);
return 1;
}
@ -272,8 +271,8 @@ SystemAlarmsAlarmOptions AlarmsGetHighestSeverity()
// Go through alarms and find the highest severity
uint32_t n = 0;
while (n < SYSTEMALARMS_ALARM_NUMELEM && highest != SYSTEMALARMS_ALARM_CRITICAL) {
if (cast_struct_to_array(alarmsData, alarmsData.Actuator)[n] > highest) {
highest = cast_struct_to_array(alarmsData, alarmsData.Actuator)[n];
if (SystemAlarmsAlarmToArray(alarmsData)[n] > highest) {
highest = SystemAlarmsAlarmToArray(alarmsData)[n];
}
n++;
}

72
flight/libraries/auxmagsupport.c Normal file
View File

@ -0,0 +1,72 @@
/**
******************************************************************************
*
* @file auxmagsupport.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Functions to handle aux mag data and calibration.
* --
* @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 <stdint.h>
#include "inc/auxmagsupport.h"
#include "CoordinateConversions.h"
static float mag_bias[3] = { 0, 0, 0 };
static float mag_transform[3][3] = {
{ 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }
};
AuxMagSettingsTypeOptions option;
void auxmagsupport_reload_settings()
{
AuxMagSettingsTypeGet(&option);
float a[3][3];
float b[3][3];
float rotz;
AuxMagSettingsmag_transformArrayGet((float *)a);
AuxMagSettingsOrientationGet(&rotz);
rotz = DEG2RAD(rotz);
rot_about_axis_z(rotz, b);
matrix_mult_3x3f(a, b, mag_transform);
AuxMagSettingsmag_biasArrayGet(mag_bias);
}
void auxmagsupport_publish_samples(float mags[3], uint8_t status)
{
float mag_out[3];
mags[0] -= mag_bias[0];
mags[1] -= mag_bias[1];
mags[2] -= mag_bias[2];
rot_mult(mag_transform, mags, mag_out);
AuxMagSensorData data;
data.x = mag_out[0];
data.y = mag_out[1];
data.z = mag_out[2];
data.Status = status;
AuxMagSensorSet(&data);
}
AuxMagSettingsTypeOptions auxmagsupport_get_type()
{
return option;
}

54
flight/libraries/fifo_buffer.c
View File

@ -148,14 +148,18 @@ uint16_t fifoBuf_getDataPeek(t_fifo_buffer *buf, void *data, uint16_t len)
uint16_t i = 0;
while (num_bytes > 0) {
uint16_t j = buf_size - rd;
if (j > num_bytes) {
j = num_bytes;
uint16_t block_len = buf_size - rd;
if (block_len > num_bytes) {
block_len = num_bytes;
}
memcpy(p + i, buff + rd, j);
i += j;
num_bytes -= j;
rd += j;
if (block_len == 1) {
*((uint8_t *)(p + i)) = *((uint8_t *)(buff + rd));
} else {
memcpy(p + i, buff + rd, block_len);
}
i += block_len;
num_bytes -= block_len;
rd += block_len;
if (rd >= buf_size) {
rd = 0;
}
@ -184,14 +188,18 @@ uint16_t fifoBuf_getData(t_fifo_buffer *buf, void *data, uint16_t len)
uint16_t i = 0;
while (num_bytes > 0) {
uint16_t j = buf_size - rd;
if (j > num_bytes) {
j = num_bytes;
uint16_t block_len = buf_size - rd;
if (block_len > num_bytes) {
block_len = num_bytes;
}
memcpy(p + i, buff + rd, j);
i += j;
num_bytes -= j;
rd += j;
if (block_len == 1) {
*((uint8_t *)(p + i)) = *((uint8_t *)(buff + rd));
} else {
memcpy(p + i, buff + rd, block_len);
}
i += block_len;
num_bytes -= block_len;
rd += block_len;
if (rd >= buf_size) {
rd = 0;
}
@ -243,14 +251,18 @@ uint16_t fifoBuf_putData(t_fifo_buffer *buf, const void *data, uint16_t len)
uint16_t i = 0;
while (num_bytes > 0) {
uint16_t j = buf_size - wr;
if (j > num_bytes) {
j = num_bytes;
uint16_t block_len = buf_size - wr;
if (block_len > num_bytes) {
block_len = num_bytes;
}
memcpy(buff + wr, p + i, j);
i += j;
num_bytes -= j;
wr += j;
if (block_len == 1) {
*((uint8_t *)(buff + wr)) = *((uint8_t *)(p + i));
} else {
memcpy(buff + wr, p + i, block_len);
}
i += block_len;
num_bytes -= block_len;
wr += block_len;
if (wr >= buf_size) {
wr = 0;
}

85
flight/libraries/inc/CoordinateConversions.h
View File

@ -50,6 +50,21 @@ void RPY2Quaternion(const float rpy[3], float q[4]);
// ** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion **
void Quaternion2R(float q[4], float Rbe[3][3]);
// ** Find first row of Rbe, that rotates a vector from earth fixed to body frame, from quaternion **
// ** This vector corresponds to the fuselage/roll vector xB **
void QuaternionC2xB(const float q0, const float q1, const float q2, const float q3, float x[3]);
void Quaternion2xB(const float q[4], float x[3]);
// ** Find second row of Rbe, that rotates a vector from earth fixed to body frame, from quaternion **
// ** This vector corresponds to the spanwise/pitch vector yB **
void QuaternionC2yB(const float q0, const float q1, const float q2, const float q3, float y[3]);
void Quaternion2yB(const float q[4], float y[3]);
// ** Find third row of Rbe, that rotates a vector from earth fixed to body frame, from quaternion **
// ** This vector corresponds to the vertical/yaw vector zB **
void QuaternionC2zB(const float q0, const float q1, const float q2, const float q3, float z[3]);
void Quaternion2zB(const float q[4], float z[3]);
// ****** Express LLA in a local NED Base Frame ********
void LLA2Base(int32_t LLAi[3], double BaseECEF[3], float Rne[3][3], float NED[3]);
@ -82,7 +97,7 @@ void rot_mult(float R[3][3], const float vec[3], float vec_out[3]);
* @param b
* @param result
*/
inline void matrix_mult_3x3f(float a[3][3], float b[3][3], float result[3][3])
static inline void matrix_mult_3x3f(float a[3][3], float b[3][3], float result[3][3])
{
result[0][0] = a[0][0] * b[0][0] + a[1][0] * b[0][1] + a[2][0] * b[0][2];
result[0][1] = a[0][1] * b[0][0] + a[1][1] * b[0][1] + a[2][1] * b[0][2];
@ -97,5 +112,73 @@ inline void matrix_mult_3x3f(float a[3][3], float b[3][3], float result[3][3])
result[2][2] = a[0][2] * b[2][0] + a[1][2] * b[2][1] + a[2][2] * b[2][2];
}
inline void matrix_inline_scale_3f(float a[3][3], float scale)
{
a[0][0] *= scale;
a[0][1] *= scale;
a[0][2] *= scale;
a[1][0] *= scale;
a[1][1] *= scale;
a[1][2] *= scale;
a[2][0] *= scale;
a[2][1] *= scale;
a[2][2] *= scale;
}
inline void rot_about_axis_x(const float rotation, float R[3][3])
{
float s = sinf(rotation);
float c = cosf(rotation);
R[0][0] = 1;
R[0][1] = 0;
R[0][2] = 0;
R[1][0] = 0;
R[1][1] = c;
R[1][2] = -s;
R[2][0] = 0;
R[2][1] = s;
R[2][2] = c;
}
inline void rot_about_axis_y(const float rotation, float R[3][3])
{
float s = sinf(rotation);
float c = cosf(rotation);
R[0][0] = c;
R[0][1] = 0;
R[0][2] = s;
R[1][0] = 0;
R[1][1] = 1;
R[1][2] = 0;
R[2][0] = -s;
R[2][1] = 0;
R[2][2] = c;
}
inline void rot_about_axis_z(const float rotation, float R[3][3])
{
float s = sinf(rotation);
float c = cosf(rotation);
R[0][0] = c;
R[0][1] = -s;
R[0][2] = 0;
R[1][0] = s;
R[1][1] = c;
R[1][2] = 0;
R[2][0] = 0;
R[2][1] = 0;
R[2][2] = 1;
}
#endif // COORDINATECONVERSIONS_H_

51
flight/libraries/inc/auxmagsupport.h Normal file
View File

@ -0,0 +1,51 @@
/**
******************************************************************************
*
* @file auxmagsupport.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Functions to handle aux mag data and calibration.
* --
* @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 AUXMAGSUPPORT_H_
#define AUXMAGSUPPORT_H_
#include <openpilot.h>
#include <auxmagsettings.h>
#include <auxmagsensor.h>
/**
* @brief reload Aux Mag settings
*/
void auxmagsupport_reload_settings();
/**
* @brief Publish a new Aux Magnetometer sample
* @param[in] mags Mag sample in milliGauss
* @param[in] status one of AuxMagSensorStatusOptions option
*/
void auxmagsupport_publish_samples(float mags[3], uint8_t status);
/**
* @brief Get the Aux Mag settings Type option
* @param[in] mags Mag sample in milliGauss
* @return one of AuxMagSettingsTypeOptions option
*/
AuxMagSettingsTypeOptions auxmagsupport_get_type();
#endif /* AUXMAGSUPPORT_H_ */

22
flight/modules/Attitude/revolution/inc/attitude.h → flight/libraries/inc/lednotification.h
View File

@ -1,14 +1,10 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup Attitude Attitude Module
* @{
*
* @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 lednotification.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief led notification library
* --
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
@ -27,11 +23,11 @@
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef ATTITUDE_H
#define ATTITUDE_H
#ifndef LEDNOTIFICATION_H_
#define LEDNOTIFICATION_H_
#include <optypes.h>
#include "openpilot.h"
void LedNotificationExtLedsRun();
int32_t AttitudeInitialize(void);
#endif // ATTITUDE_H
#endif /* LEDNOTIFICATION_H_ */

2
flight/libraries/inc/notification.h
View File

@ -27,7 +27,7 @@
#define NOTIFICATION_H
// period of each blink phase
#define LED_BLINK_PERIOD_MS 200
#define LED_BLINK_PERIOD_MS 50
// update the status snapshot used by notifcations
void NotificationUpdateStatus();

5
flight/libraries/inc/op_dfu.h
View File

@ -50,11 +50,8 @@ typedef struct {
/* Exported functions ------------------------------------------------------- */
void processComand(uint8_t *Receive_Buffer);
uint32_t baseOfAdressType(uint8_t type);
uint8_t isBiggerThanAvailable(uint8_t type, uint32_t size);
void OPDfuIni(uint8_t discover);
void DataDownload(DownloadAction);
bool flash_read(uint8_t *buffer, uint32_t adr, DFUProgType type);
#endif /* __OP_DFU_H */
/******************* (C) COPYRIGHT 2010 STMicroelectronics *****END OF FILE****/

64
flight/libraries/inc/optypes.h Normal file
View File

@ -0,0 +1,64 @@
/**
******************************************************************************
*
* @file optypes.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief OP Generic data type library
* --
* @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 UTIL_H
#define UTIL_H
#include <stdint.h>
typedef struct {
uint8_t R;
uint8_t G;
uint8_t B;
} Color_t;
extern const Color_t Color_Off;
extern const Color_t Color_Black;
extern const Color_t Color_Red;
extern const Color_t Color_Lime;
extern const Color_t Color_Blue;
extern const Color_t Color_Yellow;
extern const Color_t Color_Cian;
extern const Color_t Color_Magenta;
extern const Color_t Color_Navy;
extern const Color_t Color_Green;
extern const Color_t Color_Purple;
extern const Color_t Color_Teal;
extern const Color_t Color_Orange;
extern const Color_t Color_White;
#define COLOR_BLACK { .R = 0x00, .G = 0x00, .B = 0x00 }
#define COLOR_OFF COLOR_BLACK
#define COLOR_RED { .R = 0xFF, .G = 0x00, .B = 0x00 }
#define COLOR_LIME { .R = 0x00, .G = 0xFF, .B = 0x00 }
#define COLOR_BLUE { .R = 0x00, .G = 0x00, .B = 0xFF }
#define COLOR_YELLOW { .R = 0xFF, .G = 0xFF, .B = 0x00 }
#define COLOR_CIAN { .R = 0x00, .G = 0xFF, .B = 0xFF }
#define COLOR_MAGENTA { .R = 0xFF, .G = 0x00, .B = 0xFF }
#define COLOR_NAVY { .R = 0x00, .G = 0x00, .B = 0x80 }
#define COLOR_GREEN { .R = 0x00, .G = 0x80, .B = 0x00 }
#define COLOR_PURPLE { .R = 0x80, .G = 0x00, .B = 0x80 }
#define COLOR_TEAL { .R = 0x00, .G = 0x80, .B = 0x80 }
#define COLOR_ORANGE { .R = 0xFF, .G = 0xA5, .B = 0x00 }
#define COLOR_WHITE { .R = 0xAA, .G = 0xAA, .B = 0xAA }
#endif /* UTIL_H */

6
flight/libraries/inc/paths.h
View File

@ -30,10 +30,10 @@
struct path_status {
float fractional_progress;
float error;
float correction_direction[2];
float path_direction[2];
float path_vector[3];
float correction_vector[3];
};
void path_progress(float *start_point, float *end_point, float *cur_point, struct path_status *status, uint8_t mode);
void path_progress(PathDesiredData *path, float *cur_point, struct path_status *status);
#endif

118
flight/libraries/inc/ssp.h Normal file
View File

@ -0,0 +1,118 @@
/*******************************************************************
*
* NAME: ssp.h
*
*
*******************************************************************/
#ifndef SSP_H
#define SSP_H
/** INCLUDE FILES **/
#include <stdint.h>
/** LOCAL DEFINITIONS **/
#ifndef TRUE
#define TRUE 1
#endif
#ifndef FALSE
#define FALSE 0
#endif
#define SPP_USES_CRC
#define SSP_TX_IDLE 0 // not expecting a ACK packet (no current transmissions in progress)
#define SSP_TX_WAITING 1 // waiting for a valid ACK to arrive
#define SSP_TX_TIMEOUT 2 // failed to receive a valid ACK in the timeout period, after retrying.
#define SSP_TX_ACKED 3 // valid ACK received before timeout period.
#define SSP_TX_BUFOVERRUN 4 // amount of data to send execeds the transmission buffer sizeof
#define SSP_TX_BUSY 5 // Attempted to start a transmission while a transmission was already in progress.
// #define SSP_TX_FAIL - failure...
#define SSP_RX_IDLE 0
#define SSP_RX_RECEIVING 1
#define SSP_RX_COMPLETE 2
// types of packet that can be received
#define SSP_RX_DATA 5
#define SSP_RX_ACK 6
#define SSP_RX_SYNCH 7
typedef enum decodeState_ {
decode_len1_e = 0,
decode_seqNo_e,
decode_data_e,
decode_crc1_e,
decode_crc2_e,
decode_idle_e
} DecodeState_t;
typedef enum ReceiveState {
state_escaped_e = 0, state_unescaped_e
} ReceiveState_t;
typedef struct {
uint8_t *pbuff;
uint16_t length;
uint16_t crc;
uint8_t seqNo;
} Packet_t;
typedef struct {
uint8_t *rxBuf; // Buffer used to store rcv data
uint16_t rxBufSize; // rcv buffer size.
uint8_t *txBuf; // Length of data in buffer
uint16_t txBufSize; // CRC for data in Packet buff
uint16_t max_retry; // Maximum number of retrys for a single transmit.
int32_t timeoutLen; // how long to wait for each retry to succeed
void (*pfCallBack)(uint8_t *, uint16_t); // call back function that is called when a full packet has been received
int16_t (*pfSerialRead)(void); // function to call to read a byte from serial hardware
void (*pfSerialWrite)(uint8_t); // function used to write a byte to serial hardware for transmission
uint32_t (*pfGetTime)(void); // function returns time in number of seconds that has elapsed from a given reference point
} PortConfig_t;
typedef struct Port_tag {
void (*pfCallBack)(uint8_t *, uint16_t); // call back function that is called when a full packet has been received
int16_t (*pfSerialRead)(void); // function to read a character from the serial input stream
void (*pfSerialWrite)(uint8_t); // function to write a byte to be sent out the serial port
uint32_t (*pfGetTime)(void); // function returns time in number of seconds that has elapsed from a given reference point
uint8_t retryCount; // how many times have we tried to transmit the 'send' packet
uint8_t maxRetryCount; // max. times to try to transmit the 'send' packet
int32_t timeoutLen; // how long to wait for each retry to succeed
uint32_t timeout; // current timeout. when 'time' reaches this point we have timed out
uint8_t txSeqNo; // current 'send' packet sequence number
uint16_t rxBufPos; // current buffer position in the receive packet
uint16_t rxBufLen; // number of 'data' bytes in the buffer
uint8_t rxSeqNo; // current 'receive' packet number
uint16_t rxBufSize; // size of the receive buffer.
uint16_t txBufSize; // size of the transmit buffer.
uint8_t *txBuf; // transmit buffer. REquired to store a copy of packet data in case a retry is needed.
uint8_t *rxBuf; // receive buffer. Used to store data as a packet is received.
uint16_t sendSynch; // flag to indicate that we should send a synchronize packet to the host
// this is required when switching from the application to the bootloader
// and vice-versa. This fixes the firwmare download timeout.
// when this flag is set to true, the next time we send a packet we will first
// send a synchronize packet.
ReceiveState_t InputState;
DecodeState_t DecodeState;
uint16_t SendState;
uint16_t crc;
uint32_t RxError;
uint32_t TxError;
uint16_t flags;
} Port_t;
/** Public Data **/
/** PUBLIC FUNCTIONS **/
int16_t ssp_ReceiveProcess(Port_t *thisport);
int16_t ssp_SendProcess(Port_t *thisport);
uint16_t ssp_SendString(Port_t *thisport, char *str);
int16_t ssp_SendData(Port_t *thisport, const uint8_t *data,
const uint16_t length);
void ssp_Init(Port_t *thisport, const PortConfig_t *const info);
int16_t ssp_ReceiveByte(Port_t *thisport);
uint16_t ssp_Synchronise(Port_t *thisport);
/** EXTERNAL FUNCTIONS **/
#endif // ifndef SSP_H

50
flight/libraries/inc/stopwatch.h
View File

@ -1,11 +1,11 @@
/**
******************************************************************************
* @addtogroup CopterControlBL CopterControl BootLoader
* @brief These files contain the code to the CopterControl Bootloader.
* @addtogroup OpenPilot library
* @brief These files contain the code for stopwatch handling.
*
* @file stopwatch.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Timer functions for the LED PWM.
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Generic pios_delay based stopwatch functions.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
@ -27,7 +27,8 @@
#ifndef _STOPWATCH_H
#define _STOPWATCH_H
#include <stdint.h>
#include <pios_delay.h>
/////////////////////////////////////////////////////////////////////////////
// Global definitions
/////////////////////////////////////////////////////////////////////////////
@ -36,15 +37,46 @@
/////////////////////////////////////////////////////////////////////////////
// Global Types
/////////////////////////////////////////////////////////////////////////////
typedef struct {
uint32_t raw;
uint32_t resolution;
} stopwatch_t;
/////////////////////////////////////////////////////////////////////////////
// Prototypes
/////////////////////////////////////////////////////////////////////////////
extern s32 STOPWATCH_Init(u32 resolution, TIM_TypeDef *TIM);
extern s32 STOPWATCH_Reset(TIM_TypeDef *TIM);
extern u32 STOPWATCH_ValueGet(TIM_TypeDef *TIM);
inline int32_t STOPWATCH_Init(uint32_t resolution, stopwatch_t *stopwatch)
{
stopwatch->raw = PIOS_DELAY_GetRaw();
stopwatch->resolution = resolution;
return 0; // no error
}
/////////////////////////////////////////////////////////////////////////////
// ! Resets the stopwatch
// ! \return < 0 on errors
/////////////////////////////////////////////////////////////////////////////
inline int32_t STOPWATCH_Reset(stopwatch_t *stopwatch)
{
stopwatch->raw = PIOS_DELAY_GetRaw();
return 0; // no error
}
/////////////////////////////////////////////////////////////////////////////
// ! Returns current value of stopwatch
// ! \return stopwatch value
/////////////////////////////////////////////////////////////////////////////
inline uint32_t STOPWATCH_ValueGet(stopwatch_t *stopwatch)
{
uint32_t value = PIOS_DELAY_GetuSSince(stopwatch->raw);
if (stopwatch > 1) {
value = value / stopwatch->resolution;
}
return value;
}
/////////////////////////////////////////////////////////////////////////////

62
flight/libraries/inc/ubx_utils.h Normal file
View File

@ -0,0 +1,62 @@
/**
******************************************************************************
*
* @file ubx_utils.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief UBX Protocol utilities
* --
* @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 UBX_UTILS_H_
#define UBX_UTILS_H_
#include <stdint.h>
#include <stdbool.h>
typedef struct {
uint8_t syn1;
uint8_t syn2;
uint8_t class;
uint8_t id;
uint16_t len;
} __attribute__((packed)) UBXHeader_t;
typedef struct {
uint8_t chk1;
uint8_t chk2;
} __attribute__((packed)) UBXFooter_t;
typedef union {
uint8_t binarystream[0];
struct {
UBXHeader_t header;
uint8_t payload[0];
} packet;
} UBXPacket_t;
#define UBX_HEADER_LEN (sizeof(UBXHeader_t))
#define UBX_SYN1 0xB5
#define UBX_SYN2 0x62
bool ubx_getLastSentence(uint8_t *data, uint16_t bufferCount, uint8_t * *lastSentence, uint16_t *lenght);
void ubx_appendChecksum(UBXPacket_t *pkt);
void ubx_buildPacket(UBXPacket_t *pkt, uint8_t packetClass, uint8_t packetId, uint16_t len);
#endif /* UBX_UTILS_H_ */

259
flight/libraries/lednotification.c Normal file
View File

@ -0,0 +1,259 @@
/**
******************************************************************************
*
* @file lednotification.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief led notification library.
* --
* @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 "inc/lednotification.h"
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <FreeRTOS.h>
#include <pios.h>
#include <pios_notify.h>
#include <pios_ws2811.h>
// Private defines
// Maximum number of notifications enqueued when a higher priority notification is added
#define MAX_BACKGROUND_NOTIFICATIONS 6
#define MAX_HANDLED_LED 1
#define BACKGROUND_SEQUENCE 0
#define RESET_STEP -1
#define GET_CURRENT_MILLIS (xTaskGetTickCount() * portTICK_RATE_MS)
// Private data types definition
// this is the status for a single notification led set
typedef struct {
int8_t queued_priorities[MAX_BACKGROUND_NOTIFICATIONS]; // slot 0 is reserved for background
LedSequence_t queued_sequences[MAX_BACKGROUND_NOTIFICATIONS]; // slot 0 is reserved for background
uint32_t next_run_time;
uint32_t sequence_starting_time;
int8_t active_sequence_num; // active queued sequence or BACKGROUND_SEQUENCE
bool running; // is this led running?
bool step_phase_on; // true = step on phase, false = step off phase
uint8_t next_sequence_step; // (step number to be executed) << 1 || (0x00 = on phase, 0x01 = off phase)
uint8_t next_step_rep; // next repetition number for next step (valid if step.repeats >1)
uint8_t next_sequence_rep; // next sequence repetition counter (valid if sequence.repeats > 1)
uint8_t led_set_start; // first target led for this set
uint8_t led_set_end; // last target led for this set
} NotifierLedStatus_t;
static bool led_status_initialized = false;
NotifierLedStatus_t led_status[MAX_HANDLED_LED];
static void InitExtLed()
{
memset(led_status, 0, sizeof(NotifierLedStatus_t) * MAX_HANDLED_LED);
const uint32_t now = GET_CURRENT_MILLIS;
for (uint8_t l = 0; l < MAX_HANDLED_LED; l++) {
led_status[l].led_set_start = 0;
led_status[l].led_set_end = PIOS_WS2811_NUMLEDS - 1;
led_status[l].next_run_time = now + 500; // start within half a second
for (uint8_t i = 0; i < MAX_BACKGROUND_NOTIFICATIONS; i++) {
led_status[l].queued_priorities[i] = NOTIFY_PRIORITY_BACKGROUND;
}
}
}
/**
* restart current sequence
*/
static void restart_sequence(NotifierLedStatus_t *status, bool immediate)
{
status->next_sequence_step = 0;
status->next_step_rep = 0;
status->step_phase_on = true;
status->running = true;
if (immediate) {
uint32_t currentTime = GET_CURRENT_MILLIS;
status->next_run_time = currentTime;
}
status->sequence_starting_time = status->next_run_time;
}
/**
* modify background sequence or enqueue a new sequence to play
*/
static void push_queued_sequence(ExtLedNotification_t *new_notification, NotifierLedStatus_t *status)
{
int8_t updated_sequence;
int8_t lowest_priority_index = -1;
int8_t lowest_priority = new_notification->priority;
if (new_notification->priority == NOTIFY_PRIORITY_BACKGROUND) {
lowest_priority_index = BACKGROUND_SEQUENCE;
} else {
// slot 0 is reserved for Background sequence
for (int8_t i = 1; i < MAX_BACKGROUND_NOTIFICATIONS; i++) {
if (status->queued_priorities[i] < lowest_priority) {
lowest_priority_index = i;
lowest_priority = status->queued_priorities[i];
}
}
}
// no items with priority lower than the one we are trying to enqueue. skip
if (lowest_priority_index < 0) {
return;
}
status->queued_priorities[lowest_priority_index] = new_notification->priority;
status->queued_sequences[lowest_priority_index] = new_notification->sequence;
updated_sequence = lowest_priority_index;;
// check whether we should preempt the current notification and play this new one
if (status->queued_priorities[status->active_sequence_num] < new_notification->priority) {
status->active_sequence_num = updated_sequence;
}
if (status->active_sequence_num == updated_sequence) {
restart_sequence(status, true);
}
}
static bool pop_queued_sequence(NotifierLedStatus_t *status)
{
if (status->active_sequence_num > BACKGROUND_SEQUENCE) {
// set the last active slot as empty
status->queued_priorities[status->active_sequence_num] = NOTIFY_PRIORITY_BACKGROUND;
// search the highest priority item
int8_t highest_priority_index = BACKGROUND_SEQUENCE;
int8_t highest_priority = NOTIFY_PRIORITY_BACKGROUND;
for (int8_t i = 1; i < MAX_BACKGROUND_NOTIFICATIONS; i++) {
if (status->queued_priorities[i] > highest_priority) {
highest_priority_index = i;
highest_priority = status->queued_priorities[i];
}
}
// set the next sequence to activate (or BACKGROUND_SEQUENCE when all slots are empty)
status->active_sequence_num = highest_priority_index;
return highest_priority_index != BACKGROUND_SEQUENCE;
}
// background sequence was completed
return false;
}
/**
* advance current sequence pointers for next step
*/
static void advance_sequence(NotifierLedStatus_t *status)
{
LedSequence_t *activeSequence = &status->queued_sequences[status->active_sequence_num];
uint8_t step = status->next_sequence_step;
LedStep_t *currentStep = &activeSequence->steps[step];
// Next step will be the OFF phase, so just update the time and
if (status->step_phase_on) {
// next will be the off phase
status->next_run_time += currentStep->time_on;
status->step_phase_on = false;
// check if off phase should be skipped
if (currentStep->time_off != 0) {
return;
}
}
// next step is ON phase. check whether to repeat current step or move to next one
status->next_run_time += currentStep->time_off;
status->step_phase_on = true;
if (status->next_step_rep + 1 < currentStep->repeats) {
// setup next repetition
status->next_step_rep++;
return;
}
// move to next step
LedStep_t *nextStep = (step + 1 < NOTIFY_SEQUENCE_MAX_STEPS) ? &activeSequence->steps[step + 1] : 0;
// next step is null, check whether sequence must be repeated or it must move to lower priority queued or background sequences
if (NOTIFY_IS_NULL_STEP(nextStep)) {
if (activeSequence->repeats == -1 || status->next_sequence_rep + 1 < activeSequence->repeats) {
status->next_sequence_rep++;
// restart the sequence
restart_sequence(status, false);
return;
}
if (status->active_sequence_num != BACKGROUND_SEQUENCE) {
// no repeat, pop enqueued or background sequences
pop_queued_sequence(status);
restart_sequence(status, false);
status->next_sequence_rep = 0;
} else {
status->running = false;
}
} else {
status->next_step_rep = 0;
status->next_sequence_step++;
}
}
/**
* run a led set
*/
static void run_led(NotifierLedStatus_t *status)
{
const uint32_t currentTime = GET_CURRENT_MILLIS;
if (!status->running || currentTime < status->next_run_time) {
return;
}
status->next_run_time = currentTime;
uint8_t step = status->next_sequence_step;
LedSequence_t *activeSequence = &status->queued_sequences[status->active_sequence_num];
const Color_t color = status->step_phase_on ? activeSequence->steps[step].color : Color_Off;
for (uint8_t i = status->led_set_start; i <= status->led_set_end; i++) {
PIOS_WS2811_setColorRGB(color, i, false);
}
PIOS_WS2811_Update();
advance_sequence(status);
}
void LedNotificationExtLedsRun()
{
// handle incoming sequences
if (!led_status_initialized) {
InitExtLed();
led_status_initialized = true;
}
static ExtLedNotification_t *newNotification;
newNotification = PIOS_NOTIFY_GetNewExtLedSequence(true);
if (newNotification) {
push_queued_sequence(newNotification, &led_status[0]);
}
// Run Leds
for (uint8_t i = 0; i < MAX_HANDLED_LED; i++) {
run_led(&led_status[i]);
}
}

100
flight/libraries/math/butterworth.c Normal file
View File

@ -0,0 +1,100 @@
/**
******************************************************************************
* @addtogroup OpenPilot Math Utilities
* @{
* @addtogroup Butterworth low pass filter
* @{
*
* @file butterworth.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Direct form two of a second order Butterworth low pass filter
*
* @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 "openpilot.h"
#include "math.h"
#include "butterworth.h"
/**
* Initialization function for coefficients of a second order Butterworth biquadratic filter in direct from 2.
* Note that b1 = 2 * b0 and b2 = b0 is use here and in the sequel.
* @param[in] ff Cut-off frequency ratio
* @param[out] filterPtr Pointer to filter coefficients
* @returns Nothing
*/
void InitButterWorthDF2Filter(const float ff, struct ButterWorthDF2Filter *filterPtr)
{
const float ita = 1.0f / tanf(M_PI_F * ff);
const float b0 = 1.0f / (1.0f + M_SQRT2_F * ita + ita * ita);
const float a1 = 2.0f * b0 * (ita * ita - 1.0f);
const float a2 = -b0 * (1.0f - M_SQRT2_F * ita + ita * ita);
filterPtr->b0 = b0;
filterPtr->a1 = a1;
filterPtr->a2 = a2;
}
/**
* Initialization function for intermediate values of a second order Butterworth biquadratic filter in direct from 2.
* Obtained by solving a linear equation system.
* @param[in] x0 Prescribed value
* @param[in] filterPtr Pointer to filter coefficients
* @param[out] wn1Ptr Pointer to first intermediate value
* @param[out] wn2Ptr Pointer to second intermediate value
* @returns Nothing
*/
void InitButterWorthDF2Values(const float x0, const struct ButterWorthDF2Filter *filterPtr, float *wn1Ptr, float *wn2Ptr)
{
const float b0 = filterPtr->b0;
const float a1 = filterPtr->a1;
const float a2 = filterPtr->a2;
const float a11 = 2.0f + a1;
const float a12 = 1.0f + a2;
const float a21 = 2.0f + a1 * a1 + a2;
const float a22 = 1.0f + a1 * a2;
const float det = a11 * a22 - a12 * a21;
const float rhs1 = x0 / b0 - x0;
const float rhs2 = x0 / b0 - x0 + a1 * x0;
*wn1Ptr = (a22 * rhs1 - a12 * rhs2) / det;
*wn2Ptr = (-a21 * rhs1 + a11 * rhs2) / det;
}
/**
* Second order Butterworth biquadratic filter in direct from 2, such that only two values wn1=w[n-1] and wn2=w[n-2] need to be stored.
* Function takes care of updating the values wn1 and wn2.
* @param[in] xn New raw value
* @param[in] filterPtr Pointer to filter coefficients
* @param[out] wn1Ptr Pointer to first intermediate value
* @param[out] wn2Ptr Pointer to second intermediate value
* @returns Filtered value
*/
float FilterButterWorthDF2(const float xn, const struct ButterWorthDF2Filter *filterPtr, float *wn1Ptr, float *wn2Ptr)
{
const float wn = xn + filterPtr->a1 * (*wn1Ptr) + filterPtr->a2 * (*wn2Ptr);
const float val = filterPtr->b0 * (wn + 2.0f * (*wn1Ptr) + (*wn2Ptr));
*wn2Ptr = *wn1Ptr;
*wn1Ptr = wn;
return val;
}

46
flight/libraries/math/butterworth.h Normal file
View File

@ -0,0 +1,46 @@
/**
******************************************************************************
* @addtogroup OpenPilot Math Utilities
* @{
* @addtogroup Butterworth low pass filter
* @{
*
* @file butterworth.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Direct form two of a second order Butterworth low pass filter
*
* @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 BUTTERWORTH_H
#define BUTTERWORTH_H
// Coefficients of second order Butterworth biquadratic filter in direct from 2
struct ButterWorthDF2Filter {
float b0;
float a1;
float a2;
};
// Function declarations
void InitButterWorthDF2Filter(const float ff, struct ButterWorthDF2Filter *filterPtr);
void InitButterWorthDF2Values(const float x0, const struct ButterWorthDF2Filter *filterPtr, float *wn1Ptr, float *wn2Ptr);
float FilterButterWorthDF2(const float xn, const struct ButterWorthDF2Filter *filterPtr, float *wn1Ptr, float *wn2Ptr);
#endif

107
flight/libraries/math/mathmisc.h
View File

@ -31,6 +31,9 @@
#ifndef MATHMISC_H
#define MATHMISC_H
#include <math.h>
#include <stdint.h>
// returns min(boundary1,boundary2) if val<min(boundary1,boundary2)
// returns max(boundary1,boundary2) if val>max(boundary1,boundary2)
// returns val if min(boundary1,boundary2)<=val<=max(boundary1,boundary2)
@ -52,4 +55,108 @@ static inline float boundf(float val, float boundary1, float boundary2)
return val;
}
static inline float squaref(float x)
{
return x * x;
}
static inline float vector_lengthf(float *vector, const uint8_t dim)
{
float length = 0.0f;
for (int t = 0; t < dim; t++) {
length += squaref(vector[t]);
}
return sqrtf(length);
}
static inline void vector_normalizef(float *vector, const uint8_t dim)
{
float length = vector_lengthf(vector, dim);
if (length <= 0.0f || isnan(length)) {
return;
}
for (int t = 0; t < dim; t++) {
vector[t] /= length;
}
}
typedef struct pointf {
float x;
float y;
} pointf;
// Returns the y value, given x, on the line passing through the points p0 and p1.
static inline float y_on_line(float x, const pointf *p0, const pointf *p1)
{
// Setup line y = m * x + b.
const float dY1 = p1->y - p0->y;
const float dX1 = p1->x - p0->x;
const float m = dY1 / dX1; // == dY0 / dX0 == (p0.y - b) / (p0.x - 0.0f) ==>
const float b = p0->y - m * p0->x;
// Get the y value on the line.
return m * x + b;
}
// Returns the y value, given x, on the curve defined by the points array.
// The fist and last line of the curve extends beyond the first resp. last points.
static inline float y_on_curve(float x, const pointf points[], int num_points)
{
// Find the two points x is within.
// If x is smaller than the first point's x value, use the first line of the curve.
// If x is larger than the last point's x value, user the last line of the curve.
int end_point = num_points - 1;
for (int i = 1; i < num_points; i++) {
if (x < points[i].x) {
end_point = i;
break;
}
}
// Find the y value on the selected line.
return y_on_line(x, &points[end_point - 1], &points[end_point]);
}
// Fast inverse square root implementation from "quake3-1.32b/code/game/q_math.c"
// http://en.wikipedia.org/wiki/Fast_inverse_square_root
static inline float fast_invsqrtf(float number)
{
float x2, y;
const float threehalfs = 1.5F;
union {
float f;
uint32_t u;
} i;
x2 = number * 0.5F;
y = number;
i.f = y; // evil floating point bit level hacking
i.u = 0x5f3759df - (i.u >> 1); // what the fxck?
y = i.f;
y = y * (threehalfs - (x2 * y * y)); // 1st iteration
// y = y * ( threehalfs - ( x2 * y * y ) ); // 2nd iteration, this can be removed
return y;
}
/**
* Ultrafast pow() aproximation needed for expo
* Based on Algorithm by Martin Ankerl
*/
static inline float fastPow(float a, float b)
{
union {
double d;
int32_t x[2];
} u = { (double)a };
u.x[1] = (int32_t)(b * (u.x[1] - 1072632447) + 1072632447);
u.x[0] = 0;
return (float)u.d;
}
#endif /* MATHMISC_H */

15
flight/libraries/math/pid.c
View File

@ -76,12 +76,12 @@ float pid_apply(struct pid *pid, const float err, float dT)
* This version of apply uses setpoint weighting for the derivative component so the gain
* on the gyro derivative can be different than the gain on the setpoint derivative
*/
float pid_apply_setpoint(struct pid *pid, const float factor, const float setpoint, const float measured, float dT)
float pid_apply_setpoint(struct pid *pid, const pid_scaler *scaler, const float setpoint, const float measured, float dT)
{
float err = setpoint - measured;
// Scale up accumulator by 1000 while computing to avoid losing precision
pid->iAccumulator += err * (factor * pid->i * dT * 1000.0f);
pid->iAccumulator += err * (scaler->i * pid->i * dT * 1000.0f);
pid->iAccumulator = boundf(pid->iAccumulator, pid->iLim * -1000.0f, pid->iLim * 1000.0f);
// Calculate DT1 term,
@ -89,11 +89,14 @@ float pid_apply_setpoint(struct pid *pid, const float factor, const float setpoi
float diff = ((deriv_gamma * setpoint - measured) - pid->lastErr);
pid->lastErr = (deriv_gamma * setpoint - measured);
if (pid->d > 0.0f && dT > 0.0f) {
dterm = pid->lastDer + dT / (dT + deriv_tau) * ((factor * diff * pid->d / dT) - pid->lastDer);
pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff)
} // 7.9577e-3 means 20 Hz f_cutoff
// low pass filter derivative term. below formula is the same as
// dterm = (1-alpha)*pid->lastDer + alpha * (...)/dT
// with alpha = dT/(deriv_tau+dT)
dterm = pid->lastDer + dT / (dT + deriv_tau) * ((scaler->d * diff * pid->d / dT) - pid->lastDer);
pid->lastDer = dterm;
}
return (err * factor * pid->p) + pid->iAccumulator / 1000.0f + dterm;
return (err * scaler->p * pid->p) + pid->iAccumulator / 1000.0f + dterm;
}
/**

10
flight/libraries/math/pid.h
View File

@ -31,6 +31,8 @@
#ifndef PID_H
#define PID_H
#include "mathmisc.h"
// !
struct pid {
float p;
@ -42,9 +44,15 @@ struct pid {
float lastDer;
};
typedef struct pid_scaler {
float p;
float i;
float d;
} pid_scaler;
// ! Methods to use the pid structures
float pid_apply(struct pid *pid, const float err, float dT);
float pid_apply_setpoint(struct pid *pid, const float factor, const float setpoint, const float measured, float dT);
float pid_apply_setpoint(struct pid *pid, const pid_scaler *scaler, const float setpoint, const float measured, float dT);
void pid_zero(struct pid *pid);
void pid_configure(struct pid *pid, float p, float i, float d, float iLim);
void pid_configure_derivative(float cutoff, float gamma);

16
flight/libraries/notification.c
View File

@ -25,10 +25,14 @@
*/
#include "inc/notification.h"
#include <openpilot.h>
#include <pios_struct_helper.h>
#include <systemalarms.h>
#include <flightstatus.h>
#include <pios_notify.h>
#include <stdbool.h>
#define GET_CURRENT_MILLIS (xTaskGetTickCount() * portTICK_RATE_MS)
// Private data types definition
#ifdef PIOS_LED_ALARM
#define ALARM_LED_ON() PIOS_LED_On(PIOS_LED_ALARM)
@ -137,6 +141,8 @@ static bool handleAlarms(uint16_t *r_pattern, uint16_t *b_pattern);
static bool handleNotifications(pios_notify_notification runningNotification, uint16_t *r_pattern, uint16_t *b_pattern);
static void handleFlightMode(uint16_t *r_pattern, uint16_t *b_pattern);
static void handleHeartbeat(uint16_t *r_pattern, uint16_t *b_pattern);
void NotificationUpdateStatus()
{
started = true;
@ -150,7 +156,7 @@ void NotificationUpdateStatus()
void NotificationOnboardLedsRun()
{
static portTickType lastRunTimestamp;
static uint32_t lastRunTimestamp;
static uint16_t r_pattern;
static uint16_t b_pattern;
static uint8_t cycleCount; // count the number of cycles
@ -164,11 +170,13 @@ void NotificationOnboardLedsRun()
STATUS_LENGHT
} status;
if (!started || (xTaskGetTickCount() - lastRunTimestamp) < (LED_BLINK_PERIOD_MS * portTICK_RATE_MS / 4)) {
const uint32_t current_timestamp = GET_CURRENT_MILLIS;
if (!started || (current_timestamp - lastRunTimestamp) < LED_BLINK_PERIOD_MS) {
return;
}
lastRunTimestamp = xTaskGetTickCount();
lastRunTimestamp = current_timestamp;
// the led will show various status information, subdivided in three phases
// - Notification
// - Alarm

81
flight/libraries/op_dfu.c
View File

@ -31,7 +31,6 @@
#include <stdbool.h>
#include "op_dfu.h"
#include "pios_bl_helper.h"
#include "pios_com_msg.h"
#include <pios_board_info.h>
// programmable devices
Device devicesTable[10];
@ -71,7 +70,12 @@ DFUTransfer downType = 0;
/* Extern variables ----------------------------------------------------------*/
extern DFUStates DeviceState;
extern uint8_t JumpToApp;
extern int32_t platform_senddata(const uint8_t *msg, uint16_t msg_len);
/* Private function prototypes -----------------------------------------------*/
static uint32_t baseOfAdressType(uint8_t type);
static uint8_t isBiggerThanAvailable(uint8_t type, uint32_t size);
static void OPDfuIni(uint8_t discover);
bool flash_read(uint8_t *buffer, uint32_t adr, DFUProgType type);
/* Private functions ---------------------------------------------------------*/
void sendData(uint8_t *buf, uint16_t size);
uint32_t CalcFirmCRC(void);
@ -109,35 +113,40 @@ void DataDownload(__attribute__((unused)) DownloadAction action)
sendData(SendBuffer + 1, 63);
}
}
static uint32_t unpack_uint32(uint8_t *buffer)
{
uint32_t ret = buffer[0] << 24;
ret += buffer[1] << 16;
ret += buffer[2] << 8;
ret += buffer[3];
return ret;
}
static void pack_uint32(uint32_t value, uint8_t *buffer)
{
buffer[0] = value >> 24;
buffer[1] = value >> 16;
buffer[2] = value >> 8;
buffer[3] = value;
}
void processComand(uint8_t *xReceive_Buffer)
{
Command = xReceive_Buffer[COMMAND];
#ifdef DEBUG_SSP
char str[63] = { 0 };
sprintf(str, "Received COMMAND:%d|", Command);
PIOS_COM_SendString(PIOS_COM_TELEM_USB, str);
#endif
EchoReqFlag = (Command >> 7);
EchoAnsFlag = (Command >> 6) & 0x01;
StartFlag = (Command >> 5) & 0x01;
Count = xReceive_Buffer[COUNT] << 24;
Count += xReceive_Buffer[COUNT + 1] << 16;
Count += xReceive_Buffer[COUNT + 2] << 8;
Count += xReceive_Buffer[COUNT + 3];
Data = xReceive_Buffer[DATA] << 24;
Data += xReceive_Buffer[DATA + 1] << 16;
Data += xReceive_Buffer[DATA + 2] << 8;
Data += xReceive_Buffer[DATA + 3];
Count = unpack_uint32(&xReceive_Buffer[COUNT]);
Data = unpack_uint32(&xReceive_Buffer[DATA]);
Data0 = xReceive_Buffer[DATA];
Data1 = xReceive_Buffer[DATA + 1];
Data2 = xReceive_Buffer[DATA + 2];
Data3 = xReceive_Buffer[DATA + 3];
for (uint32_t i = 0; i < 3; i++) {
Opt[i] = xReceive_Buffer[DATA + 4 * (i + 1)] << 24 |
xReceive_Buffer[DATA + 4 * (i + 1) + 1] << 16 |
xReceive_Buffer[DATA + 4 * (i + 1) + 2] << 8 |
xReceive_Buffer[DATA + 4 * (i + 1) + 3];
Opt[i] = unpack_uint32(&xReceive_Buffer[DATA + 4 * (i + 1)]);
}
Command = Command & 0b00011111;
@ -182,10 +191,7 @@ void processComand(uint8_t *xReceive_Buffer)
TransferType = Data0;
SizeOfTransfer = Count;
Next_Packet = 1;
Expected_CRC = Data2 << 24;
Expected_CRC += Data3 << 16;
Expected_CRC += xReceive_Buffer[DATA + 4] << 8;
Expected_CRC += xReceive_Buffer[DATA + 5];
Expected_CRC = unpack_uint32(&xReceive_Buffer[DATA + 2]);
SizeOfLastPacket = Data1;
if (isBiggerThanAvailable(TransferType, (SizeOfTransfer - 1)
@ -229,10 +235,7 @@ void processComand(uint8_t *xReceive_Buffer)
case Self_flash:
for (uint8_t x = 0; x < numberOfWords; ++x) {
offset = 4 * x;
Data = xReceive_Buffer[DATA + offset] << 24;
Data += xReceive_Buffer[DATA + 1 + offset] << 16;
Data += xReceive_Buffer[DATA + 2 + offset] << 8;
Data += xReceive_Buffer[DATA + 3 + offset];
Data = unpack_uint32(&xReceive_Buffer[DATA + offset]);
aux = baseOfAdressType(TransferType) + (uint32_t)(
Count * 14 * 4 + x * 4);
result = 0;
@ -286,18 +289,12 @@ void processComand(uint8_t *xReceive_Buffer)
Buffer[8] = WRFlags >> 8;
Buffer[9] = WRFlags;
} else {
Buffer[2] = devicesTable[Data0 - 1].sizeOfCode >> 24;
Buffer[3] = devicesTable[Data0 - 1].sizeOfCode >> 16;
Buffer[4] = devicesTable[Data0 - 1].sizeOfCode >> 8;
Buffer[5] = devicesTable[Data0 - 1].sizeOfCode;
pack_uint32(devicesTable[Data0 - 1].sizeOfCode, &Buffer[2]);
Buffer[6] = Data0;
Buffer[7] = devicesTable[Data0 - 1].BL_Version;
Buffer[8] = devicesTable[Data0 - 1].sizeOfDescription;
Buffer[9] = devicesTable[Data0 - 1].devID;
Buffer[10] = devicesTable[Data0 - 1].FW_Crc >> 24;
Buffer[11] = devicesTable[Data0 - 1].FW_Crc >> 16;
Buffer[12] = devicesTable[Data0 - 1].FW_Crc >> 8;
Buffer[13] = devicesTable[Data0 - 1].FW_Crc;
pack_uint32(devicesTable[Data0 - 1].FW_Crc, &Buffer[10]);
Buffer[14] = devicesTable[Data0 - 1].devID >> 8;
Buffer[15] = devicesTable[Data0 - 1].devID;
}
@ -341,14 +338,7 @@ void processComand(uint8_t *xReceive_Buffer)
}
break;
case Download_Req:
#ifdef DEBUG_SSP
sprintf(str, "COMMAND:DOWNLOAD_REQ 1 Status=%d|", DeviceState);
PIOS_COM_SendString(PIOS_COM_TELEM_USB, str);
#endif
if (DeviceState == DFUidle) {
#ifdef DEBUG_SSP
PIOS_COM_SendString(PIOS_COM_TELEM_USB, "COMMAND:DOWNLOAD_REQ 1|");
#endif
downType = Data0;
downPacketTotal = Count;
downSizeOfLastPacket = Data1;
@ -370,10 +360,7 @@ void processComand(uint8_t *xReceive_Buffer)
Buffer[0] = 0x01;
Buffer[1] = Status_Rep;
if (DeviceState == wrong_packet_received) {
Buffer[2] = Aditionals >> 24;
Buffer[3] = Aditionals >> 16;
Buffer[4] = Aditionals >> 8;
Buffer[5] = Aditionals;
pack_uint32(Aditionals, &Buffer[2]);
} else {
Buffer[2] = 0;
Buffer[3] = ((uint16_t)Aditionals) >> 8;
@ -469,7 +456,7 @@ uint32_t CalcFirmCRC()
}
void sendData(uint8_t *buf, uint16_t size)
{
PIOS_COM_MSG_Send(PIOS_COM_TELEM_USB, buf, size);
platform_senddata(buf, size);
}
bool flash_read(uint8_t *buffer, uint32_t adr, DFUProgType type)

42
flight/libraries/optypes.c Normal file
View File

@ -0,0 +1,42 @@
/**
******************************************************************************
*
* @file optypes.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief OP Generic data type library
* --
* @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 <optypes.h>
const Color_t Color_Off = COLOR_OFF;
const Color_t Color_Black = COLOR_BLACK;
const Color_t Color_Red = COLOR_RED;
const Color_t Color_Lime = COLOR_LIME;
const Color_t Color_Blue = COLOR_BLUE;
const Color_t Color_Yellow = COLOR_YELLOW;
const Color_t Color_Cian = COLOR_CIAN;
const Color_t Color_Magenta = COLOR_MAGENTA;
const Color_t Color_Navy = COLOR_NAVY;
const Color_t Color_Green = COLOR_GREEN;
const Color_t Color_Purple = COLOR_PURPLE;
const Color_t Color_Teal = COLOR_TEAL;
const Color_t Color_Orange = COLOR_ORANGE;
const Color_t Color_White = COLOR_WHITE;

202
flight/libraries/paths.c
View File

@ -26,49 +26,53 @@
#include <pios.h>
#include <pios_math.h>
#include "paths.h"
#include <mathmisc.h>
#include "uavobjectmanager.h" // <--.
#include "pathdesired.h" // <-- needed only for correct ENUM macro usage with path modes (PATHDESIRED_MODE_xxx,
#include "paths.h"
// no direct UAVObject usage allowed in this file
// private functions
static void path_endpoint(float *start_point, float *end_point, float *cur_point, struct path_status *status);
static void path_vector(float *start_point, float *end_point, float *cur_point, struct path_status *status);
static void path_circle(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool clockwise);
static void path_endpoint(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode);
static void path_vector(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode);
static void path_circle(PathDesiredData *path, float *cur_point, struct path_status *status, bool clockwise);
/**
* @brief Compute progress along path and deviation from it
* @param[in] start_point Starting point
* @param[in] end_point Ending point
* @param[in] path PathDesired structure
* @param[in] cur_point Current location
* @param[in] mode Path following mode
* @param[out] status Structure containing progress along path and deviation
*/
void path_progress(float *start_point, float *end_point, float *cur_point, struct path_status *status, uint8_t mode)
void path_progress(PathDesiredData *path, float *cur_point, struct path_status *status)
{
switch (mode) {
switch (path->Mode) {
case PATHDESIRED_MODE_FLYVECTOR:
return path_vector(path, cur_point, status, true);
break;
case PATHDESIRED_MODE_DRIVEVECTOR:
return path_vector(start_point, end_point, cur_point, status);
return path_vector(path, cur_point, status, false);
break;
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_DRIVECIRCLERIGHT:
return path_circle(start_point, end_point, cur_point, status, 1);
return path_circle(path, cur_point, status, 1);
break;
case PATHDESIRED_MODE_FLYCIRCLELEFT:
case PATHDESIRED_MODE_DRIVECIRCLELEFT:
return path_circle(start_point, end_point, cur_point, status, 0);
return path_circle(path, cur_point, status, 0);
break;
case PATHDESIRED_MODE_FLYENDPOINT:
return path_endpoint(path, cur_point, status, true);
break;
case PATHDESIRED_MODE_DRIVEENDPOINT:
default:
// use the endpoint as default failsafe if called in unknown modes
return path_endpoint(start_point, end_point, cur_point, status);
return path_endpoint(path, cur_point, status, false);
break;
}
@ -76,135 +80,153 @@ void path_progress(float *start_point, float *end_point, float *cur_point, struc
/**
* @brief Compute progress towards endpoint. Deviation equals distance
* @param[in] start_point Starting point
* @param[in] end_point Ending point
* @param[in] path PathDesired
* @param[in] cur_point Current location
* @param[out] status Structure containing progress along path and deviation
* @param[in] mode3D set true to include altitude in distance and progress calculation
*/
static void path_endpoint(float *start_point, float *end_point, float *cur_point, struct path_status *status)
static void path_endpoint(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode3D)
{
float path_north, path_east, diff_north, diff_east;
float diff[3];
float dist_path, dist_diff;
// we do not correct in this mode
status->correction_direction[0] = status->correction_direction[1] = 0;
// Distance to go
path_north = end_point[0] - start_point[0];
path_east = end_point[1] - start_point[1];
status->path_vector[0] = path->End.North - path->Start.North;
status->path_vector[1] = path->End.East - path->Start.East;
status->path_vector[2] = mode3D ? path->End.Down - path->Start.Down : 0.0f;
// Current progress location relative to end
diff_north = end_point[0] - cur_point[0];
diff_east = end_point[1] - cur_point[1];
diff[0] = path->End.North - cur_point[0];
diff[1] = path->End.East - cur_point[1];
diff[2] = mode3D ? path->End.Down - cur_point[2] : 0.0f;
dist_diff = sqrtf(diff_north * diff_north + diff_east * diff_east);
dist_path = sqrtf(path_north * path_north + path_east * path_east);
dist_diff = vector_lengthf(diff, 3);
dist_path = vector_lengthf(status->path_vector, 3);
if (dist_diff < 1e-6f) {
status->fractional_progress = 1;
status->error = 0;
status->path_direction[0] = status->path_direction[1] = 0;
status->error = 0.0f;
status->correction_vector[0] = status->correction_vector[1] = status->correction_vector[2] = 0.0f;
// we have no base movement direction in this mode
status->path_vector[0] = status->path_vector[1] = status->path_vector[2] = 0.0f;
return;
}
status->fractional_progress = 1 - dist_diff / (1 + dist_path);
if (fmaxf(dist_path, 1.0f) > dist_diff) {
status->fractional_progress = 1 - dist_diff / fmaxf(dist_path, 1.0f);
} else {
status->fractional_progress = 0; // we don't want fractional_progress to become negative
}
status->error = dist_diff;
// Compute direction to travel
status->path_direction[0] = diff_north / dist_diff;
status->path_direction[1] = diff_east / dist_diff;
// Compute correction vector
status->correction_vector[0] = diff[0];
status->correction_vector[1] = diff[1];
status->correction_vector[2] = diff[2];
// base movement direction in this mode is a constant velocity offset on top of correction in the same direction
status->path_vector[0] = path->EndingVelocity * status->correction_vector[0] / dist_diff;
status->path_vector[1] = path->EndingVelocity * status->correction_vector[1] / dist_diff;
status->path_vector[2] = path->EndingVelocity * status->correction_vector[2] / dist_diff;
}
/**
* @brief Compute progress along path and deviation from it
* @param[in] start_point Starting point
* @param[in] end_point Ending point
* @param[in] path PathDesired
* @param[in] cur_point Current location
* @param[out] status Structure containing progress along path and deviation
* @param[in] mode3D set true to include altitude in distance and progress calculation
*/
static void path_vector(float *start_point, float *end_point, float *cur_point, struct path_status *status)
static void path_vector(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode3D)
{
float path_north, path_east, diff_north, diff_east;
float diff[3];
float dist_path;
float dot;
float normal[2];
float velocity;
float track_point[3];
// Distance to go
path_north = end_point[0] - start_point[0];
path_east = end_point[1] - start_point[1];
status->path_vector[0] = path->End.North - path->Start.North;
status->path_vector[1] = path->End.East - path->Start.East;
status->path_vector[2] = mode3D ? path->End.Down - path->Start.Down : 0.0f;
// Current progress location relative to start
diff_north = cur_point[0] - start_point[0];
diff_east = cur_point[1] - start_point[1];
diff[0] = cur_point[0] - path->Start.North;
diff[1] = cur_point[1] - path->Start.East;
diff[2] = mode3D ? cur_point[2] - path->Start.Down : 0.0f;
dot = path_north * diff_north + path_east * diff_east;
dist_path = sqrtf(path_north * path_north + path_east * path_east);
dot = status->path_vector[0] * diff[0] + status->path_vector[1] * diff[1] + status->path_vector[2] * diff[2];
dist_path = vector_lengthf(status->path_vector, 3);
if (dist_path < 1e-6f) {
// if the path is too short, we cannot determine vector direction.
if (dist_path > 1e-6f) {
// Compute direction to travel & progress
status->fractional_progress = dot / (dist_path * dist_path);
} else {
// Fly towards the endpoint to prevent flying away,
// but assume progress=1 either way.
path_endpoint(start_point, end_point, cur_point, status);
path_endpoint(path, cur_point, status, mode3D);
status->fractional_progress = 1;
return;
}
// Compute point on track that is closest to our current position.
track_point[0] = status->fractional_progress * status->path_vector[0] + path->Start.North;
track_point[1] = status->fractional_progress * status->path_vector[1] + path->Start.East;
track_point[2] = status->fractional_progress * status->path_vector[2] + path->Start.Down;
// Compute the normal to the path
normal[0] = -path_east / dist_path;
normal[1] = path_north / dist_path;
status->correction_vector[0] = track_point[0] - cur_point[0];
status->correction_vector[1] = track_point[1] - cur_point[1];
status->correction_vector[2] = track_point[2] - cur_point[2];
status->fractional_progress = dot / (dist_path * dist_path);
status->error = normal[0] * diff_north + normal[1] * diff_east;
status->error = vector_lengthf(status->correction_vector, 3);
// Compute direction to correct error
status->correction_direction[0] = (status->error > 0) ? -normal[0] : normal[0];
status->correction_direction[1] = (status->error > 0) ? -normal[1] : normal[1];
// Now just want magnitude of error
status->error = fabs(status->error);
// Compute direction to travel
status->path_direction[0] = path_north / dist_path;
status->path_direction[1] = path_east / dist_path;
// correct movement vector to current velocity
velocity = path->StartingVelocity + boundf(status->fractional_progress, 0.0f, 1.0f) * (path->EndingVelocity - path->StartingVelocity);
status->path_vector[0] = velocity * status->path_vector[0] / dist_path;
status->path_vector[1] = velocity * status->path_vector[1] / dist_path;
status->path_vector[2] = velocity * status->path_vector[2] / dist_path;
}
/**
* @brief Compute progress along circular path and deviation from it
* @param[in] start_point Starting point
* @param[in] end_point Center point
* @param[in] path PathDesired
* @param[in] cur_point Current location
* @param[out] status Structure containing progress along path and deviation
*/
static void path_circle(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool clockwise)
static void path_circle(PathDesiredData *path, float *cur_point, struct path_status *status, bool clockwise)
{
float radius_north, radius_east, diff_north, diff_east;
float radius_north, radius_east, diff_north, diff_east, diff_down;
float radius, cradius;
float normal[2];
float progress;
float a_diff, a_radius;
// Radius
radius_north = end_point[0] - start_point[0];
radius_east = end_point[1] - start_point[1];
radius_north = path->End.North - path->Start.North;
radius_east = path->End.East - path->Start.East;
// Current location relative to center
diff_north = cur_point[0] - end_point[0];
diff_east = cur_point[1] - end_point[1];
diff_north = cur_point[0] - path->End.North;
diff_east = cur_point[1] - path->End.East;
diff_down = cur_point[2] - path->End.Down;
radius = sqrtf(powf(radius_north, 2) + powf(radius_east, 2));
cradius = sqrtf(powf(diff_north, 2) + powf(diff_east, 2));
radius = sqrtf(squaref(radius_north) + squaref(radius_east));
cradius = sqrtf(squaref(diff_north) + squaref(diff_east));
// circles are always horizontal (for now - TODO: allow 3d circles - problem: clockwise/counterclockwise does no longer apply)
status->path_vector[2] = 0.0f;
// error is current radius minus wanted radius - positive if too close
status->error = radius - cradius;
if (cradius < 1e-6f) {
// cradius is zero, just fly somewhere and make sure correction is still a normal
// cradius is zero, just fly somewhere
status->fractional_progress = 1;
status->error = radius;
status->correction_direction[0] = 0;
status->correction_direction[1] = 1;
status->path_direction[0] = 1;
status->path_direction[1] = 0;
return;
}
status->correction_vector[0] = 0;
status->correction_vector[1] = 0;
status->path_vector[0] = path->EndingVelocity;
status->path_vector[1] = 0;
} else {
if (clockwise) {
// Compute the normal to the radius clockwise
normal[0] = -diff_east / cradius;
@ -228,28 +250,28 @@ static void path_circle(float *start_point, float *end_point, float *cur_point,
progress = (a_diff - a_radius + M_PI_F) / (2.0f * M_PI_F);
if (progress < 0) {
if (progress < 0.0f) {
progress += 1.0f;
} else if (progress >= 1.0f) {
progress -= 1.0f;
}
if (clockwise) {
progress = 1 - progress;
progress = 1.0f - progress;
}
status->fractional_progress = progress;
// error is current radius minus wanted radius - positive if too close
status->error = radius - cradius;
// Compute direction to travel
status->path_vector[0] = normal[0] * path->EndingVelocity;
status->path_vector[1] = normal[1] * path->EndingVelocity;
// Compute direction to correct error
status->correction_direction[0] = (status->error > 0 ? 1 : -1) * diff_north / cradius;
status->correction_direction[1] = (status->error > 0 ? 1 : -1) * diff_east / cradius;
status->correction_vector[0] = status->error * diff_north / cradius;
status->correction_vector[1] = status->error * diff_east / cradius;
}
// Compute direction to travel
status->path_direction[0] = normal[0];
status->path_direction[1] = normal[1];
status->correction_vector[2] = -diff_down;
status->error = fabs(status->error);
}

35
flight/libraries/plans.c
View File

@ -70,8 +70,6 @@ void plan_setup_positionHold()
FlightModeSettingsPositionHoldOffsetData offset;
FlightModeSettingsPositionHoldOffsetGet(&offset);
float startingVelocity;
FlightModeSettingsPositionHoldStartingVelocityGet(&startingVelocity);
pathDesired.End.North = positionState.North;
pathDesired.End.East = positionState.East;
@ -79,7 +77,7 @@ void plan_setup_positionHold()
pathDesired.Start.North = positionState.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter
pathDesired.Start.East = positionState.East;
pathDesired.Start.Down = positionState.Down;
pathDesired.StartingVelocity = startingVelocity;
pathDesired.StartingVelocity = 0.0f;
pathDesired.EndingVelocity = 0.0f;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
@ -110,8 +108,6 @@ void plan_setup_returnToBase()
destDown = MIN(positionStateDown, takeoffLocation.Down) - destDown;
FlightModeSettingsPositionHoldOffsetData offset;
FlightModeSettingsPositionHoldOffsetGet(&offset);
float startingVelocity;
FlightModeSettingsPositionHoldStartingVelocityGet(&startingVelocity);
pathDesired.End.North = takeoffLocation.North;
pathDesired.End.East = takeoffLocation.East;
@ -121,16 +117,27 @@ void plan_setup_returnToBase()
pathDesired.Start.East = takeoffLocation.East;
pathDesired.Start.Down = destDown;
pathDesired.StartingVelocity = startingVelocity;
pathDesired.StartingVelocity = 0.0f;
pathDesired.EndingVelocity = 0.0f;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
}
static PiOSDeltatimeConfig landdT;
void plan_setup_land()
{
float descendspeed;
plan_setup_positionHold();
FlightModeSettingsLandingVelocityGet(&descendspeed);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.StartingVelocity = descendspeed;
pathDesired.EndingVelocity = descendspeed;
PathDesiredSet(&pathDesired);
PIOS_DELTATIME_Init(&landdT, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
}
/**
@ -138,12 +145,18 @@ void plan_setup_land()
*/
void plan_run_land()
{
float downPos, descendspeed;
PathDesiredEndData pathDesiredEnd;
PathDesiredEndGet(&pathDesiredEnd);
PositionStateDownGet(&downPos); // current down position
PathDesiredEndGet(&pathDesiredEnd); // desired position
PathDesiredEndingVelocityGet(&descendspeed);
PositionStateDownGet(&pathDesiredEnd.Down);
pathDesiredEnd.Down += 5;
// desired position is updated to match the desired descend speed but don't run ahead
// too far if the current position can't keep up. This normaly means we have landed.
if (pathDesiredEnd.Down - downPos < 10) {
pathDesiredEnd.Down += descendspeed * PIOS_DELTATIME_GetAverageSeconds(&landdT);
}
PathDesiredEndSet(&pathDesiredEnd);
}
@ -366,8 +379,6 @@ void plan_setup_AutoCruise()
FlightModeSettingsPositionHoldOffsetData offset;
FlightModeSettingsPositionHoldOffsetGet(&offset);
float startingVelocity;
FlightModeSettingsPositionHoldStartingVelocityGet(&startingVelocity);
// initialization is flight in direction of the nose.
// the velocity is not relevant, as it will be reset by the run function even during first call
@ -387,7 +398,7 @@ void plan_setup_AutoCruise()
pathDesired.Start.North = pathDesired.End.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter
pathDesired.Start.East = pathDesired.End.East;
pathDesired.Start.Down = pathDesired.End.Down;
pathDesired.StartingVelocity = startingVelocity;
pathDesired.StartingVelocity = 0.0f;
pathDesired.EndingVelocity = 0.0f;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;

2
flight/libraries/rscode/ecc.h
View File

@ -66,7 +66,7 @@ extern int pBytes[MAXDEG];
extern int synBytes[MAXDEG];
/* print debugging info */
extern int DEBUG;
//extern int DEBUG;
/* Reed Solomon encode/decode routines */
void initialize_ecc (void);

2
flight/libraries/rscode/rs.c
View File

@ -38,7 +38,7 @@ int synBytes[MAXDEG];
/* generator polynomial */
int genPoly[MAXDEG*2];
int DEBUG = FALSE;
//int DEBUG = FALSE;
static void
compute_genpoly (int nbytes, int genpoly[]);

2
flight/libraries/sanitycheck.c
View File

@ -149,7 +149,6 @@ int32_t configuration_check()
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOCRUISE:
ADDSEVERITY(!coptercontrol);
ADDSEVERITY(PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER));
ADDSEVERITY(navCapableFusion);
break;
default:
@ -263,6 +262,7 @@ 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:

795
flight/libraries/ssp.c Normal file
View File

@ -0,0 +1,795 @@
/***********************************************************************************************************
*
* NAME: ssp.c
* DESCRIPTION: simple serial protocol - packet based serial transport layer.
* AUTHOR: Joe Hlebasko
* HISTORY: Created 1/1/2010
*
* Packet Formats
* Format:
* +------+----+------+---------------------------+--------+
* | 225 | L1 | S# | App Data (0-254 bytes) | CRC 16 |
* +------+----+------+---------------------------+--------+
*
* 225 = sync byte, indicates start of a packet
* L1 = 1 byte for size of data payload. (sequence number is part of data payload.)
* S# = 1 byte for sequence number.
* Seq of 0 = seq # synchronise request, forces other end to reset receive sequence number to 1.
* sender of synchronise request will reset the tx seq number to 1
* Seq # of 1..127 = normal data packets. Sequence number is incremented by for each transmitted
* packet. Rolls over from 127 to 1.
* if most sig. bit is set then the packet is an ACK packet of data packet sequence number of the
* lower 7 bits (1..127)
* App Data may contain 0..254 bytes. The sequence number is consider part of the payload.
* CRC 16 - 16 bits of CRC values of Sequence # and data bytes.
*
* Protocol has two types of packets: data and ack packets. ACK packets have the most sig. bit set in the
* sequence number, this implies that valid sequence numbers are 1..127
*
* This protocol uses the concept of sequences numbers to determine if a given packet has been received. This
* requires both devices to be able to synchronize sequence numbers. This is accomplished by sending a packet
* length 1 and sequence number = 0. The receive then resets it's transmit sequence number to 1.
*
* ACTIVE_SYNCH is a version that will automatically send a synch request if it receives a synch packet. Only
* one device in the communication should do otherwise you end up with an endless loops of synchronization.
* Right now each side needs to manually issues a synch request.
*
* This protocol is best used in cases where one device is the master and the other is the slave, or a don't
* speak unless spoken to type of approach.
*
* The following are items are required to initialize a port for communications:
* 1. The number attempts for each packet
* 2. time to wait for an ack.
* 3. pointer to buffer to be used for receiving.
* 4. pointer to a buffer to be used for transmission
* 5. length of each buffer (rx and tx)
* 6. Four functions:
* 1. write byte = writes a byte out the serial port (or other comm device)
* 2. read byte = retrieves a byte from the serial port. Returns -1 if a byte is not available
* 3. callback = function to call when a valid data packet has been received. This function is responsible
* to do what needs to be done with the data when it is received. The primary mission of this function
* should be to copy the data to a private buffer out of the working receive buffer to prevent overrun.
* processing should be kept to a minimum.
* 4. get time = function should return the current time. Note that time units are not specified it just
* needs to be some measure of time that increments as time passes by. The timeout values for a given
* port should the units used/returned by the get time function.
*
* All of the state information of a communication port is contained in a Port_t structure. This allows this
* module to operature on multiple communication ports with a single code base.
*
* The ssp_ReceiveProcess and ssp_SendProcess functions need to be called to process data through the
* respective state machines. Typical implementation would have a serial ISR to pull bytes out of the UART
* and place into a circular buffer. The serial read function would then pull bytes out this buffer
* processing. The TX side has the write function placing bytes into a circular buffer with the TX ISR
* pulling bytes out of the buffer and putting into the UART. It is possible to run the receive process from
* the receive ISR but care must be taken on processing data when it is received to avoid holding up the ISR
* and sending ACK packets from the receive ISR.
*
***********************************************************************************************************/
/** INCLUDE FILES **/
#include <stdint.h>
#include <string.h>
#include <stdio.h>
#include <pios.h>
#include "ssp.h"
/** PRIVATE DEFINITIONS **/
#define SYNC 225 // Sync character used in Serial Protocol
#define ESC 224 // ESC character used in Serial Protocol
#define ESC_SYNC 1 // ESC_SYNC character used in Serial Protocol
#define ACK_BIT 0x80 // Ack bit, bit 7 of sequence number, 1 = Acknowledge, 0 =
// new packet
// packet location definitions.
#define LENGTH 0
#define SEQNUM 1
#define DATA 2
// Make larger sized integers from smaller sized integers
#define MAKEWORD16(ub, lb) ((uint16_t)0x0000 | ((uint16_t)(ub) << 8) | (uint16_t)(lb))
#define MAKEWORD32(uw, lw) ((uint32_t)(0x0UL | ((uint32_t)(uw) << 16) | (uint32_t)(lw)))
#define MAKEWORD32B(b3, b2, b1, b0) ((uint32_t)((uint32_t)(b3) << 24) | ((uint32_t)(b2) << 16) | ((uint32_t)(b1) << 8) | ((uint32_t)(b0))
// Used to extract smaller integers from larger sized intergers
#define LOWERBYTE(w) (uint8_t)((w) & 0x00ff)
#define UPPERBYTE(w) (uint8_t)(((w) & 0xff00) >> 8)
#define UPPERWORD(lw) (uint16_t)(((lw) & 0xffff0000) >> 16)
#define LOWERWORD(lw) (uint16_t)((lw) & 0x0000ffff)
// Macros to operate on a target and bitmask.
#define CLEARBIT(a, b) ((a) = (a) & ~(b))
#define SETBIT(a, b) ((a) = (a) | (b))
#define TOGGLEBIT(a, b) ((a) = (a) ^ (b))
// test bit macros operate using a bit mask.
#define ISBITSET(a, b) (((a) & (b)) == (b) ? TRUE : FALSE)
#define ISBITCLEAR(a, b) ((~(a) & (b)) == (b) ? TRUE : FALSE)
/** PRIVATE FUNCTIONS **/
// static void sf_SendSynchPacket( Port_t *thisport );
static uint16_t sf_checksum(uint16_t crc, uint8_t data);
static void sf_write_byte(Port_t *thisport, uint8_t c);
static void sf_SetSendTimeout(Port_t *thisport);
static uint16_t sf_CheckTimeout(Port_t *thisport);
static int16_t sf_DecodeState(Port_t *thisport, uint8_t c);
static int16_t sf_ReceiveState(Port_t *thisport, uint8_t c);
static void sf_SendPacket(Port_t *thisport);
static void sf_SendAckPacket(Port_t *thisport, uint8_t seqNumber);
static void sf_MakePacket(uint8_t *buf, const uint8_t *pdata, uint16_t length,
uint8_t seqNo);
static int16_t sf_ReceivePacket(Port_t *thisport);
/* Flag bit masks...*/
#define SENT_SYNCH (0x01)
#define ACK_RECEIVED (0x02)
#define ACK_EXPECTED (0x04)
#define SSP_AWAITING_ACK 0
#define SSP_ACKED 1
#define SSP_IDLE 2
/** PRIVATE DATA **/
static const uint16_t CRC_TABLE[] = { 0x0000, 0xC0C1, 0xC181, 0x0140, 0xC301,
0x03C0, 0x0280, 0xC241, 0xC601, 0x06C0,0x0780, 0xC741, 0x0500, 0xC5C1,
0xC481, 0x0440, 0xCC01, 0x0CC0, 0x0D80,0xCD41, 0x0F00, 0xCFC1, 0xCE81,
0x0E40, 0x0A00, 0xCAC1, 0xCB81, 0x0B40,0xC901, 0x09C0, 0x0880, 0xC841,
0xD801, 0x18C0, 0x1980, 0xD941, 0x1B00,0xDBC1, 0xDA81, 0x1A40, 0x1E00,
0xDEC1, 0xDF81, 0x1F40, 0xDD01, 0x1DC0,0x1C80, 0xDC41, 0x1400, 0xD4C1,
0xD581, 0x1540, 0xD701, 0x17C0, 0x1680,0xD641, 0xD201, 0x12C0, 0x1380,
0xD341, 0x1100, 0xD1C1, 0xD081, 0x1040,0xF001, 0x30C0, 0x3180, 0xF141,
0x3300, 0xF3C1, 0xF281, 0x3240, 0x3600,0xF6C1, 0xF781, 0x3740, 0xF501,
0x35C0, 0x3480, 0xF441, 0x3C00, 0xFCC1,0xFD81, 0x3D40, 0xFF01, 0x3FC0,
0x3E80, 0xFE41, 0xFA01, 0x3AC0, 0x3B80,0xFB41, 0x3900, 0xF9C1, 0xF881,
0x3840, 0x2800, 0xE8C1, 0xE981, 0x2940,0xEB01, 0x2BC0, 0x2A80, 0xEA41,
0xEE01, 0x2EC0, 0x2F80, 0xEF41, 0x2D00,0xEDC1, 0xEC81, 0x2C40, 0xE401,
0x24C0, 0x2580, 0xE541, 0x2700, 0xE7C1,0xE681, 0x2640, 0x2200, 0xE2C1,
0xE381, 0x2340, 0xE101, 0x21C0, 0x2080,0xE041, 0xA001, 0x60C0, 0x6180,
0xA141, 0x6300, 0xA3C1, 0xA281, 0x6240,0x6600, 0xA6C1, 0xA781, 0x6740,
0xA501, 0x65C0, 0x6480, 0xA441, 0x6C00,0xACC1, 0xAD81, 0x6D40, 0xAF01,
0x6FC0, 0x6E80, 0xAE41, 0xAA01, 0x6AC0,0x6B80, 0xAB41, 0x6900, 0xA9C1,
0xA881, 0x6840, 0x7800, 0xB8C1, 0xB981,0x7940, 0xBB01, 0x7BC0, 0x7A80,
0xBA41, 0xBE01, 0x7EC0, 0x7F80, 0xBF41,0x7D00, 0xBDC1, 0xBC81, 0x7C40,
0xB401, 0x74C0, 0x7580, 0xB541, 0x7700,0xB7C1, 0xB681, 0x7640, 0x7200,
0xB2C1, 0xB381, 0x7340, 0xB101, 0x71C0,0x7080, 0xB041, 0x5000, 0x90C1,
0x9181, 0x5140, 0x9301, 0x53C0, 0x5280,0x9241, 0x9601, 0x56C0, 0x5780,
0x9741, 0x5500, 0x95C1, 0x9481, 0x5440,0x9C01, 0x5CC0, 0x5D80, 0x9D41,
0x5F00, 0x9FC1, 0x9E81, 0x5E40, 0x5A00,0x9AC1, 0x9B81, 0x5B40, 0x9901,
0x59C0, 0x5880, 0x9841, 0x8801, 0x48C0,0x4980, 0x8941, 0x4B00, 0x8BC1,
0x8A81, 0x4A40, 0x4E00, 0x8EC1, 0x8F81,0x4F40, 0x8D01, 0x4DC0, 0x4C80,
0x8C41, 0x4400, 0x84C1, 0x8581, 0x4540,0x8701, 0x47C0, 0x4680, 0x8641,
0x8201, 0x42C0, 0x4380, 0x8341, 0x4100,0x81C1, 0x8081, 0x4040 };
/** EXTERNAL DATA **/
/** EXTERNAL FUNCTIONS **/
/** VERIFICATION FUNCTIONS **/
/***********************************************************************************************************/
/*!
* \brief Initializes the communication port for use
* \param thisport = pointer to port structure to initialize
* \param info = config struct with default values.
* \return None.
*
* \note
* Must be called before calling the Send or REceive process functions.
*/
void ssp_Init(Port_t *thisport, const PortConfig_t *const info)
{
thisport->pfCallBack = info->pfCallBack;
thisport->pfSerialRead = info->pfSerialRead;
thisport->pfSerialWrite = info->pfSerialWrite;
thisport->pfGetTime = info->pfGetTime;
thisport->maxRetryCount = info->max_retry;
thisport->timeoutLen = info->timeoutLen;
thisport->txBufSize = info->txBufSize;
thisport->rxBufSize = info->rxBufSize;
thisport->txBuf = info->txBuf;
thisport->rxBuf = info->rxBuf;
thisport->retryCount = 0;
thisport->sendSynch = FALSE; // TRUE;
thisport->rxSeqNo = 255;
thisport->txSeqNo = 255;
thisport->SendState = SSP_IDLE;
}
/*!
* \brief Runs the send process, checks for receipt of ack, timeouts and resends if needed.
* \param thisport = which port to use
* \return SSP_TX_WAITING - waiting for a valid ACK to arrive
* \return SSP_TX_TIMEOUT - failed to receive a valid ACK in the timeout period, after retrying.
* \return SSP_TX_IDLE - not expecting a ACK packet (no current transmissions in progress)
* \return SSP_TX_ACKED - valid ACK received before timeout period.
*
* \note
*
*/
int16_t ssp_SendProcess(Port_t *thisport)
{
int16_t value = SSP_TX_WAITING;
if (thisport->SendState == SSP_AWAITING_ACK) {
if (sf_CheckTimeout(thisport) == TRUE) {
if (thisport->retryCount < thisport->maxRetryCount) {
// Try again
sf_SendPacket(thisport);
sf_SetSendTimeout(thisport);
value = SSP_TX_WAITING;
} else {
// Give up, # of trys has exceded the limit
value = SSP_TX_TIMEOUT;
CLEARBIT(thisport->flags, ACK_RECEIVED);
thisport->SendState = SSP_IDLE;
}
} else {
value = SSP_TX_WAITING;
}
} else if (thisport->SendState == SSP_ACKED) {
SETBIT(thisport->flags, ACK_RECEIVED);
value = SSP_TX_ACKED;
thisport->SendState = SSP_IDLE;
} else {
thisport->SendState = SSP_IDLE;
value = SSP_TX_IDLE;
}
return value;
}
/*!
* \brief Runs the receive process. fetches a byte at a time and runs the byte through the protocol receive state machine.
* \param thisport - which port to use.
* \return receive status.
*
* \note
*
*/
int16_t ssp_ReceiveProcess(Port_t *thisport)
{
int16_t b;
int16_t packet_status = SSP_RX_IDLE;
do {
b = thisport->pfSerialRead(); // attempt to read a char from the serial buffer
if (b != -1) {
packet_status = sf_ReceiveState(thisport, b); // process the newly received byte in the receive state machine
}
// keep going until either we received a full packet or there are no more bytes to process
} while (packet_status != SSP_RX_COMPLETE && b != -1);
return packet_status;
}
/*!
* \brief processes a single byte through the receive state machine.
* \param thisport = which port to use
* \return current receive status
*
* \note
*
*/
int16_t ssp_ReceiveByte(Port_t *thisport)
{
int16_t b;
int16_t packet_status = SSP_RX_IDLE;
b = thisport->pfSerialRead();
if (b != -1) {
packet_status = sf_ReceiveState(thisport, b);
}
return packet_status;
}
/*!
* \brief Sends a data packet and blocks until timeout or ack is received.
* \param thisport = which port to use
* \param data = pointer to data to send
* \param length = number of data bytes to send. Must be less than 254
* \return true = ack was received within number of retries
* \return false = ack was not received.
*
* \note
*
*/
uint16_t ssp_SendDataBlock(Port_t *thisport, uint8_t *data, uint16_t length)
{
int16_t packet_status = SSP_TX_WAITING;
packet_status = ssp_SendData(thisport, data, length); // send the data
while (packet_status == SSP_TX_WAITING) { // check the status
(void)ssp_ReceiveProcess(thisport); // process any bytes received.
packet_status = ssp_SendProcess(thisport); // check the send status
}
return packet_status == SSP_TX_ACKED; // figure out what happened to the packet
}
/*!
* \brief sends a chunk of data and does not block
* \param thisport = which port to use
* \param data = pointer to data to send
* \param length = number of bytes to send
* \return SSP_TX_BUFOVERRUN = tried to send too much data
* \return SSP_TX_WAITING = data sent and waiting for an ack to arrive
* \return SSP_TX_BUSY = a packet has already been sent, but not yet acked
*
* \note
*
*/
int16_t ssp_SendData(Port_t *thisport, const uint8_t *data,
const uint16_t length)
{
int16_t value = SSP_TX_WAITING;
if ((length + 2) > thisport->txBufSize) {
// TRYING to send too much data.
value = SSP_TX_BUFOVERRUN;
} else if (thisport->SendState == SSP_IDLE) {
#ifdef ACTIVE_SYNCH
if (thisport->sendSynch == TRUE) {
sf_SendSynchPacket(thisport);
}
#endif
#ifdef SYNCH_SEND
if (length == 0) {
// TODO this method could allow a task/user to start a synchronisation step if a zero is mistakenly passed to this function.
// could add a check for a NULL data pointer, or use some sort of static flag that can only be accessed by a static function
// that must be called before calling this function.
// we are attempting to send a synch packet
thisport->txSeqNo = 0; // make this zero to cause the other end to re-synch with us
SETBIT(thisport->flags, SENT_SYNCH);
} else {
// we are sending a data packet
CLEARBIT(thisport->txSeqNo, ACK_BIT); // make sure we are not sending a ACK packet
thisport->txSeqNo++; // update the sequence number.
if (thisport->txSeqNo > 0x7F) { // check for sequence number rollover
thisport->txSeqNo = 1; // if we do have rollover then reset to 1 not zero,
// zero is reserviced for synchronization requests
}
}
#else
CLEARBIT(thisport->txSeqNo, ACK_BIT); // make sure we are not sending a ACK packet
thisport->txSeqNo++; // update the sequence number.
if (thisport->txSeqNo > 0x7F) { // check for sequence number rollover
thisport->txSeqNo = 1; // if we do have rollover then reset to 1 not zero,
// zero is reserved for synchronization requests
}
#endif /* ifdef SYNCH_SEND */
CLEARBIT(thisport->flags, ACK_RECEIVED);
thisport->SendState = SSP_AWAITING_ACK;
value = SSP_TX_WAITING;
thisport->retryCount = 0; // zero out the retry counter for this transmission
sf_MakePacket(thisport->txBuf, data, length, thisport->txSeqNo);
sf_SendPacket(thisport); // punch out the packet to the serial port
sf_SetSendTimeout(thisport); // do the timeout values
} else {
// error we are already sending a packet. Need to wait for the current packet to be acked or timeout.
value = SSP_TX_BUSY;
}
return value;
}
/*!
* \brief Attempts to synchronize the sequence numbers with the other end of the connectin.
* \param thisport = which port to use
* \return true = success
* \return false = failed to receive an ACK to our synch request
*
* \note
* A. send a packet with a sequence number equal to zero
* B. if timed out then:
* send synch packet again
* increment try counter
* if number of tries exceed maximum try limit then exit
* C. goto A
*/
uint16_t ssp_Synchronise(Port_t *thisport)
{
int16_t packet_status;
#ifndef USE_SENDPACKET_DATA
thisport->txSeqNo = 0; // make this zero to cause the other end to re-synch with us
SETBIT(thisport->flags, SENT_SYNCH);
// TODO - should this be using ssp_SendPacketData()??
sf_MakePacket(thisport->txBuf, NULL, 0, thisport->txSeqNo); // construct the packet
sf_SendPacket(thisport);
sf_SetSendTimeout(thisport);
thisport->SendState = SSP_AWAITING_ACK;
packet_status = SSP_TX_WAITING;
#else
packet_status = ssp_SendData(thisport, NULL, 0);
#endif
while (packet_status == SSP_TX_WAITING) { // we loop until we time out.
(void)ssp_ReceiveProcess(thisport); // do the receive process
packet_status = ssp_SendProcess(thisport); // do the send process
}
thisport->sendSynch = FALSE;
return packet_status == SSP_TX_ACKED;
}
/*!
* \brief sends out a preformatted packet for a give port
* \param thisport = which port to use.
* \return none.
*
* \note
* Packet should be formed through the use of sf_MakePacket before calling this function.
*/
static void sf_SendPacket(Port_t *thisport)
{
// add 3 to packet data length for: 1 length + 2 CRC (packet overhead)
uint8_t packetLen = thisport->txBuf[LENGTH] + 3;
// use the raw serial write function so the SYNC byte does not get 'escaped'
thisport->pfSerialWrite(SYNC);
for (uint8_t x = 0; x < packetLen; x++) {
sf_write_byte(thisport, thisport->txBuf[x]);
}
thisport->retryCount++;
}
/*!
* \brief converts data to transport layer protocol packet format.
* \param txbuf = buffer to use when forming the packet
* \param pdata = pointer to data to use
* \param length = number of bytes to use
* \param seqNo = sequence number of this packet
* \return none.
*
* \note
* 1. This function does not try to interpret ACK or SYNCH packets. This should
* be done by the caller of this function.
* 2. This function will attempt to format all data upto the size of the tx buffer.
* Any extra data beyond that will be ignored.
* 3. TODO: Should this function return an error if data length to be sent is greater th tx buffer size?
*
*/
void sf_MakePacket(uint8_t *txBuf, const uint8_t *pdata, uint16_t length,
uint8_t seqNo)
{
uint16_t crc = 0xffff;
uint16_t bufPos = 0;
uint8_t b;
// add 1 for the seq. number
txBuf[LENGTH] = length + 1;
txBuf[SEQNUM] = seqNo;
crc = sf_checksum(crc, seqNo);
length = length + 2; // add two for the length and seqno bytes which are added before the loop.
for (bufPos = 2; bufPos < length; bufPos++) {
b = *pdata++;
txBuf[bufPos] = b;
crc = sf_checksum(crc, b); // update CRC value
}
txBuf[bufPos++] = LOWERBYTE(crc);
txBuf[bufPos] = UPPERBYTE(crc);
}
/*!
* \brief sends out an ack packet to given sequence number
* \param thisport = which port to use
* \param seqNumber = sequence number of the packet we would like to ack
* \return none.
*
* \note
*
*/
static void sf_SendAckPacket(Port_t *thisport, uint8_t seqNumber)
{
uint8_t AckSeqNumber = SETBIT(seqNumber, ACK_BIT);
// create the packet, note we pass AckSequenceNumber directly
sf_MakePacket(thisport->txBuf, NULL, 0, AckSeqNumber);
sf_SendPacket(thisport);
// we don't set the timeout for an ACK because we don't ACK our ACKs in this protocol
}
/*!
* \brief writes a byte out the output channel. Adds escape byte where needed
* \param thisport = which port to use
* \param c = byte to send
* \return none.
*
* \note
*
*/
static void sf_write_byte(Port_t *thisport, uint8_t c)
{
if (c == SYNC) { // check for SYNC byte
thisport->pfSerialWrite(ESC); // since we are not starting a packet we must ESCAPE the SYNCH byte
thisport->pfSerialWrite(ESC_SYNC); // now send the escaped synch char
} else if (c == ESC) { // Check for ESC character
thisport->pfSerialWrite(ESC); // if it is, we need to send it twice
thisport->pfSerialWrite(ESC);
} else {
thisport->pfSerialWrite(c); // otherwise write the byte to serial port
}
}
/************************************************************************************************************
*
* NAME: uint16_t ssp_crc16( uint16_t crc, uint16_t data )
* DESCRIPTION: Uses crc_table to calculate new crc
* ARGUMENTS:
* arg1: crc
* arg2: data - byte to calculate into CRC
* RETURN: New crc
* CREATED: 5/8/02
*
*************************************************************************************************************/
/*!
* \brief calculates the new CRC value for 'data'
* \param crc = current CRC value
* \param data = new byte
* \return updated CRC value
*
* \note
*
*/
static uint16_t sf_checksum(uint16_t crc, uint8_t data)
{
#ifdef SPP_USES_CRC
return (crc >> 8) ^ CRC_TABLE[(crc ^ data) & 0x00FF];
#else
uint8_t cka = crc & 0xff;
uint8_t ckb = (crc >> 8) & 0xff;
cka += data;
ckb += cka;
return cka | ckb << 8;
#endif
}
/*!
* \brief sets the timeout for the given packet
* \param thisport = which port to use
* \return none.
*
* \note
*
*/
static void sf_SetSendTimeout(Port_t *thisport)
{
uint32_t timeout;
timeout = thisport->pfGetTime() + thisport->timeoutLen;
thisport->timeout = timeout;
}
/*!
* \brief checks to see if a timeout occured
* \param thisport = which port to use
* \return true = a timeout has occurred
* \return false = has not timed out
*
* \note
*
*/
static uint16_t sf_CheckTimeout(Port_t *thisport)
{
uint16_t retval = FALSE;
uint32_t current_time;
current_time = thisport->pfGetTime();
if (current_time > thisport->timeout) {
retval = TRUE;
}
return retval;
}
/****************************************************************************
* NAME: sf_ReceiveState
* DESC: Implements the receive state handling code for escaped and unescaped data
* ARGS: thisport - which port to operate on
* c - incoming byte
* RETURN:
* CREATED:
* NOTES:
* 1. change from using pointer to functions.
****************************************************************************/
/*!
* \brief implements the receive state handling code for escaped and unescaped data
* \param thisport = which port to use
* \param c = byte to process through the receive state machine
* \return receive status
*
* \note
*
*/
static int16_t sf_ReceiveState(Port_t *thisport, uint8_t c)
{
int16_t retval = SSP_RX_RECEIVING;
switch (thisport->InputState) {
case state_unescaped_e:
if (c == SYNC) {
thisport->DecodeState = decode_len1_e;
} else if (c == ESC) {
thisport->InputState = state_escaped_e;
} else {
retval = sf_DecodeState(thisport, c);
}
break; // end of unescaped state.
case state_escaped_e:
thisport->InputState = state_unescaped_e;
if (c == SYNC) {
thisport->DecodeState = decode_len1_e;
} else if (c == ESC_SYNC) {
retval = sf_DecodeState(thisport, SYNC);
} else {
retval = sf_DecodeState(thisport, c);
}
break; // end of the escaped state.
default:
break;
}
return retval;
}
/****************************************************************************
* NAME: sf_DecodeState
* DESC: Implements the receive state finite state machine
* ARGS: thisport - which port to operate on
* c - incoming byte
* RETURN:
* CREATED:
* NOTES:
* 1. change from using pointer to functions.
****************************************************************************/
/*!
* \brief implements the receiving decoding state machine
* \param thisport = which port to use
* \param c = byte to process
* \return receive status
*
* \note
*
*/
static int16_t sf_DecodeState(Port_t *thisport, uint8_t c)
{
int16_t retval;
switch (thisport->DecodeState) {
case decode_idle_e:
// 'c' is ignored in this state as the only way to leave the idle state is
// recognition of the SYNC byte in the sf_ReceiveState function.
retval = SSP_RX_IDLE;
break;
case decode_len1_e:
thisport->rxBuf[LENGTH] = c;
thisport->rxBufLen = c;
if (thisport->rxBufLen <= thisport->rxBufSize) {
thisport->DecodeState = decode_seqNo_e;
retval = SSP_RX_RECEIVING;
} else {
thisport->DecodeState = decode_idle_e;
retval = SSP_RX_IDLE;
}
break;
case decode_seqNo_e:
thisport->rxBuf[SEQNUM] = c;
thisport->crc = 0xffff;
thisport->rxBufLen--; // subtract 1 for the seq. no.
thisport->rxBufPos = 2;
thisport->crc = sf_checksum(thisport->crc, c);
if (thisport->rxBufLen > 0) {
thisport->DecodeState = decode_data_e;
} else {
thisport->DecodeState = decode_crc1_e;
}
retval = SSP_RX_RECEIVING;
break;
case decode_data_e:
thisport->rxBuf[(thisport->rxBufPos)++] = c;
thisport->crc = sf_checksum(thisport->crc, c);
if (thisport->rxBufPos == (thisport->rxBufLen + 2)) {
thisport->DecodeState = decode_crc1_e;
}
retval = SSP_RX_RECEIVING;
break;
case decode_crc1_e:
thisport->crc = sf_checksum(thisport->crc, c);
thisport->DecodeState = decode_crc2_e;
retval = SSP_RX_RECEIVING;
break;
case decode_crc2_e:
thisport->DecodeState = decode_idle_e;
// verify the CRC value for the packet
if (sf_checksum(thisport->crc, c) == 0) {
// TODO shouldn't the return value of sf_ReceivePacket() be checked?
sf_ReceivePacket(thisport);
retval = SSP_RX_COMPLETE;
} else {
thisport->RxError++;
retval = SSP_RX_IDLE;
}
break;
default:
thisport->DecodeState = decode_idle_e; // unknown state so reset to idle state and wait for the next start of a packet.
retval = SSP_RX_IDLE;
break;
}
return retval;
}
/************************************************************************************************************
*
* NAME: int16_t sf_ReceivePacket( )
* DESCRIPTION: Receive one packet, assumed that data is in rec.buff[]
* ARGUMENTS:
* RETURN: 0 . no new packet was received, could be ack or same packet
* 1 . new packet received
* SSP_PACKET_?
* SSP_PACKET_COMPLETE
* SSP_PACKET_ACK
* CREATED: 5/8/02
*
*************************************************************************************************************/
/*!
* \brief receive one packet. calls the callback function if needed.
* \param thisport = which port to use
* \return true = valid data packet received.
* \return false = otherwise
*
* \note
*
* Created: Oct 7, 2010 12:07:22 AM by joe
*/
static int16_t sf_ReceivePacket(Port_t *thisport)
{
int16_t value = FALSE;
if (ISBITSET(thisport->rxBuf[SEQNUM], ACK_BIT)) {
// Received an ACK packet, need to check if it matches the previous sent packet
if ((thisport->rxBuf[SEQNUM] & 0x7F) == (thisport->txSeqNo & 0x7f)) {
// It matches the last packet sent by us
SETBIT(thisport->txSeqNo, ACK_BIT);
thisport->SendState = SSP_ACKED;
value = FALSE;
}
// else ignore the ACK packet
} else {
// Received a 'data' packet, figure out what type of packet we received...
if (thisport->rxBuf[SEQNUM] == 0) {
// Synchronize sequence number with host
#ifdef ACTIVE_SYNCH
thisport->sendSynch = TRUE;
#endif
sf_SendAckPacket(thisport, thisport->rxBuf[SEQNUM]);
thisport->rxSeqNo = 0;
value = FALSE;
} else if (thisport->rxBuf[SEQNUM] == thisport->rxSeqNo) {
// Already seen this packet, just ack it, don't act on the packet.
sf_SendAckPacket(thisport, thisport->rxBuf[SEQNUM]);
value = FALSE;
} else {
// New Packet
thisport->rxSeqNo = thisport->rxBuf[SEQNUM];
// Let the application do something with the data/packet.
if (thisport->pfCallBack != NULL) {
// skip the first two bytes (length and seq. no.) in the buffer.
thisport->pfCallBack(&(thisport->rxBuf[2]), thisport->rxBufLen);
}
// after we send the ACK, it is possible for the host to send a new packet.
// Thus the application needs to copy the data and reset the receive buffer
// inside of thisport->pfCallBack()
sf_SendAckPacket(thisport, thisport->rxBuf[SEQNUM]);
value = TRUE;
}
}
return value;
}

126
flight/libraries/stopwatch.c
View File

@ -1,126 +0,0 @@
/**
******************************************************************************
* @addtogroup CopterControlBL CopterControl BootLoader
* @{
*
* @file stopwatch.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Timer functions for the LED PWM.
* @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 files
/////////////////////////////////////////////////////////////////////////////
#include "stm32f10x_tim.h"
/////////////////////////////////////////////////////////////////////////////
// Local definitions
/////////////////////////////////////////////////////////////////////////////
uint32_t STOPWATCH_Init(u32 resolution, TIM_TypeDef *TIM)
{
uint32_t STOPWATCH_TIMER_RCC;
switch ((uint32_t)TIM) {
case (uint32_t)TIM1:
STOPWATCH_TIMER_RCC = RCC_APB2Periph_TIM1;
break;
case (uint32_t)TIM2:
STOPWATCH_TIMER_RCC = RCC_APB1Periph_TIM2;
break;
case (uint32_t)TIM3:
STOPWATCH_TIMER_RCC = RCC_APB1Periph_TIM3;
break;
case (uint32_t)TIM4:
STOPWATCH_TIMER_RCC = RCC_APB1Periph_TIM4;
break;
case (uint32_t)TIM5:
STOPWATCH_TIMER_RCC = RCC_APB1Periph_TIM5;
break;
case (uint32_t)TIM6:
STOPWATCH_TIMER_RCC = RCC_APB1Periph_TIM6;
break;
case (uint32_t)TIM7:
STOPWATCH_TIMER_RCC = RCC_APB1Periph_TIM7;
break;
case (uint32_t)TIM8:
STOPWATCH_TIMER_RCC = RCC_APB2Periph_TIM8;
break;
default:
/* Unsupported timer */
while (1) {
;
}
}
// enable timer clock
if (STOPWATCH_TIMER_RCC == RCC_APB2Periph_TIM1 || STOPWATCH_TIMER_RCC
== RCC_APB2Periph_TIM8) {
RCC_APB2PeriphClockCmd(STOPWATCH_TIMER_RCC, ENABLE);
} else {
RCC_APB1PeriphClockCmd(STOPWATCH_TIMER_RCC, ENABLE);
}
// time base configuration
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_TimeBaseStructure.TIM_Period = 0xffff; // max period
TIM_TimeBaseStructure.TIM_Prescaler = (72 * resolution) - 1; // <resolution> uS accuracy @ 72 MHz
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM, &TIM_TimeBaseStructure);
// enable interrupt request
TIM_ITConfig(TIM, TIM_IT_Update, ENABLE);
// start counter
TIM_Cmd(TIM, ENABLE);
return 0; // no error
}
/////////////////////////////////////////////////////////////////////////////
// ! Resets the stopwatch
// ! \return < 0 on errors
/////////////////////////////////////////////////////////////////////////////
uint32_t STOPWATCH_Reset(TIM_TypeDef *TIM)
{
// reset counter
TIM->CNT = 1; // set to 1 instead of 0 to avoid new IRQ request
TIM_ClearITPendingBit(TIM, TIM_IT_Update);
return 0; // no error
}
/////////////////////////////////////////////////////////////////////////////
// ! Returns current value of stopwatch
// ! \return 1..65535: valid stopwatch value
// ! \return 0xffffffff: counter overrun
/////////////////////////////////////////////////////////////////////////////
uint32_t STOPWATCH_ValueGet(TIM_TypeDef *TIM)
{
uint32_t value = TIM->CNT;
if (TIM_GetITStatus(TIM, TIM_IT_Update) != RESET) {
value = 0xffffffff;
}
return value;
}

77
flight/libraries/ubx_utils.c Normal file
View File

@ -0,0 +1,77 @@
/**
******************************************************************************
*
* @file ubx_utils.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief UBX Protocol utilities.
* --
* @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 <ubx_utils.h>
bool ubx_getLastSentence(uint8_t *data, uint16_t bufferCount, uint8_t * *lastSentence, uint16_t *length)
{
const uint8_t packet_overhead = UBX_HEADER_LEN + 2;
uint8_t *current = data + bufferCount - packet_overhead;
while (current >= data) {
// look for a ubx a sentence
if (current[0] == UBX_SYN1 && current[1] == UBX_SYN2) {
// check whether it fits the current buffer (whole sentence is into buffer)
uint16_t len = current[4] + (current[5] << 8);
if (len + packet_overhead + current <= data + bufferCount) {
*lastSentence = current;
*length = len + packet_overhead;
return true;
}
}
current--;
}
// no complete sentence found
return false;
}
void ubx_buildPacket(UBXPacket_t *pkt, uint8_t packetClass, uint8_t packetId, uint16_t len)
{
pkt->packet.header.syn1 = UBX_SYN1;
pkt->packet.header.syn2 = UBX_SYN2;
// don't make any assumption on alignments...
((uint8_t *)&pkt->packet.header.len)[0] = len & 0xFF;
((uint8_t *)&pkt->packet.header.len)[1] = (len >> 8) & 0xFF;
pkt->packet.header.class = packetClass;
pkt->packet.header.id = packetId;
ubx_appendChecksum(pkt);
}
void ubx_appendChecksum(UBXPacket_t *pkt)
{
uint8_t chkA = 0;
uint8_t chkB = 0;
uint16_t len = ((uint8_t *)&pkt->packet.header.len)[0] | ((uint8_t *)&pkt->packet.header.len)[1] << 8;
// From class field to the end of payload
for (uint8_t i = 2; i < len + UBX_HEADER_LEN; i++) {
chkA += pkt->binarystream[i];
chkB += chkA;
}
;
pkt->packet.payload[len] = chkA;
pkt->packet.payload[len + 1] = chkB;
}

14
flight/modules/Airspeed/airspeed.c
View File

@ -44,7 +44,7 @@
#include "baro_airspeed_ms4525do.h"
#include "baro_airspeed_etasv3.h"
#include "baro_airspeed_mpxv.h"
#include "gps_airspeed.h"
#include "imu_airspeed.h"
#include "airspeedalarm.h"
#include "taskinfo.h"
@ -99,7 +99,7 @@ int32_t AirspeedInitialize()
HwSettingsInitialize();
uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
HwSettingsOptionalModulesGet(optionalModules);
HwSettingsOptionalModulesArrayGet(optionalModules);
if (optionalModules[HWSETTINGS_OPTIONALMODULES_AIRSPEED] == HWSETTINGS_OPTIONALMODULES_ENABLED) {
@ -136,7 +136,7 @@ MODULE_INITCALL(AirspeedInitialize, AirspeedStart);
static void airspeedTask(__attribute__((unused)) void *parameters)
{
AirspeedSettingsUpdatedCb(AirspeedSettingsHandle());
bool gpsAirspeedInitialized = false;
bool imuAirspeedInitialized = false;
AirspeedSensorData airspeedData;
AirspeedSensorGet(&airspeedData);
@ -164,9 +164,9 @@ static void airspeedTask(__attribute__((unused)) void *parameters)
AirspeedSensorSet(&airspeedData);
break;
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GROUNDSPEEDBASEDWINDESTIMATION:
if (!gpsAirspeedInitialized) {
gpsAirspeedInitialized = true;
gps_airspeedInitialize();
if (!imuAirspeedInitialized) {
imuAirspeedInitialized = true;
imu_airspeedInitialize(&airspeedSettings);
}
break;
}
@ -192,7 +192,7 @@ static void airspeedTask(__attribute__((unused)) void *parameters)
break;
#endif
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GROUNDSPEEDBASEDWINDESTIMATION:
gps_airspeedGet(&airspeedData, &airspeedSettings);
imu_airspeedGet(&airspeedData, &airspeedSettings);
break;
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_NONE:
// no need to check so often until a sensor is enabled

170
flight/modules/Airspeed/gps_airspeed.c
View File

@ -1,170 +0,0 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AirspeedModule Airspeed Module
* @brief Use GPS data to estimate airspeed
* @{
*
* @file gps_airspeed.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Airspeed 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
*/
#include "openpilot.h"
#include "velocitystate.h"
#include "attitudestate.h"
#include "airspeedsensor.h"
#include "airspeedsettings.h"
#include "gps_airspeed.h"
#include "CoordinateConversions.h"
#include "airspeedalarm.h"
#include <pios_math.h>
// Private constants
#define GPS_AIRSPEED_BIAS_KP 0.1f // Needs to be settable in a UAVO
#define GPS_AIRSPEED_BIAS_KI 0.1f // Needs to be settable in a UAVO
#define SAMPLING_DELAY_MS_GPS 100 // Needs to be settable in a UAVO
#define GPS_AIRSPEED_TIME_CONSTANT_MS 500.0f // Needs to be settable in a UAVO
// Private types
struct GPSGlobals {
float RbeCol1_old[3];
float gpsVelOld_N;
float gpsVelOld_E;
float gpsVelOld_D;
float oldAirspeed;
};
// Private variables
static struct GPSGlobals *gps;
// Private functions
/*
* Initialize function loads first data sets, and allocates memory for structure.
*/
void gps_airspeedInitialize()
{
// This method saves memory in case we don't use the GPS module.
gps = (struct GPSGlobals *)pios_malloc(sizeof(struct GPSGlobals));
// GPS airspeed calculation variables
VelocityStateInitialize();
VelocityStateData gpsVelData;
VelocityStateGet(&gpsVelData);
gps->gpsVelOld_N = gpsVelData.North;
gps->gpsVelOld_E = gpsVelData.East;
gps->gpsVelOld_D = gpsVelData.Down;
gps->oldAirspeed = 0.0f;
AttitudeStateData attData;
AttitudeStateGet(&attData);
float Rbe[3][3];
float q[4] = { attData.q1, attData.q2, attData.q3, attData.q4 };
// Calculate rotation matrix
Quaternion2R(q, Rbe);
gps->RbeCol1_old[0] = Rbe[0][0];
gps->RbeCol1_old[1] = Rbe[0][1];
gps->RbeCol1_old[2] = Rbe[0][2];
}
/*
* Calculate airspeed as a function of GPS groundspeed and vehicle attitude.
* From "IMU Wind Estimation (Theory)", by William Premerlani.
* The idea is that V_gps=V_air+V_wind. If we assume wind constant, =>
* V_gps_2-V_gps_1 = (V_air_2+V_wind_2) -(V_air_1+V_wind_1) = V_air_2 - V_air_1.
* If we assume airspeed constant, => V_gps_2-V_gps_1 = |V|*(f_2 - f1),
* where "f" is the fuselage vector in earth coordinates.
* We then solve for |V| = |V_gps_2-V_gps_1|/ |f_2 - f1|.
*/
void gps_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *airspeedSettings)
{
float Rbe[3][3];
{ // Scoping to save memory. We really just need Rbe.
AttitudeStateData attData;
AttitudeStateGet(&attData);
float q[4] = { attData.q1, attData.q2, attData.q3, attData.q4 };
// Calculate rotation matrix
Quaternion2R(q, Rbe);
}
// Calculate the cos(angle) between the two fuselage basis vectors
float cosDiff = (Rbe[0][0] * gps->RbeCol1_old[0]) + (Rbe[0][1] * gps->RbeCol1_old[1]) + (Rbe[0][2] * gps->RbeCol1_old[2]);
// If there's more than a 5 degree difference between two fuselage measurements, then we have sufficient delta to continue.
if (fabsf(cosDiff) < cosf(DEG2RAD(5.0f))) {
VelocityStateData gpsVelData;
VelocityStateGet(&gpsVelData);
if (gpsVelData.North * gpsVelData.North + gpsVelData.East * gpsVelData.East + gpsVelData.Down * gpsVelData.Down < 1.0f) {
airspeedData->CalibratedAirspeed = 0;
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
return; // do not calculate if gps velocity is insufficient...
}
// Calculate the norm^2 of the difference between the two GPS vectors
float normDiffGPS2 = powf(gpsVelData.North - gps->gpsVelOld_N, 2.0f) + powf(gpsVelData.East - gps->gpsVelOld_E, 2.0f) + powf(gpsVelData.Down - gps->gpsVelOld_D, 2.0f);
// Calculate the norm^2 of the difference between the two fuselage vectors
float normDiffAttitude2 = powf(Rbe[0][0] - gps->RbeCol1_old[0], 2.0f) + powf(Rbe[0][1] - gps->RbeCol1_old[1], 2.0f) + powf(Rbe[0][2] - gps->RbeCol1_old[2], 2.0f);
// Airspeed magnitude is the ratio between the two difference norms
float airspeed = sqrtf(normDiffGPS2 / normDiffAttitude2);
if (!IS_REAL(airspeedData->CalibratedAirspeed)) {
airspeedData->CalibratedAirspeed = 0;
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
} else {
// need a low pass filter to filter out spikes in non coordinated maneuvers
airspeedData->CalibratedAirspeed = (1.0f - airspeedSettings->GroundSpeedBasedEstimationLowPassAlpha) * gps->oldAirspeed + airspeedSettings->GroundSpeedBasedEstimationLowPassAlpha * airspeed;
gps->oldAirspeed = airspeedData->CalibratedAirspeed;
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
AirspeedAlarm(SYSTEMALARMS_ALARM_OK);
}
// Save old variables for next pass
gps->gpsVelOld_N = gpsVelData.North;
gps->gpsVelOld_E = gpsVelData.East;
gps->gpsVelOld_D = gpsVelData.Down;
gps->RbeCol1_old[0] = Rbe[0][0];
gps->RbeCol1_old[1] = Rbe[0][1];
gps->RbeCol1_old[2] = Rbe[0][2];
}
}
/**
* @}
* @}
*/

306
flight/modules/Airspeed/imu_airspeed.c Normal file
View File

@ -0,0 +1,306 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AirspeedModule Airspeed Module
* @brief Use attitude and velocity data to estimate airspeed
* @{
*
* @file imu_airspeed.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief IMU based airspeed calculation
*
* @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 "openpilot.h"
#include "velocitystate.h"
#include "attitudestate.h"
#include "airspeedsensor.h"
#include "airspeedsettings.h"
#include "imu_airspeed.h"
#include "CoordinateConversions.h"
#include "butterworth.h"
#include <pios_math.h>
// Private constants
#define EPS 1e-6f
#define EPS_REORIENTATION 1e-10f
#define EPS_VELOCITY 1.f
// Private types
// structure with smoothed fuselage orientation, ground speed, wind vector and their changes in time
struct IMUGlobals {
// Butterworth filters
struct ButterWorthDF2Filter filter;
struct ButterWorthDF2Filter prefilter;
float ff, ffV;
// storage variables for Butterworth filter
float pn1, pn2;
float yn1, yn2;
float v1n1, v1n2;
float v2n1, v2n2;
float v3n1, v3n2;
float Vw1n1, Vw1n2;
float Vw2n1, Vw2n2;
float Vw3n1, Vw3n2;
float Vw1, Vw2, Vw3;
// storage variables for derivative calculation
float pOld, yOld;
float v1Old, v2Old, v3Old;
};
// Private variables
static struct IMUGlobals *imu;
// Private functions
// a simple square inline function based on multiplication faster than powf(x,2.0f)
static inline float Sq(float x)
{
return x * x;
}
// ****** find pitch, yaw from quaternion ********
static void Quaternion2PY(const float q0, const float q1, const float q2, const float q3, float *pPtr, float *yPtr, bool principalArg)
{
float R13, R11, R12;
const float q0s = q0 * q0;
const float q1s = q1 * q1;
const float q2s = q2 * q2;
const float q3s = q3 * q3;
R13 = 2.0f * (q1 * q3 - q0 * q2);
R11 = q0s + q1s - q2s - q3s;
R12 = 2.0f * (q1 * q2 + q0 * q3);
*pPtr = asinf(-R13); // pitch always between -pi/2 to pi/2
const float y_ = atan2f(R12, R11);
// use old yaw contained in y to add multiples of 2pi to have a continuous yaw if user does not want the principal argument
// else simply copy atan2 result into result
if (principalArg) {
*yPtr = y_;
} else {
// calculate needed mutliples of 2pi to avoid jumps
// number of cycles accumulated in old yaw
const int32_t cycles = (int32_t)(*yPtr / M_2PI_F);
// look for a jump by substracting the modulus, i.e. there is maximally one jump.
// take slightly less than 2pi, because the jump will always be lower than 2pi
const int32_t mod = (int32_t)((y_ - (*yPtr - cycles * M_2PI_F)) / (M_2PI_F * 0.8f));
*yPtr = y_ + M_2PI_F * (cycles - mod);
}
}
static void PY2xB(const float p, const float y, float x[3])
{
const float cosp = cosf(p);
x[0] = cosp * cosf(y);
x[1] = cosp * sinf(y);
x[2] = -sinf(p);
}
static void PY2DeltaxB(const float p, const float y, const float xB[3], float x[3])
{
const float cosp = cosf(p);
x[0] = xB[0] - cosp * cosf(y);
x[1] = xB[1] - cosp * sinf(y);
x[2] = xB[2] - -sinf(p);
}
/*
* Initialize function loads first data sets, and allocates memory for structure.
*/
void imu_airspeedInitialize(const AirspeedSettingsData *airspeedSettings)
{
// pre-filter frequency rate
const float ff = (float)(airspeedSettings->SamplePeriod) / 1000.0f / airspeedSettings->IMUBasedEstimationLowPassPeriod1;
// filter frequency rate
const float ffV = (float)(airspeedSettings->SamplePeriod) / 1000.0f / airspeedSettings->IMUBasedEstimationLowPassPeriod2;
// This method saves memory in case we don't use the module.
imu = (struct IMUGlobals *)pios_malloc(sizeof(struct IMUGlobals));
// airspeed calculation variables
VelocityStateInitialize();
VelocityStateData velData;
VelocityStateGet(&velData);
AttitudeStateData attData;
AttitudeStateGet(&attData);
// initialize filters for given ff and ffV
InitButterWorthDF2Filter(ffV, &(imu->filter));
InitButterWorthDF2Filter(ff, &(imu->prefilter));
imu->ffV = ffV;
imu->ff = ff;
// get pitch and yaw from quarternion; principal argument for yaw
Quaternion2PY(attData.q1, attData.q2, attData.q3, attData.q4, &(imu->pOld), &(imu->yOld), true);
InitButterWorthDF2Values(imu->pOld, &(imu->prefilter), &(imu->pn1), &(imu->pn2));
InitButterWorthDF2Values(imu->yOld, &(imu->prefilter), &(imu->yn1), &(imu->yn2));
// use current NED speed as vOld vector and as initial value for filter
imu->v1Old = velData.North;
imu->v2Old = velData.East;
imu->v3Old = velData.Down;
InitButterWorthDF2Values(imu->v1Old, &(imu->prefilter), &(imu->v1n1), &(imu->v1n2));
InitButterWorthDF2Values(imu->v2Old, &(imu->prefilter), &(imu->v2n1), &(imu->v2n2));
InitButterWorthDF2Values(imu->v3Old, &(imu->prefilter), &(imu->v3n1), &(imu->v3n2));
// initial guess for windspeed is zero
imu->Vw3 = imu->Vw2 = imu->Vw1 = 0.0f;
InitButterWorthDF2Values(0.0f, &(imu->filter), &(imu->Vw1n1), &(imu->Vw1n2));
imu->Vw3n1 = imu->Vw2n1 = imu->Vw1n1;
imu->Vw3n2 = imu->Vw2n2 = imu->Vw1n2;
}
/*
* Calculate airspeed as a function of groundspeed and vehicle attitude.
* Adapted from "IMU Wind Estimation (Theory)", by William Premerlani.
* The idea is that V_gps=V_air+V_wind. If we assume wind constant, =>
* V_gps_2-V_gps_1 = (V_air_2+V_wind_2) -(V_air_1+V_wind_1) = V_air_2 - V_air_1.
* If we assume airspeed constant, => V_gps_2-V_gps_1 = |V|*(f_2 - f1),
* where "f" is the fuselage vector in earth coordinates.
* We then solve for |V| = |V_gps_2-V_gps_1|/ |f_2 - f1|.
* Adapted to: |V| = (V_gps_2-V_gps_1) dot (f2_-f_1) / |f_2 - f1|^2.
*
* See OP-1317 imu_wind_estimation.pdf for details on the adaptation
* Need a low pass filter to filter out spikes in non coordinated maneuvers
* A two step Butterworth second order filter is used. In the first step fuselage vector xB
* and ground speed vector Vel are filtered. The fuselage vector is filtered through its pitch
* and yaw to keep a unit length. After building the differenced dxB and dVel are produced and
* the airspeed calculated. The calculated airspeed is filtered again with a Butterworth filter
*/
void imu_airspeedGet(AirspeedSensorData *airspeedData, const AirspeedSettingsData *airspeedSettings)
{
// pre-filter frequency rate
const float ff = (float)(airspeedSettings->SamplePeriod) / 1000.0f / airspeedSettings->IMUBasedEstimationLowPassPeriod1;
// filter frequency rate
const float ffV = (float)(airspeedSettings->SamplePeriod) / 1000.0f / airspeedSettings->IMUBasedEstimationLowPassPeriod2;
// check for a change in filter frequency rate. if yes, then actualize filter constants and intermediate values
if (fabsf(ffV - imu->ffV) > EPS) {
InitButterWorthDF2Filter(ffV, &(imu->filter));
InitButterWorthDF2Values(imu->Vw1, &(imu->filter), &(imu->Vw1n1), &(imu->Vw1n2));
InitButterWorthDF2Values(imu->Vw2, &(imu->filter), &(imu->Vw2n1), &(imu->Vw2n2));
InitButterWorthDF2Values(imu->Vw3, &(imu->filter), &(imu->Vw3n1), &(imu->Vw3n2));
}
if (fabsf(ff - imu->ff) > EPS) {
InitButterWorthDF2Filter(ff, &(imu->prefilter));
InitButterWorthDF2Values(imu->pOld, &(imu->prefilter), &(imu->pn1), &(imu->pn2));
InitButterWorthDF2Values(imu->yOld, &(imu->prefilter), &(imu->yn1), &(imu->yn2));
InitButterWorthDF2Values(imu->v1Old, &(imu->prefilter), &(imu->v1n1), &(imu->v1n2));
InitButterWorthDF2Values(imu->v2Old, &(imu->prefilter), &(imu->v2n1), &(imu->v2n2));
InitButterWorthDF2Values(imu->v3Old, &(imu->prefilter), &(imu->v3n1), &(imu->v3n2));
}
float normVel2;
float normDiffAttitude2;
float dvdtDotdfdt;
float xB[3];
// get values and conduct smoothing of ground speed and orientation independently of the calculation of airspeed
{ // Scoping to save memory
AttitudeStateData attData;
AttitudeStateGet(&attData);
VelocityStateData velData;
VelocityStateGet(&velData);
float p = imu->pOld, y = imu->yOld;
float dxB[3];
// get pitch and roll Euler angles from quaternion
// do not calculate the principlal argument of yaw, i.e. use old yaw to add multiples of 2pi to have a continuous yaw
Quaternion2PY(attData.q1, attData.q2, attData.q3, attData.q4, &p, &y, false);
// filter pitch and roll Euler angles instead of fuselage vector to guarantee a unit length at all times
p = FilterButterWorthDF2(p, &(imu->prefilter), &(imu->pn1), &(imu->pn2));
y = FilterButterWorthDF2(y, &(imu->prefilter), &(imu->yn1), &(imu->yn2));
// transform pitch and yaw into fuselage vector xB and xBold
PY2xB(p, y, xB);
// calculate change in fuselage vector by substraction of old value
PY2DeltaxB(imu->pOld, imu->yOld, xB, dxB);
// filter ground speed from VelocityState
const float fv1n = FilterButterWorthDF2(velData.North, &(imu->prefilter), &(imu->v1n1), &(imu->v1n2));
const float fv2n = FilterButterWorthDF2(velData.East, &(imu->prefilter), &(imu->v2n1), &(imu->v2n2));
const float fv3n = FilterButterWorthDF2(velData.Down, &(imu->prefilter), &(imu->v3n1), &(imu->v3n2));
// calculate norm of ground speed
normVel2 = Sq(fv1n) + Sq(fv2n) + Sq(fv3n);
// calculate norm of orientation change
normDiffAttitude2 = Sq(dxB[0]) + Sq(dxB[1]) + Sq(dxB[2]);
// cauclate scalar product between groundspeed change and orientation change
dvdtDotdfdt = (fv1n - imu->v1Old) * dxB[0] + (fv2n - imu->v2Old) * dxB[1] + (fv3n - imu->v3Old) * dxB[2];
// actualise old values
imu->pOld = p;
imu->yOld = y;
imu->v1Old = fv1n;
imu->v2Old = fv2n;
imu->v3Old = fv3n;
}
// Some reorientation needed to be able to calculate airspeed, calculate only for sufficient velocity
// a negative scalar product is a clear sign that we are not really able to calculate the airspeed
// NOTE: normVel2 check against EPS_VELOCITY might make problems during hovering maneuvers in fixed wings
if (normDiffAttitude2 > EPS_REORIENTATION && normVel2 > EPS_VELOCITY && dvdtDotdfdt > 0.f) {
// Airspeed modulus: |v| = dv/dt * dxB/dt / |dxB/dt|^2
// airspeed is always REAL because normDiffAttitude2 > EPS_REORIENTATION > 0 and REAL dvdtDotdfdt
const float airspeed = dvdtDotdfdt / normDiffAttitude2;
// groundspeed = airspeed + wind ---> wind = groundspeed - airspeed
const float wind[3] = { imu->v1Old - xB[0] * airspeed,
imu->v2Old - xB[1] * airspeed,
imu->v3Old - xB[2] * airspeed };
// filter raw wind
imu->Vw1 = FilterButterWorthDF2(wind[0], &(imu->filter), &(imu->Vw1n1), &(imu->Vw1n2));
imu->Vw2 = FilterButterWorthDF2(wind[1], &(imu->filter), &(imu->Vw2n1), &(imu->Vw2n2));
imu->Vw3 = FilterButterWorthDF2(wind[2], &(imu->filter), &(imu->Vw3n1), &(imu->Vw3n2));
} // else leave wind estimation unchanged
{ // Scoping to save memory
// airspeed = groundspeed - wind
const float Vair[3] = {
imu->v1Old - imu->Vw1,
imu->v2Old - imu->Vw2,
imu->v3Old - imu->Vw3
};
// project airspeed into fuselage vector
airspeedData->CalibratedAirspeed = Vair[0] * xB[0] + Vair[1] * xB[1] + Vair[2] * xB[2];
}
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
AlarmsClear(SYSTEMALARMS_ALARM_AIRSPEED);
}
/**
* @}
* @}
*/

14
flight/modules/Airspeed/inc/gps_airspeed.h → flight/modules/Airspeed/inc/imu_airspeed.h
View File

@ -3,10 +3,10 @@
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AirspeedModule Airspeed Module
* @brief Calculate airspeed as a function of the difference between sequential GPS velocity and attitude measurements
* @brief Calculate airspeed as a function of the difference between sequential ground velocity and attitude measurements
* @{
*
* @file gps_airspeed.h
* @file imu_airspeed.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Airspeed module, reads temperature and pressure from BMP085
*
@ -28,13 +28,13 @@
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef GPS_AIRSPEED_H
#define GPS_AIRSPEED_H
#ifndef IMU_AIRSPEED_H
#define IMU_AIRSPEED_H
void gps_airspeedInitialize();
void gps_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *airspeedSettings);
void imu_airspeedInitialize(const AirspeedSettingsData *airspeedSettings);
void imu_airspeedGet(AirspeedSensorData *airspeedData, const AirspeedSettingsData *airspeedSettings);
#endif // GPS_AIRSPEED_H
#endif // IMU_AIRSPEED_H
/**
* @}

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

@ -41,6 +41,7 @@
#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
@ -50,6 +51,14 @@
#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
@ -57,6 +66,11 @@ 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);
@ -166,13 +180,22 @@ static void altitudeTask(__attribute__((unused)) void *parameters)
temp = PIOS_MS5611_GetTemperature();
press = PIOS_MS5611_GetPressure();
if (tempCorrectionEnabled) {
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 = temp > baroCorrectionExtent.max ? baroCorrectionExtent.max :
(temp < baroCorrectionExtent.min ? baroCorrectionExtent.min : temp);
press -= baroCorrection.a + ((baroCorrection.d * ctemp + baroCorrection.c) * ctemp + baroCorrection.b) * ctemp;
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)) {

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

@ -64,8 +64,8 @@
#include "CoordinateConversions.h"
#include <pios_notify.h>
#include <mathmisc.h>
#include <pios_constants.h>
#include <pios_instrumentation_helper.h>
PERF_DEFINE_COUNTER(counterUpd);
@ -82,10 +82,19 @@ PERF_DEFINE_COUNTER(counterAtt);
#define STACK_SIZE_BYTES 540
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
#define SENSOR_PERIOD 4
// Attitude module loop interval (defined by sensor rate in pios_config.h)
static const uint32_t sensor_period_ms = ((uint32_t)1000.0f / PIOS_SENSOR_RATE);
#define UPDATE_RATE 25.0f
#define UPDATE_EXPECTED (1.0f / 500.0f)
// Interval in number of sample to recalculate temp bias
#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 UPDATE_EXPECTED (1.0f / PIOS_SENSOR_RATE)
#define UPDATE_MIN 1.0e-6f
#define UPDATE_MAX 1.0f
#define UPDATE_ALPHA 1.0e-2f
@ -130,6 +139,10 @@ static bool apply_accel_temp = false;
static AccelGyroSettingsgyro_temp_coeffData gyro_temp_coeff;;
static AccelGyroSettingsaccel_temp_coeffData accel_temp_coeff;
static AccelGyroSettingstemp_calibrated_extentData temp_calibrated_extent;
static float temperature = NAN;
static float accel_temp_bias[3] = { 0 };
static float gyro_temp_bias[3] = { 0 };
static uint8_t temp_calibration_count = 0;
// Accel and Gyro scaling (this is the product of sensor scale and adjustement in AccelGyroSettings
static AccelGyroSettingsgyro_scaleData gyro_scale;
@ -142,8 +155,7 @@ static volatile int32_t trim_accels[3];
static volatile int32_t trim_samples;
int32_t const MAX_TRIM_FLIGHT_SAMPLES = 65535;
#define GRAV 9.81f
#define STD_CC_ACCEL_SCALE (GRAV * 0.004f)
#define STD_CC_ACCEL_SCALE (PIOS_CONST_MKS_GRAV_ACCEL_F * 0.004f)
/* 0.004f is gravity / LSB */
#define STD_CC_ANALOG_GYRO_NEUTRAL 1665
#define STD_CC_ANALOG_GYRO_GAIN 0.42f
@ -222,12 +234,6 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
// Set critical error and wait until the accel is producing data
while (PIOS_ADXL345_FifoElements() == 0) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
}
bool cc3d = BOARDISCC3D;
if (cc3d) {
@ -236,6 +242,11 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
#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);
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
}
accel_test = PIOS_ADXL345_Test();
#endif
@ -257,7 +268,7 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
settingsUpdatedCb(AttitudeSettingsHandle());
PIOS_DELTATIME_Init(&dtconfig, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
portTickType lastSysTime = xTaskGetTickCount();
// Main task loop
while (1) {
FlightStatusData flightStatus;
@ -317,6 +328,7 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
PERF_MEASURE_PERIOD(counterPeriod);
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
}
vTaskDelayUntil(&lastSysTime, sensor_period_ms / portTICK_PERIOD_MS);
}
}
@ -446,43 +458,82 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
static struct pios_mpu6000_data mpu6000_data;
static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *gyrosData)
{
float accels[3], gyros[3];
float accels[3] = { 0 };
float gyros[3] = { 0 };
float temp = 0;
uint8_t count = 0;
#if defined(PIOS_INCLUDE_MPU6000)
xQueueHandle queue = PIOS_MPU6000_GetQueue();
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;
if (xQueueReceive(queue, (void *)&mpu6000_data, SENSOR_PERIOD) == errQUEUE_EMPTY) {
accels[0] += mpu6000_data.accel_x;
accels[1] += mpu6000_data.accel_y;
accels[2] += mpu6000_data.accel_z;
temp += mpu6000_data.temperature;
count++;
// check if further samples are already in queue
ret = xQueueReceive(queue, (void *)&mpu6000_data, 0);
}
PERF_TRACK_VALUE(counterAccelSamples, count);
if (!count) {
return -1; // Error, no data
}
// Do not read raw sensor data in simulation mode
if (GyroStateReadOnly() || AccelStateReadOnly()) {
return 0;
}
float invcount = 1.0f / count;
PERF_TIMED_SECTION_START(counterUpd);
gyros[0] = mpu6000_data.gyro_x * gyro_scale.X;
gyros[1] = mpu6000_data.gyro_y * gyro_scale.Y;
gyros[2] = mpu6000_data.gyro_z * gyro_scale.Z;
gyros[0] *= gyro_scale.X * invcount;
gyros[1] *= gyro_scale.Y * invcount;
gyros[2] *= gyro_scale.Z * invcount;
accels[0] = mpu6000_data.accel_x * accel_scale.X;
accels[1] = mpu6000_data.accel_y * accel_scale.Y;
accels[2] = mpu6000_data.accel_z * accel_scale.Z;
float ctemp = mpu6000_data.temperature > temp_calibrated_extent.max ? temp_calibrated_extent.max :
(mpu6000_data.temperature < temp_calibrated_extent.min ? temp_calibrated_extent.min
: mpu6000_data.temperature);
accels[0] *= accel_scale.X * invcount;
accels[1] *= accel_scale.Y * invcount;
accels[2] *= accel_scale.Z * invcount;
temp *= invcount;
if (isnan(temperature)) {
temperature = temp;
}
temperature = temp_alpha * (temp - temperature) + temperature;
if ((apply_gyro_temp || apply_accel_temp) && !temp_calibration_count) {
temp_calibration_count = TEMP_CALIB_INTERVAL;
float ctemp = boundf(temperature, temp_calibrated_extent.max, temp_calibrated_extent.min);
if (apply_gyro_temp) {
gyros[0] -= (gyro_temp_coeff.X + gyro_temp_coeff.X2 * ctemp) * ctemp;
gyros[1] -= (gyro_temp_coeff.Y + gyro_temp_coeff.Y2 * ctemp) * ctemp;
gyros[2] -= (gyro_temp_coeff.Z + gyro_temp_coeff.Z2 * ctemp) * ctemp;
gyro_temp_bias[0] = (gyro_temp_coeff.X + gyro_temp_coeff.X2 * ctemp) * ctemp;
gyro_temp_bias[1] = (gyro_temp_coeff.Y + gyro_temp_coeff.Y2 * ctemp) * ctemp;
gyro_temp_bias[2] = (gyro_temp_coeff.Z + gyro_temp_coeff.Z2 * ctemp) * ctemp;
}
if (apply_accel_temp) {
accels[0] -= accel_temp_coeff.X * ctemp;
accels[1] -= accel_temp_coeff.Y * ctemp;
accels[2] -= accel_temp_coeff.Z * ctemp;
accel_temp_bias[0] = accel_temp_coeff.X * ctemp;
accel_temp_bias[1] = accel_temp_coeff.Y * ctemp;
accel_temp_bias[2] = accel_temp_coeff.Z * ctemp;
}
}
temp_calibration_count--;
if (apply_gyro_temp) {
gyros[0] -= gyro_temp_bias[0];
gyros[1] -= gyro_temp_bias[1];
gyros[2] -= gyro_temp_bias[2];
}
if (apply_accel_temp) {
accels[0] -= accel_temp_bias[0];
accels[1] -= accel_temp_bias[1];
accels[2] -= accel_temp_bias[2];
}
// gyrosData->temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f;
// accelsData->temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f;
@ -567,24 +618,24 @@ __attribute__((optimize("O3"))) static void updateAttitude(AccelStateData *accel
CrossProduct((const float *)accels_filtered, (const float *)grot_filtered, accel_err);
// Account for accel magnitude
float accel_mag = sqrtf(accels_filtered[0] * accels_filtered[0] + accels_filtered[1] * accels_filtered[1] + accels_filtered[2] * accels_filtered[2]);
if (accel_mag < 1.0e-3f) {
float inv_accel_mag = fast_invsqrtf(accels_filtered[0] * accels_filtered[0] + accels_filtered[1] * accels_filtered[1] + accels_filtered[2] * accels_filtered[2]);
if (inv_accel_mag > 1e3f) {
return;
}
// Account for filtered gravity vector magnitude
float grot_mag;
float inv_grot_mag;
if (accel_filter_enabled) {
grot_mag = sqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]);
inv_grot_mag = fast_invsqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]);
} else {
grot_mag = 1.0f;
inv_grot_mag = 1.0f;
}
if (grot_mag < 1.0e-3f) {
if (inv_grot_mag > 1e3f) {
return;
}
const float invMag = 1.0f / (accel_mag * grot_mag);
const float invMag = (inv_accel_mag * inv_grot_mag);
accel_err[0] *= invMag;
accel_err[1] *= invMag;
accel_err[2] *= invMag;
@ -625,21 +676,20 @@ __attribute__((optimize("O3"))) static void updateAttitude(AccelStateData *accel
}
// Renomalize
float qmag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
float inv_qmag = fast_invsqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
// If quaternion has become inappropriately short or is nan reinit.
// THIS SHOULD NEVER ACTUALLY HAPPEN
if ((fabsf(qmag) < 1e-3f) || isnan(qmag)) {
if ((fabsf(inv_qmag) > 1e3f) || isnan(inv_qmag)) {
q[0] = 1;
q[1] = 0;
q[2] = 0;
q[3] = 0;
} else {
const float invQmag = 1.0f / qmag;
q[0] = q[0] * invQmag;
q[1] = q[1] * invQmag;
q[2] = q[2] * invQmag;
q[3] = q[3] * invQmag;
q[0] = q[0] * inv_qmag;
q[1] = q[1] * inv_qmag;
q[2] = q[2] * inv_qmag;
q[3] = q[3] * inv_qmag;
}
AttitudeStateData attitudeState;
@ -757,7 +807,7 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
accelGyroSettings.accel_scale.X = trim_accels[0] / trim_samples;
accelGyroSettings.accel_scale.Y = trim_accels[1] / trim_samples;
// Z should average -grav
accelGyroSettings.accel_scale.Z = trim_accels[2] / trim_samples + GRAV;
accelGyroSettings.accel_scale.Z = trim_accels[2] / trim_samples + PIOS_CONST_MKS_GRAV_ACCEL_F;
attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL;
AttitudeSettingsSet(&attitudeSettings);
} else {

1348
flight/modules/Attitude/revolution/attitude.c
View File

@ -1,1348 +0,0 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup Attitude Copter Control Attitude Estimation
* @brief Acquires sensor data and computes attitude estimate
* Specifically updates the the @ref AttitudeState "AttitudeState" and @ref AttitudeRaw "AttitudeRaw" settings objects
* @{
*
* @file attitude.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.
*
* @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
*/
/**
* Input objects: None, takes sensor data via pios
* Output objects: @ref AttitudeRaw @ref AttitudeState
*
* This module computes an attitude estimate from the sensor data
*
* The module executes in its own thread.
*
* UAVObjects are automatically generated by the UAVObjectGenerator from
* the object definition XML file.
*
* Modules have no API, all communication to other modules is done through UAVObjects.
* However modules may use the API exposed by shared libraries.
* See the OpenPilot wiki for more details.
* http://www.openpilot.org/OpenPilot_Application_Architecture
*
*/
#include <openpilot.h>
#include <pios_struct_helper.h>
#include "attitude.h"
#include "accelsensor.h"
#include "accelstate.h"
#include "airspeedsensor.h"
#include "airspeedstate.h"
#include "attitudestate.h"
#include "attitudesettings.h"
#include "barosensor.h"
#include "flightstatus.h"
#include "gpspositionsensor.h"
#include "gpsvelocitysensor.h"
#include "gyrostate.h"
#include "gyrosensor.h"
#include "homelocation.h"
#include "magsensor.h"
#include "magstate.h"
#include "positionstate.h"
#include "ekfconfiguration.h"
#include "ekfstatevariance.h"
#include "revocalibration.h"
#include "revosettings.h"
#include "velocitystate.h"
#include "taskinfo.h"
#include "CoordinateConversions.h"
// Private constants
#define STACK_SIZE_BYTES 2048
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
#define FAILSAFE_TIMEOUT_MS 10
#define CALIBRATION_DELAY 4000
#define CALIBRATION_DURATION 6000
// low pass filter configuration to calculate offset
// of barometric altitude sensor
// reasoning: updates at: 10 Hz, tau= 300 s settle time
// exp(-(1/f) / tau ) ~=~ 0.9997
#define BARO_OFFSET_LOWPASS_ALPHA 0.9997f
// simple IAS to TAS aproximation - 2% increase per 1000ft
// since we do not have flowing air temperature information
#define IAS2TAS(alt) (1.0f + (0.02f * (alt) / 304.8f))
// Private types
// Private variables
static xTaskHandle attitudeTaskHandle;
static xQueueHandle gyroQueue;
static xQueueHandle accelQueue;
static xQueueHandle magQueue;
static xQueueHandle airspeedQueue;
static xQueueHandle baroQueue;
static xQueueHandle gpsQueue;
static xQueueHandle gpsVelQueue;
static AttitudeSettingsData attitudeSettings;
static HomeLocationData homeLocation;
static RevoCalibrationData revoCalibration;
static EKFConfigurationData ekfConfiguration;
static RevoSettingsData revoSettings;
static FlightStatusData flightStatus;
const uint32_t SENSOR_QUEUE_SIZE = 10;
static bool volatile variance_error = true;
static bool volatile initialization_required = true;
static uint32_t volatile running_algorithm = 0xffffffff; // we start with no algorithm running
static float rollPitchBiasRate = 0;
// Accel filtering
static float accel_alpha = 0;
static bool accel_filter_enabled = false;
static float accels_filtered[3];
static float grot_filtered[3];
// Private functions
static void AttitudeTask(void *parameters);
static int32_t updateAttitudeComplementary(bool first_run);
static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode);
static void settingsUpdatedCb(UAVObjEvent *objEv);
static int32_t getNED(GPSPositionSensorData *gpsPosition, float *NED);
static void magOffsetEstimation(MagSensorData *mag);
// check for invalid values
static inline bool invalid(float data)
{
if (isnan(data) || isinf(data)) {
return true;
}
return false;
}
// check for invalid variance values
static inline bool invalid_var(float data)
{
if (invalid(data)) {
return true;
}
if (data < 1e-15f) { // var should not be close to zero. And not negative either.
return true;
}
return false;
}
/**
* 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 AttitudeInitialize(void)
{
GyroSensorInitialize();
GyroStateInitialize();
AccelSensorInitialize();
AccelStateInitialize();
MagSensorInitialize();
MagStateInitialize();
AirspeedSensorInitialize();
AirspeedStateInitialize();
BaroSensorInitialize();
GPSPositionSensorInitialize();
GPSVelocitySensorInitialize();
AttitudeSettingsInitialize();
AttitudeStateInitialize();
PositionStateInitialize();
VelocityStateInitialize();
RevoSettingsInitialize();
RevoCalibrationInitialize();
EKFConfigurationInitialize();
EKFStateVarianceInitialize();
FlightStatusInitialize();
// Initialize this here while we aren't setting the homelocation in GPS
HomeLocationInitialize();
// Initialize quaternion
AttitudeStateData attitude;
AttitudeStateGet(&attitude);
attitude.q1 = 1.0f;
attitude.q2 = 0.0f;
attitude.q3 = 0.0f;
attitude.q4 = 0.0f;
AttitudeStateSet(&attitude);
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
RevoSettingsConnectCallback(&settingsUpdatedCb);
RevoCalibrationConnectCallback(&settingsUpdatedCb);
HomeLocationConnectCallback(&settingsUpdatedCb);
EKFConfigurationConnectCallback(&settingsUpdatedCb);
FlightStatusConnectCallback(&settingsUpdatedCb);
return 0;
}
/**
* Start the task. Expects all objects to be initialized by this point.
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AttitudeStart(void)
{
// Create the queues for the sensors
gyroQueue = xQueueCreate(1, sizeof(UAVObjEvent));
accelQueue = xQueueCreate(1, sizeof(UAVObjEvent));
magQueue = xQueueCreate(1, sizeof(UAVObjEvent));
airspeedQueue = xQueueCreate(1, sizeof(UAVObjEvent));
baroQueue = xQueueCreate(1, sizeof(UAVObjEvent));
gpsQueue = xQueueCreate(1, sizeof(UAVObjEvent));
gpsVelQueue = xQueueCreate(1, sizeof(UAVObjEvent));
// Start main task
xTaskCreate(AttitudeTask, "Attitude", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &attitudeTaskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ATTITUDE, attitudeTaskHandle);
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE);
#endif
GyroSensorConnectQueue(gyroQueue);
AccelSensorConnectQueue(accelQueue);
MagSensorConnectQueue(magQueue);
AirspeedSensorConnectQueue(airspeedQueue);
BaroSensorConnectQueue(baroQueue);
GPSPositionSensorConnectQueue(gpsQueue);
GPSVelocitySensorConnectQueue(gpsVelQueue);
return 0;
}
MODULE_INITCALL(AttitudeInitialize, AttitudeStart);
/**
* Module thread, should not return.
*/
static void AttitudeTask(__attribute__((unused)) void *parameters)
{
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
// Force settings update to make sure rotation loaded
settingsUpdatedCb(NULL);
// Wait for all the sensors be to read
vTaskDelay(100);
// Main task loop - TODO: make it run as delayed callback
while (1) {
int32_t ret_val = -1;
bool first_run = false;
if (initialization_required) {
initialization_required = false;
first_run = true;
}
// This function blocks on data queue
switch (running_algorithm) {
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY:
ret_val = updateAttitudeComplementary(first_run);
break;
case REVOSETTINGS_FUSIONALGORITHM_INS13GPSOUTDOOR:
ret_val = updateAttitudeINSGPS(first_run, true);
break;
case REVOSETTINGS_FUSIONALGORITHM_INS13INDOOR:
ret_val = updateAttitudeINSGPS(first_run, false);
break;
default:
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
break;
}
if (ret_val != 0) {
initialization_required = true;
}
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
#endif
}
}
static inline void apply_accel_filter(const float *raw, float *filtered)
{
if (accel_filter_enabled) {
filtered[0] = filtered[0] * accel_alpha + raw[0] * (1 - accel_alpha);
filtered[1] = filtered[1] * accel_alpha + raw[1] * (1 - accel_alpha);
filtered[2] = filtered[2] * accel_alpha + raw[2] * (1 - accel_alpha);
} else {
filtered[0] = raw[0];
filtered[1] = raw[1];
filtered[2] = raw[2];
}
}
float accel_mag;
float qmag;
float attitudeDt;
float mag_err[3];
static int32_t updateAttitudeComplementary(bool first_run)
{
UAVObjEvent ev;
GyroSensorData gyroSensorData;
GyroStateData gyroStateData;
AccelSensorData accelSensorData;
static int32_t timeval;
float dT;
static uint8_t init = 0;
static float gyro_bias[3] = { 0, 0, 0 };
static bool magCalibrated = true;
static uint32_t initStartupTime = 0;
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
if (xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE ||
xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE) {
// When one of these is updated so should the other
// Do not set attitude timeout warnings in simulation mode
if (!AttitudeStateReadOnly()) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING);
return -1;
}
}
AccelSensorGet(&accelSensorData);
// TODO: put in separate filter
AccelStateData accelState;
accelState.x = accelSensorData.x;
accelState.y = accelSensorData.y;
accelState.z = accelSensorData.z;
AccelStateSet(&accelState);
// During initialization and
if (first_run) {
#if defined(PIOS_INCLUDE_HMC5883)
// To initialize we need a valid mag reading
if (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) != pdTRUE) {
return -1;
}
MagSensorData magData;
MagSensorGet(&magData);
#else
MagSensorData magData;
magData.x = 100.0f;
magData.y = 0.0f;
magData.z = 0.0f;
#endif
float magBias[3];
RevoCalibrationmag_biasArrayGet(magBias);
// don't trust Mag for initial orientation if it has not been calibrated
if (magBias[0] < 1e-6f && magBias[1] < 1e-6f && magBias[2] < 1e-6f) {
magCalibrated = false;
magData.x = 100.0f;
magData.y = 0.0f;
magData.z = 0.0f;
}
AttitudeStateData attitudeState;
AttitudeStateGet(&attitudeState);
init = 0;
// Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly,
// so pseudo "north" vector can be estimated even if the board is not level
attitudeState.Roll = atan2f(-accelSensorData.y, -accelSensorData.z);
float zn = cosf(attitudeState.Roll) * magData.z + sinf(attitudeState.Roll) * magData.y;
float yn = cosf(attitudeState.Roll) * magData.y - sinf(attitudeState.Roll) * magData.z;
// rotate accels z vector according to roll
float azn = cosf(attitudeState.Roll) * accelSensorData.z + sinf(attitudeState.Roll) * accelSensorData.y;
attitudeState.Pitch = atan2f(accelSensorData.x, -azn);
float xn = cosf(attitudeState.Pitch) * magData.x + sinf(attitudeState.Pitch) * zn;
attitudeState.Yaw = atan2f(-yn, xn);
// TODO: This is still a hack
// Put this in a proper generic function in CoordinateConversion.c
// should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags)
// should calculate the rotation in 3d space using proper cross product math
// SUBTODO: formulate the math required
attitudeState.Roll = RAD2DEG(attitudeState.Roll);
attitudeState.Pitch = RAD2DEG(attitudeState.Pitch);
attitudeState.Yaw = RAD2DEG(attitudeState.Yaw);
RPY2Quaternion(&attitudeState.Roll, &attitudeState.q1);
AttitudeStateSet(&attitudeState);
timeval = PIOS_DELAY_GetRaw();
// wait calibration_delay only at powerup
if (xTaskGetTickCount() < 3000) {
initStartupTime = 0;
} else {
initStartupTime = xTaskGetTickCount() - CALIBRATION_DELAY;
}
// Zero gyro bias
// This is really needed after updating calibration settings.
gyro_bias[0] = 0.0f;
gyro_bias[1] = 0.0f;
gyro_bias[2] = 0.0f;
return 0;
}
if ((xTaskGetTickCount() - initStartupTime < CALIBRATION_DURATION + CALIBRATION_DELAY) &&
(xTaskGetTickCount() - initStartupTime > CALIBRATION_DELAY)) {
// For first CALIBRATION_DURATION seconds after CALIBRATION_DELAY from startup
// Zero gyro bias assuming it is steady, smoothing the gyro input value applying rollPitchBiasRate.
attitudeSettings.AccelKp = 1.0f;
attitudeSettings.AccelKi = 0.0f;
attitudeSettings.YawBiasRate = 0.23f;
accel_filter_enabled = false;
rollPitchBiasRate = 0.01f;
attitudeSettings.MagKp = magCalibrated ? 1.0f : 0.0f;
init = 0;
} else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
attitudeSettings.AccelKp = 1.0f;
attitudeSettings.AccelKi = 0.0f;
attitudeSettings.YawBiasRate = 0.23f;
accel_filter_enabled = false;
rollPitchBiasRate = 0.01f;
attitudeSettings.MagKp = magCalibrated ? 1.0f : 0.0f;
init = 0;
} else if (init == 0) {
// Reload settings (all the rates)
AttitudeSettingsGet(&attitudeSettings);
rollPitchBiasRate = 0.0f;
if (accel_alpha > 0.0f) {
accel_filter_enabled = true;
}
init = 1;
}
GyroSensorGet(&gyroSensorData);
gyroStateData.x = gyroSensorData.x;
gyroStateData.y = gyroSensorData.y;
gyroStateData.z = gyroSensorData.z;
// Compute the dT using the cpu clock
dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f;
timeval = PIOS_DELAY_GetRaw();
float q[4];
AttitudeStateData attitudeState;
AttitudeStateGet(&attitudeState);
float grot[3];
float accel_err[3];
// Get the current attitude estimate
quat_copy(&attitudeState.q1, q);
// Apply smoothing to accel values, to reduce vibration noise before main calculations.
apply_accel_filter((const float *)&accelSensorData.x, accels_filtered);
// Rotate gravity to body frame and cross with accels
grot[0] = -(2.0f * (q[1] * q[3] - q[0] * q[2]));
grot[1] = -(2.0f * (q[2] * q[3] + q[0] * q[1]));
grot[2] = -(q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
apply_accel_filter(grot, grot_filtered);
CrossProduct((const float *)accels_filtered, (const float *)grot_filtered, accel_err);
// Account for accel magnitude
accel_mag = accels_filtered[0] * accels_filtered[0] + accels_filtered[1] * accels_filtered[1] + accels_filtered[2] * accels_filtered[2];
accel_mag = sqrtf(accel_mag);
float grot_mag;
if (accel_filter_enabled) {
grot_mag = sqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]);
} else {
grot_mag = 1.0f;
}
// TODO! check grot_mag & accel vector magnitude values for correctness.
accel_err[0] /= (accel_mag * grot_mag);
accel_err[1] /= (accel_mag * grot_mag);
accel_err[2] /= (accel_mag * grot_mag);
if (xQueueReceive(magQueue, &ev, 0) != pdTRUE) {
// Rotate gravity to body frame and cross with accels
float brot[3];
float Rbe[3][3];
MagSensorData mag;
Quaternion2R(q, Rbe);
MagSensorGet(&mag);
// TODO: separate filter!
if (revoCalibration.MagBiasNullingRate > 0) {
magOffsetEstimation(&mag);
}
MagStateData mags;
mags.x = mag.x;
mags.y = mag.y;
mags.z = mag.z;
MagStateSet(&mags);
// If the mag is producing bad data don't use it (normally bad calibration)
if (!isnan(mag.x) && !isinf(mag.x) && !isnan(mag.y) && !isinf(mag.y) && !isnan(mag.z) && !isinf(mag.z)) {
rot_mult(Rbe, homeLocation.Be, brot);
float mag_len = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z);
mag.x /= mag_len;
mag.y /= mag_len;
mag.z /= mag_len;
float bmag = sqrtf(brot[0] * brot[0] + brot[1] * brot[1] + brot[2] * brot[2]);
brot[0] /= bmag;
brot[1] /= bmag;
brot[2] /= bmag;
// Only compute if neither vector is null
if (bmag < 1.0f || mag_len < 1.0f) {
mag_err[0] = mag_err[1] = mag_err[2] = 0.0f;
} else {
CrossProduct((const float *)&mag.x, (const float *)brot, mag_err);
}
}
} else {
mag_err[0] = mag_err[1] = mag_err[2] = 0.0f;
}
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
// Correct rates based on integral coefficient
gyroStateData.x -= gyro_bias[0];
gyroStateData.y -= gyro_bias[1];
gyroStateData.z -= gyro_bias[2];
gyro_bias[0] -= accel_err[0] * attitudeSettings.AccelKi - (gyroStateData.x) * rollPitchBiasRate;
gyro_bias[1] -= accel_err[1] * attitudeSettings.AccelKi - (gyroStateData.y) * rollPitchBiasRate;
gyro_bias[2] -= -mag_err[2] * attitudeSettings.MagKi - (gyroStateData.z) * rollPitchBiasRate;
// save gyroscope state
GyroStateSet(&gyroStateData);
// Correct rates based on proportional coefficient
gyroStateData.x += accel_err[0] * attitudeSettings.AccelKp / dT;
gyroStateData.y += accel_err[1] * attitudeSettings.AccelKp / dT;
gyroStateData.z += accel_err[2] * attitudeSettings.AccelKp / dT + mag_err[2] * attitudeSettings.MagKp / dT;
// Work out time derivative from INSAlgo writeup
// Also accounts for the fact that gyros are in deg/s
float qdot[4];
qdot[0] = DEG2RAD(-q[1] * gyroStateData.x - q[2] * gyroStateData.y - q[3] * gyroStateData.z) * dT / 2;
qdot[1] = DEG2RAD(q[0] * gyroStateData.x - q[3] * gyroStateData.y + q[2] * gyroStateData.z) * dT / 2;
qdot[2] = DEG2RAD(q[3] * gyroStateData.x + q[0] * gyroStateData.y - q[1] * gyroStateData.z) * dT / 2;
qdot[3] = DEG2RAD(-q[2] * gyroStateData.x + q[1] * gyroStateData.y + q[0] * gyroStateData.z) * dT / 2;
// Take a time step
q[0] = q[0] + qdot[0];
q[1] = q[1] + qdot[1];
q[2] = q[2] + qdot[2];
q[3] = q[3] + qdot[3];
if (q[0] < 0.0f) {
q[0] = -q[0];
q[1] = -q[1];
q[2] = -q[2];
q[3] = -q[3];
}
// Renomalize
qmag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
q[0] = q[0] / qmag;
q[1] = q[1] / qmag;
q[2] = q[2] / qmag;
q[3] = q[3] / qmag;
// If quaternion has become inappropriately short or is nan reinit.
// THIS SHOULD NEVER ACTUALLY HAPPEN
if ((fabsf(qmag) < 1.0e-3f) || isnan(qmag)) {
q[0] = 1.0f;
q[1] = 0.0f;
q[2] = 0.0f;
q[3] = 0.0f;
}
quat_copy(q, &attitudeState.q1);
// Convert into eueler degrees (makes assumptions about RPY order)
Quaternion2RPY(&attitudeState.q1, &attitudeState.Roll);
AttitudeStateSet(&attitudeState);
// Flush these queues for avoid errors
xQueueReceive(baroQueue, &ev, 0);
if (xQueueReceive(gpsQueue, &ev, 0) == pdTRUE && homeLocation.Set == HOMELOCATION_SET_TRUE) {
float NED[3];
// Transform the GPS position into NED coordinates
GPSPositionSensorData gpsPosition;
GPSPositionSensorGet(&gpsPosition);
getNED(&gpsPosition, NED);
PositionStateData positionState;
PositionStateGet(&positionState);
positionState.North = NED[0];
positionState.East = NED[1];
positionState.Down = NED[2];
PositionStateSet(&positionState);
}
if (xQueueReceive(gpsVelQueue, &ev, 0) == pdTRUE) {
// Transform the GPS position into NED coordinates
GPSVelocitySensorData gpsVelocity;
GPSVelocitySensorGet(&gpsVelocity);
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
velocityState.North = gpsVelocity.North;
velocityState.East = gpsVelocity.East;
velocityState.Down = gpsVelocity.Down;
VelocityStateSet(&velocityState);
}
if (xQueueReceive(airspeedQueue, &ev, 0) == pdTRUE) {
// Calculate true airspeed from indicated airspeed
AirspeedSensorData airspeedSensor;
AirspeedSensorGet(&airspeedSensor);
AirspeedStateData airspeed;
AirspeedStateGet(&airspeed);
PositionStateData positionState;
PositionStateGet(&positionState);
if (airspeedSensor.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE) {
// we have airspeed available
airspeed.CalibratedAirspeed = airspeedSensor.CalibratedAirspeed;
airspeed.TrueAirspeed = (airspeedSensor.TrueAirspeed < 0.f) ? airspeed.CalibratedAirspeed *IAS2TAS(homeLocation.Altitude - positionState.Down) : airspeedSensor.TrueAirspeed;
AirspeedStateSet(&airspeed);
}
}
if (!init && flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
} else if (variance_error) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
}
return 0;
}
#include "insgps.h"
int32_t ins_failed = 0;
extern struct NavStruct Nav;
int32_t init_stage = 0;
/**
* @brief Use the INSGPS fusion algorithm in either indoor or outdoor mode (use GPS)
* @params[in] first_run This is the first run so trigger reinitialization
* @params[in] outdoor_mode If true use the GPS for position, if false weakly pull to (0,0)
* @return 0 for success, -1 for failure
*/
static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
{
UAVObjEvent ev;
GyroSensorData gyroSensorData;
AccelSensorData accelSensorData;
MagStateData magData;
AirspeedSensorData airspeedData;
BaroSensorData baroData;
GPSPositionSensorData gpsData;
GPSVelocitySensorData gpsVelData;
static bool mag_updated = false;
static bool baro_updated;
static bool airspeed_updated;
static bool gps_updated;
static bool gps_vel_updated;
static bool value_error = false;
static float baroOffset = 0.0f;
static uint32_t ins_last_time = 0;
static bool inited;
float NED[3] = { 0.0f, 0.0f, 0.0f };
float vel[3] = { 0.0f, 0.0f, 0.0f };
float zeros[3] = { 0.0f, 0.0f, 0.0f };
// Perform the update
uint16_t sensors = 0;
float dT;
// Wait until the gyro and accel object is updated, if a timeout then go to failsafe
if ((xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE) ||
(xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE)) {
// Do not set attitude timeout warnings in simulation mode
if (!AttitudeStateReadOnly()) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING);
return -1;
}
}
if (inited) {
mag_updated = 0;
baro_updated = 0;
airspeed_updated = 0;
gps_updated = 0;
gps_vel_updated = 0;
}
if (first_run) {
inited = false;
init_stage = 0;
mag_updated = 0;
baro_updated = 0;
airspeed_updated = 0;
gps_updated = 0;
gps_vel_updated = 0;
ins_last_time = PIOS_DELAY_GetRaw();
return 0;
}
mag_updated |= (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE);
baro_updated |= xQueueReceive(baroQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE;
airspeed_updated |= xQueueReceive(airspeedQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE;
// Check if we are running simulation
if (!GPSPositionSensorReadOnly()) {
gps_updated |= (xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode;
} else {
gps_updated |= pdTRUE && outdoor_mode;
}
if (!GPSVelocitySensorReadOnly()) {
gps_vel_updated |= (xQueueReceive(gpsVelQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode;
} else {
gps_vel_updated |= pdTRUE && outdoor_mode;
}
// Get most recent data
GyroSensorGet(&gyroSensorData);
AccelSensorGet(&accelSensorData);
// TODO: separate filter!
if (mag_updated) {
MagSensorData mags;
MagSensorGet(&mags);
if (revoCalibration.MagBiasNullingRate > 0) {
magOffsetEstimation(&mags);
}
magData.x = mags.x;
magData.y = mags.y;
magData.z = mags.z;
MagStateSet(&magData);
} else {
MagStateGet(&magData);
}
BaroSensorGet(&baroData);
AirspeedSensorGet(&airspeedData);
GPSPositionSensorGet(&gpsData);
GPSVelocitySensorGet(&gpsVelData);
// TODO: put in separate filter
AccelStateData accelState;
accelState.x = accelSensorData.x;
accelState.y = accelSensorData.y;
accelState.z = accelSensorData.z;
AccelStateSet(&accelState);
value_error = false;
// safety checks
if (invalid(gyroSensorData.x) ||
invalid(gyroSensorData.y) ||
invalid(gyroSensorData.z) ||
invalid(accelSensorData.x) ||
invalid(accelSensorData.y) ||
invalid(accelSensorData.z)) {
// cannot run process update, raise error!
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
return 0;
}
if (invalid(magData.x) ||
invalid(magData.y) ||
invalid(magData.z)) {
// magnetometers can be ignored for a while
mag_updated = false;
value_error = true;
}
// Don't require HomeLocation.Set to be true but at least require a mag configuration (allows easily
// switching between indoor and outdoor mode with Set = false)
if ((homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1] + homeLocation.Be[2] * homeLocation.Be[2] < 1e-5f)) {
mag_updated = false;
value_error = true;
}
if (invalid(baroData.Altitude)) {
baro_updated = false;
value_error = true;
}
if (invalid(airspeedData.CalibratedAirspeed)) {
airspeed_updated = false;
value_error = true;
}
if (invalid(gpsData.Altitude)) {
gps_updated = false;
value_error = true;
}
if (invalid_var(ekfConfiguration.R.GPSPosNorth) ||
invalid_var(ekfConfiguration.R.GPSPosEast) ||
invalid_var(ekfConfiguration.R.GPSPosDown) ||
invalid_var(ekfConfiguration.R.GPSVelNorth) ||
invalid_var(ekfConfiguration.R.GPSVelEast) ||
invalid_var(ekfConfiguration.R.GPSVelDown)) {
gps_updated = false;
value_error = true;
}
if (invalid(gpsVelData.North) ||
invalid(gpsVelData.East) ||
invalid(gpsVelData.Down)) {
gps_vel_updated = false;
value_error = true;
}
// Discard airspeed if sensor not connected
if (airspeedData.SensorConnected != AIRSPEEDSENSOR_SENSORCONNECTED_TRUE) {
airspeed_updated = false;
}
// Have a minimum requirement for gps usage
if ((gpsData.Satellites < 7) ||
(gpsData.PDOP > 4.0f) ||
(gpsData.Latitude == 0 && gpsData.Longitude == 0) ||
(homeLocation.Set != HOMELOCATION_SET_TRUE)) {
gps_updated = false;
gps_vel_updated = false;
}
if (!inited) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
} else if (value_error) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
} else if (variance_error) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
} else if (outdoor_mode && gpsData.Satellites < 7) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
}
dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f;
ins_last_time = PIOS_DELAY_GetRaw();
// This should only happen at start up or at mode switches
if (dT > 0.01f) {
dT = 0.01f;
} else if (dT <= 0.001f) {
dT = 0.001f;
}
if (!inited && mag_updated && baro_updated && (gps_updated || !outdoor_mode) && !variance_error) {
// Don't initialize until all sensors are read
if (init_stage == 0) {
// Reset the INS algorithm
INSGPSInit();
INSSetMagVar((float[3]) { ekfConfiguration.R.MagX,
ekfConfiguration.R.MagY,
ekfConfiguration.R.MagZ }
);
INSSetAccelVar((float[3]) { ekfConfiguration.Q.AccelX,
ekfConfiguration.Q.AccelY,
ekfConfiguration.Q.AccelZ }
);
INSSetGyroVar((float[3]) { ekfConfiguration.Q.GyroX,
ekfConfiguration.Q.GyroY,
ekfConfiguration.Q.GyroZ }
);
INSSetGyroBiasVar((float[3]) { ekfConfiguration.Q.GyroDriftX,
ekfConfiguration.Q.GyroDriftY,
ekfConfiguration.Q.GyroDriftZ }
);
INSSetBaroVar(ekfConfiguration.R.BaroZ);
// Initialize the gyro bias
float gyro_bias[3] = { 0.0f, 0.0f, 0.0f };
INSSetGyroBias(gyro_bias);
float pos[3] = { 0.0f, 0.0f, 0.0f };
if (outdoor_mode) {
GPSPositionSensorData gpsPosition;
GPSPositionSensorGet(&gpsPosition);
// Transform the GPS position into NED coordinates
getNED(&gpsPosition, pos);
// Initialize barometric offset to current GPS NED coordinate
baroOffset = -pos[2] - baroData.Altitude;
} else {
// Initialize barometric offset to homelocation altitude
baroOffset = -baroData.Altitude;
pos[2] = -(baroData.Altitude + baroOffset);
}
// xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS);
// MagSensorGet(&magData);
AttitudeStateData attitudeState;
AttitudeStateGet(&attitudeState);
// Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly,
// so pseudo "north" vector can be estimated even if the board is not level
attitudeState.Roll = atan2f(-accelSensorData.y, -accelSensorData.z);
float zn = cosf(attitudeState.Roll) * magData.z + sinf(attitudeState.Roll) * magData.y;
float yn = cosf(attitudeState.Roll) * magData.y - sinf(attitudeState.Roll) * magData.z;
// rotate accels z vector according to roll
float azn = cosf(attitudeState.Roll) * accelSensorData.z + sinf(attitudeState.Roll) * accelSensorData.y;
attitudeState.Pitch = atan2f(accelSensorData.x, -azn);
float xn = cosf(attitudeState.Pitch) * magData.x + sinf(attitudeState.Pitch) * zn;
attitudeState.Yaw = atan2f(-yn, xn);
// TODO: This is still a hack
// Put this in a proper generic function in CoordinateConversion.c
// should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags)
// should calculate the rotation in 3d space using proper cross product math
// SUBTODO: formulate the math required
attitudeState.Roll = RAD2DEG(attitudeState.Roll);
attitudeState.Pitch = RAD2DEG(attitudeState.Pitch);
attitudeState.Yaw = RAD2DEG(attitudeState.Yaw);
RPY2Quaternion(&attitudeState.Roll, &attitudeState.q1);
AttitudeStateSet(&attitudeState);
float q[4] = { attitudeState.q1, attitudeState.q2, attitudeState.q3, attitudeState.q4 };
INSSetState(pos, zeros, q, zeros, zeros);
INSResetP(cast_struct_to_array(ekfConfiguration.P, ekfConfiguration.P.AttitudeQ1));
} else {
// Run prediction a bit before any corrections
// Because the sensor module remove the bias we need to add it
// back in here so that the INS algorithm can track it correctly
float gyros[3] = { DEG2RAD(gyroSensorData.x), DEG2RAD(gyroSensorData.y), DEG2RAD(gyroSensorData.z) };
INSStatePrediction(gyros, &accelSensorData.x, dT);
AttitudeStateData attitude;
AttitudeStateGet(&attitude);
attitude.q1 = Nav.q[0];
attitude.q2 = Nav.q[1];
attitude.q3 = Nav.q[2];
attitude.q4 = Nav.q[3];
Quaternion2RPY(&attitude.q1, &attitude.Roll);
AttitudeStateSet(&attitude);
}
init_stage++;
if (init_stage > 10) {
inited = true;
}
return 0;
}
if (!inited) {
return 0;
}
// Because the sensor module remove the bias we need to add it
// back in here so that the INS algorithm can track it correctly
float gyros[3] = { DEG2RAD(gyroSensorData.x), DEG2RAD(gyroSensorData.y), DEG2RAD(gyroSensorData.z) };
// Advance the state estimate
INSStatePrediction(gyros, &accelSensorData.x, dT);
// Copy the attitude into the UAVO
AttitudeStateData attitude;
AttitudeStateGet(&attitude);
attitude.q1 = Nav.q[0];
attitude.q2 = Nav.q[1];
attitude.q3 = Nav.q[2];
attitude.q4 = Nav.q[3];
Quaternion2RPY(&attitude.q1, &attitude.Roll);
AttitudeStateSet(&attitude);
// Advance the covariance estimate
INSCovariancePrediction(dT);
if (mag_updated) {
sensors |= MAG_SENSORS;
}
if (baro_updated) {
sensors |= BARO_SENSOR;
}
INSSetMagNorth(homeLocation.Be);
if (gps_updated && outdoor_mode) {
INSSetPosVelVar((float[3]) { ekfConfiguration.R.GPSPosNorth,
ekfConfiguration.R.GPSPosEast,
ekfConfiguration.R.GPSPosDown },
(float[3]) { ekfConfiguration.R.GPSVelNorth,
ekfConfiguration.R.GPSVelEast,
ekfConfiguration.R.GPSVelDown }
);
sensors |= POS_SENSORS;
if (0) { // Old code to take horizontal velocity from GPS Position update
sensors |= HORIZ_SENSORS;
vel[0] = gpsData.Groundspeed * cosf(DEG2RAD(gpsData.Heading));
vel[1] = gpsData.Groundspeed * sinf(DEG2RAD(gpsData.Heading));
vel[2] = 0.0f;
}
// Transform the GPS position into NED coordinates
getNED(&gpsData, NED);
// Track barometric altitude offset with a low pass filter
baroOffset = BARO_OFFSET_LOWPASS_ALPHA * baroOffset +
(1.0f - BARO_OFFSET_LOWPASS_ALPHA)
* (-NED[2] - baroData.Altitude);
} else if (!outdoor_mode) {
INSSetPosVelVar((float[3]) { ekfConfiguration.FakeR.FakeGPSPosIndoor,
ekfConfiguration.FakeR.FakeGPSPosIndoor,
ekfConfiguration.FakeR.FakeGPSPosIndoor },
(float[3]) { ekfConfiguration.FakeR.FakeGPSVelIndoor,
ekfConfiguration.FakeR.FakeGPSVelIndoor,
ekfConfiguration.FakeR.FakeGPSVelIndoor }
);
vel[0] = vel[1] = vel[2] = 0.0f;
NED[0] = NED[1] = 0.0f;
NED[2] = -(baroData.Altitude + baroOffset);
sensors |= HORIZ_SENSORS | HORIZ_POS_SENSORS;
sensors |= POS_SENSORS | VERT_SENSORS;
}
if (gps_vel_updated && outdoor_mode) {
sensors |= HORIZ_SENSORS | VERT_SENSORS;
vel[0] = gpsVelData.North;
vel[1] = gpsVelData.East;
vel[2] = gpsVelData.Down;
}
// Copy the position into the UAVO
PositionStateData positionState;
PositionStateGet(&positionState);
positionState.North = Nav.Pos[0];
positionState.East = Nav.Pos[1];
positionState.Down = Nav.Pos[2];
PositionStateSet(&positionState);
// airspeed correction needs current positionState
if (airspeed_updated) {
// we have airspeed available
AirspeedStateData airspeed;
AirspeedStateGet(&airspeed);
airspeed.CalibratedAirspeed = airspeedData.CalibratedAirspeed;
airspeed.TrueAirspeed = (airspeedData.TrueAirspeed < 0.f) ? airspeed.CalibratedAirspeed *IAS2TAS(homeLocation.Altitude - positionState.Down) : airspeedData.TrueAirspeed;
AirspeedStateSet(&airspeed);
if (!gps_vel_updated && !gps_updated) {
// feed airspeed into EKF, treat wind as 1e2 variance
sensors |= HORIZ_SENSORS | VERT_SENSORS;
INSSetPosVelVar((float[3]) { ekfConfiguration.FakeR.FakeGPSPosIndoor,
ekfConfiguration.FakeR.FakeGPSPosIndoor,
ekfConfiguration.FakeR.FakeGPSPosIndoor },
(float[3]) { ekfConfiguration.FakeR.FakeGPSVelAirspeed,
ekfConfiguration.FakeR.FakeGPSVelAirspeed,
ekfConfiguration.FakeR.FakeGPSVelAirspeed }
);
// rotate airspeed vector into NED frame - airspeed is measured in X axis only
float R[3][3];
Quaternion2R(Nav.q, R);
float vtas[3] = { airspeed.TrueAirspeed, 0.0f, 0.0f };
rot_mult(R, vtas, vel);
}
}
/*
* TODO: Need to add a general sanity check for all the inputs to make sure their kosher
* although probably should occur within INS itself
*/
if (sensors) {
INSCorrection(&magData.x, NED, vel, (baroData.Altitude + baroOffset), sensors);
}
// Copy the velocity into the UAVO
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
velocityState.North = Nav.Vel[0];
velocityState.East = Nav.Vel[1];
velocityState.Down = Nav.Vel[2];
VelocityStateSet(&velocityState);
GyroStateData gyroState;
gyroState.x = RAD2DEG(gyros[0] - RAD2DEG(Nav.gyro_bias[0]));
gyroState.y = RAD2DEG(gyros[1] - RAD2DEG(Nav.gyro_bias[1]));
gyroState.z = RAD2DEG(gyros[2] - RAD2DEG(Nav.gyro_bias[2]));
GyroStateSet(&gyroState);
EKFStateVarianceData vardata;
EKFStateVarianceGet(&vardata);
INSGetP(cast_struct_to_array(vardata.P, vardata.P.AttitudeQ1));
EKFStateVarianceSet(&vardata);
return 0;
}
/**
* @brief Convert the GPS LLA position into NED coordinates
* @note this method uses a taylor expansion around the home coordinates
* to convert to NED which allows it to be done with all floating
* calculations
* @param[in] Current GPS coordinates
* @param[out] NED frame coordinates
* @returns 0 for success, -1 for failure
*/
float T[3];
static int32_t getNED(GPSPositionSensorData *gpsPosition, float *NED)
{
float dL[3] = { DEG2RAD((gpsPosition->Latitude - homeLocation.Latitude) / 10.0e6f),
DEG2RAD((gpsPosition->Longitude - homeLocation.Longitude) / 10.0e6f),
(gpsPosition->Altitude + gpsPosition->GeoidSeparation - homeLocation.Altitude) };
NED[0] = T[0] * dL[0];
NED[1] = T[1] * dL[1];
NED[2] = T[2] * dL[2];
return 0;
}
static void settingsUpdatedCb(UAVObjEvent *ev)
{
if (ev == NULL || ev->obj == FlightStatusHandle()) {
FlightStatusGet(&flightStatus);
}
if (ev == NULL || ev->obj == RevoCalibrationHandle()) {
RevoCalibrationGet(&revoCalibration);
}
// change of these settings require reinitialization of the EKF
// when an error flag has been risen, we also listen to flightStatus updates,
// since we are waiting for the system to get disarmed so we can reinitialize safely.
if (ev == NULL ||
ev->obj == EKFConfigurationHandle() ||
ev->obj == RevoSettingsHandle() ||
(variance_error == true && ev->obj == FlightStatusHandle())
) {
bool error = false;
EKFConfigurationGet(&ekfConfiguration);
int t;
for (t = 0; t < EKFCONFIGURATION_P_NUMELEM; t++) {
if (invalid_var(cast_struct_to_array(ekfConfiguration.P, ekfConfiguration.P.AttitudeQ1)[t])) {
error = true;
}
}
for (t = 0; t < EKFCONFIGURATION_Q_NUMELEM; t++) {
if (invalid_var(cast_struct_to_array(ekfConfiguration.Q, ekfConfiguration.Q.AccelX)[t])) {
error = true;
}
}
for (t = 0; t < EKFCONFIGURATION_R_NUMELEM; t++) {
if (invalid_var(cast_struct_to_array(ekfConfiguration.R, ekfConfiguration.R.BaroZ)[t])) {
error = true;
}
}
RevoSettingsGet(&revoSettings);
// Reinitialization of the EKF is not desired during flight.
// It will be delayed until the board is disarmed by raising the error flag.
// We will not prevent the initial initialization though, since the board could be in always armed mode.
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && !initialization_required) {
error = true;
}
if (error) {
variance_error = true;
} else {
// trigger reinitialization - possibly with new algorithm
running_algorithm = revoSettings.FusionAlgorithm;
variance_error = false;
initialization_required = true;
}
}
if (ev == NULL || ev->obj == HomeLocationHandle()) {
HomeLocationGet(&homeLocation);
// Compute matrix to convert deltaLLA to NED
float lat, alt;
lat = DEG2RAD(homeLocation.Latitude / 10.0e6f);
alt = homeLocation.Altitude;
T[0] = alt + 6.378137E6f;
T[1] = cosf(lat) * (alt + 6.378137E6f);
T[2] = -1.0f;
// TODO: convert positionState to new reference frame and gracefully update EKF state!
// needed for long range flights where the reference coordinate is adjusted in flight
}
if (ev == NULL || ev->obj == AttitudeSettingsHandle()) {
AttitudeSettingsGet(&attitudeSettings);
// Calculate accel filter alpha, in the same way as for gyro data in stabilization module.
const float fakeDt = 0.0015f;
if (attitudeSettings.AccelTau < 0.0001f) {
accel_alpha = 0; // not trusting this to resolve to 0
accel_filter_enabled = false;
} else {
accel_alpha = expf(-fakeDt / attitudeSettings.AccelTau);
accel_filter_enabled = true;
}
}
}
/**
* Perform an update of the @ref MagBias based on
* Magmeter Offset Cancellation: Theory and Implementation,
* revisited William Premerlani, October 14, 2011
*/
static void magOffsetEstimation(MagSensorData *mag)
{
#if 0
// Constants, to possibly go into a UAVO
static const float MIN_NORM_DIFFERENCE = 50;
static float B2[3] = { 0, 0, 0 };
MagBiasData magBias;
MagBiasGet(&magBias);
// Remove the current estimate of the bias
mag->x -= magBias.x;
mag->y -= magBias.y;
mag->z -= magBias.z;
// First call
if (B2[0] == 0 && B2[1] == 0 && B2[2] == 0) {
B2[0] = mag->x;
B2[1] = mag->y;
B2[2] = mag->z;
return;
}
float B1[3] = { mag->x, mag->y, mag->z };
float norm_diff = sqrtf(powf(B2[0] - B1[0], 2) + powf(B2[1] - B1[1], 2) + powf(B2[2] - B1[2], 2));
if (norm_diff > MIN_NORM_DIFFERENCE) {
float norm_b1 = sqrtf(B1[0] * B1[0] + B1[1] * B1[1] + B1[2] * B1[2]);
float norm_b2 = sqrtf(B2[0] * B2[0] + B2[1] * B2[1] + B2[2] * B2[2]);
float scale = cal.MagBiasNullingRate * (norm_b2 - norm_b1) / norm_diff;
float b_error[3] = { (B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale };
magBias.x += b_error[0];
magBias.y += b_error[1];
magBias.z += b_error[2];
MagBiasSet(&magBias);
// Store this value to compare against next update
B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2];
}
#else // if 0
static float magBias[3] = { 0 };
// Remove the current estimate of the bias
mag->x -= magBias[0];
mag->y -= magBias[1];
mag->z -= magBias[2];
AttitudeStateData attitude;
AttitudeStateGet(&attitude);
const float Rxy = sqrtf(homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1]);
const float Rz = homeLocation.Be[2];
const float rate = revoCalibration.MagBiasNullingRate;
float Rot[3][3];
float B_e[3];
float xy[2];
float delta[3];
// Get the rotation matrix
Quaternion2R(&attitude.q1, Rot);
// Rotate the mag into the NED frame
B_e[0] = Rot[0][0] * mag->x + Rot[1][0] * mag->y + Rot[2][0] * mag->z;
B_e[1] = Rot[0][1] * mag->x + Rot[1][1] * mag->y + Rot[2][1] * mag->z;
B_e[2] = Rot[0][2] * mag->x + Rot[1][2] * mag->y + Rot[2][2] * mag->z;
float cy = cosf(DEG2RAD(attitude.Yaw));
float sy = sinf(DEG2RAD(attitude.Yaw));
xy[0] = cy * B_e[0] + sy * B_e[1];
xy[1] = -sy * B_e[0] + cy * B_e[1];
float xy_norm = sqrtf(xy[0] * xy[0] + xy[1] * xy[1]);
delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]);
delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]);
delta[2] = -rate * (Rz - B_e[2]);
if (!isnan(delta[0]) && !isinf(delta[0]) &&
!isnan(delta[1]) && !isinf(delta[1]) &&
!isnan(delta[2]) && !isinf(delta[2])) {
magBias[0] += delta[0];
magBias[1] += delta[1];
magBias[2] += delta[2];
}
#endif // if 0
}
/**
* @}
* @}
*/

29
flight/modules/CameraStab/camerastab.c
View File

@ -52,7 +52,6 @@
#include "camerastabsettings.h"
#include "cameradesired.h"
#include "hwsettings.h"
#include <pios_struct_helper.h>
//
// Configuration
@ -172,22 +171,22 @@ static void attitudeUpdated(UAVObjEvent *ev)
// process axes
for (uint8_t i = 0; i < CAMERASTABSETTINGS_INPUT_NUMELEM; i++) {
// read and process control input
if (cast_struct_to_array(cameraStab.Input, cameraStab.Input.Roll)[i] != CAMERASTABSETTINGS_INPUT_NONE) {
if (AccessoryDesiredInstGet(cast_struct_to_array(cameraStab.Input, cameraStab.Input.Roll)[i] -
if (CameraStabSettingsInputToArray(cameraStab.Input)[i] != CAMERASTABSETTINGS_INPUT_NONE) {
if (AccessoryDesiredInstGet(CameraStabSettingsInputToArray(cameraStab.Input)[i] -
CAMERASTABSETTINGS_INPUT_ACCESSORY0, &accessory) == 0) {
float input_rate;
switch (cast_struct_to_array(cameraStab.StabilizationMode, cameraStab.StabilizationMode.Roll)[i]) {
switch (CameraStabSettingsStabilizationModeToArray(cameraStab.StabilizationMode)[i]) {
case CAMERASTABSETTINGS_STABILIZATIONMODE_ATTITUDE:
csd->inputs[i] = accessory.AccessoryVal *
cast_struct_to_array(cameraStab.InputRange, cameraStab.InputRange.Roll)[i];
CameraStabSettingsInputRangeToArray(cameraStab.InputRange)[i];
break;
case CAMERASTABSETTINGS_STABILIZATIONMODE_AXISLOCK:
input_rate = accessory.AccessoryVal *
cast_struct_to_array(cameraStab.InputRate, cameraStab.InputRate.Roll)[i];
CameraStabSettingsInputRateToArray(cameraStab.InputRate)[i];
if (fabsf(input_rate) > cameraStab.MaxAxisLockRate) {
csd->inputs[i] = boundf(csd->inputs[i] + input_rate * 0.001f * dT_millis,
-cast_struct_to_array(cameraStab.InputRange, cameraStab.InputRange.Roll)[i],
cast_struct_to_array(cameraStab.InputRange, cameraStab.InputRange.Roll)[i]);
-CameraStabSettingsInputRangeToArray(cameraStab.InputRange)[i],
CameraStabSettingsInputRangeToArray(cameraStab.InputRange)[i]);
}
break;
default:
@ -214,14 +213,14 @@ static void attitudeUpdated(UAVObjEvent *ev)
}
#ifdef USE_GIMBAL_LPF
if (cast_struct_to_array(cameraStab.ResponseTime, cameraStab.ResponseTime.Roll)[i]) {
float rt = (float)cast_struct_to_array(cameraStab.ResponseTime, cameraStab.ResponseTime.Roll)[i];
if (CameraStabSettingsResponseTimeToArray(cameraStab.ResponseTime)[i]) {
float rt = (float)CameraStabSettingsResponseTimeToArray(cameraStab.ResponseTime)[i];
attitude = csd->attitudeFiltered[i] = ((rt * csd->attitudeFiltered[i]) + (dT_millis * attitude)) / (rt + dT_millis);
}
#endif
#ifdef USE_GIMBAL_FF
if (cast_struct_to_array(cameraStab.FeedForward, cameraStab.FeedForward.Roll)[i]) {
if (CameraStabSettingsFeedForwardToArray(cameraStab.FeedForward)[i]) {
applyFeedForward(i, dT_millis, &attitude, &cameraStab);
}
#endif
@ -229,7 +228,7 @@ static void attitudeUpdated(UAVObjEvent *ev)
// bounding for elevon mixing occurs on the unmixed output
// to limit the range of the mixed output you must limit the range
// of both the unmixed pitch and unmixed roll
float output = boundf((attitude + csd->inputs[i]) / cast_struct_to_array(cameraStab.OutputRange, cameraStab.OutputRange.Roll)[i], -1.0f, 1.0f);
float output = boundf((attitude + csd->inputs[i]) / CameraStabSettingsOutputRangeToArray(cameraStab.OutputRange)[i], -1.0f, 1.0f);
// set output channels
switch (i) {
@ -316,12 +315,12 @@ void applyFeedForward(uint8_t index, float dT_millis, float *attitude, CameraSta
// apply feed forward
float accumulator = csd->ffFilterAccumulator[index];
accumulator += (*attitude - csd->ffLastAttitude[index]) *
(float)cast_struct_to_array(cameraStab->FeedForward, cameraStab->FeedForward.Roll)[index] * gimbalTypeCorrection;
(float)CameraStabSettingsFeedForwardToArray(cameraStab->FeedForward)[index] * gimbalTypeCorrection;
csd->ffLastAttitude[index] = *attitude;
*attitude += accumulator;
float filter = (float)((accumulator > 0.0f) ? cast_struct_to_array(cameraStab->AccelTime, cameraStab->AccelTime.Roll)[index] :
cast_struct_to_array(cameraStab->DecelTime, cameraStab->DecelTime.Roll)[index]) / dT_millis;
float filter = (float)((accumulator > 0.0f) ? CameraStabSettingsAccelTimeToArray(cameraStab->AccelTime)[index] :
CameraStabSettingsDecelTimeToArray(cameraStab->DecelTime)[index]) / dT_millis;
if (filter < 1.0f) {
filter = 1.0f;
}

11
flight/modules/ComUsbBridge/ComUsbBridge.c
View File

@ -42,7 +42,7 @@
static void com2UsbBridgeTask(void *parameters);
static void usb2ComBridgeTask(void *parameters);
static void updateSettings();
static void updateSettings(UAVObjEvent *ev);
// ****************
// Private constants
@ -106,8 +106,7 @@ static int32_t comUsbBridgeInitialize(void)
HwSettingsOptionalModulesGet(&optionalModules);
if (usart_port && vcp_port &&
(optionalModules.ComUsbBridge == HWSETTINGS_OPTIONALMODULES_ENABLED)) {
if (usart_port && vcp_port) {
bridge_enabled = true;
} else {
bridge_enabled = false;
@ -119,8 +118,8 @@ static int32_t comUsbBridgeInitialize(void)
PIOS_Assert(com2usb_buf);
usb2com_buf = pios_malloc(BRIDGE_BUF_LEN);
PIOS_Assert(usb2com_buf);
updateSettings();
HwSettingsConnectCallback(&updateSettings);
updateSettings(0);
}
return 0;
@ -170,7 +169,7 @@ static void usb2ComBridgeTask(__attribute__((unused)) void *parameters)
}
static void updateSettings()
static void updateSettings(__attribute__((unused)) UAVObjEvent *ev)
{
if (usart_port) {
// Retrieve settings

30
flight/modules/Extensions/MagBaro/magbaro.c
View File

@ -64,7 +64,8 @@ static int32_t alt_ds_pres = 0;
static int alt_ds_count = 0;
#endif
#if defined(PIOS_INCLUDE_HMC5883)
#if defined(PIOS_INCLUDE_HMC5X83)
pios_hmc5x83_dev_t mag_handle = 0;
int32_t mag_test;
static float mag_bias[3] = { 0, 0, 0 };
static float mag_scale[3] = { 1, 1, 1 };
@ -108,7 +109,7 @@ int32_t MagBaroInitialize()
#endif
if (magbaroEnabled) {
#if defined(PIOS_INCLUDE_HMC5883)
#if defined(PIOS_INCLUDE_HMC5X83)
MagSensorInitialize();
#endif
@ -127,15 +128,16 @@ MODULE_INITCALL(MagBaroInitialize, MagBaroStart);
/**
* Module thread, should not return.
*/
#if defined(PIOS_INCLUDE_HMC5883)
static const struct pios_hmc5883_cfg pios_hmc5883_cfg = {
#ifdef PIOS_HMC5883_HAS_GPIOS
#if defined(PIOS_INCLUDE_HMC5X83)
static const struct pios_hmc5x83_cfg pios_hmc5x83_cfg = {
#ifdef PIOS_HMC5X83_HAS_GPIOS
.exti_cfg = 0,
#endif
.M_ODR = PIOS_HMC5883_ODR_15,
.Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL,
.Gain = PIOS_HMC5883_GAIN_1_9,
.Mode = PIOS_HMC5883_MODE_CONTINUOUS,
.M_ODR = PIOS_HMC5x83_ODR_15,
.Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL,
.Gain = PIOS_HMC5x83_GAIN_1_9,
.Mode = PIOS_HMC5x83_MODE_CONTINUOUS,
.Driver = &PIOS_HMC5x83_I2C_DRIVER,
};
#endif
@ -148,9 +150,9 @@ static void magbaroTask(__attribute__((unused)) void *parameters)
PIOS_BMP085_Init();
#endif
#if defined(PIOS_INCLUDE_HMC5883)
#if defined(PIOS_INCLUDE_HMC5X83)
MagSensorData mag;
PIOS_HMC5883_Init(&pios_hmc5883_cfg);
mag_handle = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, PIOS_I2C_MAIN_ADAPTER, 0);
uint32_t mag_update_time = PIOS_DELAY_GetRaw();
#endif
@ -197,10 +199,10 @@ static void magbaroTask(__attribute__((unused)) void *parameters)
}
#endif /* if defined(PIOS_INCLUDE_BMP085) */
#if defined(PIOS_INCLUDE_HMC5883)
if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 100000) {
#if defined(PIOS_INCLUDE_HMC5X83)
if (PIOS_HMC5x83_NewDataAvailable(mag_handle) || PIOS_DELAY_DiffuS(mag_update_time) > 100000) {
int16_t values[3];
PIOS_HMC5883_ReadMag(values);
PIOS_HMC5x83_ReadMag(mag_handle, values);
float mags[3] = { (float)values[1] * mag_scale[0] - mag_bias[0],
(float)values[0] * mag_scale[1] - mag_bias[1],
-(float)values[2] * mag_scale[2] - mag_bias[2] };

3
flight/modules/FirmwareIAP/firmwareiap.c
View File

@ -176,6 +176,7 @@ static void FirmwareIAPCallback(UAVObjEvent *ev)
case IAP_STATE_STEP_2:
if (data.Command == IAP_CMD_STEP_3) {
if (delta > iap_time_3_low_end && delta < iap_time_3_high_end) {
#ifndef PIOS_APPS_MINIMAL
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
@ -184,7 +185,7 @@ static void FirmwareIAPCallback(UAVObjEvent *ev)
iap_state = IAP_STATE_READY;
break;
}
#endif
// we've met the three sequence of command numbers
// we've met the time requirements.
PIOS_IAP_SetRequest1();

772
flight/modules/FixedWingPathFollower/fixedwingpathfollower.c
View File

@ -1,772 +0,0 @@
/**
******************************************************************************
*
* @file fixedwingpathfollower.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint
* and sets @ref AttitudeDesired. It only does this when the FlightMode field
* of @ref ManualControlCommand is Auto.
*
* @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
*/
/**
* Input object: ActiveWaypoint
* Input object: PositionState
* Input object: ManualControlCommand
* Output object: AttitudeDesired
*
* This module will periodically update the value of the AttitudeDesired object.
*
* The module executes in its own thread in this example.
*
* Modules have no API, all communication to other modules is done through UAVObjects.
* However modules may use the API exposed by shared libraries.
* See the OpenPilot wiki for more details.
* http://www.openpilot.org/OpenPilot_Application_Architecture
*
*/
#include <openpilot.h>
#include "hwsettings.h"
#include "attitudestate.h"
#include "pathdesired.h" // object that will be updated by the module
#include "positionstate.h"
#include "flightstatus.h"
#include "pathstatus.h"
#include "airspeedstate.h"
#include "fixedwingpathfollowersettings.h"
#include "fixedwingpathfollowerstatus.h"
#include "homelocation.h"
#include "stabilizationdesired.h"
#include "stabilizationsettings.h"
#include "systemsettings.h"
#include "velocitydesired.h"
#include "velocitystate.h"
#include "taskinfo.h"
#include <pios_struct_helper.h>
#include <sanitycheck.h>
#include "sin_lookup.h"
#include "paths.h"
#include "CoordinateConversions.h"
// Private constants
#define MAX_QUEUE_SIZE 4
#define STACK_SIZE_BYTES 1548
#define TASK_PRIORITY (tskIDLE_PRIORITY + 2)
// Private variables
static bool followerEnabled = false;
static xTaskHandle pathfollowerTaskHandle;
static PathDesiredData pathDesired;
static PathStatusData pathStatus;
static FixedWingPathFollowerSettingsData fixedwingpathfollowerSettings;
// Private functions
static void pathfollowerTask(void *parameters);
static void SettingsUpdatedCb(UAVObjEvent *ev);
static void updatePathVelocity();
static uint8_t updateFixedDesiredAttitude();
static void updateFixedAttitude();
static void airspeedStateUpdatedCb(UAVObjEvent *ev);
static bool correctCourse(float *C, float *V, float *F, float s);
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t FixedWingPathFollowerStart()
{
if (followerEnabled) {
// Start main task
xTaskCreate(pathfollowerTask, "PathFollower", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &pathfollowerTaskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_PATHFOLLOWER, pathfollowerTaskHandle);
}
return 0;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t FixedWingPathFollowerInitialize()
{
HwSettingsInitialize();
HwSettingsOptionalModulesData optionalModules;
HwSettingsOptionalModulesGet(&optionalModules);
FrameType_t frameType = GetCurrentFrameType();
if ((optionalModules.FixedWingPathFollower == HWSETTINGS_OPTIONALMODULES_ENABLED) ||
(frameType == FRAME_TYPE_FIXED_WING)) {
followerEnabled = true;
FixedWingPathFollowerSettingsInitialize();
FixedWingPathFollowerStatusInitialize();
PathDesiredInitialize();
PathStatusInitialize();
VelocityDesiredInitialize();
AirspeedStateInitialize();
} else {
followerEnabled = false;
}
return 0;
}
MODULE_INITCALL(FixedWingPathFollowerInitialize, FixedWingPathFollowerStart);
static float northVelIntegral = 0.0f;
static float eastVelIntegral = 0.0f;
static float downVelIntegral = 0.0f;
static float courseIntegral = 0.0f;
static float speedIntegral = 0.0f;
static float powerIntegral = 0.0f;
static float airspeedErrorInt = 0.0f;
// correct speed by measured airspeed
static float indicatedAirspeedStateBias = 0.0f;
/**
* Module thread, should not return.
*/
static void pathfollowerTask(__attribute__((unused)) void *parameters)
{
SystemSettingsData systemSettings;
FlightStatusData flightStatus;
portTickType lastUpdateTime;
AirspeedStateConnectCallback(airspeedStateUpdatedCb);
FixedWingPathFollowerSettingsConnectCallback(SettingsUpdatedCb);
PathDesiredConnectCallback(SettingsUpdatedCb);
FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings);
PathDesiredGet(&pathDesired);
// Main task loop
lastUpdateTime = xTaskGetTickCount();
while (1) {
// Conditions when this runs:
// 1. Must have FixedWing type airframe
// 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR
// FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path
SystemSettingsGet(&systemSettings);
if ((systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL)) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
vTaskDelay(1000);
continue;
}
// Continue collecting data if not enough time
vTaskDelayUntil(&lastUpdateTime, fixedwingpathfollowerSettings.UpdatePeriod / portTICK_RATE_MS);
FlightStatusGet(&flightStatus);
PathStatusGet(&pathStatus);
uint8_t result;
// Check the combinations of flightmode and pathdesired mode
if (flightStatus.ControlChain.PathFollower == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
if (flightStatus.ControlChain.PathPlanner == FLIGHTSTATUS_CONTROLCHAIN_FALSE) {
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
updatePathVelocity();
result = updateFixedDesiredAttitude();
if (result) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
}
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
}
} else {
pathStatus.UID = pathDesired.UID;
pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
switch (pathDesired.Mode) {
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_FLYCIRCLELEFT:
updatePathVelocity();
result = updateFixedDesiredAttitude();
if (result) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
} else {
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
}
break;
case PATHDESIRED_MODE_FIXEDATTITUDE:
updateFixedAttitude(pathDesired.ModeParameters);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_DISARMALARM:
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
break;
default:
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
break;
}
}
} else {
// Be cleaner and get rid of global variables
northVelIntegral = 0.0f;
eastVelIntegral = 0.0f;
downVelIntegral = 0.0f;
courseIntegral = 0.0f;
speedIntegral = 0.0f;
powerIntegral = 0.0f;
}
PathStatusSet(&pathStatus);
}
}
/**
* Compute desired velocity from the current position and path
*
* Takes in @ref PositionState and compares it to @ref PathDesired
* and computes @ref VelocityDesired
*/
static void updatePathVelocity()
{
PositionStateData positionState;
PositionStateGet(&positionState);
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
// look ahead fixedwingpathfollowerSettings.CourseFeedForward seconds
float cur[3] = { positionState.North + (velocityState.North * fixedwingpathfollowerSettings.CourseFeedForward),
positionState.East + (velocityState.East * fixedwingpathfollowerSettings.CourseFeedForward),
positionState.Down + (velocityState.Down * fixedwingpathfollowerSettings.CourseFeedForward) };
struct path_status progress;
path_progress(cast_struct_to_array(pathDesired.Start, pathDesired.Start.North),
cast_struct_to_array(pathDesired.End, pathDesired.End.North),
cur, &progress, pathDesired.Mode);
float groundspeed;
float altitudeSetpoint;
switch (pathDesired.Mode) {
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_DRIVECIRCLERIGHT:
case PATHDESIRED_MODE_FLYCIRCLELEFT:
case PATHDESIRED_MODE_DRIVECIRCLELEFT:
groundspeed = pathDesired.EndingVelocity;
altitudeSetpoint = pathDesired.End.Down;
break;
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_DRIVEENDPOINT:
case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_DRIVEVECTOR:
default:
groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) *
boundf(progress.fractional_progress, 0.0f, 1.0f);
altitudeSetpoint = pathDesired.Start.Down + (pathDesired.End.Down - pathDesired.Start.Down) *
boundf(progress.fractional_progress, 0.0f, 1.0f);
break;
}
// make sure groundspeed is not zero
if (groundspeed < 1e-6f) {
groundspeed = 1e-6f;
}
// calculate velocity - can be zero if waypoints are too close
VelocityDesiredData velocityDesired;
velocityDesired.North = progress.path_direction[0];
velocityDesired.East = progress.path_direction[1];
float error_speed = progress.error * fixedwingpathfollowerSettings.HorizontalPosP;
// if a plane is crossing its desired flightpath facing the wrong way (away from flight direction)
// it would turn towards the flightpath to get on its desired course. This however would reverse the correction vector
// once it crosses the flightpath again, which would make it again turn towards the flightpath (but away from its desired heading)
// leading to an S-shape snake course the wrong way
// this only happens especially if HorizontalPosP is too high, as otherwise the angle between velocity desired and path_direction won't
// turn steep unless there is enough space complete the turn before crossing the flightpath
// in this case the plane effectively needs to be turned around
// indicators:
// difference between correction_direction and velocitystate >90 degrees and
// difference between path_direction and velocitystate >90 degrees ( 4th sector, facing away from everything )
// fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore)
if ( // calculating angles < 90 degrees through dot products
((progress.path_direction[0] * velocityState.North + progress.path_direction[1] * velocityState.East) < 0.0f) &&
((progress.correction_direction[0] * velocityState.North + progress.correction_direction[1] * velocityState.East) < 0.0f)) {
error_speed = 0.0f;
}
// calculate correction - can also be zero if correction vector is 0 or no error present
velocityDesired.North += progress.correction_direction[0] * error_speed;
velocityDesired.East += progress.correction_direction[1] * error_speed;
// scale to correct length
float l = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East);
if (l > 1e-9f) {
velocityDesired.North *= groundspeed / l;
velocityDesired.East *= groundspeed / l;
}
float downError = altitudeSetpoint - positionState.Down;
velocityDesired.Down = downError * fixedwingpathfollowerSettings.VerticalPosP;
// update pathstatus
pathStatus.error = progress.error;
pathStatus.fractional_progress = progress.fractional_progress;
VelocityDesiredSet(&velocityDesired);
}
/**
* Compute desired attitude from a fixed preset
*
*/
static void updateFixedAttitude(float *attitude)
{
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
stabDesired.Roll = attitude[0];
stabDesired.Pitch = attitude[1];
stabDesired.Yaw = attitude[2];
stabDesired.Thrust = attitude[3];
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
StabilizationDesiredSet(&stabDesired);
}
/**
* Compute desired attitude from the desired velocity
*
* Takes in @ref NedState which has the acceleration in the
* NED frame as the feedback term and then compares the
* @ref VelocityState against the @ref VelocityDesired
*/
static uint8_t updateFixedDesiredAttitude()
{
uint8_t result = 1;
float dT = fixedwingpathfollowerSettings.UpdatePeriod / 1000.0f; // Convert from [ms] to [s]
VelocityDesiredData velocityDesired;
VelocityStateData velocityState;
StabilizationDesiredData stabDesired;
AttitudeStateData attitudeState;
StabilizationSettingsData stabSettings;
FixedWingPathFollowerStatusData fixedwingpathfollowerStatus;
AirspeedStateData airspeedState;
SystemSettingsData systemSettings;
float groundspeedProjection;
float indicatedAirspeedState;
float indicatedAirspeedDesired;
float airspeedError;
float pitchCommand;
float descentspeedDesired;
float descentspeedError;
float powerCommand;
float airspeedVector[2];
float fluidMovement[2];
float courseComponent[2];
float courseError;
float courseCommand;
FixedWingPathFollowerStatusGet(&fixedwingpathfollowerStatus);
VelocityStateGet(&velocityState);
StabilizationDesiredGet(&stabDesired);
VelocityDesiredGet(&velocityDesired);
AttitudeStateGet(&attitudeState);
StabilizationSettingsGet(&stabSettings);
AirspeedStateGet(&airspeedState);
SystemSettingsGet(&systemSettings);
/**
* Compute speed error and course
*/
// missing sensors for airspeed-direction we have to assume within
// reasonable error that measured airspeed is actually the airspeed
// component in forward pointing direction
// airspeedVector is normalized
airspeedVector[0] = cos_lookup_deg(attitudeState.Yaw);
airspeedVector[1] = sin_lookup_deg(attitudeState.Yaw);
// current ground speed projected in forward direction
groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1];
// note that airspeedStateBias is ( calibratedAirspeed - groundspeedProjection ) at the time of measurement,
// but thanks to accelerometers, groundspeedProjection reacts faster to changes in direction
// than airspeed and gps sensors alone
indicatedAirspeedState = groundspeedProjection + indicatedAirspeedStateBias;
// fluidMovement is a vector describing the aproximate movement vector of
// the surrounding fluid in 2d space (aka wind vector)
fluidMovement[0] = velocityState.North - (indicatedAirspeedState * airspeedVector[0]);
fluidMovement[1] = velocityState.East - (indicatedAirspeedState * airspeedVector[1]);
// calculate the movement vector we need to fly to reach velocityDesired -
// taking fluidMovement into account
courseComponent[0] = velocityDesired.North - fluidMovement[0];
courseComponent[1] = velocityDesired.East - fluidMovement[1];
indicatedAirspeedDesired = boundf(sqrtf(courseComponent[0] * courseComponent[0] + courseComponent[1] * courseComponent[1]),
fixedwingpathfollowerSettings.HorizontalVelMin,
fixedwingpathfollowerSettings.HorizontalVelMax);
// if we could fly at arbitrary speeds, we'd just have to move towards the
// courseComponent vector as previously calculated and we'd be fine
// unfortunately however we are bound by min and max air speed limits, so
// we need to recalculate the correct course to meet at least the
// velocityDesired vector direction at our current speed
// this overwrites courseComponent
bool valid = correctCourse(courseComponent, (float *)&velocityDesired.North, fluidMovement, indicatedAirspeedDesired);
// Error condition: wind speed too high, we can't go where we want anymore
fixedwingpathfollowerStatus.Errors.Wind = 0;
if ((!valid) &&
fixedwingpathfollowerSettings.Safetymargins.Wind > 0.5f) { // alarm switched on
fixedwingpathfollowerStatus.Errors.Wind = 1;
result = 0;
}
// Airspeed error
airspeedError = indicatedAirspeedDesired - indicatedAirspeedState;
// Vertical speed error
descentspeedDesired = boundf(
velocityDesired.Down,
-fixedwingpathfollowerSettings.VerticalVelMax,
fixedwingpathfollowerSettings.VerticalVelMax);
descentspeedError = descentspeedDesired - velocityState.Down;
// Error condition: plane too slow or too fast
fixedwingpathfollowerStatus.Errors.Highspeed = 0;
fixedwingpathfollowerStatus.Errors.Lowspeed = 0;
if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedwingpathfollowerSettings.Safetymargins.Overspeed) {
fixedwingpathfollowerStatus.Errors.Overspeed = 1;
result = 0;
}
if (indicatedAirspeedState > fixedwingpathfollowerSettings.HorizontalVelMax * fixedwingpathfollowerSettings.Safetymargins.Highspeed) {
fixedwingpathfollowerStatus.Errors.Highspeed = 1;
result = 0;
}
if (indicatedAirspeedState < fixedwingpathfollowerSettings.HorizontalVelMin * fixedwingpathfollowerSettings.Safetymargins.Lowspeed) {
fixedwingpathfollowerStatus.Errors.Lowspeed = 1;
result = 0;
}
if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedwingpathfollowerSettings.Safetymargins.Stallspeed) {
fixedwingpathfollowerStatus.Errors.Stallspeed = 1;
result = 0;
}
/**
* Compute desired thrust command
*/
// compute saturated integral error thrust response. Make integral leaky for better performance. Approximately 30s time constant.
if (fixedwingpathfollowerSettings.PowerPI.Ki > 0.0f) {
powerIntegral = boundf(powerIntegral + -descentspeedError * dT,
-fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki,
fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki
) * (1.0f - 1.0f / (1.0f + 30.0f / dT));
} else {
powerIntegral = 0.0f;
}
// Compute the cross feed from vertical speed to pitch, with saturation
float speedErrorToPowerCommandComponent = boundf(
(airspeedError / fixedwingpathfollowerSettings.HorizontalVelMin) * fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Kp,
-fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Max,
fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Max
);
// Compute final thrust response
powerCommand = -descentspeedError * fixedwingpathfollowerSettings.PowerPI.Kp +
powerIntegral * fixedwingpathfollowerSettings.PowerPI.Ki +
speedErrorToPowerCommandComponent;
// Output internal state to telemetry
fixedwingpathfollowerStatus.Error.Power = descentspeedError;
fixedwingpathfollowerStatus.ErrorInt.Power = powerIntegral;
fixedwingpathfollowerStatus.Command.Power = powerCommand;
// set thrust
stabDesired.Thrust = boundf(fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand,
fixedwingpathfollowerSettings.ThrustLimit.Min,
fixedwingpathfollowerSettings.ThrustLimit.Max);
// Error condition: plane cannot hold altitude at current speed.
fixedwingpathfollowerStatus.Errors.Lowpower = 0;
if (fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand >= fixedwingpathfollowerSettings.ThrustLimit.Max && // thrust at maximum
velocityState.Down > 0.0f && // we ARE going down
descentspeedDesired < 0.0f && // we WANT to go up
airspeedError > 0.0f && // we are too slow already
fixedwingpathfollowerSettings.Safetymargins.Lowpower > 0.5f) { // alarm switched on
fixedwingpathfollowerStatus.Errors.Lowpower = 1;
result = 0;
}
// Error condition: plane keeps climbing despite minimum thrust (opposite of above)
fixedwingpathfollowerStatus.Errors.Highpower = 0;
if (fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand <= fixedwingpathfollowerSettings.ThrustLimit.Min && // thrust at minimum
velocityState.Down < 0.0f && // we ARE going up
descentspeedDesired > 0.0f && // we WANT to go down
airspeedError < 0.0f && // we are too fast already
fixedwingpathfollowerSettings.Safetymargins.Highpower > 0.5f) { // alarm switched on
fixedwingpathfollowerStatus.Errors.Highpower = 1;
result = 0;
}
/**
* Compute desired pitch command
*/
if (fixedwingpathfollowerSettings.SpeedPI.Ki > 0) {
// Integrate with saturation
airspeedErrorInt = boundf(airspeedErrorInt + airspeedError * dT,
-fixedwingpathfollowerSettings.SpeedPI.ILimit / fixedwingpathfollowerSettings.SpeedPI.Ki,
fixedwingpathfollowerSettings.SpeedPI.ILimit / fixedwingpathfollowerSettings.SpeedPI.Ki);
}
// Compute the cross feed from vertical speed to pitch, with saturation
float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Kp,
-fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Max,
fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Max
);
// Compute the pitch command as err*Kp + errInt*Ki + X_feed.
pitchCommand = -(airspeedError * fixedwingpathfollowerSettings.SpeedPI.Kp
+ airspeedErrorInt * fixedwingpathfollowerSettings.SpeedPI.Ki
) + verticalSpeedToPitchCommandComponent;
fixedwingpathfollowerStatus.Error.Speed = airspeedError;
fixedwingpathfollowerStatus.ErrorInt.Speed = airspeedErrorInt;
fixedwingpathfollowerStatus.Command.Speed = pitchCommand;
stabDesired.Pitch = boundf(fixedwingpathfollowerSettings.PitchLimit.Neutral + pitchCommand,
fixedwingpathfollowerSettings.PitchLimit.Min,
fixedwingpathfollowerSettings.PitchLimit.Max);
// Error condition: high speed dive
fixedwingpathfollowerStatus.Errors.Pitchcontrol = 0;
if (fixedwingpathfollowerSettings.PitchLimit.Neutral + pitchCommand >= fixedwingpathfollowerSettings.PitchLimit.Max && // pitch demand is full up
velocityState.Down > 0.0f && // we ARE going down
descentspeedDesired < 0.0f && // we WANT to go up
airspeedError < 0.0f && // we are too fast already
fixedwingpathfollowerSettings.Safetymargins.Pitchcontrol > 0.5f) { // alarm switched on
fixedwingpathfollowerStatus.Errors.Pitchcontrol = 1;
result = 0;
}
/**
* Compute desired roll command
*/
courseError = RAD2DEG(atan2f(courseComponent[1], courseComponent[0])) - attitudeState.Yaw;
if (courseError < -180.0f) {
courseError += 360.0f;
}
if (courseError > 180.0f) {
courseError -= 360.0f;
}
// overlap calculation. Theres a dead zone behind the craft where the
// counter-yawing of some craft while rolling could render a desired right
// turn into a desired left turn. Making the turn direction based on
// current roll angle keeps the plane committed to a direction once chosen
if (courseError < -180.0f + (fixedwingpathfollowerSettings.ReverseCourseOverlap * 0.5f)
&& attitudeState.Roll > 0.0f) {
courseError += 360.0f;
}
if (courseError > 180.0f - (fixedwingpathfollowerSettings.ReverseCourseOverlap * 0.5f)
&& attitudeState.Roll < 0.0f) {
courseError -= 360.0f;
}
courseIntegral = boundf(courseIntegral + courseError * dT * fixedwingpathfollowerSettings.CoursePI.Ki,
-fixedwingpathfollowerSettings.CoursePI.ILimit,
fixedwingpathfollowerSettings.CoursePI.ILimit);
courseCommand = (courseError * fixedwingpathfollowerSettings.CoursePI.Kp +
courseIntegral);
fixedwingpathfollowerStatus.Error.Course = courseError;
fixedwingpathfollowerStatus.ErrorInt.Course = courseIntegral;
fixedwingpathfollowerStatus.Command.Course = courseCommand;
stabDesired.Roll = boundf(fixedwingpathfollowerSettings.RollLimit.Neutral +
courseCommand,
fixedwingpathfollowerSettings.RollLimit.Min,
fixedwingpathfollowerSettings.RollLimit.Max);
// TODO: find a check to determine loss of directional control. Likely needs some check of derivative
/**
* Compute desired yaw command
*/
// TODO implement raw control mode for yaw and base on Accels.Y
stabDesired.Yaw = 0.0f;
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
StabilizationDesiredSet(&stabDesired);
FixedWingPathFollowerStatusSet(&fixedwingpathfollowerStatus);
return result;
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings);
PathDesiredGet(&pathDesired);
}
static void airspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
AirspeedStateData airspeedState;
VelocityStateData velocityState;
AirspeedStateGet(&airspeedState);
VelocityStateGet(&velocityState);
float airspeedVector[2];
float yaw;
AttitudeStateYawGet(&yaw);
airspeedVector[0] = cos_lookup_deg(yaw);
airspeedVector[1] = sin_lookup_deg(yaw);
// vector projection of groundspeed on airspeed vector to handle both forward and backwards movement
float groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1];
indicatedAirspeedStateBias = airspeedState.CalibratedAirspeed - groundspeedProjection;
// note - we do fly by Indicated Airspeed (== calibrated airspeed) however
// since airspeed is updated less often than groundspeed, we use sudden
// changes to groundspeed to offset the airspeed by the same measurement.
// This has a side effect that in the absence of any airspeed updates, the
// pathfollower will fly using groundspeed.
}
/**
* Function to calculate course vector C based on airspeed s, fluid movement F
* and desired movement vector V
* parameters in: V,F,s
* parameters out: C
* returns true if a valid solution could be found for V,F,s, false if not
* C will be set to a best effort attempt either way
*/
static bool correctCourse(float *C, float *V, float *F, float s)
{
// Approach:
// Let Sc be a circle around origin marking possible movement vectors
// of the craft with airspeed s (all possible options for C)
// Let Vl be a line through the origin along movement vector V where fr any
// point v on line Vl v = k * (V / |V|) = k' * V
// Let Wl be a line parallel to Vl where for any point v on line Vl exists
// a point w on WL with w = v - F
// Then any intersection between circle Sc and line Wl represents course
// vector which would result in a movement vector
// V' = k * ( V / |V|) = k' * V
// If there is no intersection point, S is insufficient to compensate
// for F and we can only try to fly in direction of V (thus having wind drift
// but at least making progress orthogonal to wind)
s = fabsf(s);
float f = sqrtf(F[0] * F[0] + F[1] * F[1]);
// normalize Cn=V/|V|, |V| must be >0
float v = sqrtf(V[0] * V[0] + V[1] * V[1]);
if (v < 1e-6f) {
// if |V|=0, we aren't supposed to move, turn into the wind
// (this allows hovering)
C[0] = -F[0];
C[1] = -F[1];
// if desired airspeed matches fluidmovement a hover is actually
// intended so return true
return fabsf(f - s) < 1e-3f;
}
float Vn[2] = { V[0] / v, V[1] / v };
// project F on V
float fp = F[0] * Vn[0] + F[1] * Vn[1];
// find component Fo of F that is orthogonal to V
// (which is exactly the distance between Vl and Wl)
float Fo[2] = { F[0] - (fp * Vn[0]), F[1] - (fp * Vn[1]) };
float fo2 = Fo[0] * Fo[0] + Fo[1] * Fo[1];
// find k where k * Vn = C - Fo
// |C|=s is the hypothenuse in any rectangular triangle formed by k * Vn and Fo
// so k^2 + fo^2 = s^2 (since |Vn|=1)
float k2 = s * s - fo2;
if (k2 <= -1e-3f) {
// there is no solution, we will be drifted off either way
// fallback: fly stupidly in direction of V and hope for the best
C[0] = V[0];
C[1] = V[1];
return false;
} else if (k2 <= 1e-3f) {
// there is exactly one solution: -Fo
C[0] = -Fo[0];
C[1] = -Fo[1];
return true;
}
// we have two possible solutions k positive and k negative as there are
// two intersection points between Wl and Sc
// which one is better? two criteria:
// 1. we MUST move in the right direction, if any k leads to -v its invalid
// 2. we should minimize the speed error
float k = sqrt(k2);
float C1[2] = { -k * Vn[0] - Fo[0], -k * Vn[1] - Fo[1] };
float C2[2] = { k *Vn[0] - Fo[0], k * Vn[1] - Fo[1] };
// project C+F on Vn to find signed resulting movement vector length
float vp1 = (C1[0] + F[0]) * Vn[0] + (C1[1] + F[1]) * Vn[1];
float vp2 = (C2[0] + F[0]) * Vn[0] + (C2[1] + F[1]) * Vn[1];
if (vp1 >= 0.0f && fabsf(v - vp1) < fabsf(v - vp2)) {
// in this case the angle between course and resulting movement vector
// is greater than 90 degrees - so we actually fly backwards
C[0] = C1[0];
C[1] = C1[1];
return true;
}
C[0] = C2[0];
C[1] = C2[1];
if (vp2 >= 0.0f) {
// in this case the angle between course and movement vector is less than
// 90 degrees, but we do move in the right direction
return true;
} else {
// in this case we actually get driven in the opposite direction of V
// with both solutions for C
// this might be reached in headwind stronger than maximum allowed
// airspeed.
return false;
}
}

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

@ -40,26 +40,40 @@
#include "gpssettings.h"
#include "taskinfo.h"
#include "hwsettings.h"
#include "auxmagsensor.h"
#include "WorldMagModel.h"
#include "CoordinateConversions.h"
#include <pios_com.h>
#include "GPS.h"
#include "NMEA.h"
#include "UBX.h"
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
#include "inc/ubx_autoconfig.h"
#endif
#include <pios_instrumentation_helper.h>
PERF_DEFINE_COUNTER(counterBytesIn);
PERF_DEFINE_COUNTER(counterRate);
PERF_DEFINE_COUNTER(counterParse);
// ****************
// Private functions
static void gpsTask(void *parameters);
static void updateSettings();
static void updateHwSettings();
#ifdef PIOS_GPS_SETS_HOMELOCATION
static void setHomeLocation(GPSPositionSensorData *gpsData);
static float GravityAccel(float latitude, float longitude, float altitude);
#endif
#ifdef PIOS_INCLUDE_GPS_UBX_PARSER
void AuxMagSettingsUpdatedCb(UAVObjEvent *ev);
#ifndef PIOS_GPS_MINIMAL
void updateGpsSettings(UAVObjEvent *ev);
#endif
#endif
// ****************
// Private constants
@ -69,17 +83,24 @@ static float GravityAccel(float latitude, float longitude, float altitude);
// the new location with Set = true.
#define GPS_HOMELOCATION_SET_DELAY 5000
#define GPS_LOOP_DELAY_MS 6
#ifdef PIOS_GPS_SETS_HOMELOCATION
// Unfortunately need a good size stack for the WMM calculation
#define STACK_SIZE_BYTES 1024
#else
#if defined(PIOS_GPS_MINIMAL)
#define GPS_READ_BUFFER 32
#define STACK_SIZE_BYTES 500
#else
#define STACK_SIZE_BYTES 650
#endif // PIOS_GPS_MINIMAL
#endif // PIOS_GPS_SETS_HOMELOCATION
#ifndef GPS_READ_BUFFER
#define GPS_READ_BUFFER 128
#endif
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
// ****************
@ -153,8 +174,16 @@ int32_t GPSInitialize(void)
GPSTimeInitialize();
GPSSatellitesInitialize();
HomeLocationInitialize();
updateSettings();
#ifdef PIOS_INCLUDE_GPS_UBX_PARSER
AuxMagSensorInitialize();
AuxMagSettingsInitialize();
GPSExtendedStatusInitialize();
// Initialize mag parameters
AuxMagSettingsUpdatedCb(NULL);
AuxMagSettingsConnectCallback(AuxMagSettingsUpdatedCb);
#endif
updateHwSettings();
#else
if (gpsPort && gpsEnabled) {
GPSPositionSensorInitialize();
@ -166,7 +195,7 @@ int32_t GPSInitialize(void)
#if defined(PIOS_GPS_SETS_HOMELOCATION)
HomeLocationInitialize();
#endif
updateSettings();
updateHwSettings();
}
#endif /* if defined(REVOLUTION) */
@ -184,7 +213,9 @@ int32_t GPSInitialize(void)
gps_rx_buffer = NULL;
}
PIOS_Assert(gps_rx_buffer);
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
GPSSettingsConnectCallback(updateGpsSettings);
#endif
return 0;
}
@ -215,22 +246,64 @@ static void gpsTask(__attribute__((unused)) void *parameters)
timeOfLastCommandMs = timeNowMs;
GPSPositionSensorGet(&gpspositionsensor);
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
updateGpsSettings(0);
#endif
TickType_t xLastWakeTime;
xLastWakeTime = xTaskGetTickCount();
PERF_INIT_COUNTER(counterBytesIn, 0x97510001);
PERF_INIT_COUNTER(counterRate, 0x97510002);
PERF_INIT_COUNTER(counterParse, 0x97510003);
uint8_t c[GPS_READ_BUFFER];
// Loop forever
while (1) {
uint8_t c;
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX) {
char *buffer = 0;
uint16_t count = 0;
uint8_t status;
GPSPositionSensorStatusGet(&status);
ubx_autoconfig_run(&buffer, &count, status != GPSPOSITIONSENSOR_STATUS_NOGPS);
// Something to send?
if (count) {
PIOS_COM_SendBuffer(gpsPort, (uint8_t *)buffer, count);
}
}
#endif
// This blocks the task until there is something on the buffer
while (PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, xDelay) > 0) {
uint16_t cnt;
while ((cnt = PIOS_COM_ReceiveBuffer(gpsPort, c, GPS_READ_BUFFER, xDelay)) > 0) {
PERF_TIMED_SECTION_START(counterParse);
PERF_TRACK_VALUE(counterBytesIn, cnt);
PERF_MEASURE_PERIOD(counterRate);
int res;
switch (gpsSettings.DataProtocol) {
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
case GPSSETTINGS_DATAPROTOCOL_NMEA:
res = parse_nmea_stream(c, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
res = parse_nmea_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
break;
#endif
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
case GPSSETTINGS_DATAPROTOCOL_UBX:
res = parse_ubx_stream(c, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
{
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
int32_t ac_status = ubx_autoconfig_get_status();
static uint8_t lastStatus = 0;
gpspositionsensor.AutoConfigStatus =
ac_status == UBX_AUTOCONFIG_STATUS_DISABLED ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED :
ac_status == UBX_AUTOCONFIG_STATUS_DONE ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DONE :
ac_status == UBX_AUTOCONFIG_STATUS_ERROR ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR :
GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING;
if (gpspositionsensor.AutoConfigStatus != lastStatus) {
GPSPositionSensorAutoConfigStatusSet(&gpspositionsensor.AutoConfigStatus);
lastStatus = gpspositionsensor.AutoConfigStatus;
}
#endif
res = parse_ubx_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
}
break;
#endif
default:
@ -238,6 +311,7 @@ static void gpsTask(__attribute__((unused)) void *parameters)
break;
}
PERF_TIMED_SECTION_END(counterParse);
if (res == PARSER_COMPLETE) {
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
timeOfLastUpdateMs = timeNowMs;
@ -247,7 +321,8 @@ static void gpsTask(__attribute__((unused)) void *parameters)
// Check for GPS timeout
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS) {
if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS ||
(gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX && gpspositionsensor.AutoConfigStatus == GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR)) {
// we have not received any valid GPS sentences for a while.
// either the GPS is not plugged in or a hardware problem or the GPS has locked up.
uint8_t status = GPSPOSITIONSENSOR_STATUS_NOGPS;
@ -257,7 +332,7 @@ static void gpsTask(__attribute__((unused)) void *parameters)
// we appear to be receiving GPS sentences OK, we've had an update
// criteria for GPS-OK taken from this post...
// http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220
if ((gpspositionsensor.PDOP < gpsSettings.MaxPDOP) && (gpspositionsensor.Satellites >= gpsSettings.MinSattelites) &&
if ((gpspositionsensor.PDOP < gpsSettings.MaxPDOP) && (gpspositionsensor.Satellites >= gpsSettings.MinSatellites) &&
(gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) &&
(gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) {
AlarmsClear(SYSTEMALARMS_ALARM_GPS);
@ -284,6 +359,7 @@ static void gpsTask(__attribute__((unused)) void *parameters)
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL);
}
}
vTaskDelayUntil(&xLastWakeTime, GPS_LOOP_DELAY_MS / portTICK_RATE_MS);
}
}
@ -346,7 +422,7 @@ static void setHomeLocation(GPSPositionSensorData *gpsData)
* like protocol, etc. Thus the HwSettings object which contains the
* GPS port speed is used for now.
*/
static void updateSettings()
static void updateHwSettings()
{
if (gpsPort) {
// Retrieve settings
@ -376,10 +452,92 @@ static void updateSettings()
case HWSETTINGS_GPSSPEED_115200:
PIOS_COM_ChangeBaud(gpsPort, 115200);
break;
case HWSETTINGS_GPSSPEED_230400:
PIOS_COM_ChangeBaud(gpsPort, 230400);
break;
}
}
}
#ifdef PIOS_INCLUDE_GPS_UBX_PARSER
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;
uint8_t ubxDynamicModel;
uint8_t ubxSbasMode;
ubx_autoconfig_settings_t newconfig;
uint8_t ubxSbasSats;
GPSSettingsUbxRateGet(&newconfig.navRate);
GPSSettingsUbxAutoConfigGet(&ubxAutoConfig);
newconfig.autoconfigEnabled = ubxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_DISABLED ? false : true;
newconfig.storeSettings = ubxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGUREANDSTORE;
GPSSettingsUbxDynamicModelGet(&ubxDynamicModel);
newconfig.dynamicModel = ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PORTABLE ? UBX_DYNMODEL_PORTABLE :
ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_STATIONARY ? UBX_DYNMODEL_STATIONARY :
ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PEDESTRIAN ? UBX_DYNMODEL_PEDESTRIAN :
ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AUTOMOTIVE ? UBX_DYNMODEL_AUTOMOTIVE :
ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_SEA ? UBX_DYNMODEL_SEA :
ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE1G ? UBX_DYNMODEL_AIRBORNE1G :
ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE2G ? UBX_DYNMODEL_AIRBORNE2G :
ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE4G ? UBX_DYNMODEL_AIRBORNE4G :
UBX_DYNMODEL_AIRBORNE1G;
GPSSettingsUbxSBASModeGet(&ubxSbasMode);
switch ((GPSSettingsUbxSBASModeOptions)ubxSbasMode) {
case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTION:
case GPSSETTINGS_UBXSBASMODE_CORRECTION:
case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTIONINTEGRITY:
case GPSSETTINGS_UBXSBASMODE_CORRECTIONINTEGRITY:
newconfig.SBASCorrection = true;
break;
default:
newconfig.SBASCorrection = false;
}
switch ((GPSSettingsUbxSBASModeOptions)ubxSbasMode) {
case GPSSETTINGS_UBXSBASMODE_RANGING:
case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTION:
case GPSSETTINGS_UBXSBASMODE_RANGINGINTEGRITY:
case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTIONINTEGRITY:
newconfig.SBASRanging = true;
break;
default:
newconfig.SBASRanging = false;
}
switch ((GPSSettingsUbxSBASModeOptions)ubxSbasMode) {
case GPSSETTINGS_UBXSBASMODE_INTEGRITY:
case GPSSETTINGS_UBXSBASMODE_RANGINGINTEGRITY:
case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTIONINTEGRITY:
case GPSSETTINGS_UBXSBASMODE_CORRECTIONINTEGRITY:
newconfig.SBASIntegrity = true;
break;
default:
newconfig.SBASIntegrity = false;
}
GPSSettingsUbxSBASChannelsUsedGet(&newconfig.SBASChannelsUsed);
GPSSettingsUbxSBASSatsGet(&ubxSbasSats);
newconfig.SBASSats = ubxSbasSats == GPSSETTINGS_UBXSBASSATS_WAAS ? UBX_SBAS_SATS_WAAS :
ubxSbasSats == GPSSETTINGS_UBXSBASSATS_EGNOS ? UBX_SBAS_SATS_EGNOS :
ubxSbasSats == GPSSETTINGS_UBXSBASSATS_MSAS ? UBX_SBAS_SATS_MSAS :
ubxSbasSats == GPSSETTINGS_UBXSBASSATS_GAGAN ? UBX_SBAS_SATS_GAGAN :
ubxSbasSats == GPSSETTINGS_UBXSBASSATS_SDCM ? UBX_SBAS_SATS_SDCM : UBX_SBAS_SATS_AUTOSCAN;
ubx_autoconfig_set(newconfig);
}
#endif /* if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) */
#endif /* ifdef PIOS_INCLUDE_GPS_UBX_PARSER */
/**
* @}
* @}

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

@ -106,12 +106,16 @@ static const struct nmea_parser nmea_parsers[] = {
#endif // PIOS_GPS_MINIMAL
};
int parse_nmea_stream(uint8_t c, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats)
int parse_nmea_stream(uint8_t *rx, uint8_t len, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats)
{
int ret = PARSER_INCOMPLETE;
static uint8_t rx_count = 0;
static bool start_flag = false;
static bool found_cr = false;
uint8_t c;
for (int i = 0; i < len; i++) {
c = rx[i];
// detect start while acquiring stream
if (!start_flag && (c == '$')) { // NMEA identifier found
start_flag = true;
@ -128,7 +132,7 @@ int parse_nmea_stream(uint8_t c, char *gps_rx_buffer, GPSPositionSensorData *Gps
start_flag = false;
found_cr = false;
rx_count = 0;
return PARSER_OVERRUN;
ret = PARSER_OVERRUN;
} else {
gps_rx_buffer[rx_count] = c;
rx_count++;
@ -161,7 +165,7 @@ int parse_nmea_stream(uint8_t c, char *gps_rx_buffer, GPSPositionSensorData *Gps
// PIOS_DEBUG_PinHigh(2);
gpsRxStats->gpsRxChkSumError++;
// PIOS_DEBUG_PinLow(2);
return PARSER_ERROR;
ret = PARSER_ERROR;
} else { // Valid checksum, use this packet to update the GPS position
if (!NMEA_update_position(&gps_rx_buffer[1], GpsData)) {
// PIOS_DEBUG_PinHigh(2);
@ -171,10 +175,11 @@ int parse_nmea_stream(uint8_t c, char *gps_rx_buffer, GPSPositionSensorData *Gps
gpsRxStats->gpsRxReceived++;
};
return PARSER_COMPLETE;
ret = PARSER_COMPLETE;
}
}
return PARSER_INCOMPLETE;
}
return ret;
}
static const struct nmea_parser *NMEA_find_parser_by_prefix(const char *prefix)
@ -489,7 +494,7 @@ static bool nmeaProcessGPGGA(GPSPositionSensorData *GpsData, bool *gpsDataUpdate
// geoid separation
GpsData->GeoidSeparation = NMEA_real_to_float(param[11]);
GpsData->SensorType = GPSPOSITIONSENSOR_SENSORTYPE_NMEA;
return true;
}
@ -687,8 +692,8 @@ static bool nmeaProcessGPGSV(__attribute__((unused)) GPSPositionSensorData *GpsD
// Get sat info
gsv_partial.PRN[sat_index] = atoi(param[parIdx++]);
gsv_partial.Elevation[sat_index] = NMEA_real_to_float(param[parIdx++]);
gsv_partial.Azimuth[sat_index] = NMEA_real_to_float(param[parIdx++]);
gsv_partial.Elevation[sat_index] = atoi(param[parIdx++]);
gsv_partial.Azimuth[sat_index] = atoi(param[parIdx++]);
gsv_partial.SNR[sat_index] = atoi(param[parIdx++]);
#ifdef NMEA_DEBUG_GSV
DEBUG_MSG(" %d", gsv_partial.PRN[sat_index]);

289
flight/modules/GPS/UBX.c
View File

@ -30,16 +30,84 @@
#include "openpilot.h"
#include "pios.h"
#include "pios_math.h"
#include <pios_helpers.h>
#include <pios_delay.h>
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
#include "inc/UBX.h"
#include "inc/GPS.h"
#include <string.h>
#include <auxmagsupport.h>
#include "UBX.h"
#include "GPS.h"
static bool useMag = false;
static GPSPositionSensorSensorTypeOptions sensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN;
static bool usePvt = false;
static uint32_t lastPvtTime = 0;
// parse table item
typedef struct {
uint8_t msgClass;
uint8_t msgID;
void (*handler)(struct UBXPacket *, GPSPositionSensorData *GpsPosition);
} ubx_message_handler;
// parsing functions, roughly ordered by reception rate (higher rate messages on top)
static void parse_ubx_op_mag(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
static void parse_ubx_nav_pvt(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
static void parse_ubx_nav_posllh(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
static void parse_ubx_nav_velned(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
static void parse_ubx_nav_sol(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
static void parse_ubx_nav_dop(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
#ifndef PIOS_GPS_MINIMAL
static void parse_ubx_nav_timeutc(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
static void parse_ubx_nav_svinfo(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
static void parse_ubx_op_sys(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
#endif
static void parse_ubx_ack_ack(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
static void parse_ubx_ack_nak(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
static void parse_ubx_mon_ver(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
const ubx_message_handler ubx_handler_table[] = {
{ .msgClass = UBX_CLASS_OP_CUST, .msgID = UBX_ID_OP_MAG, .handler = &parse_ubx_op_mag },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_PVT, .handler = &parse_ubx_nav_pvt },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_POSLLH, .handler = &parse_ubx_nav_posllh },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_VELNED, .handler = &parse_ubx_nav_velned },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SOL, .handler = &parse_ubx_nav_sol },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_DOP, .handler = &parse_ubx_nav_dop },
#ifndef PIOS_GPS_MINIMAL
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SVINFO, .handler = &parse_ubx_nav_svinfo },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_TIMEUTC, .handler = &parse_ubx_nav_timeutc },
{ .msgClass = UBX_CLASS_OP_CUST, .msgID = UBX_ID_OP_SYS, .handler = &parse_ubx_op_sys },
#endif
{ .msgClass = UBX_CLASS_ACK, .msgID = UBX_ID_ACK_ACK, .handler = &parse_ubx_ack_ack },
{ .msgClass = UBX_CLASS_ACK, .msgID = UBX_ID_ACK_NAK, .handler = &parse_ubx_ack_nak },
{ .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_VER, .handler = &parse_ubx_mon_ver },
};
#define UBX_HANDLER_TABLE_SIZE NELEMENTS(ubx_handler_table)
// detected hw version
int32_t ubxHwVersion = -1;
// Last received Ack/Nak
struct UBX_ACK_ACK ubxLastAck;
struct UBX_ACK_NAK ubxLastNak;
// If a PVT sentence is received in the last UBX_PVT_TIMEOUT (ms) timeframe it disables VELNED/POSLLH/SOL/TIMEUTC
#define UBX_PVT_TIMEOUT (1000)
// parse incoming character stream for messages in UBX binary format
int parse_ubx_stream(uint8_t c, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats)
int parse_ubx_stream(uint8_t *rx, uint8_t len, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats)
{
int ret = PARSER_INCOMPLETE; // message not (yet) complete
enum proto_states {
START,
UBX_SY2,
@ -52,11 +120,13 @@ int parse_ubx_stream(uint8_t c, char *gps_rx_buffer, GPSPositionSensorData *GpsD
UBX_CHK2,
FINISHED
};
uint8_t c;
static enum proto_states proto_state = START;
static uint8_t rx_count = 0;
struct UBXPacket *ubx = (struct UBXPacket *)gps_rx_buffer;
for (int i = 0; i < len; i++) {
c = rx[i];
switch (proto_state) {
case START: // detect protocol
if (c == UBX_SYNC1) { // first UBX sync char found
@ -121,14 +191,14 @@ int parse_ubx_stream(uint8_t c, char *gps_rx_buffer, GPSPositionSensorData *GpsD
}
if (proto_state == START) {
return PARSER_ERROR; // parser couldn't use this byte
ret = (ret != PARSER_COMPLETE) ? PARSER_ERROR : PARSER_COMPLETE; // parser couldn't use this byte
} else if (proto_state == FINISHED) {
gpsRxStats->gpsRxReceived++;
proto_state = START;
return PARSER_COMPLETE; // message complete & processed
ret = PARSER_COMPLETE; // message complete & processed
}
return PARSER_INCOMPLETE; // message not (yet) complete
}
return ret;
}
@ -193,8 +263,13 @@ bool checksum_ubx_message(struct UBXPacket *ubx)
}
}
void parse_ubx_nav_posllh(struct UBX_NAV_POSLLH *posllh, GPSPositionSensorData *GpsPosition)
static void parse_ubx_nav_posllh(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition)
{
if (usePvt) {
return;
}
struct UBX_NAV_POSLLH *posllh = &ubx->payload.nav_posllh;
if (check_msgtracker(posllh->iTOW, POSLLH_RECEIVED)) {
if (GpsPosition->Status != GPSPOSITIONSENSOR_STATUS_NOFIX) {
GpsPosition->Altitude = (float)posllh->hMSL * 0.001f;
@ -205,8 +280,12 @@ void parse_ubx_nav_posllh(struct UBX_NAV_POSLLH *posllh, GPSPositionSensorData *
}
}
void parse_ubx_nav_sol(struct UBX_NAV_SOL *sol, GPSPositionSensorData *GpsPosition)
static void parse_ubx_nav_sol(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition)
{
if (usePvt) {
return;
}
struct UBX_NAV_SOL *sol = &ubx->payload.nav_sol;
if (check_msgtracker(sol->iTOW, SOL_RECEIVED)) {
GpsPosition->Satellites = sol->numSV;
@ -226,8 +305,10 @@ void parse_ubx_nav_sol(struct UBX_NAV_SOL *sol, GPSPositionSensorData *GpsPositi
}
}
void parse_ubx_nav_dop(struct UBX_NAV_DOP *dop, GPSPositionSensorData *GpsPosition)
static void parse_ubx_nav_dop(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition)
{
struct UBX_NAV_DOP *dop = &ubx->payload.nav_dop;
if (check_msgtracker(dop->iTOW, DOP_RECEIVED)) {
GpsPosition->HDOP = (float)dop->hDOP * 0.01f;
GpsPosition->VDOP = (float)dop->vDOP * 0.01f;
@ -235,10 +316,13 @@ void parse_ubx_nav_dop(struct UBX_NAV_DOP *dop, GPSPositionSensorData *GpsPositi
}
}
void parse_ubx_nav_velned(struct UBX_NAV_VELNED *velned, GPSPositionSensorData *GpsPosition)
static void parse_ubx_nav_velned(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition)
{
if (usePvt) {
return;
}
GPSVelocitySensorData GpsVelocity;
struct UBX_NAV_VELNED *velned = &ubx->payload.nav_velned;
if (check_msgtracker(velned->iTOW, VELNED_RECEIVED)) {
if (GpsPosition->Status != GPSPOSITIONSENSOR_STATUS_NOFIX) {
GpsVelocity.North = (float)velned->velN / 100.0f;
@ -251,9 +335,58 @@ void parse_ubx_nav_velned(struct UBX_NAV_VELNED *velned, GPSPositionSensorData *
}
}
#if !defined(PIOS_GPS_MINIMAL)
void parse_ubx_nav_timeutc(struct UBX_NAV_TIMEUTC *timeutc)
static void parse_ubx_nav_pvt(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition)
{
lastPvtTime = PIOS_DELAY_GetuS();
GPSVelocitySensorData GpsVelocity;
struct UBX_NAV_PVT *pvt = &ubx->payload.nav_pvt;
check_msgtracker(pvt->iTOW, (ALL_RECEIVED));
GpsVelocity.North = (float)pvt->velN * 0.001f;
GpsVelocity.East = (float)pvt->velE * 0.001f;
GpsVelocity.Down = (float)pvt->velD * 0.001f;
GPSVelocitySensorSet(&GpsVelocity);
GpsPosition->Groundspeed = (float)pvt->gSpeed * 0.001f;
GpsPosition->Heading = (float)pvt->heading * 1.0e-5f;
GpsPosition->Altitude = (float)pvt->hMSL * 0.001f;
GpsPosition->GeoidSeparation = (float)(pvt->height - pvt->hMSL) * 0.001f;
GpsPosition->Latitude = pvt->lat;
GpsPosition->Longitude = pvt->lon;
GpsPosition->Satellites = pvt->numSV;
GpsPosition->PDOP = pvt->pDOP * 0.01f;
if (pvt->flags & PVT_FLAGS_GNSSFIX_OK) {
GpsPosition->Status = pvt->fixType == PVT_FIX_TYPE_3D ?
GPSPOSITIONSENSOR_STATUS_FIX3D : GPSPOSITIONSENSOR_STATUS_FIX2D;
} else {
GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_NOFIX;
}
#if !defined(PIOS_GPS_MINIMAL)
if (pvt->valid & PVT_VALID_VALIDTIME) {
// Time is valid, set GpsTime
GPSTimeData GpsTime;
GpsTime.Year = pvt->year;
GpsTime.Month = pvt->month;
GpsTime.Day = pvt->day;
GpsTime.Hour = pvt->hour;
GpsTime.Minute = pvt->min;
GpsTime.Second = pvt->sec;
GPSTimeSet(&GpsTime);
}
#endif
}
#if !defined(PIOS_GPS_MINIMAL)
static void parse_ubx_nav_timeutc(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition)
{
if (usePvt) {
return;
}
struct UBX_NAV_TIMEUTC *timeutc = &ubx->payload.nav_timeutc;
// Test if time is valid
if ((timeutc->valid & TIMEUTC_VALIDTOW) && (timeutc->valid & TIMEUTC_VALIDWKN)) {
// Time is valid, set GpsTime
@ -275,16 +408,17 @@ void parse_ubx_nav_timeutc(struct UBX_NAV_TIMEUTC *timeutc)
#endif /* if !defined(PIOS_GPS_MINIMAL) */
#if !defined(PIOS_GPS_MINIMAL)
void parse_ubx_nav_svinfo(struct UBX_NAV_SVINFO *svinfo)
static void parse_ubx_nav_svinfo(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition)
{
uint8_t chan;
GPSSatellitesData svdata;
struct UBX_NAV_SVINFO *svinfo = &ubx->payload.nav_svinfo;
svdata.SatsInView = 0;
for (chan = 0; chan < svinfo->numCh; chan++) {
if (svdata.SatsInView < GPSSATELLITES_PRN_NUMELEM) {
svdata.Azimuth[svdata.SatsInView] = (float)svinfo->sv[chan].azim;
svdata.Elevation[svdata.SatsInView] = (float)svinfo->sv[chan].elev;
svdata.Azimuth[svdata.SatsInView] = svinfo->sv[chan].azim;
svdata.Elevation[svdata.SatsInView] = svinfo->sv[chan].elev;
svdata.PRN[svdata.SatsInView] = svinfo->sv[chan].svid;
svdata.SNR[svdata.SatsInView] = svinfo->sv[chan].cno;
svdata.SatsInView++;
@ -292,8 +426,8 @@ void parse_ubx_nav_svinfo(struct UBX_NAV_SVINFO *svinfo)
}
// fill remaining slots (if any)
for (chan = svdata.SatsInView; chan < GPSSATELLITES_PRN_NUMELEM; chan++) {
svdata.Azimuth[chan] = (float)0.0f;
svdata.Elevation[chan] = (float)0.0f;
svdata.Azimuth[chan] = 0;
svdata.Elevation[chan] = 0;
svdata.PRN[chan] = 0;
svdata.SNR[chan] = 0;
}
@ -302,45 +436,112 @@ void parse_ubx_nav_svinfo(struct UBX_NAV_SVINFO *svinfo)
}
#endif /* if !defined(PIOS_GPS_MINIMAL) */
static void parse_ubx_ack_ack(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition)
{
struct UBX_ACK_ACK *ack_ack = &ubx->payload.ack_ack;
ubxLastAck = *ack_ack;
}
static void parse_ubx_ack_nak(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition)
{
struct UBX_ACK_NAK *ack_nak = &ubx->payload.ack_nak;
ubxLastNak = *ack_nak;
}
static void parse_ubx_mon_ver(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition)
{
struct UBX_MON_VER *mon_ver = &ubx->payload.mon_ver;
ubxHwVersion = atoi(mon_ver->hwVersion);
sensorType = (ubxHwVersion >= 80000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX8 :
((ubxHwVersion >= 70000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX7 : GPSPOSITIONSENSOR_SENSORTYPE_UBX);
}
#if !defined(PIOS_GPS_MINIMAL)
static void parse_ubx_op_sys(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition)
{
struct UBX_OP_SYSINFO *sysinfo = &ubx->payload.op_sysinfo;
GPSExtendedStatusData data;
data.FlightTime = sysinfo->flightTime;
data.BoardType[0] = sysinfo->board_type;
data.BoardType[1] = sysinfo->board_revision;
memcpy(&data.FirmwareHash, &sysinfo->sha1sum, GPSEXTENDEDSTATUS_FIRMWAREHASH_NUMELEM);
memcpy(&data.FirmwareTag, &sysinfo->commit_tag_name, GPSEXTENDEDSTATUS_FIRMWARETAG_NUMELEM);
data.Options = sysinfo->options;
data.Status = GPSEXTENDEDSTATUS_STATUS_GPSV9;
GPSExtendedStatusSet(&data);
}
#endif
static void parse_ubx_op_mag(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition)
{
if (!useMag) {
return;
}
struct UBX_OP_MAG *mag = &ubx->payload.op_mag;
float mags[3] = { mag->x, mag->y, mag->z };
auxmagsupport_publish_samples(mags, AUXMAGSENSOR_STATUS_OK);
}
// UBX message parser
// returns UAVObjectID if a UAVObject structure is ready for further processing
uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition)
{
uint32_t id = 0;
static bool ubxInitialized = false;
switch (ubx->header.class) {
case UBX_CLASS_NAV:
switch (ubx->header.id) {
case UBX_ID_POSLLH:
parse_ubx_nav_posllh(&ubx->payload.nav_posllh, GpsPosition);
break;
case UBX_ID_DOP:
parse_ubx_nav_dop(&ubx->payload.nav_dop, GpsPosition);
break;
case UBX_ID_SOL:
parse_ubx_nav_sol(&ubx->payload.nav_sol, GpsPosition);
break;
case UBX_ID_VELNED:
parse_ubx_nav_velned(&ubx->payload.nav_velned, GpsPosition);
break;
#if !defined(PIOS_GPS_MINIMAL)
case UBX_ID_TIMEUTC:
parse_ubx_nav_timeutc(&ubx->payload.nav_timeutc);
break;
case UBX_ID_SVINFO:
parse_ubx_nav_svinfo(&ubx->payload.nav_svinfo);
break;
#endif
if (!ubxInitialized) {
// initialize dop values. If no DOP sentence is received it is safer to initialize them to a high value rather than 0.
GpsPosition->HDOP = 99.99f;
GpsPosition->PDOP = 99.99f;
GpsPosition->VDOP = 99.99f;
ubxInitialized = true;
}
// is it using PVT?
usePvt = (lastPvtTime) && (PIOS_DELAY_GetuSSince(lastPvtTime) < UBX_PVT_TIMEOUT * 1000);
for (uint8_t i = 0; i < UBX_HANDLER_TABLE_SIZE; i++) {
const ubx_message_handler *handler = &ubx_handler_table[i];
if (handler->msgClass == ubx->header.class && handler->msgID == ubx->header.id) {
handler->handler(ubx, GpsPosition);
break;
}
}
GpsPosition->SensorType = sensorType;
if (msgtracker.msg_received == ALL_RECEIVED) {
GPSPositionSensorSet(GpsPosition);
msgtracker.msg_received = NONE_RECEIVED;
id = GPSPOSITIONSENSOR_OBJID;
} else {
uint8_t status;
GPSPositionSensorStatusGet(&status);
if (status == GPSPOSITIONSENSOR_STATUS_NOGPS) {
// Some ubx thing has been received so GPS is there
status = GPSPOSITIONSENSOR_STATUS_NOFIX;
GPSPositionSensorStatusSet(&status);
}
}
return id;
}
void load_mag_settings()
{
auxmagsupport_reload_settings();
if (auxmagsupport_get_type() != AUXMAGSETTINGS_TYPE_GPSV9) {
useMag = false;
const uint8_t status = AUXMAGSENSOR_STATUS_NONE;
// next sample from other external mags will provide the right status if present
AuxMagSensorStatusSet((uint8_t *)&status);
} else {
useMag = true;
}
}
#endif // PIOS_INCLUDE_GPS_UBX_PARSER

1
flight/modules/GPS/inc/GPS.h
View File

@ -38,6 +38,7 @@
#include "gpssatellites.h"
#include "gpspositionsensor.h"
#include "gpstime.h"
#include "auxmagsettings.h"
#define NO_PARSER -3 // no parser available
#define PARSER_OVERRUN -2 // message buffer overrun before completing the message

2
flight/modules/GPS/inc/NMEA.h
View File

@ -39,6 +39,6 @@
extern bool NMEA_update_position(char *nmea_sentence, GPSPositionSensorData *GpsData);
extern bool NMEA_checksum(char *nmea_sentence);
extern int parse_nmea_stream(uint8_t, char *, GPSPositionSensorData *, struct GPS_RX_STATS *);
extern int parse_nmea_stream(uint8_t *, uint8_t, char *, GPSPositionSensorData *, struct GPS_RX_STATS *);
#endif /* NMEA_H */

201
flight/modules/GPS/inc/UBX.h
View File

@ -32,8 +32,12 @@
#define UBX_H
#include "openpilot.h"
#include "gpspositionsensor.h"
#include "gpsextendedstatus.h"
#include "auxmagsensor.h"
#include "GPS.h"
#define UBX_HW_VERSION_8 80000
#define UBX_HW_VERSION_7 70000
#define UBX_SYNC1 0xb5 // UBX protocol synchronization characters
#define UBX_SYNC2 0x62
@ -41,17 +45,86 @@
// From u-blox6 receiver protocol specification
// Messages classes
#define UBX_CLASS_NAV 0x01
typedef enum {
UBX_CLASS_NAV = 0x01,
UBX_CLASS_ACK = 0x05,
UBX_CLASS_CFG = 0x06,
UBX_CLASS_MON = 0x0A,
UBX_CLASS_OP_CUST = 0x99,
UBX_CLASS_AID = 0x0B,
// unused class IDs, used for disabling them
UBX_CLASS_RXM = 0x02,
} ubx_class;
// Message IDs
#define UBX_ID_POSLLH 0x02
#define UBX_ID_STATUS 0x03
#define UBX_ID_DOP 0x04
#define UBX_ID_SOL 0x06
#define UBX_ID_VELNED 0x12
#define UBX_ID_TIMEUTC 0x21
#define UBX_ID_SVINFO 0x30
typedef enum {
UBX_ID_NAV_POSLLH = 0x02,
UBX_ID_NAV_STATUS = 0x03,
UBX_ID_NAV_DOP = 0x04,
UBX_ID_NAV_SOL = 0x06,
UBX_ID_NAV_VELNED = 0x12,
UBX_ID_NAV_TIMEUTC = 0x21,
UBX_ID_NAV_SVINFO = 0x30,
UBX_ID_NAV_PVT = 0x07,
UBX_ID_NAV_AOPSTATUS = 0x60,
UBX_ID_NAV_CLOCK = 0x22,
UBX_ID_NAV_DGPS = 0x31,
UBX_ID_NAV_POSECEF = 0x01,
UBX_ID_NAV_SBAS = 0x32,
UBX_ID_NAV_TIMEGPS = 0x20,
UBX_ID_NAV_VELECEF = 0x11
} ubx_class_nav_id;
typedef enum {
UBX_ID_OP_SYS = 0x01,
UBX_ID_OP_MAG = 0x02,
} ubx_class_op_id;
typedef enum {
UBX_ID_MON_VER = 0x04,
// unused messages IDs, used for disabling them
UBX_ID_MON_HW2 = 0x0B,
UBX_ID_MON_HW = 0x09,
UBX_ID_MON_IO = 0x02,
UBX_ID_MON_MSGPP = 0x06,
UBX_ID_MON_RXBUFF = 0x07,
UBX_ID_MON_RXR = 0x21,
UBX_ID_MON_TXBUF = 0x08,
} ubx_class_mon_id;
typedef enum {
UBX_ID_CFG_NAV5 = 0x24,
UBX_ID_CFG_RATE = 0x08,
UBX_ID_CFG_MSG = 0x01,
UBX_ID_CFG_CFG = 0x09,
UBX_ID_CFG_SBAS = 0x16,
} ubx_class_cfg_id;
typedef enum {
UBX_ID_ACK_ACK = 0x01,
UBX_ID_ACK_NAK = 0x00,
} ubx_class_ack_id;
typedef enum {
UBX_ID_AID_ALM = 0x0B,
UBX_ID_AID_ALPSRV = 0x32,
UBX_ID_AID_ALP = 0x50,
UBX_ID_AID_AOP = 0x33,
UBX_ID_AID_DATA = 0x10,
UBX_ID_AID_EPH = 0x31,
UBX_ID_AID_HUI = 0x02,
UBX_ID_AID_INI = 0x01,
UBX_ID_AID_REQ = 0x00,
} ubx_class_aid_id;
typedef enum {
UBX_ID_RXM_ALM = 0x30,
UBX_ID_RXM_EPH = 0x31,
UBX_ID_RXM_RAW = 0x10,
UBX_ID_RXM_SFRB = 0x11,
UBX_ID_RXM_SVSI = 0x20,
} ubx_class_rxm_id;
// private structures
// Geodetic Position Solution
@ -156,6 +229,59 @@ struct UBX_NAV_TIMEUTC {
uint8_t valid; // Validity Flags
};
#define PVT_VALID_VALIDDATE 0x01
#define PVT_VALID_VALIDTIME 0x02
#define PVT_VALID_FULLYRESOLVED 0x04
#define PVT_FIX_TYPE_NO_FIX 0
#define PVT_FIX_TYPE_DEAD_RECKON 0x01 // Dead Reckoning only
#define PVT_FIX_TYPE_2D 0x02 // 2D-Fix
#define PVT_FIX_TYPE_3D 0x03 // 3D-Fix
#define PVT_FIX_TYPE_GNSS_DEAD_RECKON 0x04 // GNSS + dead reckoning combined
#define PVT_FIX_TYPE_TIME_ONLY 0x05 // Time only fix
#define PVT_FLAGS_GNSSFIX_OK (1 << 0)
#define PVT_FLAGS_DIFFSOLN (1 << 1)
#define PVT_FLAGS_PSMSTATE_ENABLED (1 << 2)
#define PVT_FLAGS_PSMSTATE_ACQUISITION (2 << 2)
#define PVT_FLAGS_PSMSTATE_TRACKING (3 << 2)
#define PVT_FLAGS_PSMSTATE_PO_TRACKING (4 << 2)
#define PVT_FLAGS_PSMSTATE_INACTIVE (5 << 2)
// PVT Navigation Position Velocity Time Solution
struct UBX_NAV_PVT {
uint32_t iTOW;
uint16_t year;
uint8_t month;
uint8_t day;
uint8_t hour;
uint8_t min;
uint8_t sec;
uint8_t valid;
uint32_t tAcc;
int32_t nano;
uint8_t fixType;
uint8_t flags;
uint8_t reserved1;
uint8_t numSV;
int32_t lon;
int32_t lat;
int32_t height;
int32_t hMSL;
uint32_t hAcc;
uint32_t vAcc;
int32_t velN;
int32_t velE;
int32_t velD;
int32_t gSpeed;
int32_t heading;
uint32_t sAcc;
uint32_t headingAcc;
uint16_t pDOP;
uint16_t reserved2;
uint32_t reserved3;
} __attribute__((packed));
// Space Vehicle (SV) Information
// Single SV information block
@ -191,17 +317,67 @@ struct UBX_NAV_SVINFO {
struct UBX_NAV_SVINFO_SV sv[MAX_SVS]; // Repeated 'numCh' times
};
// ACK message class
struct UBX_ACK_ACK {
uint8_t clsID; // ClassID
uint8_t msgID; // MessageID
};
struct UBX_ACK_NAK {
uint8_t clsID; // ClassID
uint8_t msgID; // MessageID
};
// MON message Class
#define UBX_MON_MAX_EXT 5
struct UBX_MON_VER {
char swVersion[30];
char hwVersion[10];
#if UBX_MON_MAX_EXT > 0
char extension[UBX_MON_MAX_EXT][30];
#endif
};
// OP custom messages
struct UBX_OP_SYSINFO {
uint32_t flightTime;
uint16_t options;
uint8_t board_type;
uint8_t board_revision;
uint8_t commit_tag_name[26];
uint8_t sha1sum[8];
} __attribute__((packed));
// OP custom messages
struct UBX_OP_MAG {
int16_t x;
int16_t y;
int16_t z;
uint16_t Status;
};
typedef union {
uint8_t payload[0];
// Nav Class
struct UBX_NAV_POSLLH nav_posllh;
struct UBX_NAV_STATUS nav_status;
struct UBX_NAV_DOP nav_dop;
struct UBX_NAV_SOL nav_sol;
struct UBX_NAV_VELNED nav_velned;
struct UBX_NAV_PVT nav_pvt;
#if !defined(PIOS_GPS_MINIMAL)
struct UBX_NAV_TIMEUTC nav_timeutc;
struct UBX_NAV_SVINFO nav_svinfo;
#endif
// Ack Class
struct UBX_ACK_ACK ack_ack;
struct UBX_ACK_NAK ack_nak;
// Mon Class
struct UBX_MON_VER mon_ver;
struct UBX_OP_SYSINFO op_sysinfo;
struct UBX_OP_MAG op_mag;
} UBXPayload;
struct UBXHeader {
@ -217,8 +393,15 @@ struct UBXPacket {
UBXPayload payload;
};
// Used by AutoConfig code
extern int32_t ubxHwVersion;
extern struct UBX_ACK_ACK ubxLastAck;
extern struct UBX_ACK_NAK ubxLastNak;
bool checksum_ubx_message(struct UBXPacket *);
uint32_t parse_ubx_message(struct UBXPacket *, GPSPositionSensorData *);
int parse_ubx_stream(uint8_t, char *, GPSPositionSensorData *, struct GPS_RX_STATS *);
int parse_ubx_stream(uint8_t *rx, uint8_t len, char *, GPSPositionSensorData *, struct GPS_RX_STATS *);
void load_mag_settings();
#endif /* UBX_H */

192
flight/modules/GPS/inc/ubx_autoconfig.h Normal file
View File

@ -0,0 +1,192 @@
/**
******************************************************************************
*
* @file %FILENAME%
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @addtogroup [Group]
* @{
* @addtogroup %CLASS%
* @{
* @brief [Brief]
*****************************************************************************/
/*
* 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 UBX_AUTOCONFIG_H_
#define UBX_AUTOCONFIG_H_
#include "UBX.h"
#include <stdint.h>
#include <stdbool.h>
// defines
// TODO: NEO8 max rate is for Rom version, flash is limited to 10Hz, need to handle that.
#define UBX_MAX_RATE_VER8 18
#define UBX_MAX_RATE_VER7 10
#define UBX_MAX_RATE 5
// time to wait before reinitializing the fsm due to disconnection
#define UBX_CONNECTION_TIMEOUT (2000 * 1000)
// times between retries in case an error does occurs
#define UBX_ERROR_RETRY_TIMEOUT (1000 * 1000)
// timeout for ack reception
#define UBX_REPLY_TIMEOUT (500 * 1000)
// max retries in case of timeout
#define UBX_MAX_RETRIES 5
// pause between each configuration step
#define UBX_STEP_WAIT_TIME (10 * 1000)
// types
typedef enum {
UBX_AUTOCONFIG_STATUS_DISABLED = 0,
UBX_AUTOCONFIG_STATUS_RUNNING,
UBX_AUTOCONFIG_STATUS_DONE,
UBX_AUTOCONFIG_STATUS_ERROR
} ubx_autoconfig_status_t;
// Enumeration options for field UBXDynamicModel
typedef enum {
UBX_DYNMODEL_PORTABLE = 0,
UBX_DYNMODEL_STATIONARY = 2,
UBX_DYNMODEL_PEDESTRIAN = 3,
UBX_DYNMODEL_AUTOMOTIVE = 4,
UBX_DYNMODEL_SEA = 5,
UBX_DYNMODEL_AIRBORNE1G = 6,
UBX_DYNMODEL_AIRBORNE2G = 7,
UBX_DYNMODEL_AIRBORNE4G = 8
} ubx_config_dynamicmodel_t;
typedef enum {
UBX_SBAS_SATS_AUTOSCAN = 0,
UBX_SBAS_SATS_WAAS = 1,
UBX_SBAS_SATS_EGNOS = 2,
UBX_SBAS_SATS_MSAS = 3,
UBX_SBAS_SATS_GAGAN = 4,
UBX_SBAS_SATS_SDCM = 5
} ubx_config_sats_t;
#define UBX_
typedef struct {
bool autoconfigEnabled;
bool storeSettings;
bool SBASRanging;
bool SBASCorrection;
bool SBASIntegrity;
ubx_config_sats_t SBASSats;
uint8_t SBASChannelsUsed;
int8_t navRate;
ubx_config_dynamicmodel_t dynamicModel;
} ubx_autoconfig_settings_t;
// Mask for "all supported devices": battery backed RAM, Flash, EEPROM, SPI Flash
#define UBX_CFG_CFG_ALL_DEVICES_MASK (0x01 | 0x02 | 0x04 | 0x10)
// Sent messages for configuration support
typedef struct {
uint32_t clearMask;
uint32_t saveMask;
uint32_t loadMask;
uint8_t deviceMask;
} __attribute__((packed)) ubx_cfg_cfg_t;
typedef struct {
uint16_t mask;
uint8_t dynModel;
uint8_t fixMode;
int32_t fixedAlt;
uint32_t fixedAltVar;
int8_t minElev;
uint8_t drLimit;
uint16_t pDop;
uint16_t tDop;
uint16_t pAcc;
uint16_t tAcc;
uint8_t staticHoldThresh;
uint8_t dgpsTimeOut;
uint8_t cnoThreshNumSVs;
uint8_t cnoThresh;
uint16_t reserved2;
uint32_t reserved3;
uint32_t reserved4;
} __attribute__((packed)) ubx_cfg_nav5_t;
typedef struct {
uint16_t measRate;
uint16_t navRate;
uint16_t timeRef;
} __attribute__((packed)) ubx_cfg_rate_t;
typedef struct {
uint8_t msgClass;
uint8_t msgID;
uint8_t rate;
} __attribute__((packed)) ubx_cfg_msg_t;
#define UBX_CFG_SBAS_MODE_ENABLED 0x01
#define UBX_CFG_SBAS_MODE_TEST 0x02
#define UBX_CFG_SBAS_USAGE_RANGE 0x01
#define UBX_CFG_SBAS_USAGE_DIFFCORR 0x02
#define UBX_CFG_SBAS_USAGE_INTEGRITY 0x04
// SBAS used satellite PNR bitmask (120-151)
// -------------------------------------1---------1---------1---------1
// -------------------------------------5---------4---------3---------2
// ------------------------------------10987654321098765432109876543210
// WAAS 122, 133, 134, 135, 138---------|---------|---------|---------|
#define UBX_CFG_SBAS_SCANMODE1_WAAS 0b00000000000001001110000000000100
// EGNOS 120, 124, 126, 131-------------|---------|---------|---------|
#define UBX_CFG_SBAS_SCANMODE1_EGNOS 0b00000000000000000000100001010001
// MSAS 129, 137------------------------|---------|---------|---------|
#define UBX_CFG_SBAS_SCANMODE1_MSAS 0b00000000000000100000001000000000
// GAGAN 127, 128-----------------------|---------|---------|---------|
#define UBX_CFG_SBAS_SCANMODE1_GAGAN 0b00000000000000000000000110000000
// SDCM 125, 140, 141-------------------|---------|---------|---------|
#define UBX_CFG_SBAS_SCANMODE1_SDCM 0b00000000001100000000000000100000
#define UBX_CFG_SBAS_SCANMODE2 0x00
typedef struct {
uint8_t mode;
uint8_t usage;
uint8_t maxSBAS;
uint8_t scanmode2;
uint32_t scanmode1;
} __attribute__((packed)) ubx_cfg_sbas_t;
typedef struct {
uint8_t prolog[2];
uint8_t class;
uint8_t id;
uint16_t len;
} __attribute__((packed)) UBXSentHeader_t;
typedef union {
uint8_t buffer[0];
struct {
UBXSentHeader_t header;
union {
ubx_cfg_cfg_t cfg_cfg;
ubx_cfg_msg_t cfg_msg;
ubx_cfg_nav5_t cfg_nav5;
ubx_cfg_rate_t cfg_rate;
ubx_cfg_sbas_t cfg_sbas;
} payload;
uint8_t resvd[2]; // added space for checksum bytes
} message;
} __attribute__((packed)) UBXSentPacket_t;
void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send, bool gps_connected);
void ubx_autoconfig_set(ubx_autoconfig_settings_t config);
int32_t ubx_autoconfig_get_status();
#endif /* UBX_AUTOCONFIG_H_ */

434
flight/modules/GPS/ubx_autoconfig.c Normal file
View File

@ -0,0 +1,434 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup GSPModule GPS Module
* @brief Support code for UBX AutoConfig
* @{
*
* @file ubx_autoconfig.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Support code for UBX AutoConfig
* @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 "inc/ubx_autoconfig.h"
#include <pios_mem.h>
// private type definitions
typedef enum {
INIT_STEP_DISABLED = 0,
INIT_STEP_START,
INIT_STEP_ASK_VER,
INIT_STEP_WAIT_VER,
INIT_STEP_ENABLE_SENTENCES,
INIT_STEP_ENABLE_SENTENCES_WAIT_ACK,
INIT_STEP_CONFIGURE,
INIT_STEP_CONFIGURE_WAIT_ACK,
INIT_STEP_DONE,
INIT_STEP_ERROR,
} initSteps_t;
typedef struct {
initSteps_t currentStep; // Current configuration "fsm" status
uint32_t lastStepTimestampRaw; // timestamp of last operation
uint32_t lastConnectedRaw; // timestamp of last time gps was connected
UBXSentPacket_t working_packet; // outbound "buffer"
ubx_autoconfig_settings_t currentSettings;
int8_t lastConfigSent; // index of last configuration string sent
struct UBX_ACK_ACK requiredAck; // Class and id of the message we are waiting for an ACK from GPS
uint8_t retryCount;
} status_t;
ubx_cfg_msg_t msg_config_ubx6[] = {
// messages to disable
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_CLOCK, .rate = 0 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_POSECEF, .rate = 0 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SBAS, .rate = 0 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_TIMEGPS, .rate = 0 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_VELECEF, .rate = 0 },
{ .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_HW, .rate = 0 },
{ .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_HW2, .rate = 0 },
{ .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_IO, .rate = 0 },
{ .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_MSGPP, .rate = 0 },
{ .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_RXBUFF, .rate = 0 },
{ .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_RXR, .rate = 0 },
{ .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_TXBUF, .rate = 0 },
{ .msgClass = UBX_CLASS_RXM, .msgID = UBX_ID_RXM_SVSI, .rate = 0 },
// message to enable
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_POSLLH, .rate = 1 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_DOP, .rate = 1 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SOL, .rate = 1 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_STATUS, .rate = 1 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_VELNED, .rate = 1 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_TIMEUTC, .rate = 1 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SVINFO, .rate = 10 },
};
ubx_cfg_msg_t msg_config_ubx7[] = {
// messages to disable
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_AOPSTATUS, .rate = 0 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_CLOCK, .rate = 0 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_DGPS, .rate = 0 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_POSECEF, .rate = 0 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SBAS, .rate = 0 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_TIMEGPS, .rate = 0 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_VELECEF, .rate = 0 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SOL, .rate = 0 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_STATUS, .rate = 0 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_VELNED, .rate = 0 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_TIMEUTC, .rate = 0 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_POSLLH, .rate = 0 },
{ .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_HW, .rate = 0 },
{ .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_HW2, .rate = 0 },
{ .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_IO, .rate = 0 },
{ .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_MSGPP, .rate = 0 },
{ .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_RXBUFF, .rate = 0 },
{ .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_RXR, .rate = 0 },
{ .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_TXBUF, .rate = 0 },
{ .msgClass = UBX_CLASS_RXM, .msgID = UBX_ID_RXM_SVSI, .rate = 0 },
// message to enable
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_PVT, .rate = 1 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_DOP, .rate = 1 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SVINFO, .rate = 10 },
};
// private defines
#define LAST_CONFIG_SENT_START (-1)
#define LAST_CONFIG_SENT_COMPLETED (-2)
// private variables
// enable the autoconfiguration system
static bool enabled;
static status_t *status = 0;
static void append_checksum(UBXSentPacket_t *packet)
{
uint8_t i;
uint8_t ck_a = 0;
uint8_t ck_b = 0;
uint16_t len = packet->message.header.len + sizeof(UBXSentHeader_t);
for (i = 2; i < len; i++) {
ck_a += packet->buffer[i];
ck_b += ck_a;
}
packet->buffer[len] = ck_a;
packet->buffer[len + 1] = ck_b;
}
/**
* prepare a packet to be sent, fill the header and appends the checksum.
* return the total packet lenght comprising header and checksum
*/
static uint16_t prepare_packet(UBXSentPacket_t *packet, uint8_t classID, uint8_t messageID, uint16_t len)
{
packet->message.header.prolog[0] = UBX_SYNC1;
packet->message.header.prolog[1] = UBX_SYNC2;
packet->message.header.class = classID;
packet->message.header.id = messageID;
packet->message.header.len = len;
append_checksum(packet);
return packet->message.header.len + sizeof(UBXSentHeader_t) + 2; // header + payload + checksum
}
static void build_request(UBXSentPacket_t *packet, uint8_t classID, uint8_t messageID, uint16_t *bytes_to_send)
{
*bytes_to_send = prepare_packet(packet, classID, messageID, 0);
}
void config_rate(uint16_t *bytes_to_send)
{
memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_rate_t));
// if rate is less than 1 uses the highest rate for current hardware
uint16_t rate = status->currentSettings.navRate > 0 ? status->currentSettings.navRate : 99;
if (ubxHwVersion < UBX_HW_VERSION_7 && rate > UBX_MAX_RATE) {
rate = UBX_MAX_RATE;
} else if (ubxHwVersion < UBX_HW_VERSION_8 && rate > UBX_MAX_RATE_VER7) {
rate = UBX_MAX_RATE_VER7;
} else if (ubxHwVersion >= UBX_HW_VERSION_8 && rate > UBX_MAX_RATE_VER8) {
rate = UBX_MAX_RATE_VER8;
}
uint16_t period = 1000 / rate;
status->working_packet.message.payload.cfg_rate.measRate = period;
status->working_packet.message.payload.cfg_rate.navRate = 1; // must be set to 1
status->working_packet.message.payload.cfg_rate.timeRef = 1; // 0 = UTC Time, 1 = GPS Time
*bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_RATE, sizeof(ubx_cfg_rate_t));
status->requiredAck.clsID = UBX_CLASS_CFG;
status->requiredAck.msgID = UBX_ID_CFG_RATE;
}
void config_nav(uint16_t *bytes_to_send)
{
memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_nav5_t));
status->working_packet.message.payload.cfg_nav5.dynModel = status->currentSettings.dynamicModel;
status->working_packet.message.payload.cfg_nav5.fixMode = 2; // 1=2D only, 2=3D only, 3=Auto 2D/3D
// mask LSB=dyn|minEl|posFixMode|drLim|posMask|statisticHoldMask|dgpsMask|......|reservedBit0 = MSB
status->working_packet.message.payload.cfg_nav5.mask = 0x01 + 0x04; // Dyn Model | posFixMode configuration
*bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_NAV5, sizeof(ubx_cfg_nav5_t));
status->requiredAck.clsID = UBX_CLASS_CFG;
status->requiredAck.msgID = UBX_ID_CFG_NAV5;
}
void config_sbas(uint16_t *bytes_to_send)
{
memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_sbas_t));
status->working_packet.message.payload.cfg_sbas.maxSBAS = status->currentSettings.SBASChannelsUsed < 4 ?
status->currentSettings.SBASChannelsUsed : 3;
status->working_packet.message.payload.cfg_sbas.usage =
(status->currentSettings.SBASCorrection ? UBX_CFG_SBAS_USAGE_DIFFCORR : 0) |
(status->currentSettings.SBASIntegrity ? UBX_CFG_SBAS_USAGE_INTEGRITY : 0) |
(status->currentSettings.SBASRanging ? UBX_CFG_SBAS_USAGE_RANGE : 0);
// If sbas is used for anything then set mode as enabled
status->working_packet.message.payload.cfg_sbas.mode =
status->working_packet.message.payload.cfg_sbas.usage != 0 ? UBX_CFG_SBAS_MODE_ENABLED : 0;
status->working_packet.message.payload.cfg_sbas.scanmode1 =
status->currentSettings.SBASSats == UBX_SBAS_SATS_WAAS ? UBX_CFG_SBAS_SCANMODE1_WAAS :
status->currentSettings.SBASSats == UBX_SBAS_SATS_EGNOS ? UBX_CFG_SBAS_SCANMODE1_EGNOS :
status->currentSettings.SBASSats == UBX_SBAS_SATS_MSAS ? UBX_CFG_SBAS_SCANMODE1_MSAS :
status->currentSettings.SBASSats == UBX_SBAS_SATS_GAGAN ? UBX_CFG_SBAS_SCANMODE1_GAGAN :
status->currentSettings.SBASSats == UBX_SBAS_SATS_SDCM ? UBX_CFG_SBAS_SCANMODE1_SDCM : UBX_SBAS_SATS_AUTOSCAN;
status->working_packet.message.payload.cfg_sbas.scanmode2 = UBX_CFG_SBAS_SCANMODE2;
*bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_SBAS, sizeof(ubx_cfg_sbas_t));
status->requiredAck.clsID = UBX_CLASS_CFG;
status->requiredAck.msgID = UBX_ID_CFG_SBAS;
}
void config_save(uint16_t *bytes_to_send)
{
memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_cfg_t));
// mask LSB=ioPort|msgConf|infMsg|navConf|rxmConf|||||rinvConf|antConf|....|= MSB
status->working_packet.message.payload.cfg_cfg.saveMask = 0x02 | 0x08; // msgConf + navConf
status->working_packet.message.payload.cfg_cfg.deviceMask = UBX_CFG_CFG_ALL_DEVICES_MASK;
*bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_CFG, sizeof(ubx_cfg_cfg_t));
status->requiredAck.clsID = UBX_CLASS_CFG;
status->requiredAck.msgID = UBX_ID_CFG_CFG;
}
static void configure(uint16_t *bytes_to_send)
{
switch (status->lastConfigSent) {
case LAST_CONFIG_SENT_START:
config_rate(bytes_to_send);
break;
case LAST_CONFIG_SENT_START + 1:
config_nav(bytes_to_send);
break;
case LAST_CONFIG_SENT_START + 2:
config_sbas(bytes_to_send);
if (!status->currentSettings.storeSettings) {
// skips saving
status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED;
}
break;
case LAST_CONFIG_SENT_START + 3:
config_save(bytes_to_send);
status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED;
return;
default:
status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED;
return;
}
}
static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send)
{
int8_t msg = status->lastConfigSent + 1;
uint8_t msg_count = (ubxHwVersion >= UBX_HW_VERSION_7) ?
NELEMENTS(msg_config_ubx7) : NELEMENTS(msg_config_ubx6);
ubx_cfg_msg_t *msg_config = (ubxHwVersion >= UBX_HW_VERSION_7) ?
&msg_config_ubx7[0] : &msg_config_ubx6[0];
if (msg >= 0 && msg < msg_count) {
status->working_packet.message.payload.cfg_msg = msg_config[msg];
*bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_MSG, sizeof(ubx_cfg_msg_t));
status->requiredAck.clsID = UBX_CLASS_CFG;
status->requiredAck.msgID = UBX_ID_CFG_MSG;
} else {
status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED;
}
}
void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send, bool gps_connected)
{
*bytes_to_send = 0;
*buffer = (char *)status->working_packet.buffer;
if (!status || !enabled || status->currentStep == INIT_STEP_DISABLED) {
return; // autoconfig not enabled
}
if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_STEP_WAIT_TIME) {
return;
}
// when gps is disconnected it will replay the autoconfig sequence.
if (!gps_connected) {
if (status->currentStep == INIT_STEP_DONE) {
// if some operation is in progress and it is not running into issues it maybe that
// the disconnection is only due to wrong message rates, so reinit only when done.
// errors caused by disconnection are handled by error retry logic
if (PIOS_DELAY_DiffuS(status->lastConnectedRaw) > UBX_CONNECTION_TIMEOUT) {
status->currentStep = INIT_STEP_START;
return;
}
}
} else {
// reset connection timer
status->lastConnectedRaw = PIOS_DELAY_GetRaw();
}
switch (status->currentStep) {
case INIT_STEP_ERROR:
if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) > UBX_ERROR_RETRY_TIMEOUT) {
status->currentStep = INIT_STEP_START;
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
}
return;
case INIT_STEP_DISABLED:
case INIT_STEP_DONE:
return;
case INIT_STEP_START:
case INIT_STEP_ASK_VER:
// clear ack
ubxLastAck.clsID = 0x00;
ubxLastAck.msgID = 0x00;
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
build_request(&status->working_packet, UBX_CLASS_MON, UBX_ID_MON_VER, bytes_to_send);
status->currentStep = INIT_STEP_WAIT_VER;
break;
case INIT_STEP_WAIT_VER:
if (ubxHwVersion > 0) {
status->lastConfigSent = LAST_CONFIG_SENT_START;
status->currentStep = INIT_STEP_ENABLE_SENTENCES;
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
return;
}
if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) > UBX_REPLY_TIMEOUT) {
status->currentStep = INIT_STEP_ASK_VER;
}
return;
case INIT_STEP_ENABLE_SENTENCES:
case INIT_STEP_CONFIGURE:
{
bool step_configure = (status->currentStep == INIT_STEP_CONFIGURE);
if (step_configure) {
configure(bytes_to_send);
} else {
enable_sentences(bytes_to_send);
}
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
if (status->lastConfigSent == LAST_CONFIG_SENT_COMPLETED) {
status->lastConfigSent = LAST_CONFIG_SENT_START;
status->currentStep = step_configure ? INIT_STEP_DONE : INIT_STEP_CONFIGURE;
} else {
status->currentStep = step_configure ? INIT_STEP_CONFIGURE_WAIT_ACK : INIT_STEP_ENABLE_SENTENCES_WAIT_ACK;
}
return;
}
case INIT_STEP_ENABLE_SENTENCES_WAIT_ACK:
case INIT_STEP_CONFIGURE_WAIT_ACK: // Wait for an ack from GPS
{
bool step_configure = (status->currentStep == INIT_STEP_CONFIGURE_WAIT_ACK);
if (ubxLastAck.clsID == status->requiredAck.clsID && ubxLastAck.msgID == status->requiredAck.msgID) {
// clear ack
ubxLastAck.clsID = 0x00;
ubxLastAck.msgID = 0x00;
// Continue with next configuration option
status->retryCount = 0;
status->lastConfigSent++;
} else if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) > UBX_REPLY_TIMEOUT) {
// timeout, resend the message or abort
status->retryCount++;
if (status->retryCount > UBX_MAX_RETRIES) {
status->currentStep = INIT_STEP_ERROR;
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
return;
}
}
// no abort condition, continue or retries.,
if (step_configure) {
status->currentStep = INIT_STEP_CONFIGURE;
} else {
status->currentStep = INIT_STEP_ENABLE_SENTENCES;
}
return;
}
}
}
void ubx_autoconfig_set(ubx_autoconfig_settings_t config)
{
enabled = false;
if (config.autoconfigEnabled) {
if (!status) {
status = (status_t *)pios_malloc(sizeof(status_t));
memset(status, 0, sizeof(status_t));
status->currentStep = INIT_STEP_DISABLED;
}
status->currentSettings = config;
status->currentStep = INIT_STEP_START;
enabled = true;
} else {
if (!status) {
status->currentStep = INIT_STEP_DISABLED;
}
}
}
int32_t ubx_autoconfig_get_status()
{
if (!status || !enabled) {
return UBX_AUTOCONFIG_STATUS_DISABLED;
}
switch (status->currentStep) {
case INIT_STEP_ERROR:
return UBX_AUTOCONFIG_STATUS_ERROR;
case INIT_STEP_DISABLED:
return UBX_AUTOCONFIG_STATUS_DISABLED;
case INIT_STEP_DONE:
return UBX_AUTOCONFIG_STATUS_DONE;
default:
break;
}
return UBX_AUTOCONFIG_STATUS_RUNNING;
}

3
flight/modules/ManualControl/armhandler.c
View File

@ -29,7 +29,6 @@
*/
#include "inc/manualcontrol.h"
#include <pios_struct_helper.h>
#include <sanitycheck.h>
#include <manualcontrolcommand.h>
#include <accessorydesired.h>
@ -259,7 +258,7 @@ static bool okToArm(void)
// Check each alarm
for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) {
if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[i] >= SYSTEMALARMS_ALARM_CRITICAL) { // found an alarm thats set
if (SystemAlarmsAlarmToArray(alarms.Alarm)[i] >= SYSTEMALARMS_ALARM_CRITICAL) { // found an alarm thats set
if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) {
continue;
}

6
flight/modules/ManualControl/inc/manualcontrol.h
View File

@ -99,6 +99,7 @@ void takeOffLocationHandlerInit();
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VIRTUALBAR == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ACRO == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \
@ -116,6 +117,7 @@ void takeOffLocationHandlerInit();
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_VIRTUALBAR == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_ACRO == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_RATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \
@ -133,6 +135,7 @@ void takeOffLocationHandlerInit();
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_VIRTUALBAR == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_ACRO == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_RATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \
@ -150,6 +153,7 @@ void takeOffLocationHandlerInit();
((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_VIRTUALBAR == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) && \
((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_ACRO == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) && \
((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_RATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \
@ -167,6 +171,7 @@ void takeOffLocationHandlerInit();
((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_VIRTUALBAR == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) && \
((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_ACRO == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) && \
((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_RATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \
@ -184,6 +189,7 @@ void takeOffLocationHandlerInit();
((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_VIRTUALBAR == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) && \
((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_ACRO == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) && \
((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_RATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \

44
flight/modules/ManualControl/stabilizedhandler.c
View File

@ -29,7 +29,7 @@
*/
#include "inc/manualcontrol.h"
#include <pios_struct_helper.h>
#include <mathmisc.h>
#include <manualcontrolcommand.h>
#include <stabilizationdesired.h>
#include <flightmodesettings.h>
@ -40,6 +40,28 @@
// Private types
// Private functions
static float applyExpo(float value, float expo);
static float applyExpo(float value, float expo)
{
// note: fastPow makes a small error, therefore result needs to be bound
float exp = boundf(fastPow(1.00695f, expo), 0.5f, 2.0f);
// magic number scales expo
// so that
// expo=100 yields value**10
// expo=0 yields value**1
// expo=-100 yields value**(1/10)
// (pow(2.0,1/100)~=1.00695)
if (value > 0.0f) {
return boundf(fastPow(value, exp), 0.0f, 1.0f);
} else if (value < -0.0f) {
return boundf(-fastPow(-value, exp), -1.0f, 0.0f);
} else {
return 0.0f;
}
}
/**
@ -65,32 +87,35 @@ void stabilizedHandler(bool newinit)
StabilizationBankData stabSettings;
StabilizationBankGet(&stabSettings);
cmd.Roll = applyExpo(cmd.Roll, stabSettings.StickExpo.Roll);
cmd.Pitch = applyExpo(cmd.Pitch, stabSettings.StickExpo.Pitch);
cmd.Yaw = applyExpo(cmd.Yaw, stabSettings.StickExpo.Yaw);
uint8_t *stab_settings;
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
stab_settings = cast_struct_to_array(settings.Stabilization1Settings, settings.Stabilization1Settings.Roll);
stab_settings = FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
stab_settings = cast_struct_to_array(settings.Stabilization2Settings, settings.Stabilization2Settings.Roll);
stab_settings = FlightModeSettingsStabilization2SettingsToArray(settings.Stabilization2Settings);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
stab_settings = cast_struct_to_array(settings.Stabilization3Settings, settings.Stabilization3Settings.Roll);
stab_settings = FlightModeSettingsStabilization3SettingsToArray(settings.Stabilization3Settings);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
stab_settings = cast_struct_to_array(settings.Stabilization4Settings, settings.Stabilization4Settings.Roll);
stab_settings = FlightModeSettingsStabilization4SettingsToArray(settings.Stabilization4Settings);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
stab_settings = cast_struct_to_array(settings.Stabilization5Settings, settings.Stabilization5Settings.Roll);
stab_settings = FlightModeSettingsStabilization5SettingsToArray(settings.Stabilization5Settings);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
stab_settings = cast_struct_to_array(settings.Stabilization6Settings, settings.Stabilization6Settings.Roll);
stab_settings = FlightModeSettingsStabilization6SettingsToArray(settings.Stabilization6Settings);
break;
default:
// Major error, this should not occur because only enter this block when one of these is true
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
stab_settings = cast_struct_to_array(settings.Stabilization1Settings, settings.Stabilization1Settings.Roll);
stab_settings = FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
return;
}
@ -101,6 +126,7 @@ void stabilizedHandler(bool newinit)
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Roll * stabSettings.RollMax :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Roll * stabSettings.RollMax :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd.Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd.Roll * stabSettings.RollMax :
@ -113,6 +139,7 @@ void stabilizedHandler(bool newinit)
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Pitch * stabSettings.PitchMax :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Pitch * stabSettings.PitchMax :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd.Pitch * stabSettings.PitchMax :
@ -135,6 +162,7 @@ void stabilizedHandler(bool newinit)
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Yaw * stabSettings.YawMax :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Yaw * stabSettings.YawMax :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd.Yaw * stabSettings.YawMax :

16
flight/modules/VtolPathFollower/inc/vtolpathfollower.h → flight/modules/Notify/inc/notify.h
View File

@ -1,9 +1,9 @@
/**
******************************************************************************
*
* @file vtolpathfollower.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Module to perform path following for VTOL.
* @file notify.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Notify module, show events and status on external led.
*
* @see The GNU Public License (GPL) Version 3
*
@ -23,9 +23,11 @@
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef VTOLPATHFOLLOWER_H
#define VTOLPATHFOLLOWER_H
#ifndef NOTIFY_H_
#define NOTIFY_H_
int32_t VtolPathFollowerInitialize(void);
#endif // VTOLPATHFOLLOWER_H
int32_t NotifyInitialize(void);
#endif /* NOTIFY_H_ */

222
flight/modules/Notify/inc/sequences.h Normal file
View File

@ -0,0 +1,222 @@
/**
******************************************************************************
*
* @file sequences.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Notify module, sequences configuration.
*
* @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 SEQUENCES_H_
#define SEQUENCES_H_
#include <optypes.h>
#include <pios_notify.h>
#include <flightstatus.h>
#include <systemalarms.h>
#include <pios_helpers.h>
// This represent the list of basic light sequences, defined later
typedef enum {
NOTIFY_SEQUENCE_ARMED_FM_MANUAL = 0,
NOTIFY_SEQUENCE_ARMED_FM_STABILIZED1 = 1,
NOTIFY_SEQUENCE_ARMED_FM_STABILIZED2 = 2,
NOTIFY_SEQUENCE_ARMED_FM_STABILIZED3 = 3,
NOTIFY_SEQUENCE_ARMED_FM_STABILIZED4 = 4,
NOTIFY_SEQUENCE_ARMED_FM_STABILIZED5 = 5,
NOTIFY_SEQUENCE_ARMED_FM_STABILIZED6 = 6,
NOTIFY_SEQUENCE_ARMED_FM_AUTOTUNE = 7,
NOTIFY_SEQUENCE_ARMED_FM_GPS = 8,
NOTIFY_SEQUENCE_ARMED_FM_RTH = 9,
NOTIFY_SEQUENCE_ARMED_FM_LAND = 10,
NOTIFY_SEQUENCE_ARMED_FM_AUTO = 11,
NOTIFY_SEQUENCE_ALM_WARN_GPS = 12,
NOTIFY_SEQUENCE_ALM_ERROR_GPS = 13,
NOTIFY_SEQUENCE_ALM_WARN_BATTERY = 14,
NOTIFY_SEQUENCE_ALM_ERROR_BATTERY = 15,
NOTIFY_SEQUENCE_ALM_MAG = 16,
NOTIFY_SEQUENCE_ALM_CONFIG = 17,
NOTIFY_SEQUENCE_ALM_RECEIVER = 18,
NOTIFY_SEQUENCE_DISARMED = 19,
NOTIFY_SEQUENCE_NULL = 255, // skips any signalling for this condition
} NotifySequences;
// This structure determine sequences attached to an alarm
typedef struct {
uint32_t timeBetweenNotifications; // time in milliseconds to wait between each notification iteration
uint8_t alarmIndex; // Index of the alarm, use one of the SYSTEMALARMS_ALARM_XXXXX defines
uint8_t warnNotification; // index of the sequence to be used when alarm is in warning status(pick one from NotifySequences enum)
uint8_t errorNotification; // index of the sequence to be used when alarm is in error status(pick one from NotifySequences enum)
} AlarmDefinition_t;
// This is the list of defined light sequences
/* how each sequence is defined
* [NOTIFY_SEQUENCE_DISARMED] = { // Sequence ID
.repeats = -1, // Number of repetitions or -1 for infinite
.steps = { // List of steps (until NOTIFY_SEQUENCE_MAX_STEPS steps, default to 5)
{
.time_off = 500, // Off time for the step
.time_on = 500, // On time for the step
.color = COLOR_TEAL, // color
.repeats = 1, // repetitions for this step
},
},
},
*
* There are two kind of sequences:
* - "Background" sequences, executed if no higher priority sequence is played;
* - "Alarm" sequences, that are "modal", they temporarily suspends background sequences and plays.
* Cannot have "-1" repetitions
* At the end background sequence are resumed;
*
*/
const LedSequence_t notifications[] = {
[NOTIFY_SEQUENCE_DISARMED] = { .repeats = -1, .steps = {
{ .time_off = 500, .time_on = 500, .color = COLOR_TEAL, .repeats = 1, },
}, },
[NOTIFY_SEQUENCE_ARMED_FM_MANUAL] = { .repeats = -1, .steps = {
{ .time_off = 900, .time_on = 100, .color = COLOR_YELLOW, .repeats = 1, },
}, },
[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED1] = { .repeats = -1, .steps = {
{ .time_off = 900, .time_on = 100, .color = COLOR_BLUE, .repeats = 1, },
}, },
[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED2] = { .repeats = -1, .steps = {
{ .time_off = 100, .time_on = 100, .color = COLOR_BLUE, .repeats = 1, },
{ .time_off = 700, .time_on = 100, .color = COLOR_BLUE, .repeats = 1, },
}, },
[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED3] = { .repeats = -1, .steps = {
{ .time_off = 100, .time_on = 100, .color = COLOR_BLUE, .repeats = 2, },
{ .time_off = 500, .time_on = 100, .color = COLOR_BLUE, .repeats = 1, },
}, },
[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED4] = { .repeats = -1, .steps = {
{ .time_off = 900, .time_on = 100, .color = COLOR_PURPLE, .repeats = 1, },
}, },
[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED5] = { .repeats = -1, .steps = {
{ .time_off = 100, .time_on = 100, .color = COLOR_PURPLE, .repeats = 1, },
{ .time_off = 700, .time_on = 100, .color = COLOR_BLUE, .repeats = 1, },
}, },
[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED6] = { .repeats = -1, .steps = {
{ .time_off = 100, .time_on = 100, .color = COLOR_PURPLE, .repeats = 1, },
{ .time_off = 100, .time_on = 100, .color = COLOR_PURPLE, .repeats = 1, },
{ .time_off = 500, .time_on = 100, .color = COLOR_BLUE, .repeats = 1, },
}, },
[NOTIFY_SEQUENCE_ARMED_FM_AUTOTUNE] = { .repeats = -1, .steps = {
{ .time_off = 800, .time_on = 200, .color = COLOR_BLUE, .repeats = 1, },
}, },
[NOTIFY_SEQUENCE_ARMED_FM_GPS] = { .repeats = -1, .steps = {
{ .time_off = 800, .time_on = 200, .color = COLOR_GREEN, .repeats = 1, },
}, },
[NOTIFY_SEQUENCE_ARMED_FM_RTH] = { .repeats = -1, .steps = {
{ .time_off = 100, .time_on = 100, .color = COLOR_GREEN, .repeats = 1, },
{ .time_off = 100, .time_on = 100, .color = COLOR_YELLOW, .repeats = 1, },
}, },
[NOTIFY_SEQUENCE_ARMED_FM_LAND] = { .repeats = -1, .steps = {
{ .time_off = 100, .time_on = 100, .color = COLOR_GREEN, .repeats = 1, },
}, },
[NOTIFY_SEQUENCE_ARMED_FM_AUTO] = { .repeats = -1, .steps = {
{ .time_off = 100, .time_on = 200, .color = COLOR_GREEN, .repeats = 2, },
{ .time_off = 500, .time_on = 200, .color = COLOR_GREEN, .repeats = 1, },
}, },
[NOTIFY_SEQUENCE_ALM_WARN_GPS] = { .repeats = 2, .steps = {
{ .time_off = 300, .time_on = 300, .color = COLOR_ORANGE, .repeats = 2, },
{ .time_off = 300, .time_on = 300, .color = COLOR_GREEN, .repeats = 1, },
}, },
[NOTIFY_SEQUENCE_ALM_ERROR_GPS] = { .repeats = 2, .steps = {
{ .time_off = 300, .time_on = 300, .color = COLOR_RED, .repeats = 2, },
{ .time_off = 300, .time_on = 300, .color = COLOR_GREEN, .repeats = 1, },
}, },
[NOTIFY_SEQUENCE_ALM_WARN_BATTERY] = { .repeats = 1, .steps = {
{ .time_off = 100, .time_on = 100, .color = COLOR_ORANGE, .repeats = 10, },
}, },
[NOTIFY_SEQUENCE_ALM_ERROR_BATTERY] = { .repeats = 1, .steps = {
{ .time_off = 100, .time_on = 100, .color = COLOR_RED, .repeats = 10, },
}, },
[NOTIFY_SEQUENCE_ALM_MAG] = { .repeats = 1, .steps = {
{ .time_off = 300, .time_on = 300, .color = COLOR_RED, .repeats = 2, },
{ .time_off = 300, .time_on = 300, .color = COLOR_PURPLE, .repeats = 1, },
}, },
[NOTIFY_SEQUENCE_ALM_CONFIG] = { .repeats = 1, .steps = {
{ .time_off = 50, .time_on = 50, .color = COLOR_RED, .repeats = 9, },
{ .time_off = 500, .time_on = 50, .color = COLOR_RED, .repeats = 1, },
}, },
[NOTIFY_SEQUENCE_ALM_RECEIVER] = { .repeats = 1, .steps = {
{ .time_off = 50, .time_on = 50, .color = COLOR_ORANGE, .repeats = 9, },
{ .time_off = 500, .time_on = 50, .color = COLOR_ORANGE, .repeats = 1, },
}, },
};
// List of background sequences attached to each flight mode
const LedSequence_t *flightModeMap[] = {
[FLIGHTSTATUS_FLIGHTMODE_MANUAL] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_MANUAL],
[FLIGHTSTATUS_FLIGHTMODE_STABILIZED1] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED1],
[FLIGHTSTATUS_FLIGHTMODE_STABILIZED2] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED2],
[FLIGHTSTATUS_FLIGHTMODE_STABILIZED3] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED3],
[FLIGHTSTATUS_FLIGHTMODE_STABILIZED4] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED4],
[FLIGHTSTATUS_FLIGHTMODE_STABILIZED5] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED5],
[FLIGHTSTATUS_FLIGHTMODE_STABILIZED6] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED6],
[FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_AUTO],
[FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_AUTOTUNE],
[FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_GPS],
[FLIGHTSTATUS_FLIGHTMODE_POSITIONVARIOFPV] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_GPS],
[FLIGHTSTATUS_FLIGHTMODE_POSITIONVARIOLOS] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_GPS],
[FLIGHTSTATUS_FLIGHTMODE_POSITIONVARIONSEW] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_GPS],
[FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_RTH],
[FLIGHTSTATUS_FLIGHTMODE_LAND] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_LAND],
[FLIGHTSTATUS_FLIGHTMODE_POI] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_GPS],
[FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_GPS],
};
// List of alarms to show with attached sequences for each status
const AlarmDefinition_t alarmsMap[] = {
{
.timeBetweenNotifications = 10000,
.alarmIndex = SYSTEMALARMS_ALARM_GPS,
.warnNotification = NOTIFY_SEQUENCE_ALM_WARN_GPS,
.errorNotification = NOTIFY_SEQUENCE_ALM_ERROR_GPS,
},
{
.timeBetweenNotifications = 15000,
.alarmIndex = SYSTEMALARMS_ALARM_MAGNETOMETER,
.warnNotification = NOTIFY_SEQUENCE_NULL,
.errorNotification = NOTIFY_SEQUENCE_ALM_MAG,
},
{
.timeBetweenNotifications = 15000,
.alarmIndex = SYSTEMALARMS_ALARM_BATTERY,
.warnNotification = NOTIFY_SEQUENCE_ALM_WARN_BATTERY,
.errorNotification = NOTIFY_SEQUENCE_ALM_ERROR_BATTERY,
},
{
.timeBetweenNotifications = 5000,
.alarmIndex = SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION,
.warnNotification = NOTIFY_SEQUENCE_NULL,
.errorNotification = NOTIFY_SEQUENCE_ALM_CONFIG,
},
{
.timeBetweenNotifications = 5000,
.alarmIndex = SYSTEMALARMS_ALARM_RECEIVER,
.warnNotification = NOTIFY_SEQUENCE_ALM_RECEIVER,
.errorNotification = NOTIFY_SEQUENCE_ALM_RECEIVER,
},
};
const uint8_t alarmsMapSize = NELEMENTS(alarmsMap);
#endif /* SEQUENCES_H_ */

136
flight/modules/Notify/notify.c Normal file
View File

@ -0,0 +1,136 @@
/**
******************************************************************************
*
* @file notify.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Notify module, show events and status on external led.
*
* @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 "openpilot.h"
#include <flightstatus.h>
#include <systemalarms.h>
#include <flightbatterystate.h>
#include <lednotification.h>
#include <optypes.h>
#include <pios_notify.h>
#include <FreeRTOS.h>
#include <task.h>
#include <eventdispatcher.h>
#include "inc/notify.h"
#include "inc/sequences.h"
#include <pios_mem.h>
#include <hwsettings.h>
#define SAMPLE_PERIOD_MS 250
// private types
typedef struct {
uint32_t lastAlarmTime;
uint8_t lastAlarm;
} AlarmStatus_t;
// function declarations
static void updatedCb(UAVObjEvent *ev);
static void onTimerCb(UAVObjEvent *ev);
static void checkAlarm(uint8_t alarm, uint8_t *last_alarm, uint32_t *last_alm_time,
uint8_t warn_sequence, uint8_t error_sequence,
uint32_t timeBetweenNotifications);
static AlarmStatus_t *alarmStatus;
int32_t NotifyInitialize(void)
{
uint8_t ws281xOutStatus;
HwSettingsWS2811LED_OutGet(&ws281xOutStatus);
// Todo: Until further applications exists for WS2811 notify enabled status is tied to ws281x output configuration
bool enabled = ws281xOutStatus != HWSETTINGS_WS2811LED_OUT_DISABLED;
if (enabled) {
alarmStatus = (AlarmStatus_t *)pios_malloc(sizeof(AlarmStatus_t) * alarmsMapSize);
for (uint8_t i = 0; i < alarmsMapSize; i++) {
alarmStatus[i].lastAlarm = SYSTEMALARMS_ALARM_OK;
alarmStatus[i].lastAlarmTime = 0;
}
FlightStatusConnectCallback(&updatedCb);
static UAVObjEvent ev;
memset(&ev, 0, sizeof(UAVObjEvent));
EventPeriodicCallbackCreate(&ev, onTimerCb, SAMPLE_PERIOD_MS / portTICK_RATE_MS);
updatedCb(0);
}
return 0;
}
MODULE_INITCALL(NotifyInitialize, 0);
void updatedCb(UAVObjEvent *ev)
{
if (!ev || ev->obj == FlightStatusHandle()) {
static uint8_t last_armed = 0xff;
static uint8_t last_flightmode = 0xff;
uint8_t armed;
uint8_t flightmode;
FlightStatusArmedGet(&armed);
FlightStatusFlightModeGet(&flightmode);
if (last_armed != armed || (armed && flightmode != last_flightmode)) {
if (armed) {
PIOS_NOTIFICATION_Default_Ext_Led_Play(flightModeMap[flightmode], NOTIFY_PRIORITY_BACKGROUND);
} else {
PIOS_NOTIFICATION_Default_Ext_Led_Play(&notifications[NOTIFY_SEQUENCE_DISARMED], NOTIFY_PRIORITY_BACKGROUND);
}
}
last_armed = armed;
last_flightmode = flightmode;
}
}
void onTimerCb(__attribute__((unused)) UAVObjEvent *ev)
{
static SystemAlarmsAlarmData alarms;
SystemAlarmsAlarmGet(&alarms);
for (uint8_t i = 0; i < alarmsMapSize; i++) {
uint8_t alarm = ((uint8_t *)&alarms)[alarmsMap[i].alarmIndex];
checkAlarm(alarm,
&alarmStatus[i].lastAlarm,
&alarmStatus[i].lastAlarmTime,
alarmsMap[i].warnNotification,
alarmsMap[i].errorNotification,
alarmsMap[i].timeBetweenNotifications);
}
}
void checkAlarm(uint8_t alarm, uint8_t *last_alarm, uint32_t *last_alm_time, uint8_t warn_sequence, uint8_t error_sequence, uint32_t timeBetweenNotifications)
{
if (alarm > SYSTEMALARMS_ALARM_OK) {
uint32_t current_time = PIOS_DELAY_GetuS();
if (*last_alarm < alarm || *last_alm_time + timeBetweenNotifications * 1000 < current_time) {
uint8_t sequence = (alarm == SYSTEMALARMS_ALARM_WARNING) ? warn_sequence : error_sequence;
if (sequence != NOTIFY_SEQUENCE_NULL) {
PIOS_NOTIFICATION_Default_Ext_Led_Play(
&notifications[sequence],
alarm == SYSTEMALARMS_ALARM_WARNING ? NOTIFY_PRIORITY_REGULAR : NOTIFY_PRIORITY_CRITICAL);
}
*last_alarm = alarm;
*last_alm_time = current_time;
}
} else {
*last_alarm = SYSTEMALARMS_ALARM_OK;
}
}

1238
flight/modules/PathFollower/pathfollower.c Normal file
View File

@ -0,0 +1,1238 @@
/**
******************************************************************************
*
* @file pathfollower.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint
* and sets @ref AttitudeDesired. It only does this when the FlightMode field
* of @ref ManualControlCommand is Auto.
*
* @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
*/
/**
* Input object: PathDesired
* Input object: PositionState
* Input object: ManualControlCommand
* Output object: StabilizationDesired
*
* This module acts as "autopilot" - it controls the setpoints of stabilization
* based on current flight situation and desired flight path (PathDesired) as
* directed by flightmode selection or pathplanner
* This is a periodic delayed callback module
*
* Modules have no API, all communication to other modules is done through UAVObjects.
* However modules may use the API exposed by shared libraries.
* See the OpenPilot wiki for more details.
* http://www.openpilot.org/OpenPilot_Application_Architecture
*
*/
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
#include <paths.h>
#include <sanitycheck.h>
#include <fixedwingpathfollowersettings.h>
#include <fixedwingpathfollowerstatus.h>
#include <vtolpathfollowersettings.h>
#include <flightstatus.h>
#include <pathstatus.h>
#include <positionstate.h>
#include <velocitystate.h>
#include <velocitydesired.h>
#include <stabilizationdesired.h>
#include <airspeedstate.h>
#include <attitudestate.h>
#include <takeofflocation.h>
#include <poilocation.h>
#include <manualcontrolcommand.h>
#include <systemsettings.h>
#include <stabilizationbank.h>
// Private constants
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
#define PF_IDLE_UPDATE_RATE_MS 100
#define STACK_SIZE_BYTES 2048
#define DEADBAND_HIGH 0.10f
#define DEADBAND_LOW -0.10f
// Private types
struct Globals {
struct pid PIDposH[2];
struct pid PIDposV;
struct pid PIDvel[3];
struct pid PIDcourse;
struct pid PIDspeed;
struct pid PIDpower;
float poiRadius;
float vtolEmergencyFallback;
bool vtolEmergencyFallbackSwitch;
};
// Private variables
static DelayedCallbackInfo *pathFollowerCBInfo;
static uint32_t updatePeriod = PF_IDLE_UPDATE_RATE_MS;
static struct Globals global;
static PathStatusData pathStatus;
static PathDesiredData pathDesired;
static FixedWingPathFollowerSettingsData fixedWingPathFollowerSettings;
static VtolPathFollowerSettingsData vtolPathFollowerSettings;
// correct speed by measured airspeed
static float indicatedAirspeedStateBias = 0.0f;
// Private functions
static void pathFollowerTask(void);
static void resetGlobals();
static void SettingsUpdatedCb(UAVObjEvent *ev);
static uint8_t updateAutoPilotByFrameType();
static uint8_t updateAutoPilotFixedWing();
static uint8_t updateAutoPilotVtol();
static float updateTailInBearing();
static float updateCourseBearing();
static float updatePathBearing();
static float updatePOIBearing();
static void processPOI();
static void updatePathVelocity(float kFF, bool limited);
static uint8_t updateFixedDesiredAttitude();
static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction);
static void updateFixedAttitude();
static void updateVtolDesiredAttitudeEmergencyFallback();
static void airspeedStateUpdatedCb(UAVObjEvent *ev);
static bool correctCourse(float *C, float *V, float *F, float s);
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t PathFollowerStart()
{
// Start main task
PathStatusGet(&pathStatus);
SettingsUpdatedCb(NULL);
PIOS_CALLBACKSCHEDULER_Dispatch(pathFollowerCBInfo);
return 0;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t PathFollowerInitialize()
{
// initialize objects
FixedWingPathFollowerSettingsInitialize();
FixedWingPathFollowerStatusInitialize();
VtolPathFollowerSettingsInitialize();
FlightStatusInitialize();
PathStatusInitialize();
PathDesiredInitialize();
PositionStateInitialize();
VelocityStateInitialize();
VelocityDesiredInitialize();
StabilizationDesiredInitialize();
AirspeedStateInitialize();
AttitudeStateInitialize();
TakeOffLocationInitialize();
PoiLocationInitialize();
ManualControlCommandInitialize();
SystemSettingsInitialize();
StabilizationBankInitialize();
// reset integrals
resetGlobals();
// Create object queue
pathFollowerCBInfo = PIOS_CALLBACKSCHEDULER_Create(&pathFollowerTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_PATHFOLLOWER, STACK_SIZE_BYTES);
FixedWingPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
PathDesiredConnectCallback(SettingsUpdatedCb);
AirspeedStateConnectCallback(airspeedStateUpdatedCb);
return 0;
}
MODULE_INITCALL(PathFollowerInitialize, PathFollowerStart);
/**
* Module thread, should not return.
*/
static void pathFollowerTask(void)
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if (flightStatus.ControlChain.PathFollower != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
resetGlobals();
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_UNINITIALISED);
PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, PF_IDLE_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
return;
}
if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POI) { // TODO Hack from vtolpathfollower, move into manualcontrol!
processPOI();
}
pathStatus.UID = pathDesired.UID;
pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
switch (pathDesired.Mode) {
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_FLYCIRCLELEFT:
{
uint8_t result = updateAutoPilotByFrameType();
if (result) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
} else {
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
}
}
break;
case PATHDESIRED_MODE_FIXEDATTITUDE:
updateFixedAttitude(pathDesired.ModeParameters);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_DISARMALARM:
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
break;
default:
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
break;
}
PathStatusSet(&pathStatus);
PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, updatePeriod, CALLBACK_UPDATEMODE_SOONER);
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
FixedWingPathFollowerSettingsGet(&fixedWingPathFollowerSettings);
pid_configure(&global.PIDcourse, fixedWingPathFollowerSettings.CoursePI.Kp, fixedWingPathFollowerSettings.CoursePI.Ki, 0.0f, fixedWingPathFollowerSettings.CoursePI.ILimit);
pid_configure(&global.PIDspeed, fixedWingPathFollowerSettings.SpeedPI.Kp, fixedWingPathFollowerSettings.SpeedPI.Ki, 0.0f, fixedWingPathFollowerSettings.SpeedPI.ILimit);
pid_configure(&global.PIDpower, fixedWingPathFollowerSettings.PowerPI.Kp, fixedWingPathFollowerSettings.PowerPI.Ki, 0.0f, fixedWingPathFollowerSettings.PowerPI.ILimit);
VtolPathFollowerSettingsGet(&vtolPathFollowerSettings);
pid_configure(&global.PIDvel[0], vtolPathFollowerSettings.HorizontalVelPID.Kp, vtolPathFollowerSettings.HorizontalVelPID.Ki, vtolPathFollowerSettings.HorizontalVelPID.Kd, vtolPathFollowerSettings.HorizontalVelPID.ILimit);
pid_configure(&global.PIDvel[1], vtolPathFollowerSettings.HorizontalVelPID.Kp, vtolPathFollowerSettings.HorizontalVelPID.Ki, vtolPathFollowerSettings.HorizontalVelPID.Kd, vtolPathFollowerSettings.HorizontalVelPID.ILimit);
pid_configure(&global.PIDvel[2], vtolPathFollowerSettings.VerticalVelPID.Kp, vtolPathFollowerSettings.VerticalVelPID.Ki, vtolPathFollowerSettings.VerticalVelPID.Kd, vtolPathFollowerSettings.VerticalVelPID.ILimit);
PathDesiredGet(&pathDesired);
}
static void airspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
AirspeedStateData airspeedState;
VelocityStateData velocityState;
AirspeedStateGet(&airspeedState);
VelocityStateGet(&velocityState);
float airspeedVector[2];
float yaw;
AttitudeStateYawGet(&yaw);
airspeedVector[0] = cos_lookup_deg(yaw);
airspeedVector[1] = sin_lookup_deg(yaw);
// vector projection of groundspeed on airspeed vector to handle both forward and backwards movement
float groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1];
indicatedAirspeedStateBias = airspeedState.CalibratedAirspeed - groundspeedProjection;
// note - we do fly by Indicated Airspeed (== calibrated airspeed) however
// since airspeed is updated less often than groundspeed, we use sudden
// changes to groundspeed to offset the airspeed by the same measurement.
// This has a side effect that in the absence of any airspeed updates, the
// pathfollower will fly using groundspeed.
}
/**
* reset integrals
*/
static void resetGlobals()
{
pid_zero(&global.PIDposH[0]);
pid_zero(&global.PIDposH[1]);
pid_zero(&global.PIDposV);
pid_zero(&global.PIDvel[0]);
pid_zero(&global.PIDvel[1]);
pid_zero(&global.PIDvel[2]);
pid_zero(&global.PIDcourse);
pid_zero(&global.PIDspeed);
pid_zero(&global.PIDpower);
global.poiRadius = 0.0f;
global.vtolEmergencyFallback = 0;
global.vtolEmergencyFallbackSwitch = false;
}
static uint8_t updateAutoPilotByFrameType()
{
FrameType_t frameType = GetCurrentFrameType();
if (frameType == FRAME_TYPE_CUSTOM || frameType == FRAME_TYPE_GROUND) {
switch (vtolPathFollowerSettings.TreatCustomCraftAs) {
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING:
frameType = FRAME_TYPE_FIXED_WING;
break;
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL:
frameType = FRAME_TYPE_MULTIROTOR;
break;
}
}
switch (frameType) {
case FRAME_TYPE_MULTIROTOR:
case FRAME_TYPE_HELI:
updatePeriod = vtolPathFollowerSettings.UpdatePeriod;
return updateAutoPilotVtol();
break;
case FRAME_TYPE_FIXED_WING:
default:
updatePeriod = fixedWingPathFollowerSettings.UpdatePeriod;
return updateAutoPilotFixedWing();
break;
}
}
/**
* fixed wing autopilot:
* straight forward:
* 1. update path velocity for limited motion crafts
* 2. update attitude according to default fixed wing pathfollower algorithm
*/
static uint8_t updateAutoPilotFixedWing()
{
pid_configure(&global.PIDposH[0], fixedWingPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f);
pid_configure(&global.PIDposH[1], fixedWingPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f);
pid_configure(&global.PIDposV, fixedWingPathFollowerSettings.VerticalPosP, 0.0f, 0.0f, 0.0f);
updatePathVelocity(fixedWingPathFollowerSettings.CourseFeedForward, true);
return updateFixedDesiredAttitude();
}
/**
* vtol autopilot
* use hover capable algorithm with unlimeted movement calculation. if that fails (flyaway situation due to compass failure)
* fall back to emergency fallback autopilot to keep minimum amount of flight control
*/
static uint8_t updateAutoPilotVtol()
{
enum { RETURN_0 = 0, RETURN_1 = 1, RETURN_RESULT } returnmode;
enum { FOLLOWER_REGULAR, FOLLOWER_FALLBACK } followermode;
uint8_t result = 0;
// decide on behaviour based on settings and system state
if (global.vtolEmergencyFallbackSwitch) {
returnmode = RETURN_0;
followermode = FOLLOWER_FALLBACK;
} else {
if (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ALWAYS) {
returnmode = RETURN_1;
followermode = FOLLOWER_FALLBACK;
} else {
returnmode = RETURN_RESULT;
followermode = FOLLOWER_REGULAR;
}
}
// vertical positon control PID loops works the same in both regular and fallback modes, setup according to settings
pid_configure(&global.PIDposV, vtolPathFollowerSettings.VerticalPosP, 0.0f, 0.0f, 0.0f);
switch (followermode) {
case FOLLOWER_REGULAR:
{
// horizontal position control PID loop works according to settings in regular mode, allowing integral terms
pid_configure(&global.PIDposH[0], vtolPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f);
pid_configure(&global.PIDposH[1], vtolPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f);
updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, false);
// yaw behaviour is configurable in vtolpathfollower, select yaw control algorithm
bool yaw_attitude = true;
float yaw = 0.0f;
switch (vtolPathFollowerSettings.YawControl) {
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MANUAL:
yaw_attitude = false;
break;
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_TAILIN:
yaw = updateTailInBearing();
break;
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MOVEMENTDIRECTION:
yaw = updateCourseBearing();
break;
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_PATHDIRECTION:
yaw = updatePathBearing();
break;
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_POI:
yaw = updatePOIBearing();
break;
}
result = updateVtolDesiredAttitude(yaw_attitude, yaw);
// switch to emergency follower if follower indicates problems
if (!result && (vtolPathFollowerSettings.FlyawayEmergencyFallback != VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DISABLED)) {
global.vtolEmergencyFallbackSwitch = true;
}
}
break;
case FOLLOWER_FALLBACK:
{
// fallback loop only cares about intended horizontal flight direction, simplify control behaviour accordingly
pid_configure(&global.PIDposH[0], 1.0f, 0.0f, 0.0f, 0.0f);
pid_configure(&global.PIDposH[1], 1.0f, 0.0f, 0.0f, 0.0f);
updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, true);
// emergency follower has no return value
updateVtolDesiredAttitudeEmergencyFallback();
}
break;
}
switch (returnmode) {
case RETURN_RESULT:
return result;
default:
// returns either 0 or 1 according to enum definition above
return returnmode;
}
}
/**
* Compute bearing of current takeoff location
*/
static float updateTailInBearing()
{
PositionStateData p;
PositionStateGet(&p);
TakeOffLocationData t;
TakeOffLocationGet(&t);
// atan2f always returns in between + and - 180 degrees
return RAD2DEG(atan2f(p.East - t.East, p.North - t.North));
}
/**
* Compute bearing of current movement direction
*/
static float updateCourseBearing()
{
VelocityStateData v;
VelocityStateGet(&v);
// atan2f always returns in between + and - 180 degrees
return RAD2DEG(atan2f(v.East, v.North));
}
/**
* Compute bearing of current path direction
*/
static float updatePathBearing()
{
PositionStateData positionState;
PositionStateGet(&positionState);
float cur[3] = { positionState.North,
positionState.East,
positionState.Down };
struct path_status progress;
path_progress(&pathDesired, cur, &progress);
// atan2f always returns in between + and - 180 degrees
return RAD2DEG(atan2f(progress.path_vector[1], progress.path_vector[0]));
}
/**
* Compute bearing between current position and POI
*/
static float updatePOIBearing()
{
PoiLocationData poi;
PoiLocationGet(&poi);
PositionStateData positionState;
PositionStateGet(&positionState);
const float dT = updatePeriod / 1000.0f;
float dLoc[3];
float yaw = 0;
/*float elevation = 0;*/
dLoc[0] = positionState.North - poi.North;
dLoc[1] = positionState.East - poi.East;
dLoc[2] = positionState.Down - poi.Down;
if (dLoc[1] < 0) {
yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) + 180.0f;
} else {
yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) - 180.0f;
}
ManualControlCommandData manualControlData;
ManualControlCommandGet(&manualControlData);
float pathAngle = 0;
if (manualControlData.Roll > DEADBAND_HIGH) {
pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f;
} else if (manualControlData.Roll < DEADBAND_LOW) {
pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f;
}
return yaw + (pathAngle / 2.0f);
}
/**
* process POI control logic TODO: this should most likely go into manualcontrol!!!!
* TODO: the whole process of POI handling likely needs cleanup and rethinking, might be broken since manualcontrol was refactored currently
**/
static void processPOI()
{
const float dT = updatePeriod / 1000.0f;
PositionStateData positionState;
PositionStateGet(&positionState);
// TODO put commented out camera feature code back in place either
// permanently or optionally or remove it
// CameraDesiredData cameraDesired;
// CameraDesiredGet(&cameraDesired);
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
PoiLocationData poi;
PoiLocationGet(&poi);
float dLoc[3];
float yaw = 0;
// TODO camera feature
/*float elevation = 0;*/
dLoc[0] = positionState.North - poi.North;
dLoc[1] = positionState.East - poi.East;
dLoc[2] = positionState.Down - poi.Down;
if (dLoc[1] < 0) {
yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) + 180.0f;
} else {
yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) - 180.0f;
}
// distance
float distance = sqrtf(powf(dLoc[0], 2.0f) + powf(dLoc[1], 2.0f));
ManualControlCommandData manualControlData;
ManualControlCommandGet(&manualControlData);
float changeRadius = 0;
// Move closer or further, radially
if (manualControlData.Pitch > DEADBAND_HIGH) {
changeRadius = (manualControlData.Pitch - DEADBAND_HIGH) * dT * 100.0f;
} else if (manualControlData.Pitch < DEADBAND_LOW) {
changeRadius = (manualControlData.Pitch - DEADBAND_LOW) * dT * 100.0f;
}
// move along circular path
float pathAngle = 0;
if (manualControlData.Roll > DEADBAND_HIGH) {
pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f;
} else if (manualControlData.Roll < DEADBAND_LOW) {
pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f;
} else if (manualControlData.Roll >= DEADBAND_LOW && manualControlData.Roll <= DEADBAND_HIGH) {
// change radius only when not circling
global.poiRadius = distance + changeRadius;
}
// don't try to move any closer
if (global.poiRadius >= 3.0f || changeRadius > 0) {
if (fabsf(pathAngle) > 0.0f || fabsf(changeRadius) > 0.0f) {
pathDesired.End.North = poi.North + (global.poiRadius * cosf(DEG2RAD(pathAngle + yaw - 180.0f)));
pathDesired.End.East = poi.East + (global.poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f)));
pathDesired.StartingVelocity = 1.0f;
pathDesired.EndingVelocity = 0.0f;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
}
}
// not above
if (distance >= 3.0f) {
// TODO camera feature
// You can feed this into camerastabilization
/*elevation = RAD2DEG(atan2f(dLoc[2],distance));*/
// cameraDesired.Yaw=yaw;
// cameraDesired.PitchOrServo2=elevation;
// CameraDesiredSet(&cameraDesired);
}
}
/**
* Compute desired velocity from the current position and path
*/
static void updatePathVelocity(float kFF, bool limited)
{
PositionStateData positionState;
PositionStateGet(&positionState);
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
const float dT = updatePeriod / 1000.0f;
// look ahead kFF seconds
float cur[3] = { positionState.North + (velocityState.North * kFF),
positionState.East + (velocityState.East * kFF),
positionState.Down + (velocityState.Down * kFF) };
struct path_status progress;
path_progress(&pathDesired, cur, &progress);
// calculate velocity - can be zero if waypoints are too close
VelocityDesiredData velocityDesired;
velocityDesired.North = progress.path_vector[0];
velocityDesired.East = progress.path_vector[1];
velocityDesired.Down = progress.path_vector[2];
if (limited &&
// if a plane is crossing its desired flightpath facing the wrong way (away from flight direction)
// it would turn towards the flightpath to get on its desired course. This however would reverse the correction vector
// once it crosses the flightpath again, which would make it again turn towards the flightpath (but away from its desired heading)
// leading to an S-shape snake course the wrong way
// this only happens especially if HorizontalPosP is too high, as otherwise the angle between velocity desired and path_direction won't
// turn steep unless there is enough space complete the turn before crossing the flightpath
// in this case the plane effectively needs to be turned around
// indicators:
// difference between correction_direction and velocitystate >90 degrees and
// difference between path_direction and velocitystate >90 degrees ( 4th sector, facing away from everything )
// fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore)
// calculating angles < 90 degrees through dot products
(vector_lengthf(progress.path_vector, 2) > 1e-6f) &&
((progress.path_vector[0] * velocityState.North + progress.path_vector[1] * velocityState.East) < 0.0f) &&
((progress.correction_vector[0] * velocityState.North + progress.correction_vector[1] * velocityState.East) < 0.0f)) {
;
} else {
// calculate correction
velocityDesired.North += pid_apply(&global.PIDposH[0], progress.correction_vector[0], dT);
velocityDesired.East += pid_apply(&global.PIDposH[1], progress.correction_vector[1], dT);
}
velocityDesired.Down += pid_apply(&global.PIDposV, progress.correction_vector[2], dT);
// update pathstatus
pathStatus.error = progress.error;
pathStatus.fractional_progress = progress.fractional_progress;
pathStatus.path_direction_north = progress.path_vector[0];
pathStatus.path_direction_east = progress.path_vector[1];
pathStatus.path_direction_down = progress.path_vector[2];
pathStatus.correction_direction_north = progress.correction_vector[0];
pathStatus.correction_direction_east = progress.correction_vector[1];
pathStatus.correction_direction_down = progress.correction_vector[2];
VelocityDesiredSet(&velocityDesired);
}
/**
* Compute desired attitude from the desired velocity for fixed wing craft
*/
static uint8_t updateFixedDesiredAttitude()
{
uint8_t result = 1;
const float dT = updatePeriod / 1000.0f; // Convert from [ms] to [s]
VelocityDesiredData velocityDesired;
VelocityStateData velocityState;
StabilizationDesiredData stabDesired;
AttitudeStateData attitudeState;
FixedWingPathFollowerStatusData fixedWingPathFollowerStatus;
AirspeedStateData airspeedState;
SystemSettingsData systemSettings;
float groundspeedProjection;
float indicatedAirspeedState;
float indicatedAirspeedDesired;
float airspeedError;
float pitchCommand;
float descentspeedDesired;
float descentspeedError;
float powerCommand;
float airspeedVector[2];
float fluidMovement[2];
float courseComponent[2];
float courseError;
float courseCommand;
FixedWingPathFollowerStatusGet(&fixedWingPathFollowerStatus);
VelocityStateGet(&velocityState);
StabilizationDesiredGet(&stabDesired);
VelocityDesiredGet(&velocityDesired);
AttitudeStateGet(&attitudeState);
AirspeedStateGet(&airspeedState);
SystemSettingsGet(&systemSettings);
/**
* Compute speed error and course
*/
// missing sensors for airspeed-direction we have to assume within
// reasonable error that measured airspeed is actually the airspeed
// component in forward pointing direction
// airspeedVector is normalized
airspeedVector[0] = cos_lookup_deg(attitudeState.Yaw);
airspeedVector[1] = sin_lookup_deg(attitudeState.Yaw);
// current ground speed projected in forward direction
groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1];
// note that airspeedStateBias is ( calibratedAirspeed - groundspeedProjection ) at the time of measurement,
// but thanks to accelerometers, groundspeedProjection reacts faster to changes in direction
// than airspeed and gps sensors alone
indicatedAirspeedState = groundspeedProjection + indicatedAirspeedStateBias;
// fluidMovement is a vector describing the aproximate movement vector of
// the surrounding fluid in 2d space (aka wind vector)
fluidMovement[0] = velocityState.North - (indicatedAirspeedState * airspeedVector[0]);
fluidMovement[1] = velocityState.East - (indicatedAirspeedState * airspeedVector[1]);
// calculate the movement vector we need to fly to reach velocityDesired -
// taking fluidMovement into account
courseComponent[0] = velocityDesired.North - fluidMovement[0];
courseComponent[1] = velocityDesired.East - fluidMovement[1];
indicatedAirspeedDesired = boundf(sqrtf(courseComponent[0] * courseComponent[0] + courseComponent[1] * courseComponent[1]),
fixedWingPathFollowerSettings.HorizontalVelMin,
fixedWingPathFollowerSettings.HorizontalVelMax);
// if we could fly at arbitrary speeds, we'd just have to move towards the
// courseComponent vector as previously calculated and we'd be fine
// unfortunately however we are bound by min and max air speed limits, so
// we need to recalculate the correct course to meet at least the
// velocityDesired vector direction at our current speed
// this overwrites courseComponent
bool valid = correctCourse(courseComponent, (float *)&velocityDesired.North, fluidMovement, indicatedAirspeedDesired);
// Error condition: wind speed too high, we can't go where we want anymore
fixedWingPathFollowerStatus.Errors.Wind = 0;
if ((!valid) &&
fixedWingPathFollowerSettings.Safetymargins.Wind > 0.5f) { // alarm switched on
fixedWingPathFollowerStatus.Errors.Wind = 1;
result = 0;
}
// Airspeed error
airspeedError = indicatedAirspeedDesired - indicatedAirspeedState;
// Vertical speed error
descentspeedDesired = boundf(
velocityDesired.Down,
-fixedWingPathFollowerSettings.VerticalVelMax,
fixedWingPathFollowerSettings.VerticalVelMax);
descentspeedError = descentspeedDesired - velocityState.Down;
// Error condition: plane too slow or too fast
fixedWingPathFollowerStatus.Errors.Highspeed = 0;
fixedWingPathFollowerStatus.Errors.Lowspeed = 0;
if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedWingPathFollowerSettings.Safetymargins.Overspeed) {
fixedWingPathFollowerStatus.Errors.Overspeed = 1;
result = 0;
}
if (indicatedAirspeedState > fixedWingPathFollowerSettings.HorizontalVelMax * fixedWingPathFollowerSettings.Safetymargins.Highspeed) {
fixedWingPathFollowerStatus.Errors.Highspeed = 1;
result = 0;
}
if (indicatedAirspeedState < fixedWingPathFollowerSettings.HorizontalVelMin * fixedWingPathFollowerSettings.Safetymargins.Lowspeed) {
fixedWingPathFollowerStatus.Errors.Lowspeed = 1;
result = 0;
}
if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedWingPathFollowerSettings.Safetymargins.Stallspeed) {
fixedWingPathFollowerStatus.Errors.Stallspeed = 1;
result = 0;
}
/**
* Compute desired thrust command
*/
// Compute the cross feed from vertical speed to pitch, with saturation
float speedErrorToPowerCommandComponent = boundf(
(airspeedError / fixedWingPathFollowerSettings.HorizontalVelMin) * fixedWingPathFollowerSettings.AirspeedToPowerCrossFeed.Kp,
-fixedWingPathFollowerSettings.AirspeedToPowerCrossFeed.Max,
fixedWingPathFollowerSettings.AirspeedToPowerCrossFeed.Max
);
// Compute final thrust response
powerCommand = pid_apply(&global.PIDpower, -descentspeedError, dT) +
speedErrorToPowerCommandComponent;
// Output internal state to telemetry
fixedWingPathFollowerStatus.Error.Power = descentspeedError;
fixedWingPathFollowerStatus.ErrorInt.Power = global.PIDpower.iAccumulator;
fixedWingPathFollowerStatus.Command.Power = powerCommand;
// set thrust
stabDesired.Thrust = boundf(fixedWingPathFollowerSettings.ThrustLimit.Neutral + powerCommand,
fixedWingPathFollowerSettings.ThrustLimit.Min,
fixedWingPathFollowerSettings.ThrustLimit.Max);
// Error condition: plane cannot hold altitude at current speed.
fixedWingPathFollowerStatus.Errors.Lowpower = 0;
if (fixedWingPathFollowerSettings.ThrustLimit.Neutral + powerCommand >= fixedWingPathFollowerSettings.ThrustLimit.Max && // thrust at maximum
velocityState.Down > 0.0f && // we ARE going down
descentspeedDesired < 0.0f && // we WANT to go up
airspeedError > 0.0f && // we are too slow already
fixedWingPathFollowerSettings.Safetymargins.Lowpower > 0.5f) { // alarm switched on
fixedWingPathFollowerStatus.Errors.Lowpower = 1;
result = 0;
}
// Error condition: plane keeps climbing despite minimum thrust (opposite of above)
fixedWingPathFollowerStatus.Errors.Highpower = 0;
if (fixedWingPathFollowerSettings.ThrustLimit.Neutral + powerCommand <= fixedWingPathFollowerSettings.ThrustLimit.Min && // thrust at minimum
velocityState.Down < 0.0f && // we ARE going up
descentspeedDesired > 0.0f && // we WANT to go down
airspeedError < 0.0f && // we are too fast already
fixedWingPathFollowerSettings.Safetymargins.Highpower > 0.5f) { // alarm switched on
fixedWingPathFollowerStatus.Errors.Highpower = 1;
result = 0;
}
/**
* Compute desired pitch command
*/
// Compute the cross feed from vertical speed to pitch, with saturation
float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedWingPathFollowerSettings.VerticalToPitchCrossFeed.Kp,
-fixedWingPathFollowerSettings.VerticalToPitchCrossFeed.Max,
fixedWingPathFollowerSettings.VerticalToPitchCrossFeed.Max
);
// Compute the pitch command as err*Kp + errInt*Ki + X_feed.
pitchCommand = -pid_apply(&global.PIDspeed, airspeedError, dT) + verticalSpeedToPitchCommandComponent;
fixedWingPathFollowerStatus.Error.Speed = airspeedError;
fixedWingPathFollowerStatus.ErrorInt.Speed = global.PIDspeed.iAccumulator;
fixedWingPathFollowerStatus.Command.Speed = pitchCommand;
stabDesired.Pitch = boundf(fixedWingPathFollowerSettings.PitchLimit.Neutral + pitchCommand,
fixedWingPathFollowerSettings.PitchLimit.Min,
fixedWingPathFollowerSettings.PitchLimit.Max);
// Error condition: high speed dive
fixedWingPathFollowerStatus.Errors.Pitchcontrol = 0;
if (fixedWingPathFollowerSettings.PitchLimit.Neutral + pitchCommand >= fixedWingPathFollowerSettings.PitchLimit.Max && // pitch demand is full up
velocityState.Down > 0.0f && // we ARE going down
descentspeedDesired < 0.0f && // we WANT to go up
airspeedError < 0.0f && // we are too fast already
fixedWingPathFollowerSettings.Safetymargins.Pitchcontrol > 0.5f) { // alarm switched on
fixedWingPathFollowerStatus.Errors.Pitchcontrol = 1;
result = 0;
}
/**
* Compute desired roll command
*/
courseError = RAD2DEG(atan2f(courseComponent[1], courseComponent[0])) - attitudeState.Yaw;
if (courseError < -180.0f) {
courseError += 360.0f;
}
if (courseError > 180.0f) {
courseError -= 360.0f;
}
// overlap calculation. Theres a dead zone behind the craft where the
// counter-yawing of some craft while rolling could render a desired right
// turn into a desired left turn. Making the turn direction based on
// current roll angle keeps the plane committed to a direction once chosen
if (courseError < -180.0f + (fixedWingPathFollowerSettings.ReverseCourseOverlap * 0.5f)
&& attitudeState.Roll > 0.0f) {
courseError += 360.0f;
}
if (courseError > 180.0f - (fixedWingPathFollowerSettings.ReverseCourseOverlap * 0.5f)
&& attitudeState.Roll < 0.0f) {
courseError -= 360.0f;
}
courseCommand = pid_apply(&global.PIDcourse, courseError, dT);
fixedWingPathFollowerStatus.Error.Course = courseError;
fixedWingPathFollowerStatus.ErrorInt.Course = global.PIDcourse.iAccumulator;
fixedWingPathFollowerStatus.Command.Course = courseCommand;
stabDesired.Roll = boundf(fixedWingPathFollowerSettings.RollLimit.Neutral +
courseCommand,
fixedWingPathFollowerSettings.RollLimit.Min,
fixedWingPathFollowerSettings.RollLimit.Max);
// TODO: find a check to determine loss of directional control. Likely needs some check of derivative
/**
* Compute desired yaw command
*/
// TODO implement raw control mode for yaw and base on Accels.Y
stabDesired.Yaw = 0.0f;
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
StabilizationDesiredSet(&stabDesired);
FixedWingPathFollowerStatusSet(&fixedWingPathFollowerStatus);
return result;
}
/**
* Function to calculate course vector C based on airspeed s, fluid movement F
* and desired movement vector V
* parameters in: V,F,s
* parameters out: C
* returns true if a valid solution could be found for V,F,s, false if not
* C will be set to a best effort attempt either way
*/
static bool correctCourse(float *C, float *V, float *F, float s)
{
// Approach:
// Let Sc be a circle around origin marking possible movement vectors
// of the craft with airspeed s (all possible options for C)
// Let Vl be a line through the origin along movement vector V where fr any
// point v on line Vl v = k * (V / |V|) = k' * V
// Let Wl be a line parallel to Vl where for any point v on line Vl exists
// a point w on WL with w = v - F
// Then any intersection between circle Sc and line Wl represents course
// vector which would result in a movement vector
// V' = k * ( V / |V|) = k' * V
// If there is no intersection point, S is insufficient to compensate
// for F and we can only try to fly in direction of V (thus having wind drift
// but at least making progress orthogonal to wind)
s = fabsf(s);
float f = vector_lengthf(F, 2);
// normalize Cn=V/|V|, |V| must be >0
float v = vector_lengthf(V, 2);
if (v < 1e-6f) {
// if |V|=0, we aren't supposed to move, turn into the wind
// (this allows hovering)
C[0] = -F[0];
C[1] = -F[1];
// if desired airspeed matches fluidmovement a hover is actually
// intended so return true
return fabsf(f - s) < 1e-3f;
}
float Vn[2] = { V[0] / v, V[1] / v };
// project F on V
float fp = F[0] * Vn[0] + F[1] * Vn[1];
// find component Fo of F that is orthogonal to V
// (which is exactly the distance between Vl and Wl)
float Fo[2] = { F[0] - (fp * Vn[0]), F[1] - (fp * Vn[1]) };
float fo2 = Fo[0] * Fo[0] + Fo[1] * Fo[1];
// find k where k * Vn = C - Fo
// |C|=s is the hypothenuse in any rectangular triangle formed by k * Vn and Fo
// so k^2 + fo^2 = s^2 (since |Vn|=1)
float k2 = s * s - fo2;
if (k2 <= -1e-3f) {
// there is no solution, we will be drifted off either way
// fallback: fly stupidly in direction of V and hope for the best
C[0] = V[0];
C[1] = V[1];
return false;
} else if (k2 <= 1e-3f) {
// there is exactly one solution: -Fo
C[0] = -Fo[0];
C[1] = -Fo[1];
return true;
}
// we have two possible solutions k positive and k negative as there are
// two intersection points between Wl and Sc
// which one is better? two criteria:
// 1. we MUST move in the right direction, if any k leads to -v its invalid
// 2. we should minimize the speed error
float k = sqrt(k2);
float C1[2] = { -k * Vn[0] - Fo[0], -k * Vn[1] - Fo[1] };
float C2[2] = { k *Vn[0] - Fo[0], k * Vn[1] - Fo[1] };
// project C+F on Vn to find signed resulting movement vector length
float vp1 = (C1[0] + F[0]) * Vn[0] + (C1[1] + F[1]) * Vn[1];
float vp2 = (C2[0] + F[0]) * Vn[0] + (C2[1] + F[1]) * Vn[1];
if (vp1 >= 0.0f && fabsf(v - vp1) < fabsf(v - vp2)) {
// in this case the angle between course and resulting movement vector
// is greater than 90 degrees - so we actually fly backwards
C[0] = C1[0];
C[1] = C1[1];
return true;
}
C[0] = C2[0];
C[1] = C2[1];
if (vp2 >= 0.0f) {
// in this case the angle between course and movement vector is less than
// 90 degrees, but we do move in the right direction
return true;
} else {
// in this case we actually get driven in the opposite direction of V
// with both solutions for C
// this might be reached in headwind stronger than maximum allowed
// airspeed.
return false;
}
}
/**
* Compute desired attitude from the desired velocity
*
* Takes in @ref NedState which has the acceleration in the
* NED frame as the feedback term and then compares the
* @ref VelocityState against the @ref VelocityDesired
*/
static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction)
{
const float dT = updatePeriod / 1000.0f;
uint8_t result = 1;
VelocityDesiredData velocityDesired;
VelocityStateData velocityState;
StabilizationDesiredData stabDesired;
AttitudeStateData attitudeState;
StabilizationBankData stabSettings;
SystemSettingsData systemSettings;
float northError;
float northCommand;
float eastError;
float eastCommand;
float downError;
float downCommand;
SystemSettingsGet(&systemSettings);
VelocityStateGet(&velocityState);
VelocityDesiredGet(&velocityDesired);
StabilizationDesiredGet(&stabDesired);
VelocityDesiredGet(&velocityDesired);
AttitudeStateGet(&attitudeState);
StabilizationBankGet(&stabSettings);
// Testing code - refactor into manual control command
ManualControlCommandData manualControlData;
ManualControlCommandGet(&manualControlData);
// scale velocity if it is above configured maximum
float velH = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East);
if (velH > vtolPathFollowerSettings.HorizontalVelMax) {
velocityDesired.North *= vtolPathFollowerSettings.HorizontalVelMax / velH;
velocityDesired.East *= vtolPathFollowerSettings.HorizontalVelMax / velH;
}
if (fabsf(velocityDesired.Down) > vtolPathFollowerSettings.VerticalVelMax) {
velocityDesired.Down *= vtolPathFollowerSettings.VerticalVelMax / fabsf(velocityDesired.Down);
}
// Compute desired north command
northError = velocityDesired.North - velocityState.North;
northCommand = pid_apply(&global.PIDvel[0], northError, dT) + velocityDesired.North * vtolPathFollowerSettings.VelocityFeedforward;
// Compute desired east command
eastError = velocityDesired.East - velocityState.East;
eastCommand = pid_apply(&global.PIDvel[1], eastError, dT) + velocityDesired.East * vtolPathFollowerSettings.VelocityFeedforward;
// Compute desired down command
downError = velocityDesired.Down - velocityState.Down;
// Must flip this sign
downError = -downError;
downCommand = pid_apply(&global.PIDvel[2], downError, dT);
stabDesired.Thrust = boundf(downCommand + vtolPathFollowerSettings.ThrustLimits.Neutral, vtolPathFollowerSettings.ThrustLimits.Min, vtolPathFollowerSettings.ThrustLimits.Max);
// DEBUG HACK: allow user to skew compass on purpose to see if emergency failsafe kicks in
if (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DEBUGTEST) {
attitudeState.Yaw += 120.0f;
if (attitudeState.Yaw > 180.0f) {
attitudeState.Yaw -= 360.0f;
}
}
if ( // emergency flyaway detection
( // integral already at its limit
vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[0].iAccumulator) < 1e-6f ||
vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[1].iAccumulator) < 1e-6f
) &&
// angle between desired and actual velocity >90 degrees (by dot product)
(velocityDesired.North * velocityState.North + velocityDesired.East * velocityState.East < 0.0f) &&
// quad is moving at significant speed (during flyaway it would keep speeding up)
squaref(velocityState.North) + squaref(velocityState.East) > 1.0f
) {
global.vtolEmergencyFallback += dT;
if (global.vtolEmergencyFallback >= vtolPathFollowerSettings.FlyawayEmergencyFallbackTriggerTime) {
// after emergency timeout, trigger alarm - everything else is handled by callers
// (switch to emergency algorithm, switch to emergency waypoint in pathplanner, alarms, ...)
result = 0;
}
} else {
global.vtolEmergencyFallback = 0.0f;
}
// Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the
// craft should move similarly for 5 deg roll versus 5 deg pitch
stabDesired.Pitch = boundf(-northCommand * cosf(DEG2RAD(attitudeState.Yaw)) +
-eastCommand * sinf(DEG2RAD(attitudeState.Yaw)),
-vtolPathFollowerSettings.MaxRollPitch, vtolPathFollowerSettings.MaxRollPitch);
stabDesired.Roll = boundf(-northCommand * sinf(DEG2RAD(attitudeState.Yaw)) +
eastCommand * cosf(DEG2RAD(attitudeState.Yaw)),
-vtolPathFollowerSettings.MaxRollPitch, vtolPathFollowerSettings.MaxRollPitch);
if (vtolPathFollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) {
// For now override thrust with manual control. Disable at your risk, quad goes to China.
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
stabDesired.Thrust = manualControl.Thrust;
}
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
if (yaw_attitude) {
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Yaw = yaw_direction;
} else {
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControlData.Yaw;
}
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
StabilizationDesiredSet(&stabDesired);
return result;
}
/**
* Compute desired attitude for vtols - emergency fallback
*/
static void updateVtolDesiredAttitudeEmergencyFallback()
{
const float dT = updatePeriod / 1000.0f;
VelocityDesiredData velocityDesired;
VelocityStateData velocityState;
StabilizationDesiredData stabDesired;
float courseError;
float courseCommand;
float downError;
float downCommand;
VelocityStateGet(&velocityState);
VelocityDesiredGet(&velocityDesired);
ManualControlCommandData manualControlData;
ManualControlCommandGet(&manualControlData);
courseError = RAD2DEG(atan2f(velocityDesired.East, velocityDesired.North) - atan2f(velocityState.East, velocityState.North));
if (courseError < -180.0f) {
courseError += 360.0f;
}
if (courseError > 180.0f) {
courseError -= 360.0f;
}
courseCommand = (courseError * vtolPathFollowerSettings.EmergencyFallbackYawRate.kP);
stabDesired.Yaw = boundf(courseCommand, -vtolPathFollowerSettings.EmergencyFallbackYawRate.Max, vtolPathFollowerSettings.EmergencyFallbackYawRate.Max);
// Compute desired down command
downError = velocityDesired.Down - velocityState.Down;
// Must flip this sign
downError = -downError;
downCommand = pid_apply(&global.PIDvel[2], downError, dT);
stabDesired.Thrust = boundf(downCommand + vtolPathFollowerSettings.ThrustLimits.Neutral, vtolPathFollowerSettings.ThrustLimits.Min, vtolPathFollowerSettings.ThrustLimits.Max);
stabDesired.Roll = vtolPathFollowerSettings.EmergencyFallbackAttitude.Roll;
stabDesired.Pitch = vtolPathFollowerSettings.EmergencyFallbackAttitude.Pitch;
if (vtolPathFollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) {
// For now override thrust with manual control. Disable at your risk, quad goes to China.
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
stabDesired.Thrust = manualControl.Thrust;
}
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
StabilizationDesiredSet(&stabDesired);
}
/**
* Compute desired attitude from a fixed preset
*
*/
static void updateFixedAttitude(float *attitude)
{
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
stabDesired.Roll = attitude[0];
stabDesired.Pitch = attitude[1];
stabDesired.Yaw = attitude[2];
stabDesired.Thrust = attitude[3];
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
StabilizationDesiredSet(&stabDesired);
}

11
flight/modules/PathPlanner/pathplanner.c
View File

@ -43,7 +43,6 @@
#include "waypoint.h"
#include "waypointactive.h"
#include "flightmodesettings.h"
#include <pios_struct_helper.h>
#include "paths.h"
#include "plans.h"
@ -525,9 +524,8 @@ static uint8_t conditionLegRemaining()
float cur[3] = { positionState.North, positionState.East, positionState.Down };
struct path_status progress;
path_progress(cast_struct_to_array(pathDesired.Start, pathDesired.Start.North),
cast_struct_to_array(pathDesired.End, pathDesired.End.North),
cur, &progress, pathDesired.Mode);
path_progress(&pathDesired,
cur, &progress);
if (progress.fractional_progress >= 1.0f - pathAction.ConditionParameters[0]) {
return true;
}
@ -550,9 +548,8 @@ static uint8_t conditionBelowError()
float cur[3] = { positionState.North, positionState.East, positionState.Down };
struct path_status progress;
path_progress(cast_struct_to_array(pathDesired.Start, pathDesired.Start.North),
cast_struct_to_array(pathDesired.End, pathDesired.End.North),
cur, &progress, pathDesired.Mode);
path_progress(&pathDesired,
cur, &progress);
if (progress.error <= pathAction.ConditionParameters[0]) {
return true;
}

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

@ -33,7 +33,6 @@
*/
#include <openpilot.h>
#include <pios_struct_helper.h>
#include <accessorydesired.h>
#include <manualcontrolsettings.h>
#include <manualcontrolcommand.h>
@ -210,12 +209,12 @@ static void receiverTask(__attribute__((unused)) void *parameters)
for (uint8_t n = 0; n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) {
extern uint32_t pios_rcvr_group_map[];
if (cast_struct_to_array(settings.ChannelGroups, settings.ChannelGroups.Roll)[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
if (ManualControlSettingsChannelGroupsToArray(settings.ChannelGroups)[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
cmd.Channel[n] = PIOS_RCVR_INVALID;
} else {
cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[
cast_struct_to_array(settings.ChannelGroups, settings.ChannelGroups.Pitch)[n]],
cast_struct_to_array(settings.ChannelNumber, settings.ChannelNumber.Pitch)[n]);
ManualControlSettingsChannelGroupsToArray(settings.ChannelGroups)[n]],
ManualControlSettingsChannelNumberToArray(settings.ChannelNumber)[n]);
}
// If a channel has timed out this is not valid data and we shouldn't update anything
@ -224,9 +223,9 @@ static void receiverTask(__attribute__((unused)) void *parameters)
valid_input_detected = false;
} else {
scaledChannel[n] = scaleChannel(cmd.Channel[n],
cast_struct_to_array(settings.ChannelMax, settings.ChannelMax.Pitch)[n],
cast_struct_to_array(settings.ChannelMin, settings.ChannelMin.Pitch)[n],
cast_struct_to_array(settings.ChannelNeutral, settings.ChannelNeutral.Pitch)[n]);
ManualControlSettingsChannelMaxToArray(settings.ChannelMax)[n],
ManualControlSettingsChannelMinToArray(settings.ChannelMin)[n],
ManualControlSettingsChannelNeutralToArray(settings.ChannelNeutral)[n]);
}
}
@ -445,8 +444,8 @@ static void receiverTask(__attribute__((unused)) void *parameters)
if (pios_usb_rctx_id) {
PIOS_USB_RCTX_Update(pios_usb_rctx_id,
cmd.Channel,
cast_struct_to_array(settings.ChannelMin, settings.ChannelMin.Roll),
cast_struct_to_array(settings.ChannelMax, settings.ChannelMax.Roll),
ManualControlSettingsChannelMinToArray(settings.ChannelMin),
ManualControlSettingsChannelMaxToArray(settings.ChannelMax),
NELEMENTS(cmd.Channel));
}
#endif /* PIOS_INCLUDE_USB_RCTX */
@ -661,8 +660,8 @@ static void applyDeadband(float *value, float deadband)
*/
static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT)
{
if (cast_struct_to_array(settings->ResponseTime, settings->ResponseTime.Roll)[channel]) {
float rt = (float)cast_struct_to_array(settings->ResponseTime, settings->ResponseTime.Roll)[channel];
if (ManualControlSettingsResponseTimeToArray(settings->ResponseTime)[channel]) {
float rt = (float)ManualControlSettingsResponseTimeToArray(settings->ResponseTime)[channel];
inputFiltered[channel] = ((rt * inputFiltered[channel]) + (dT * (*value))) / (rt + dT);
*value = inputFiltered[channel];
}

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

@ -58,20 +58,36 @@
#include <accelgyrosettings.h>
#include <flightstatus.h>
#include <taskinfo.h>
#include <pios_math.h>
#include <CoordinateConversions.h>
#include <pios_board_info.h>
#include <pios_struct_helper.h>
// Private constants
#define STACK_SIZE_BYTES 1000
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
#define SENSOR_PERIOD 2
static const uint32_t sensor_period_ms = ((uint32_t)1000.0f / PIOS_SENSOR_RATE);
// Interval in number of sample to recalculate temp bias
#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 ZERO_ROT_ANGLE 0.00001f
// Private types
#define PIOS_INSTRUMENT_MODULE
#include <pios_instrumentation_helper.h>
PERF_DEFINE_COUNTER(counterGyroSamples);
PERF_DEFINE_COUNTER(counterSensorPeriod);
// Counters:
// - 0x53000001 Sensor fetch rate(period)
// - 0x53000002 number of gyro samples read for each loop
// Private functions
static void SensorsTask(void *parameters);
@ -82,6 +98,11 @@ 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 };
@ -92,6 +113,13 @@ static float mag_transform[3][3] = {
static volatile bool gyro_temp_calibrated = false;
static volatile bool accel_temp_calibrated = false;
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 float R[3][3] = {
{ 0 }
};
@ -200,8 +228,8 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
PIOS_DEBUG_Assert(0);
}
#if defined(PIOS_INCLUDE_HMC5883)
mag_test = PIOS_HMC5883_Test();
#if defined(PIOS_INCLUDE_HMC5X83)
mag_test = PIOS_HMC5x83_Test(onboard_mag);
#else
mag_test = 0;
#endif
@ -215,7 +243,8 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
vTaskDelay(10);
}
}
PERF_INIT_COUNTER(counterGyroSamples, 0x53000001);
PERF_INIT_COUNTER(counterSensorPeriod, 0x53000002);
// Main task loop
lastSysTime = xTaskGetTickCount();
bool error = false;
@ -230,7 +259,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
#endif
lastSysTime = xTaskGetTickCount();
vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_RATE_MS);
vTaskDelayUntil(&lastSysTime, sensor_period_ms / portTICK_RATE_MS);
AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL);
error = false;
} else {
@ -259,7 +288,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
count = 0;
while ((read_good = PIOS_BMA180_ReadFifo(&accel)) != 0 && !error) {
error = ((xTaskGetTickCount() - lastSysTime) > SENSOR_PERIOD) ? true : error;
error = ((xTaskGetTickCount() - lastSysTime) > sensor_period_ms) ? true : error;
}
if (error) {
// Unfortunately if the BMA180 ever misses getting read, then it will not
@ -328,6 +357,9 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
accel_samples++;
}
PERF_MEASURE_PERIOD(counterSensorPeriod);
PERF_TRACK_VALUE(counterGyroSamples, gyro_samples);
if (gyro_samples == 0) {
PIOS_MPU6000_ReadGyros(&mpu6000_data);
error = true;
@ -346,24 +378,43 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
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,
accels[1] * accel_scaling * agcal.accel_scale.Y - agcal.accel_bias.Y,
accels[2] * accel_scaling * agcal.accel_scale.Z - agcal.accel_bias.Z };
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 (accel_temp_calibrated) {
float ctemp = accelSensorData.temperature > agcal.temp_calibrated_extent.max ? agcal.temp_calibrated_extent.max :
(accelSensorData.temperature < agcal.temp_calibrated_extent.min ? agcal.temp_calibrated_extent.min
: accelSensorData.temperature);
accels_out[0] -= agcal.accel_temp_coeff.X * ctemp;
accels_out[1] -= agcal.accel_temp_coeff.Y * ctemp;
accels_out[2] -= agcal.accel_temp_coeff.Z * ctemp;
}
if (rotate) {
rot_mult(R, accels_out, accels);
accelSensorData.x = accels[0];
@ -381,18 +432,10 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
(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,
gyros[1] * gyro_scaling * agcal.gyro_scale.Y - agcal.gyro_bias.Y,
gyros[2] * gyro_scaling * agcal.gyro_scale.Z - agcal.gyro_bias.Z };
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 (gyro_temp_calibrated) {
float ctemp = gyroSensorData.temperature > agcal.temp_calibrated_extent.max ? agcal.temp_calibrated_extent.max :
(gyroSensorData.temperature < agcal.temp_calibrated_extent.min ? agcal.temp_calibrated_extent.min
: gyroSensorData.temperature);
gyros_out[0] -= (agcal.gyro_temp_coeff.X + agcal.gyro_temp_coeff.X2 * ctemp) * ctemp;
gyros_out[1] -= (agcal.gyro_temp_coeff.Y + agcal.gyro_temp_coeff.Y2 * ctemp) * ctemp;
gyros_out[2] -= (agcal.gyro_temp_coeff.Z + agcal.gyro_temp_coeff.Z2 * ctemp) * ctemp;
}
if (rotate) {
rot_mult(R, gyros_out, gyros);
gyroSensorData.x = gyros[0];
@ -409,11 +452,11 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
// 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_HMC5883)
#if defined(PIOS_INCLUDE_HMC5X83)
MagSensorData mag;
if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 150000) {
if (PIOS_HMC5x83_NewDataAvailable(onboard_mag) || PIOS_DELAY_DiffuS(mag_update_time) > 150000) {
int16_t values[3];
PIOS_HMC5883_ReadMag(values);
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] };
@ -428,13 +471,12 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
MagSensorSet(&mag);
mag_update_time = PIOS_DELAY_GetRaw();
}
#endif /* if defined(PIOS_INCLUDE_HMC5883) */
#endif /* if defined(PIOS_INCLUDE_HMC5X83) */
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
#endif
lastSysTime = xTaskGetTickCount();
vTaskDelayUntil(&lastSysTime, sensor_period_ms / portTICK_RATE_MS);
}
}
@ -490,7 +532,7 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
} else {
Quaternion2R(rotationQuat, R);
}
matrix_mult_3x3f((float(*)[3])cast_struct_to_array(cal.mag_transform, cal.mag_transform.r0c0), R, mag_transform);
matrix_mult_3x3f((float(*)[3])RevoCalibrationmag_transformToArray(cal.mag_transform), R, mag_transform);
}
/**
* @}

15
flight/modules/Stabilization/altitudeloop.c
View File

@ -42,7 +42,7 @@
#ifdef REVOLUTION
#define UPDATE_EXPECTED (1.0f / 666.0f)
#define UPDATE_EXPECTED (1.0f / PIOS_SENSOR_RATE)
#define UPDATE_MIN 1.0e-6f
#define UPDATE_MAX 1.0f
#define UPDATE_ALPHA 1.0e-2f
@ -163,8 +163,12 @@ static void altitudeHoldTask(void)
dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
switch (thrustMode) {
case ALTITUDEHOLD:
{
// altitude control loop
altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, thrustSetpoint, positionStateDown, dT);
// No scaling.
const pid_scaler scaler = { .p = 1.0f, .i = 1.0f, .d = 1.0f };
altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, &scaler, thrustSetpoint, positionStateDown, dT);
}
break;
case ALTITUDEVARIO:
altitudeHoldStatus.VelocityDesired = thrustSetpoint;
@ -181,9 +185,12 @@ static void altitudeHoldTask(void)
thrustDemand = thrustSetpoint;
break;
default:
{
// velocity control loop
thrustDemand = startThrust - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, dT);
// No scaling.
const pid_scaler scaler = { .p = 1.0f, .i = 1.0f, .d = 1.0f };
thrustDemand = startThrust - pid_apply_setpoint(&pid1, &scaler, altitudeHoldStatus.VelocityDesired, velocityStateDown, dT);
}
break;
}
}

4
flight/modules/Stabilization/inc/stabilization.h
View File

@ -59,6 +59,8 @@ typedef struct {
} monitor;
float rattitude_mode_transition_stick_position;
struct pid innerPids[3], outerPids[3];
// TPS [Roll,Pitch,Yaw][P,I,D]
bool thrust_pid_scaling_enabled[3][3];
} StabilizationData;
@ -76,7 +78,7 @@ extern StabilizationData stabSettings;
// must be same as eventdispatcher to avoid needing additional mutexes
#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
// outer loop only executes every 4th uavobject update to safe CPU
// outer loop only executes every 4th uavobject update to save CPU
#define OUTERLOOP_SKIPCOUNT 4
#endif // STABILIZATION_H

130
flight/modules/Stabilization/innerloop.c
View File

@ -32,8 +32,8 @@
*/
#include <openpilot.h>
#include <pios_struct_helper.h>
#include <pid.h>
#include <sin_lookup.h>
#include <callbackinfo.h>
#include <ratedesired.h>
#include <actuatordesired.h>
@ -43,6 +43,8 @@
#include <flightstatus.h>
#include <manualcontrolcommand.h>
#include <stabilizationbank.h>
#include <stabilizationdesired.h>
#include <actuatordesired.h>
#include <stabilization.h>
#include <relay_tuning.h>
@ -53,7 +55,7 @@
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_CRITICAL
#define UPDATE_EXPECTED (1.0f / 666.0f)
#define UPDATE_EXPECTED (1.0f / PIOS_SENSOR_RATE)
#define UPDATE_MIN 1.0e-6f
#define UPDATE_MAX 1.0f
#define UPDATE_ALPHA 1.0e-2f
@ -81,6 +83,8 @@ void stabilizationInnerloopInit()
StabilizationStatusInitialize();
FlightStatusInitialize();
ManualControlCommandInitialize();
StabilizationDesiredInitialize();
ActuatorDesiredInitialize();
#ifdef REVOLUTION
AirspeedStateInitialize();
AirspeedStateConnectCallback(AirSpeedUpdatedCb);
@ -94,6 +98,80 @@ void stabilizationInnerloopInit()
PIOS_CALLBACKSCHEDULER_Schedule(callbackHandle, FAILSAFE_TIMEOUT_MS, CALLBACK_UPDATEMODE_LATER);
}
static float get_pid_scale_source_value()
{
float value;
switch (stabSettings.stabBank.ThrustPIDScaleSource) {
case STABILIZATIONBANK_THRUSTPIDSCALESOURCE_MANUALCONTROLTHROTTLE:
ManualControlCommandThrottleGet(&value);
break;
case STABILIZATIONBANK_THRUSTPIDSCALESOURCE_STABILIZATIONDESIREDTHRUST:
StabilizationDesiredThrustGet(&value);
break;
case STABILIZATIONBANK_THRUSTPIDSCALESOURCE_ACTUATORDESIREDTHRUST:
ActuatorDesiredThrustGet(&value);
break;
default:
ActuatorDesiredThrustGet(&value);
break;
}
if (value < 0) {
value = 0.0f;
}
return value;
}
typedef struct pid_curve_scaler {
float x;
pointf points[5];
} pid_curve_scaler;
static float pid_curve_value(const pid_curve_scaler *scaler)
{
float y = y_on_curve(scaler->x, scaler->points, sizeof(scaler->points) / sizeof(scaler->points[0]));
return 1.0f + (IS_REAL(y) ? y : 0.0f);
}
static pid_scaler create_pid_scaler(int axis)
{
pid_scaler scaler;
// Always scaled with the this.
scaler.p = scaler.i = scaler.d = speedScaleFactor;
if (stabSettings.thrust_pid_scaling_enabled[axis][0]
|| stabSettings.thrust_pid_scaling_enabled[axis][1]
|| stabSettings.thrust_pid_scaling_enabled[axis][2]) {
const pid_curve_scaler curve_scaler = {
.x = get_pid_scale_source_value(),
.points = {
{ 0.00f, stabSettings.stabBank.ThrustPIDScaleCurve[0] },
{ 0.25f, stabSettings.stabBank.ThrustPIDScaleCurve[1] },
{ 0.50f, stabSettings.stabBank.ThrustPIDScaleCurve[2] },
{ 0.75f, stabSettings.stabBank.ThrustPIDScaleCurve[3] },
{ 1.00f, stabSettings.stabBank.ThrustPIDScaleCurve[4] }
}
};
float curve_value = pid_curve_value(&curve_scaler);
if (stabSettings.thrust_pid_scaling_enabled[axis][0]) {
scaler.p *= curve_value;
}
if (stabSettings.thrust_pid_scaling_enabled[axis][1]) {
scaler.i *= curve_value;
}
if (stabSettings.thrust_pid_scaling_enabled[axis][2]) {
scaler.d *= curve_value;
}
}
return scaler;
}
/**
* WARNING! This callback executes with critical flight control priority every
@ -165,21 +243,21 @@ static void stabilizationInnerloopTask()
dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
for (t = 0; t < AXES; t++) {
bool reinit = (cast_struct_to_array(enabled, enabled.Roll)[t] != previous_mode[t]);
previous_mode[t] = cast_struct_to_array(enabled, enabled.Roll)[t];
bool reinit = (StabilizationStatusInnerLoopToArray(enabled)[t] != previous_mode[t]);
previous_mode[t] = StabilizationStatusInnerLoopToArray(enabled)[t];
if (t < STABILIZATIONSTATUS_INNERLOOP_THRUST) {
if (reinit) {
stabSettings.innerPids[t].iAccumulator = 0;
}
switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
switch (StabilizationStatusInnerLoopToArray(enabled)[t]) {
case STABILIZATIONSTATUS_INNERLOOP_VIRTUALFLYBAR:
stabilization_virtual_flybar(gyro_filtered[t], rate[t], &actuatorDesiredAxis[t], dT, reinit, t, &stabSettings.settings);
break;
case STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING:
rate[t] = boundf(rate[t],
-cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t],
cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t]
-StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t],
StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t]
);
stabilization_relay_rate(rate[t] - gyro_filtered[t], &actuatorDesiredAxis[t], t, reinit);
break;
@ -198,10 +276,28 @@ static void stabilizationInnerloopTask()
case STABILIZATIONSTATUS_INNERLOOP_RATE:
// limit rate to maximum configured limits (once here instead of 5 times in outer loop)
rate[t] = boundf(rate[t],
-cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t],
cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t]
-StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t],
StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t]
);
actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], speedScaleFactor, rate[t], gyro_filtered[t], dT);
pid_scaler scaler = create_pid_scaler(t);
actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], &scaler, rate[t], gyro_filtered[t], dT);
break;
case STABILIZATIONSTATUS_INNERLOOP_ACRO:
{
float stickinput[3];
stickinput[0] = boundf(rate[0] / stabSettings.stabBank.ManualRate.Roll, -1.0f, 1.0f);
stickinput[1] = boundf(rate[1] / stabSettings.stabBank.ManualRate.Pitch, -1.0f, 1.0f);
stickinput[2] = boundf(rate[2] / stabSettings.stabBank.ManualRate.Yaw, -1.0f, 1.0f);
rate[t] = boundf(rate[t],
-StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t],
StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t]
);
pid_scaler ascaler = create_pid_scaler(t);
ascaler.i *= boundf(1.0f - (1.5f * fabsf(stickinput[t])), 0.0f, 1.0f); // this prevents Integral from getting too high while controlled manually
float arate = pid_apply_setpoint(&stabSettings.innerPids[t], &ascaler, rate[t], gyro_filtered[t], dT);
float factor = fabsf(stickinput[t]) * stabSettings.stabBank.AcroInsanityFactor;
actuatorDesiredAxis[t] = factor * stickinput[t] + (1.0f - factor) * arate;
}
break;
case STABILIZATIONSTATUS_INNERLOOP_DIRECT:
default:
@ -209,7 +305,7 @@ static void stabilizationInnerloopTask()
break;
}
} else {
switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
switch (StabilizationStatusInnerLoopToArray(enabled)[t]) {
case STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL:
actuatorDesiredAxis[t] = cruisecontrol_apply_factor(rate[t]);
break;
@ -233,6 +329,18 @@ static void stabilizationInnerloopTask()
previous_mode[t] = 255;
}
}
if (stabSettings.stabBank.EnablePiroComp == STABILIZATIONBANK_ENABLEPIROCOMP_TRUE && stabSettings.innerPids[0].iLim > 1e-3f && stabSettings.innerPids[1].iLim > 1e-3f) {
// attempted piro compensation - rotate pitch and yaw integrals (experimental)
float angleYaw = DEG2RAD(gyro_filtered[2] * dT);
float sinYaw = sinf(angleYaw);
float cosYaw = cosf(angleYaw);
float rollAcc = stabSettings.innerPids[0].iAccumulator / stabSettings.innerPids[0].iLim;
float pitchAcc = stabSettings.innerPids[1].iAccumulator / stabSettings.innerPids[1].iLim;
stabSettings.innerPids[0].iAccumulator = stabSettings.innerPids[0].iLim * (cosYaw * rollAcc + sinYaw * pitchAcc);
stabSettings.innerPids[1].iAccumulator = stabSettings.innerPids[1].iLim * (cosYaw * pitchAcc - sinYaw * rollAcc);
}
{
uint8_t armed;
FlightStatusArmedGet(&armed);

27
flight/modules/Stabilization/outerloop.c
View File

@ -32,7 +32,6 @@
*/
#include <openpilot.h>
#include <pios_struct_helper.h>
#include <pid.h>
#include <callbackinfo.h>
#include <ratedesired.h>
@ -53,7 +52,7 @@
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR
#define UPDATE_EXPECTED (1.0f / 666.0f)
#define UPDATE_EXPECTED (1.0f / PIOS_SENSOR_RATE)
#define UPDATE_MIN 1.0e-6f
#define UPDATE_MAX 1.0f
#define UPDATE_ALPHA 1.0e-2f
@ -115,7 +114,7 @@ static void stabilizationOuterloopTask()
float q_error[4];
for (t = 0; t < 3; t++) {
switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
switch (StabilizationStatusOuterLoopToArray(enabled)[t]) {
case STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE:
case STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE:
case STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING:
@ -150,14 +149,14 @@ static void stabilizationOuterloopTask()
#endif /* if defined(PIOS_QUATERNION_STABILIZATION) */
}
for (t = 0; t < AXES; t++) {
bool reinit = (cast_struct_to_array(enabled, enabled.Roll)[t] != previous_mode[t]);
previous_mode[t] = cast_struct_to_array(enabled, enabled.Roll)[t];
bool reinit = (StabilizationStatusOuterLoopToArray(enabled)[t] != previous_mode[t]);
previous_mode[t] = StabilizationStatusOuterLoopToArray(enabled)[t];
if (t < STABILIZATIONSTATUS_OUTERLOOP_THRUST) {
if (reinit) {
stabSettings.outerPids[t].iAccumulator = 0;
}
switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
switch (StabilizationStatusOuterLoopToArray(enabled)[t]) {
case STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE:
rateDesiredAxis[t] = pid_apply(&stabSettings.outerPids[t], local_error[t], dT);
break;
@ -167,11 +166,11 @@ static void stabilizationOuterloopTask()
stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f);
stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f);
stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f);
float rateDesiredAxisRate = stickinput[t] * cast_struct_to_array(stabSettings.stabBank.ManualRate, stabSettings.stabBank.ManualRate.Roll)[t];
float rateDesiredAxisRate = stickinput[t] * StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t];
// limit corrective rate to maximum rates to not give it overly large impact over manual rate when joined together
rateDesiredAxis[t] = boundf(pid_apply(&stabSettings.outerPids[t], local_error[t], dT),
-cast_struct_to_array(stabSettings.stabBank.ManualRate, stabSettings.stabBank.ManualRate.Roll)[t],
cast_struct_to_array(stabSettings.stabBank.ManualRate, stabSettings.stabBank.ManualRate.Roll)[t]
-StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t],
StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t]
);
// Compute the weighted average rate desired
// Using max() rather than sqrt() for cpu speed;
@ -232,7 +231,7 @@ static void stabilizationOuterloopTask()
stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f);
stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f);
stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f);
float rate_input = stickinput[t] * cast_struct_to_array(stabSettings.stabBank.ManualRate, stabSettings.stabBank.ManualRate.Roll)[t];
float rate_input = stickinput[t] * StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t];
float weak_leveling = local_error[t] * stabSettings.settings.WeakLevelingKp;
weak_leveling = boundf(weak_leveling, -stabSettings.settings.MaxWeakLevelingRate, stabSettings.settings.MaxWeakLevelingRate);
@ -246,7 +245,7 @@ static void stabilizationOuterloopTask()
break;
}
} else {
switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
switch (StabilizationStatusOuterLoopToArray(enabled)[t]) {
#ifdef REVOLUTION
case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE:
rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEHOLD, reinit);
@ -286,10 +285,10 @@ static void stabilizationOuterloopTask()
static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
// to reduce CPU utilisation, outer loop is not executed every state update
static uint8_t cpusafer = 0;
// to reduce CPU utilization, outer loop is not executed on every state update
static uint8_t cpusaver = 0;
if ((cpusafer++ % OUTERLOOP_SKIPCOUNT) == 0) {
if ((cpusaver++ % OUTERLOOP_SKIPCOUNT) == 0) {
// this does not need mutex protection as both eventdispatcher and stabi run in same callback task!
AttitudeStateGet(&attitude);
PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);

23
flight/modules/Stabilization/relay_tuning.c
View File

@ -32,7 +32,6 @@
*/
#include "openpilot.h"
#include <pios_struct_helper.h>
#include "stabilization.h"
#include "relaytuning.h"
#include "relaytuningsettings.h"
@ -72,8 +71,8 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit)
// On first run initialize estimates to something reasonable
if (reinit) {
rateRelayRunning[axis] = false;
cast_struct_to_array(relay.Period, relay.Period.Roll)[axis] = 200;
cast_struct_to_array(relay.Gain, relay.Gain.Roll)[axis] = 0;
RelayTuningPeriodToArray(relay.Period)[axis] = 200;
RelayTuningGainToArray(relay.Gain)[axis] = 0;
accum_sin = 0;
accum_cos = 0;
@ -96,14 +95,14 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit)
/**** The code below here is to estimate the properties of the oscillation ****/
// Make sure the period can't go below limit
if (cast_struct_to_array(relay.Period, relay.Period.Roll)[axis] < DEGLITCH_TIME) {
cast_struct_to_array(relay.Period, relay.Period.Roll)[axis] = DEGLITCH_TIME;
if (RelayTuningPeriodToArray(relay.Period)[axis] < DEGLITCH_TIME) {
RelayTuningPeriodToArray(relay.Period)[axis] = DEGLITCH_TIME;
}
// Project the error onto a sine and cosine of the same frequency
// to accumulate the average amplitude
int32_t dT = thisTime - lastHighTime;
float phase = ((float)360 * (float)dT) / cast_struct_to_array(relay.Period, relay.Period.Roll)[axis];
float phase = ((float)360 * (float)dT) / RelayTuningPeriodToArray(relay.Period)[axis];
if (phase >= 360) {
phase = 0;
}
@ -126,15 +125,15 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit)
if (rateRelayRunning[axis] == false) {
rateRelayRunning[axis] = true;
cast_struct_to_array(relay.Period, relay.Period.Roll)[axis] = 200;
cast_struct_to_array(relay.Gain, relay.Gain.Roll)[axis] = 0;
RelayTuningPeriodToArray(relay.Period)[axis] = 200;
RelayTuningGainToArray(relay.Gain)[axis] = 0;
} else {
// Low pass filter each amplitude and period
cast_struct_to_array(relay.Gain, relay.Gain.Roll)[axis] =
cast_struct_to_array(relay.Gain, relay.Gain.Roll)[axis] *
RelayTuningGainToArray(relay.Gain)[axis] =
RelayTuningGainToArray(relay.Gain)[axis] *
AMPLITUDE_ALPHA + this_gain * (1 - AMPLITUDE_ALPHA);
cast_struct_to_array(relay.Period, relay.Period.Roll)[axis] =
cast_struct_to_array(relay.Period, relay.Period.Roll)[axis] *
RelayTuningPeriodToArray(relay.Period)[axis] =
RelayTuningPeriodToArray(relay.Period)[axis] *
PERIOD_ALPHA + dT * (1 - PERIOD_ALPHA);
}
lastHighTime = thisTime;

133
flight/modules/Stabilization/stabilization.c
View File

@ -33,7 +33,6 @@
#include <openpilot.h>
#include <pios_struct_helper.h>
#include <pid.h>
#include <manualcontrolcommand.h>
#include <flightmodesettings.h>
@ -134,54 +133,58 @@ static void StabilizationDesiredUpdatedCb(__attribute__((unused)) UAVObjEvent *e
StabilizationDesiredStabilizationModeGet(&mode);
for (t = 0; t < AXES; t++) {
switch (cast_struct_to_array(mode, mode.Roll)[t]) {
switch (StabilizationDesiredStabilizationModeToArray(mode)[t]) {
case STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_DIRECT;
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_DIRECT;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE;
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_AXISLOCK;
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_AXISLOCK;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING;
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_VIRTUALFLYBAR;
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_VIRTUALFLYBAR;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO:
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_ACRO;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE;
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING;
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING;
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE;
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE;
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO;
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
break;
}
}
@ -230,6 +233,66 @@ static void SettingsBankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
StabilizationBankSet(&stabSettings.stabBank);
}
static bool use_tps_for_roll()
{
uint8_t axes = stabSettings.stabBank.ThrustPIDScaleAxes;
return axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCHYAW ||
axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCH ||
axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLYAW ||
axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLL;
}
static bool use_tps_for_pitch()
{
uint8_t axes = stabSettings.stabBank.ThrustPIDScaleAxes;
return axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCHYAW ||
axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCH ||
axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_PITCHYAW ||
axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_PITCH;
}
static bool use_tps_for_yaw()
{
uint8_t axes = stabSettings.stabBank.ThrustPIDScaleAxes;
return axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCHYAW ||
axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLYAW ||
axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_PITCHYAW ||
axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_YAW;
}
static bool use_tps_for_p()
{
uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget;
return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PI ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PD ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_P;
}
static bool use_tps_for_i()
{
uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget;
return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PI ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_ID ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_I;
}
static bool use_tps_for_d()
{
uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget;
return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PD ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_ID ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_D;
}
static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
StabilizationBankGet(&stabSettings.stabBank);
@ -269,6 +332,24 @@ static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
stabSettings.stabBank.YawPI.Ki,
0,
stabSettings.stabBank.YawPI.ILimit);
bool tps_for_axis[3] = {
use_tps_for_roll(),
use_tps_for_pitch(),
use_tps_for_yaw()
};
bool tps_for_pid[3] = {
use_tps_for_p(),
use_tps_for_i(),
use_tps_for_d()
};
for (int axis = 0; axis < 3; axis++) {
for (int pid = 0; pid < 3; pid++) {
stabSettings.thrust_pid_scaling_enabled[axis][pid] = stabSettings.stabBank.EnableThrustPIDScaling
&& tps_for_axis[axis]
&& tps_for_pid[pid];
}
}
}

3
flight/modules/Stabilization/virtualflybar.c
View File

@ -32,7 +32,6 @@
#include "openpilot.h"
#include <pios_math.h>
#include <pios_struct_helper.h>
#include "stabilization.h"
#include "stabilizationsettings.h"
@ -78,7 +77,7 @@ int stabilization_virtual_flybar(float gyro, float command, float *output, float
}
// Command signal is composed of stick input added to the gyro and virtual flybar
*output = command * cast_struct_to_array(settings->VbarSensitivity, settings->VbarSensitivity.Roll)[axis] -
*output = command * StabilizationSettingsVbarSensitivityToArray(settings->VbarSensitivity)[axis] -
gyro_gain * (vbar_integral[axis] * ki + gyro * kp);
return 0;

4
flight/modules/StateEstimation/filtercf.c
View File

@ -221,9 +221,9 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float
// During initialization and
if (this->first_run) {
#if defined(PIOS_INCLUDE_HMC5883)
#if defined(PIOS_INCLUDE_HMC5X83)
// wait until mags have been updated
if (!this->magUpdated) {
if (!this->magUpdated && this->useMag) {
return FILTERRESULT_ERROR;
}
#else

17
flight/modules/StateEstimation/filterekf.c
View File

@ -31,7 +31,6 @@
*/
#include "inc/stateestimation.h"
#include <pios_struct_helper.h>
#include <ekfconfiguration.h>
#include <ekfstatevariance.h>
@ -48,7 +47,7 @@
#define DT_ALPHA 1e-3f
#define DT_MIN 1e-6f
#define DT_MAX 1.0f
#define DT_INIT (1.0f / 666.0f) // initialize with 666 Hz (default sensor update rate on revo)
#define DT_INIT (1.0f / PIOS_SENSOR_RATE) // initialize with board sensor rate
#define IMPORT_SENSOR_IF_UPDATED(shortname, num) \
if (IS_SET(state->updated, SENSORUPDATES_##shortname)) { \
@ -165,17 +164,17 @@ static int32_t maininit(stateFilter *self)
int t;
// plausibility check
for (t = 0; t < EKFCONFIGURATION_P_NUMELEM; t++) {
if (invalid_var(cast_struct_to_array(this->ekfConfiguration.P, this->ekfConfiguration.P.AttitudeQ1)[t])) {
if (invalid_var(EKFConfigurationPToArray(this->ekfConfiguration.P)[t])) {
return 2;
}
}
for (t = 0; t < EKFCONFIGURATION_Q_NUMELEM; t++) {
if (invalid_var(cast_struct_to_array(this->ekfConfiguration.Q, this->ekfConfiguration.Q.AccelX)[t])) {
if (invalid_var(EKFConfigurationQToArray(this->ekfConfiguration.Q)[t])) {
return 2;
}
}
for (t = 0; t < EKFCONFIGURATION_R_NUMELEM; t++) {
if (invalid_var(cast_struct_to_array(this->ekfConfiguration.R, this->ekfConfiguration.R.BaroZ)[t])) {
if (invalid_var(EKFConfigurationRToArray(this->ekfConfiguration.R)[t])) {
return 2;
}
}
@ -295,7 +294,7 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
INSSetState(this->work.pos, (float *)zeros, this->work.attitude, (float *)zeros, (float *)zeros);
INSResetP(cast_struct_to_array(this->ekfConfiguration.P, this->ekfConfiguration.P.AttitudeQ1));
INSResetP(EKFConfigurationPToArray(this->ekfConfiguration.P));
} else {
// Run prediction a bit before any corrections
@ -422,12 +421,12 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
EKFStateVarianceData vardata;
EKFStateVarianceGet(&vardata);
INSGetP(cast_struct_to_array(vardata.P, vardata.P.AttitudeQ1));
INSGetP(EKFStateVariancePToArray(vardata.P));
EKFStateVarianceSet(&vardata);
int t;
for (t = 0; t < EKFSTATEVARIANCE_P_NUMELEM; t++) {
if (!IS_REAL(cast_struct_to_array(vardata.P, vardata.P.AttitudeQ1)[t]) || cast_struct_to_array(vardata.P, vardata.P.AttitudeQ1)[t] <= 0.0f) {
INSResetP(cast_struct_to_array(this->ekfConfiguration.P, this->ekfConfiguration.P.AttitudeQ1));
if (!IS_REAL(EKFStateVariancePToArray(vardata.P)[t]) || EKFStateVariancePToArray(vardata.P)[t] <= 0.0f) {
INSResetP(EKFConfigurationPToArray(this->ekfConfiguration.P));
this->init_stage = -1;
break;
}

2
flight/modules/StateEstimation/filterlla.c
View File

@ -103,7 +103,7 @@ static filterResult filter(__attribute__((unused)) stateFilter *self, stateEstim
GPSPositionSensorGet(&gpsdata);
// check if we have a valid GPS signal (not checked by StateEstimation istelf)
if ((gpsdata.PDOP < this->settings.MaxPDOP) && (gpsdata.Satellites >= this->settings.MinSattelites) &&
if ((gpsdata.PDOP < this->settings.MaxPDOP) && (gpsdata.Satellites >= this->settings.MinSatellites) &&
(gpsdata.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) &&
(gpsdata.Latitude != 0 || gpsdata.Longitude != 0)) {
int32_t LLAi[3] = {

122
flight/modules/StateEstimation/filtermag.c
View File

@ -36,8 +36,9 @@
#include <revosettings.h>
#include <systemalarms.h>
#include <homelocation.h>
#include <auxmagsettings.h>
#include <CoordinateConversions.h>
#include <mathmisc.h>
// Private constants
//
@ -47,10 +48,12 @@
struct data {
RevoCalibrationData revoCalibration;
RevoSettingsData revoSettings;
AuxMagSettingsUsageOptions auxMagUsage;
uint8_t warningcount;
uint8_t errorcount;
float homeLocationBe[3];
float magBe2;
float magBe;
float invMagBe;
float magBias[3];
};
@ -60,9 +63,9 @@ struct data {
static int32_t init(stateFilter *self);
static filterResult filter(stateFilter *self, stateEstimation *state);
static void checkMagValidity(struct data *this, float mag[3]);
static bool checkMagValidity(struct data *this, float error, bool setAlarms);
static void magOffsetEstimation(struct data *this, float mag[3]);
static float getMagError(struct data *this, float mag[3]);
int32_t filterMagInitialize(stateFilter *handle)
{
@ -80,80 +83,132 @@ static int32_t init(stateFilter *self)
this->magBias[0] = this->magBias[1] = this->magBias[2] = 0.0f;
this->warningcount = this->errorcount = 0;
HomeLocationBeGet(this->homeLocationBe);
// magBe2 holds the squared magnetic vector length (extpected)
this->magBe2 = this->homeLocationBe[0] * this->homeLocationBe[0] + this->homeLocationBe[1] * this->homeLocationBe[1] + this->homeLocationBe[2] * this->homeLocationBe[2];
// magBe holds the magnetic vector length (expected)
this->magBe = vector_lengthf(this->homeLocationBe, 3);
this->invMagBe = 1.0f / this->magBe;
RevoCalibrationGet(&this->revoCalibration);
RevoSettingsGet(&this->revoSettings);
AuxMagSettingsUsageGet(&this->auxMagUsage);
return 0;
}
static filterResult filter(stateFilter *self, stateEstimation *state)
{
struct data *this = (struct data *)self->localdata;
float auxMagError;
float boardMagError;
float temp_mag[3] = { 0 };
uint8_t temp_status = MAGSTATUS_INVALID;
uint8_t magSamples = 0;
if (IS_SET(state->updated, SENSORUPDATES_mag)) {
checkMagValidity(this, state->mag);
// Uses the external mag when available
if ((this->auxMagUsage != AUXMAGSETTINGS_USAGE_ONBOARDONLY) &&
IS_SET(state->updated, SENSORUPDATES_auxMag)) {
auxMagError = getMagError(this, state->auxMag);
// Handles alarms only if it will rely on aux mag only
bool auxMagValid = checkMagValidity(this, auxMagError, (this->auxMagUsage == AUXMAGSETTINGS_USAGE_AUXONLY));
// if we are going to use Aux only, force the update even if mag is invalid
if (auxMagValid || (this->auxMagUsage == AUXMAGSETTINGS_USAGE_AUXONLY)) {
temp_mag[0] = state->auxMag[0];
temp_mag[1] = state->auxMag[1];
temp_mag[2] = state->auxMag[2];
temp_status = MAGSTATUS_AUX;
magSamples++;
}
}
if ((this->auxMagUsage != AUXMAGSETTINGS_USAGE_AUXONLY) &&
IS_SET(state->updated, SENSORUPDATES_boardMag)) {
// TODO:mag Offset estimation works with onboard mag only right now.
if (this->revoCalibration.MagBiasNullingRate > 0) {
magOffsetEstimation(this, state->mag);
magOffsetEstimation(this, state->boardMag);
}
boardMagError = getMagError(this, state->boardMag);
// sets warning only if no mag data are available (aux is invalid or missing)
bool boardMagValid = checkMagValidity(this, boardMagError, (temp_status == MAGSTATUS_INVALID));
// force it to be set to board mag value if no data has been feed to temp_mag yet.
// this works also as a failsafe in case aux mag stops feeding data.
if (boardMagValid || (temp_status == MAGSTATUS_INVALID)) {
temp_mag[0] += state->boardMag[0];
temp_mag[1] += state->boardMag[1];
temp_mag[2] += state->boardMag[2];
temp_status = MAGSTATUS_OK;
magSamples++;
}
}
if (magSamples > 1) {
temp_mag[0] *= 0.5f;
temp_mag[1] *= 0.5f;
temp_mag[2] *= 0.5f;
}
if (temp_status != MAGSTATUS_INVALID) {
state->mag[0] = temp_mag[0];
state->mag[1] = temp_mag[1];
state->mag[2] = temp_mag[2];
state->updated |= SENSORUPDATES_mag;
}
state->magStatus = temp_status;
return FILTERRESULT_OK;
}
/**
* check validity of magnetometers
*/
static void checkMagValidity(struct data *this, float mag[3])
static bool checkMagValidity(struct data *this, float error, bool setAlarms)
{
#define ALARM_THRESHOLD 5
// mag2 holds the actual magnetic vector length (squared)
float mag2 = mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2];
// warning and error thresholds
// avoud sqrt() : minlimit<actual<maxlimit === minlimit²<actual²<maxlimit²
//
// actual = |mag|
// minlimit = |Be| - maxDeviation*|Be| = |Be| * (1 - maxDeviation)
// maxlimit = |Be| + maxDeviation*|Be| = |Be| * (1 + maxDeviation)
// minlimit² = |Be|² * ( 1 - 2*maxDeviation + maxDeviation²)
// maxlimit² = |Be|² * ( 1 + 2*maxDeviation + maxDeviation²)
//
float minWarning2 = this->magBe2 * (1.0f - 2.0f * this->revoSettings.MagnetometerMaxDeviation.Warning + this->revoSettings.MagnetometerMaxDeviation.Warning * this->revoSettings.MagnetometerMaxDeviation.Warning);
float maxWarning2 = this->magBe2 * (1.0f + 2.0f * this->revoSettings.MagnetometerMaxDeviation.Warning + this->revoSettings.MagnetometerMaxDeviation.Warning * this->revoSettings.MagnetometerMaxDeviation.Warning);
float minError2 = this->magBe2 * (1.0f - 2.0f * this->revoSettings.MagnetometerMaxDeviation.Error + this->revoSettings.MagnetometerMaxDeviation.Error * this->revoSettings.MagnetometerMaxDeviation.Error);
float maxError2 = this->magBe2 * (1.0f + 2.0f * this->revoSettings.MagnetometerMaxDeviation.Error + this->revoSettings.MagnetometerMaxDeviation.Error * this->revoSettings.MagnetometerMaxDeviation.Error);
// set errors
if (minWarning2 < mag2 && mag2 < maxWarning2) {
if (error < this->revoSettings.MagnetometerMaxDeviation.Warning) {
this->warningcount = 0;
this->errorcount = 0;
AlarmsClear(SYSTEMALARMS_ALARM_MAGNETOMETER);
} else if (minError2 < mag2 && mag2 < maxError2) {
return true;
}
if (error < this->revoSettings.MagnetometerMaxDeviation.Error) {
this->errorcount = 0;
if (this->warningcount > ALARM_THRESHOLD) {
if (setAlarms) {
AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_WARNING);
}
return false;
} else {
this->warningcount++;
return true;
}
} else {
}
if (this->errorcount > ALARM_THRESHOLD) {
if (setAlarms) {
AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_CRITICAL);
}
return false;
} else {
this->errorcount++;
}
}
// still in "grace period"
return true;
}
static float getMagError(struct data *this, float mag[3])
{
// vector norm
float magnitude = vector_lengthf(mag, 3);
// absolute value of relative error against Be
float error = fabsf(magnitude - this->magBe) * this->invMagBe;
return error;
}
/**
* Perform an update of the @ref MagBias based on
* Magmeter Offset Cancellation: Theory and Implementation,
* revisited William Premerlani, October 14, 2011
*/
static void magOffsetEstimation(struct data *this, float mag[3])
void magOffsetEstimation(struct data *this, float mag[3])
{
#if 0
// Constants, to possibly go into a UAVO
@ -244,7 +299,6 @@ static void magOffsetEstimation(struct data *this, float mag[3])
#endif // if 0
}
/**
* @}
* @}

143
flight/modules/StateEstimation/filtervelocity.c Normal file
View File

@ -0,0 +1,143 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup State Estimation
* @brief Acquires sensor data and computes state estimate
* @{
*
* @file filtervelocity.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
* @brief Barometric altitude filter, calculates altitude offset based on
* GPS altitude offset if available
*
* @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 "inc/stateestimation.h"
#include <revosettings.h>
// Private constants
#define STACK_REQUIRED 128
#define DT_ALPHA 1e-3f
#define DT_MIN 1e-6f
#define DT_MAX 1.0f
#define DT_INIT (1.0f / PIOS_SENSOR_RATE) // initialize with board sensor rate
// Private types
struct data {
float vel[3];
float velocityBias[3];
float oldPos[3];
float alpha;
uint8_t inited;
PiOSDeltatimeConfig dtconfig;
};
// Private variables
// Private functions
static int32_t init(stateFilter *self);
static filterResult filter(stateFilter *self, stateEstimation *state);
int32_t filterVelocityInitialize(stateFilter *handle)
{
handle->init = &init;
handle->filter = &filter;
handle->localdata = pios_malloc(sizeof(struct data));
return STACK_REQUIRED;
}
static int32_t init(stateFilter *self)
{
struct data *this = (struct data *)self->localdata;
this->vel[0] = 0.0f;
this->vel[1] = 0.0f;
this->vel[2] = 0.0f;
this->velocityBias[0] = 0.0f;
this->velocityBias[1] = 0.0f;
this->velocityBias[2] = 0.0f;
this->oldPos[0] = 0.0f;
this->oldPos[1] = 0.0f;
this->oldPos[2] = 0.0f;
this->inited = 0;
RevoSettingsInitialize();
RevoSettingsVelocityPostProcessingLowPassAlphaGet(&this->alpha);
PIOS_DELTATIME_Init(&this->dtconfig, DT_INIT, DT_MIN, DT_MAX, DT_ALPHA);
return 0;
}
static filterResult filter(stateFilter *self, stateEstimation *state)
{
struct data *this = (struct data *)self->localdata;
if (IS_SET(state->updated, SENSORUPDATES_pos)) {
float dT;
dT = PIOS_DELTATIME_GetAverageSeconds(&this->dtconfig);
// position updates allow us to calculate an ad-hoc velocity estimate
// it will be very noisy and likely quite wrong at every given point in time,
// but its long term average error should be quite low
if (this->inited >= 1) { // only calculate velocity estimate if previous position is known
this->vel[0] = (state->pos[0] - this->oldPos[0]) / dT;
this->vel[1] = (state->pos[1] - this->oldPos[1]) / dT;
this->vel[2] = (state->pos[2] - this->oldPos[2]) / dT;
this->inited = 2;
} else {
// mark previous position as known
this->inited = 1;
}
this->oldPos[0] = state->pos[0];
this->oldPos[1] = state->pos[1];
this->oldPos[2] = state->pos[2];
}
if (IS_SET(state->updated, SENSORUPDATES_vel)) {
// if we have both a velocity estimate from filter and a velocity estimate from postion
// we can calculate a bias estimate. It must be heavily low pass filtered to become useful
// this assumes that the bias is relatively constant over time
if (this->inited >= 2) { // only calculate bias if velocity estimate is calculated
this->velocityBias[0] *= this->alpha;
this->velocityBias[0] += (1.0f - this->alpha) * (this->vel[0] - state->vel[0]);
this->velocityBias[1] *= this->alpha;
this->velocityBias[1] += (1.0f - this->alpha) * (this->vel[1] - state->vel[1]);
this->velocityBias[2] *= this->alpha;
this->velocityBias[2] += (1.0f - this->alpha) * (this->vel[2] - state->vel[2]);
}
state->vel[0] += this->velocityBias[0];
state->vel[1] += this->velocityBias[1];
state->vel[2] += this->velocityBias[2];
}
return FILTERRESULT_OK;
}
/**
* @}
* @}
*/

9
flight/modules/StateEstimation/inc/stateestimation.h
View File

@ -47,6 +47,8 @@ typedef enum {
SENSORUPDATES_gyro = 1 << 0,
SENSORUPDATES_accel = 1 << 1,
SENSORUPDATES_mag = 1 << 2,
SENSORUPDATES_boardMag = 1 << 10,
SENSORUPDATES_auxMag = 1 << 9,
SENSORUPDATES_attitude = 1 << 3,
SENSORUPDATES_pos = 1 << 4,
SENSORUPDATES_vel = 1 << 5,
@ -55,6 +57,9 @@ typedef enum {
SENSORUPDATES_lla = 1 << 8,
} sensorUpdates;
#define MAGSTATUS_OK 1
#define MAGSTATUS_AUX 2
#define MAGSTATUS_INVALID 0
typedef struct {
float gyro[3];
float accel[3];
@ -64,6 +69,9 @@ typedef struct {
float vel[3];
float airspeed[2];
float baro[1];
float auxMag[3];
uint8_t magStatus;
float boardMag[3];
sensorUpdates updated;
} stateEstimation;
@ -77,6 +85,7 @@ typedef struct stateFilterStruct {
int32_t filterMagInitialize(stateFilter *handle);
int32_t filterBaroiInitialize(stateFilter *handle);
int32_t filterBaroInitialize(stateFilter *handle);
int32_t filterVelocityInitialize(stateFilter *handle);
int32_t filterAltitudeInitialize(stateFilter *handle);
int32_t filterAirInitialize(stateFilter *handle);
int32_t filterStationaryInitialize(stateFilter *handle);

41
flight/modules/StateEstimation/stateestimation.c
View File

@ -41,6 +41,7 @@
#include <gpspositionsensor.h>
#include <gpsvelocitysensor.h>
#include <homelocation.h>
#include <auxmagsensor.h>
#include <gyrostate.h>
#include <accelstate.h>
@ -146,6 +147,7 @@ static const filterPipeline *filterChain = NULL;
static stateFilter magFilter;
static stateFilter baroFilter;
static stateFilter baroiFilter;
static stateFilter velocityFilter;
static stateFilter altitudeFilter;
static stateFilter airFilter;
static stateFilter stationaryFilter;
@ -218,11 +220,14 @@ static const filterPipeline *ekf13iQueue = &(filterPipeline) {
.filter = &stationaryFilter,
.next = &(filterPipeline) {
.filter = &ekf13iFilter,
.next = &(filterPipeline) {
.filter = &velocityFilter,
.next = NULL,
}
}
}
}
}
};
static const filterPipeline *ekf13Queue = &(filterPipeline) {
@ -235,11 +240,14 @@ static const filterPipeline *ekf13Queue = &(filterPipeline) {
.filter = &baroFilter,
.next = &(filterPipeline) {
.filter = &ekf13Filter,
.next = &(filterPipeline) {
.filter = &velocityFilter,
.next = NULL,
}
}
}
}
}
};
// Private functions
@ -267,6 +275,7 @@ int32_t StateEstimationInitialize(void)
GyroSensorInitialize();
MagSensorInitialize();
AuxMagSensorInitialize();
BaroSensorInitialize();
AirspeedSensorInitialize();
GPSVelocitySensorInitialize();
@ -290,6 +299,7 @@ int32_t StateEstimationInitialize(void)
MagSensorConnectCallback(&sensorUpdatedCb);
BaroSensorConnectCallback(&sensorUpdatedCb);
AirspeedSensorConnectCallback(&sensorUpdatedCb);
AuxMagSensorConnectCallback(&sensorUpdatedCb);
GPSVelocitySensorConnectCallback(&sensorUpdatedCb);
GPSPositionSensorConnectCallback(&sensorUpdatedCb);
@ -298,6 +308,7 @@ int32_t StateEstimationInitialize(void)
stack_required = maxint32_t(stack_required, filterMagInitialize(&magFilter));
stack_required = maxint32_t(stack_required, filterBaroiInitialize(&baroiFilter));
stack_required = maxint32_t(stack_required, filterBaroInitialize(&baroFilter));
stack_required = maxint32_t(stack_required, filterVelocityInitialize(&velocityFilter));
stack_required = maxint32_t(stack_required, filterAltitudeInitialize(&altitudeFilter));
stack_required = maxint32_t(stack_required, filterAirInitialize(&airFilter));
stack_required = maxint32_t(stack_required, filterStationaryInitialize(&stationaryFilter));
@ -423,7 +434,8 @@ static void StateEstimationCb(void)
gyroRaw[2] = states.gyro[2];
}
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AccelSensor, accel, x, y, z);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(MagSensor, mag, x, y, z);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(MagSensor, boardMag, x, y, z);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AuxMagSensor, auxMag, x, y, z);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GPSVelocitySensor, vel, North, East, Down);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(BaroSensor, baro, Altitude, true);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_2_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(AirspeedSensor, airspeed, CalibratedAirspeed, TrueAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE);
@ -467,7 +479,26 @@ static void StateEstimationCb(void)
gyroDelta[2] = states.gyro[2] - gyroRaw[2];
}
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(AccelState, accel, x, y, z);
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(MagState, mag, x, y, z);
if (IS_SET(states.updated, SENSORUPDATES_mag)) {
MagStateData s;
MagStateGet(&s);
s.x = states.mag[0];
s.y = states.mag[1];
s.z = states.mag[2];
switch (states.magStatus) {
case MAGSTATUS_OK:
s.Source = MAGSTATE_SOURCE_ONBOARD;
break;
case MAGSTATUS_AUX:
s.Source = MAGSTATE_SOURCE_AUX;
break;
default:
s.Source = MAGSTATE_SOURCE_INVALID;
}
MagStateSet(&s);
}
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(PositionState, pos, North, East, Down);
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(VelocityState, vel, North, East, Down);
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_2_DIMENSIONS(AirspeedState, airspeed, CalibratedAirspeed, TrueAirspeed);
@ -567,7 +598,11 @@ static void sensorUpdatedCb(UAVObjEvent *ev)
}
if (ev->obj == MagSensorHandle()) {
updatedSensors |= SENSORUPDATES_mag;
updatedSensors |= SENSORUPDATES_boardMag;
}
if (ev->obj == AuxMagSensorHandle()) {
updatedSensors |= SENSORUPDATES_auxMag;
}
if (ev->obj == GPSPositionSensorHandle()) {

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

@ -39,10 +39,13 @@
*/
#include <openpilot.h>
#include <pios_struct_helper.h>
// private includes
#include "inc/systemmod.h"
#include "notification.h"
#include <notification.h>
#ifdef PIOS_INCLUDE_WS2811
#include <lednotification.h>
#endif
// UAVOs
#include <objectpersistence.h>
@ -55,6 +58,7 @@
#include <callbackinfo.h>
#include <hwsettings.h>
#include <pios_flashfs.h>
#include <pios_notify.h>
#ifdef PIOS_INCLUDE_INSTRUMENTATION
#include <instrumentation.h>
@ -420,7 +424,7 @@ static void taskMonitorForEachCallback(uint16_t task_id, const struct pios_task_
// By convention, there is a direct mapping between task monitor task_id's and members
// of the TaskInfoXXXXElem enums
PIOS_DEBUG_Assert(task_id < TASKINFO_RUNNING_NUMELEM);
cast_struct_to_array(taskData->Running, taskData->Running.System)[task_id] = task_info->is_running ? TASKINFO_RUNNING_TRUE : TASKINFO_RUNNING_FALSE;
TaskInfoRunningToArray(taskData->Running)[task_id] = task_info->is_running ? TASKINFO_RUNNING_TRUE : TASKINFO_RUNNING_FALSE;
((uint16_t *)&taskData->StackRemaining)[task_id] = task_info->stack_remaining;
((uint8_t *)&taskData->RunningTime)[task_id] = task_info->running_time_percentage;
}
@ -633,6 +637,9 @@ static void updateSystemAlarms()
void vApplicationIdleHook(void)
{
NotificationOnboardLedsRun();
#ifdef PIOS_INCLUDE_WS2811
LedNotificationExtLedsRun();
#endif
}
/**
* Called by the RTOS when a stack overflow is detected.

91
flight/modules/TxPID/txpid.c
View File

@ -51,7 +51,6 @@
*/
#include "openpilot.h"
#include <pios_struct_helper.h>
#include "txpidsettings.h"
#include "accessorydesired.h"
#include "manualcontrolcommand.h"
@ -83,6 +82,8 @@
// Private functions
static void updatePIDs(UAVObjEvent *ev);
static uint8_t update(float *var, float val);
static uint8_t updateUint8(uint8_t *var, float val);
static uint8_t updateInt8(int8_t *var, float val);
static float scale(float val, float inMin, float inMax, float outMin, float outMax);
/**
@ -194,26 +195,26 @@ static void updatePIDs(UAVObjEvent *ev)
// Loop through every enabled instance
for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) {
if (cast_struct_to_array(inst.PIDs, inst.PIDs.Instance1)[i] != TXPIDSETTINGS_PIDS_DISABLED) {
if (TxPIDSettingsPIDsToArray(inst.PIDs)[i] != TXPIDSETTINGS_PIDS_DISABLED) {
float value;
if (cast_struct_to_array(inst.Inputs, inst.Inputs.Instance1)[i] == TXPIDSETTINGS_INPUTS_THROTTLE) {
if (TxPIDSettingsInputsToArray(inst.Inputs)[i] == TXPIDSETTINGS_INPUTS_THROTTLE) {
ManualControlCommandThrottleGet(&value);
value = scale(value,
inst.ThrottleRange.Min,
inst.ThrottleRange.Max,
cast_struct_to_array(inst.MinPID, inst.MinPID.Instance1)[i],
cast_struct_to_array(inst.MaxPID, inst.MaxPID.Instance1)[i]);
TxPIDSettingsMinPIDToArray(inst.MinPID)[i],
TxPIDSettingsMaxPIDToArray(inst.MaxPID)[i]);
} else if (AccessoryDesiredInstGet(
cast_struct_to_array(inst.Inputs, inst.Inputs.Instance1)[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0,
TxPIDSettingsInputsToArray(inst.Inputs)[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0,
&accessory) == 0) {
value = scale(accessory.AccessoryVal, -1.0f, 1.0f,
cast_struct_to_array(inst.MinPID, inst.MinPID.Instance1)[i],
cast_struct_to_array(inst.MaxPID, inst.MaxPID.Instance1)[i]);
TxPIDSettingsMinPIDToArray(inst.MinPID)[i],
TxPIDSettingsMaxPIDToArray(inst.MaxPID)[i]);
} else {
continue;
}
switch (cast_struct_to_array(inst.PIDs, inst.PIDs.Instance1)[i]) {
switch (TxPIDSettingsPIDsToArray(inst.PIDs)[i]) {
case TXPIDSETTINGS_PIDS_ROLLRATEKP:
needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
break;
@ -226,6 +227,9 @@ static void updatePIDs(UAVObjEvent *ev)
case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT:
needsUpdateBank |= update(&bank.RollRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATERESP:
needsUpdateBank |= update(&bank.ManualRate.Roll, value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP:
needsUpdateBank |= update(&bank.RollPI.Kp, value);
break;
@ -235,6 +239,9 @@ static void updatePIDs(UAVObjEvent *ev)
case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT:
needsUpdateBank |= update(&bank.RollPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDERESP:
needsUpdateBank |= updateUint8(&bank.RollMax, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKP:
needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
break;
@ -247,6 +254,9 @@ static void updatePIDs(UAVObjEvent *ev)
case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT:
needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATERESP:
needsUpdateBank |= update(&bank.ManualRate.Pitch, value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP:
needsUpdateBank |= update(&bank.PitchPI.Kp, value);
break;
@ -256,6 +266,9 @@ static void updatePIDs(UAVObjEvent *ev)
case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT:
needsUpdateBank |= update(&bank.PitchPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDERESP:
needsUpdateBank |= updateUint8(&bank.PitchMax, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP:
needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
@ -272,6 +285,10 @@ static void updatePIDs(UAVObjEvent *ev)
needsUpdateBank |= update(&bank.RollRatePID.ILimit, value);
needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATERESP:
needsUpdateBank |= update(&bank.ManualRate.Roll, value);
needsUpdateBank |= update(&bank.ManualRate.Pitch, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP:
needsUpdateBank |= update(&bank.RollPI.Kp, value);
needsUpdateBank |= update(&bank.PitchPI.Kp, value);
@ -284,6 +301,10 @@ static void updatePIDs(UAVObjEvent *ev)
needsUpdateBank |= update(&bank.RollPI.ILimit, value);
needsUpdateBank |= update(&bank.PitchPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDERESP:
needsUpdateBank |= updateUint8(&bank.RollMax, value);
needsUpdateBank |= updateUint8(&bank.PitchMax, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKP:
needsUpdateBank |= update(&bank.YawRatePID.Kp, value);
break;
@ -296,6 +317,9 @@ static void updatePIDs(UAVObjEvent *ev)
case TXPIDSETTINGS_PIDS_YAWRATEILIMIT:
needsUpdateBank |= update(&bank.YawRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATERESP:
needsUpdateBank |= update(&bank.ManualRate.Yaw, value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEKP:
needsUpdateBank |= update(&bank.YawPI.Kp, value);
break;
@ -305,9 +329,28 @@ static void updatePIDs(UAVObjEvent *ev)
case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT:
needsUpdateBank |= update(&bank.YawPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDERESP:
needsUpdateBank |= updateUint8(&bank.YawMax, value);
break;
case TXPIDSETTINGS_PIDS_ROLLEXPO:
needsUpdateBank |= updateInt8(&bank.StickExpo.Roll, value);
break;
case TXPIDSETTINGS_PIDS_PITCHEXPO:
needsUpdateBank |= updateInt8(&bank.StickExpo.Pitch, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHEXPO:
needsUpdateBank |= updateInt8(&bank.StickExpo.Roll, value);
needsUpdateBank |= updateInt8(&bank.StickExpo.Pitch, value);
break;
case TXPIDSETTINGS_PIDS_YAWEXPO:
needsUpdateBank |= updateInt8(&bank.StickExpo.Yaw, value);
break;
case TXPIDSETTINGS_PIDS_GYROTAU:
needsUpdateStab |= update(&stab.GyroTau, value);
break;
case TXPIDSETTINGS_PIDS_ACROPLUSFACTOR:
needsUpdateBank |= update(&bank.AcroInsanityFactor, value);
break;
default:
PIOS_Assert(0);
}
@ -389,6 +432,36 @@ static uint8_t update(float *var, float val)
return 0;
}
/**
* Updates var using val if needed.
* \returns 1 if updated, 0 otherwise
*/
static uint8_t updateUint8(uint8_t *var, float val)
{
uint8_t roundedVal = (uint8_t)roundf(val);
if (*var != roundedVal) {
*var = roundedVal;
return 1;
}
return 0;
}
/**
* Updates var using val if needed.
* \returns 1 if updated, 0 otherwise
*/
static uint8_t updateInt8(int8_t *var, float val)
{
int8_t roundedVal = (int8_t)roundf(val);
if (*var != roundedVal) {
*var = roundedVal;
return 1;
}
return 0;
}
/**
* @}
*/

729
flight/modules/VtolPathFollower/vtolpathfollower.c
View File

@ -1,729 +0,0 @@
/**
******************************************************************************
*
* @file vtolpathfollower.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief This module compared @ref PositionState to @ref PathDesired
* and sets @ref Stabilization. It only does this when the FlightMode field
* of @ref FlightStatus is PathPlanner or RTH.
*
* @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
*/
/**
* Input object: FlightStatus
* Input object: PathDesired
* Input object: PositionState
* Output object: StabilizationDesired
*
* This module will periodically update the value of the @ref StabilizationDesired object based on
* @ref PathDesired and @PositionState when the Flight Mode selected in @FlightStatus is supported
* by this module. Otherwise another module (e.g. @ref ManualControlCommand) is expected to be
* writing to @ref StabilizationDesired.
*
* The module executes in its own thread in this example.
*
* Modules have no API, all communication to other modules is done through UAVObjects.
* However modules may use the API exposed by shared libraries.
* See the OpenPilot wiki for more details.
* http://www.openpilot.org/OpenPilot_Application_Architecture
*
*/
#include <openpilot.h>
#include <pios_struct_helper.h>
#include "vtolpathfollower.h"
#include "accelstate.h"
#include "attitudestate.h"
#include "hwsettings.h"
#include "pathdesired.h" // object that will be updated by the module
#include "positionstate.h"
#include "manualcontrolcommand.h"
#include "flightstatus.h"
#include "pathstatus.h"
#include "gpsvelocitysensor.h"
#include "gpspositionsensor.h"
#include "homelocation.h"
#include "vtolpathfollowersettings.h"
#include "nedaccel.h"
#include "stabilizationdesired.h"
#include "stabilizationsettings.h"
#include "stabilizationbank.h"
#include "systemsettings.h"
#include "velocitydesired.h"
#include "velocitystate.h"
#include "taskinfo.h"
#include "paths.h"
#include "CoordinateConversions.h"
#include <sanitycheck.h>
#include "cameradesired.h"
#include "poilearnsettings.h"
#include "poilocation.h"
#include "accessorydesired.h"
// Private constants
#define MAX_QUEUE_SIZE 4
#define STACK_SIZE_BYTES 1548
#define TASK_PRIORITY (tskIDLE_PRIORITY + 2)
// Private types
// Private variables
static xTaskHandle pathfollowerTaskHandle;
static PathStatusData pathStatus;
static VtolPathFollowerSettingsData vtolpathfollowerSettings;
static float poiRadius;
// Private functions
static void vtolPathFollowerTask(void *parameters);
static void SettingsUpdatedCb(UAVObjEvent *ev);
static void updateNedAccel();
static void updatePOIBearing();
static void updatePathVelocity();
static void updateEndpointVelocity();
static void updateFixedAttitude(float *attitude);
static void updateVtolDesiredAttitude(bool yaw_attitude);
static bool vtolpathfollower_enabled;
static void accessoryUpdated(UAVObjEvent *ev);
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t VtolPathFollowerStart()
{
if (vtolpathfollower_enabled) {
// Start main task
xTaskCreate(vtolPathFollowerTask, "VtolPathFollower", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &pathfollowerTaskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_PATHFOLLOWER, pathfollowerTaskHandle);
}
return 0;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t VtolPathFollowerInitialize()
{
HwSettingsOptionalModulesData optionalModules;
HwSettingsOptionalModulesGet(&optionalModules);
FrameType_t frameType = GetCurrentFrameType();
if ((optionalModules.VtolPathFollower == HWSETTINGS_OPTIONALMODULES_ENABLED) ||
(frameType == FRAME_TYPE_MULTIROTOR)) {
VtolPathFollowerSettingsInitialize();
NedAccelInitialize();
PathDesiredInitialize();
PathStatusInitialize();
VelocityDesiredInitialize();
CameraDesiredInitialize();
AccessoryDesiredInitialize();
PoiLearnSettingsInitialize();
PoiLocationInitialize();
vtolpathfollower_enabled = true;
} else {
vtolpathfollower_enabled = false;
}
return 0;
}
MODULE_INITCALL(VtolPathFollowerInitialize, VtolPathFollowerStart);
static float northVelIntegral = 0;
static float eastVelIntegral = 0;
static float downVelIntegral = 0;
static float northPosIntegral = 0;
static float eastPosIntegral = 0;
static float downPosIntegral = 0;
static float thrustOffset = 0;
/**
* Module thread, should not return.
*/
static void vtolPathFollowerTask(__attribute__((unused)) void *parameters)
{
SystemSettingsData systemSettings;
FlightStatusData flightStatus;
portTickType lastUpdateTime;
VtolPathFollowerSettingsConnectCallback(SettingsUpdatedCb);
AccessoryDesiredConnectCallback(accessoryUpdated);
VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
// Main task loop
lastUpdateTime = xTaskGetTickCount();
while (1) {
// Conditions when this runs:
// 1. Must have VTOL type airframe
// 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR
// FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path
SystemSettingsGet(&systemSettings);
if ((systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP)
&& (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX)
&& (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX)
&& (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO)
&& (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP)
&& (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAH)
&& (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOX)) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
vTaskDelay(1000);
continue;
}
// Continue collecting data if not enough time
vTaskDelayUntil(&lastUpdateTime, vtolpathfollowerSettings.UpdatePeriod / portTICK_RATE_MS);
// Convert the accels into the NED frame
updateNedAccel();
FlightStatusGet(&flightStatus);
PathStatusGet(&pathStatus);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
// Check the combinations of flightmode and pathdesired mode
if (flightStatus.ControlChain.PathFollower == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
if (flightStatus.ControlChain.PathPlanner == FLIGHTSTATUS_CONTROLCHAIN_FALSE) {
if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POI) {
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
updateEndpointVelocity();
updateVtolDesiredAttitude(true);
updatePOIBearing();
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
}
} else {
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
updateEndpointVelocity();
updateVtolDesiredAttitude(false);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
}
}
} else {
pathStatus.UID = pathDesired.UID;
pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
switch (pathDesired.Mode) {
// TODO: Make updateVtolDesiredAttitude and velocity report success and update PATHSTATUS_STATUS accordingly
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_FLYCIRCLELEFT:
updatePathVelocity();
updateVtolDesiredAttitude(false);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_FIXEDATTITUDE:
updateFixedAttitude(pathDesired.ModeParameters);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_DISARMALARM:
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
break;
default:
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
break;
}
PathStatusSet(&pathStatus);
}
} else {
// Be cleaner and get rid of global variables
northVelIntegral = 0;
eastVelIntegral = 0;
downVelIntegral = 0;
northPosIntegral = 0;
eastPosIntegral = 0;
downPosIntegral = 0;
// Track thrust before engaging this mode. Cheap system ident
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
thrustOffset = stabDesired.Thrust;
}
AlarmsClear(SYSTEMALARMS_ALARM_GUIDANCE);
}
}
/**
* Compute bearing and elevation between current position and POI
*/
static void updatePOIBearing()
{
const float DEADBAND_HIGH = 0.10f;
const float DEADBAND_LOW = -0.10f;
float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
PositionStateData positionState;
PositionStateGet(&positionState);
CameraDesiredData cameraDesired;
CameraDesiredGet(&cameraDesired);
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
PoiLocationData poi;
PoiLocationGet(&poi);
float dLoc[3];
float yaw = 0;
/*float elevation = 0;*/
dLoc[0] = positionState.North - poi.North;
dLoc[1] = positionState.East - poi.East;
dLoc[2] = positionState.Down - poi.Down;
if (dLoc[1] < 0) {
yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) + 180.0f;
} else {
yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) - 180.0f;
}
// distance
float distance = sqrtf(powf(dLoc[0], 2.0f) + powf(dLoc[1], 2.0f));
ManualControlCommandData manualControlData;
ManualControlCommandGet(&manualControlData);
float changeRadius = 0;
// Move closer or further, radially
if (manualControlData.Pitch > DEADBAND_HIGH) {
changeRadius = (manualControlData.Pitch - DEADBAND_HIGH) * dT * 100.0f;
} else if (manualControlData.Pitch < DEADBAND_LOW) {
changeRadius = (manualControlData.Pitch - DEADBAND_LOW) * dT * 100.0f;
}
// move along circular path
float pathAngle = 0;
if (manualControlData.Roll > DEADBAND_HIGH) {
pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f;
} else if (manualControlData.Roll < DEADBAND_LOW) {
pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f;
} else if (manualControlData.Roll >= DEADBAND_LOW && manualControlData.Roll <= DEADBAND_HIGH) {
// change radius only when not circling
poiRadius = distance + changeRadius;
}
// don't try to move any closer
if (poiRadius >= 3.0f || changeRadius > 0) {
if (fabsf(pathAngle) > 0.0f || fabsf(changeRadius) > 0.0f) {
pathDesired.End.North = poi.North + (poiRadius * cosf(DEG2RAD(pathAngle + yaw - 180.0f)));
pathDesired.End.East = poi.East + (poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f)));
pathDesired.StartingVelocity = 1.0f;
pathDesired.EndingVelocity = 0.0f;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
}
}
// not above
if (distance >= 3.0f) {
// You can feed this into camerastabilization
/*elevation = RAD2DEG(atan2f(dLoc[2],distance));*/
stabDesired.Yaw = yaw + (pathAngle / 2.0f);
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
// cameraDesired.Yaw=yaw;
// cameraDesired.PitchOrServo2=elevation;
CameraDesiredSet(&cameraDesired);
StabilizationDesiredSet(&stabDesired);
}
}
/**
* Compute desired velocity from the current position and path
*
* Takes in @ref PositionState and compares it to @ref PathDesired
* and computes @ref VelocityDesired
*/
static void updatePathVelocity()
{
float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
float downCommand;
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
PositionStateData positionState;
PositionStateGet(&positionState);
float cur[3] =
{ positionState.North, positionState.East, positionState.Down };
struct path_status progress;
path_progress(
cast_struct_to_array(pathDesired.Start, pathDesired.Start.North),
cast_struct_to_array(pathDesired.End, pathDesired.End.North),
cur, &progress, pathDesired.Mode);
float groundspeed;
switch (pathDesired.Mode) {
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_DRIVECIRCLERIGHT:
case PATHDESIRED_MODE_FLYCIRCLELEFT:
case PATHDESIRED_MODE_DRIVECIRCLELEFT:
groundspeed = pathDesired.EndingVelocity;
break;
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_DRIVEENDPOINT:
groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * boundf(progress.fractional_progress, 0, 1);
if (progress.fractional_progress > 1) {
groundspeed = 0;
}
break;
case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_DRIVEVECTOR:
default:
groundspeed = pathDesired.StartingVelocity
+ (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * boundf(progress.fractional_progress, 0, 1);
if (progress.fractional_progress > 1) {
groundspeed = 0;
}
break;
}
VelocityDesiredData velocityDesired;
velocityDesired.North = progress.path_direction[0] * groundspeed;
velocityDesired.East = progress.path_direction[1] * groundspeed;
float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI.Kp;
float correction_velocity[2] =
{ progress.correction_direction[0] * error_speed, progress.correction_direction[1] * error_speed };
float total_vel = sqrtf(powf(correction_velocity[0], 2) + powf(correction_velocity[1], 2));
float scale = 1;
if (total_vel > vtolpathfollowerSettings.HorizontalVelMax) {
scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel;
}
velocityDesired.North += progress.correction_direction[0] * error_speed * scale;
velocityDesired.East += progress.correction_direction[1] * error_speed * scale;
float altitudeSetpoint = pathDesired.Start.Down + (pathDesired.End.Down - pathDesired.Start.Down) * boundf(progress.fractional_progress, 0, 1);
float downError = altitudeSetpoint - positionState.Down;
downPosIntegral = boundf(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki,
-vtolpathfollowerSettings.VerticalPosPI.ILimit,
vtolpathfollowerSettings.VerticalPosPI.ILimit);
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI.Kp + downPosIntegral);
velocityDesired.Down = boundf(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
// update pathstatus
pathStatus.error = progress.error;
pathStatus.fractional_progress = progress.fractional_progress;
VelocityDesiredSet(&velocityDesired);
}
/**
* Compute desired velocity from the current position
*
* Takes in @ref PositionState and compares it to @ref PositionDesired
* and computes @ref VelocityDesired
*/
void updateEndpointVelocity()
{
float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
PositionStateData positionState;
VelocityDesiredData velocityDesired;
PositionStateGet(&positionState);
VelocityDesiredGet(&velocityDesired);
float northError;
float eastError;
float downError;
float northCommand;
float eastCommand;
float downCommand;
// Compute desired north command
northError = pathDesired.End.North - positionState.North;
northPosIntegral = boundf(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki,
-vtolpathfollowerSettings.HorizontalPosPI.ILimit,
vtolpathfollowerSettings.HorizontalPosPI.ILimit);
northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI.Kp + northPosIntegral);
eastError = pathDesired.End.East - positionState.East;
eastPosIntegral = boundf(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki,
-vtolpathfollowerSettings.HorizontalPosPI.ILimit,
vtolpathfollowerSettings.HorizontalPosPI.ILimit);
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI.Kp + eastPosIntegral);
// Limit the maximum velocity
float total_vel = sqrtf(powf(northCommand, 2) + powf(eastCommand, 2));
float scale = 1;
if (total_vel > vtolpathfollowerSettings.HorizontalVelMax) {
scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel;
}
velocityDesired.North = northCommand * scale;
velocityDesired.East = eastCommand * scale;
downError = pathDesired.End.Down - positionState.Down;
downPosIntegral = boundf(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki,
-vtolpathfollowerSettings.VerticalPosPI.ILimit,
vtolpathfollowerSettings.VerticalPosPI.ILimit);
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI.Kp + downPosIntegral);
velocityDesired.Down = boundf(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
VelocityDesiredSet(&velocityDesired);
}
/**
* Compute desired attitude from a fixed preset
*
*/
static void updateFixedAttitude(float *attitude)
{
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
stabDesired.Roll = attitude[0];
stabDesired.Pitch = attitude[1];
stabDesired.Yaw = attitude[2];
stabDesired.Thrust = attitude[3];
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
StabilizationDesiredSet(&stabDesired);
}
/**
* Compute desired attitude from the desired velocity
*
* Takes in @ref NedState which has the acceleration in the
* NED frame as the feedback term and then compares the
* @ref VelocityState against the @ref VelocityDesired
*/
static void updateVtolDesiredAttitude(bool yaw_attitude)
{
float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
VelocityDesiredData velocityDesired;
VelocityStateData velocityState;
StabilizationDesiredData stabDesired;
AttitudeStateData attitudeState;
NedAccelData nedAccel;
StabilizationBankData stabSettings;
SystemSettingsData systemSettings;
float northError;
float northCommand;
float eastError;
float eastCommand;
float downError;
float downCommand;
SystemSettingsGet(&systemSettings);
VelocityStateGet(&velocityState);
VelocityDesiredGet(&velocityDesired);
StabilizationDesiredGet(&stabDesired);
VelocityDesiredGet(&velocityDesired);
AttitudeStateGet(&attitudeState);
StabilizationBankGet(&stabSettings);
NedAccelGet(&nedAccel);
float northVel = 0, eastVel = 0, downVel = 0;
switch (vtolpathfollowerSettings.VelocitySource) {
case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_STATE_ESTIMATION:
northVel = velocityState.North;
eastVel = velocityState.East;
downVel = velocityState.Down;
break;
case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPS_VELNED:
{
GPSVelocitySensorData gpsVelocity;
GPSVelocitySensorGet(&gpsVelocity);
northVel = gpsVelocity.North;
eastVel = gpsVelocity.East;
downVel = gpsVelocity.Down;
}
break;
case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPS_GROUNDSPEED:
{
GPSPositionSensorData gpsPosition;
GPSPositionSensorGet(&gpsPosition);
northVel = gpsPosition.Groundspeed * cosf(DEG2RAD(gpsPosition.Heading));
eastVel = gpsPosition.Groundspeed * sinf(DEG2RAD(gpsPosition.Heading));
downVel = velocityState.Down;
}
break;
default:
PIOS_Assert(0);
break;
}
// Testing code - refactor into manual control command
ManualControlCommandData manualControlData;
ManualControlCommandGet(&manualControlData);
// Compute desired north command
northError = velocityDesired.North - northVel;
northVelIntegral = boundf(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki,
-vtolpathfollowerSettings.HorizontalVelPID.ILimit,
vtolpathfollowerSettings.HorizontalVelPID.ILimit);
northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID.Kp + northVelIntegral
- nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID.Kd
+ velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward);
// Compute desired east command
eastError = velocityDesired.East - eastVel;
eastVelIntegral = boundf(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki,
-vtolpathfollowerSettings.HorizontalVelPID.ILimit,
vtolpathfollowerSettings.HorizontalVelPID.ILimit);
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID.Kp + eastVelIntegral
- nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID.Kd
+ velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward);
// Compute desired down command
downError = velocityDesired.Down - downVel;
// Must flip this sign
downError = -downError;
downVelIntegral = boundf(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID.Ki,
-vtolpathfollowerSettings.VerticalVelPID.ILimit,
vtolpathfollowerSettings.VerticalVelPID.ILimit);
downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID.Kp + downVelIntegral
- nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID.Kd);
stabDesired.Thrust = boundf(downCommand + thrustOffset, 0, 1);
// Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the
// craft should move similarly for 5 deg roll versus 5 deg pitch
stabDesired.Pitch = boundf(-northCommand * cosf(DEG2RAD(attitudeState.Yaw)) +
-eastCommand * sinf(DEG2RAD(attitudeState.Yaw)),
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
stabDesired.Roll = boundf(-northCommand * sinf(DEG2RAD(attitudeState.Yaw)) +
eastCommand * cosf(DEG2RAD(attitudeState.Yaw)),
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
if (vtolpathfollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_FALSE) {
// For now override thrust with manual control. Disable at your risk, quad goes to China.
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
stabDesired.Thrust = manualControl.Thrust;
}
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
if (yaw_attitude) {
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
} else {
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControlData.Yaw;
}
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
StabilizationDesiredSet(&stabDesired);
}
/**
* Keep a running filtered version of the acceleration in the NED frame
*/
static void updateNedAccel()
{
float accel[3];
float q[4];
float Rbe[3][3];
float accel_ned[3];
// Collect downsampled attitude data
AccelStateData accelState;
AccelStateGet(&accelState);
accel[0] = accelState.x;
accel[1] = accelState.y;
accel[2] = accelState.z;
// rotate avg accels into earth frame and store it
AttitudeStateData attitudeState;
AttitudeStateGet(&attitudeState);
q[0] = attitudeState.q1;
q[1] = attitudeState.q2;
q[2] = attitudeState.q3;
q[3] = attitudeState.q4;
Quaternion2R(q, Rbe);
for (uint8_t i = 0; i < 3; i++) {
accel_ned[i] = 0;
for (uint8_t j = 0; j < 3; j++) {
accel_ned[i] += Rbe[j][i] * accel[j];
}
}
accel_ned[2] += 9.81f;
NedAccelData accelData;
NedAccelGet(&accelData);
accelData.North = accel_ned[0];
accelData.East = accel_ned[1];
accelData.Down = accel_ned[2];
NedAccelSet(&accelData);
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
}
static void accessoryUpdated(UAVObjEvent *ev)
{
if (ev->obj != AccessoryDesiredHandle()) {
return;
}
AccessoryDesiredData accessory;
PoiLearnSettingsData poiLearn;
PoiLearnSettingsGet(&poiLearn);
if (poiLearn.Input != POILEARNSETTINGS_INPUT_NONE) {
if (AccessoryDesiredInstGet(poiLearn.Input - POILEARNSETTINGS_INPUT_ACCESSORY0, &accessory) == 0) {
if (accessory.AccessoryVal < -0.5f) {
PositionStateData positionState;
PositionStateGet(&positionState);
PoiLocationData poi;
PoiLocationGet(&poi);
poi.North = positionState.North;
poi.East = positionState.East;
poi.Down = positionState.Down;
PoiLocationSet(&poi);
}
}
}
}

35
flight/modules/gpsp/gps9flashhandler.c Normal file
View File

@ -0,0 +1,35 @@
/**
******************************************************************************
*
* @file gps9flashhandler.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Flash handler for GPSV9.
* --
* @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 "inc/gps9flashhandler.h"
extern uintptr_t flash_id;
extern struct pios_flash_driver pios_jedec_flash_driver;
extern uintptr_t flash_id;
bool flash_available()
{
return flash_id > 0;
}

103
flight/modules/gpsp/gps9gpshandler.c Normal file
View File

@ -0,0 +1,103 @@
/**
******************************************************************************
*
* @file gps9gpshandler.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief handler for GPSV9 onboard ubx gps module.
* --
* @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 <openpilot.h>
#include <pios_struct_helper.h>
#include <pios_helpers.h>
#include <ubx_utils.h>
#include <pios_ubx_ddc.h>
#include "gps9gpshandler.h"
#include "gps9protocol.h"
uint32_t lastUnsentData = 0;
uint8_t buffer[BUFFER_SIZE];
void handleGPS()
{
bool completeSentenceSent = false;
int8_t maxCount = 2;
do {
int32_t datacounter = PIOS_UBX_DDC_GetAvailableBytes(PIOS_I2C_GPS);
if (datacounter > 0) {
uint8_t toRead = (uint32_t)datacounter > BUFFER_SIZE - lastUnsentData ? BUFFER_SIZE - lastUnsentData : (uint8_t)datacounter;
uint8_t toSend = toRead;
PIOS_UBX_DDC_ReadData(PIOS_I2C_GPS, buffer, toRead);
uint8_t *lastSentence;
uint16_t lastSentenceLength;
completeSentenceSent = ubx_getLastSentence(buffer, toRead, &lastSentence, &lastSentenceLength);
if (completeSentenceSent) {
toSend = (uint8_t)(lastSentence - buffer + lastSentenceLength);
} else {
lastUnsentData = 0;
}
PIOS_COM_SendBuffer(pios_com_main_id, buffer, toSend);
if (toRead > toSend) {
// move unsent data at the beginning of buffer to be sent next time
lastUnsentData = toRead - toSend;
memcpy(buffer, (buffer + toSend), lastUnsentData);
}
}
datacounter = PIOS_COM_ReceiveBuffer(pios_com_main_id, buffer, BUFFER_SIZE, 0);
if (datacounter > 0) {
PIOS_UBX_DDC_WriteData(PIOS_I2C_GPS, buffer, datacounter);
}
if (maxCount) {
// Note: this delay is needed as querying too quickly the UBX module's I2C(DDC)
// port causes a lot of weird issues (it stops sending nav sentences)
vTaskDelay(2 * configTICK_RATE_HZ / 1000);
}
} while (maxCount--);
}
typedef struct {
uint8_t size;
const uint8_t *sentence;
} ubx_init_sentence;
void setupGPS()
{
CfgPrtPkt cfgprt;
cfgprt.fragments.data.portID = CFG_PRT_DATA_PORTID_DDC;
cfgprt.fragments.data.reserved0 = 0;
cfgprt.fragments.data.txReady = CFG_PRT_DATA_TXREADI_DISABLED;
cfgprt.fragments.data.mode = CFG_PRT_DATA_MODE_ADDR;
cfgprt.fragments.data.reserved3 = 0;
cfgprt.fragments.data.inProtoMask = CFG_PRT_DATA_PROTO_UBX | CFG_PRT_DATA_PROTO_NMEA | CFG_PRT_DATA_PROTO_RTCM;
cfgprt.fragments.data.outProtoMask = CFG_PRT_DATA_PROTO_UBX;
cfgprt.fragments.data.flags = 0;
cfgprt.fragments.data.reserved5 = 0;
ubx_buildPacket(&cfgprt.packet, UBX_CFG_CLASS, UBX_CFG_PRT, sizeof(CfgPrtData));
PIOS_UBX_DDC_WriteData(PIOS_I2C_GPS, cfgprt.packet.binarystream, sizeof(CfgPrtPkt));
}

61
flight/modules/gpsp/gps9maghandler.c Normal file
View File

@ -0,0 +1,61 @@
/**
******************************************************************************
*
* @file gps9maghandler.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief handles GPSV9 onboard magnetometer and sends its 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
* 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 <openpilot.h>
#include <pios_struct_helper.h>
#include <pios_helpers.h>
#include <ubx_utils.h>
#include <pios_hmc5x83.h>
#include "inc/gps9protocol.h"
#define MAG_RATE_HZ 30
extern pios_hmc5x83_dev_t onboard_mag;
void handleMag()
{
#ifdef PIOS_HMC5X83_HAS_GPIOS
if (!PIOS_HMC5x83_NewDataAvailable(onboard_mag)) {
return;
}
#else
static uint32_t lastUpdate = 0;
if (PIOS_DELAY_DiffuS(lastUpdate) < (1000000 / MAG_RATE_HZ)) {
return;
}
lastUpdate = PIOS_DELAY_GetRaw();
#endif
static int16_t mag[3];
if (PIOS_HMC5x83_ReadMag(onboard_mag, mag) == 0) {
MagUbxPkt magPkt;
// swap axis so that if side with connector is aligned to revo side with connectors, mags data are aligned
magPkt.fragments.data.X = -mag[1];
magPkt.fragments.data.Y = mag[0];
magPkt.fragments.data.Z = mag[2];
magPkt.fragments.data.status = 1;
ubx_buildPacket(&magPkt.packet, UBX_OP_CUST_CLASS, UBX_OP_MAG, sizeof(MagData));
PIOS_COM_SendBuffer(pios_com_main_id, magPkt.packet.binarystream, sizeof(MagUbxPkt));
return;
}
}

263
flight/modules/gpsp/gpsdsysmod.c Normal file
View File

@ -0,0 +1,263 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @brief The OpenPilot Modules do the majority of the control in OpenPilot. The
* @ref SystemModule "System Module" starts all the other modules that then take care
* of all the telemetry and control algorithms and such. This is done through the @ref PIOS
* "PIOS Hardware abstraction layer" which then contains hardware specific implementations
* (currently only STM32 supported)
*
* @{
* @addtogroup SystemModule GPSV9 System Module
* @brief Initializes PIOS and other modules runs monitoring, executes mag and gps handlers
*
* @{
*
* @file gpsdsystemmod.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief GPS System module
*
* @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
*/
// private includes
#include "inc/gpsdsysmod.h"
#include "inc/gps9maghandler.h"
#include "inc/gps9gpshandler.h"
#include "inc/gps9flashhandler.h"
#include "inc/gps9protocol.h"
#include "pios_board_info.h"
extern uint32_t pios_com_main_id;
// Private constants
#define SYSTEM_UPDATE_PERIOD_MS 1
#define HB_LED_BLINK_ON_PERIOD_MS 100
#define HB_LED_BLINK_OFF_PERIOD_MS 1900
#define STACK_SIZE_BYTES 450
#define STAT_UPDATE_PERIOD_MS 10000
#define TASK_PRIORITY (tskIDLE_PRIORITY + 2)
// Private types
// Private variables
static xTaskHandle systemTaskHandle;
static enum { STACKOVERFLOW_NONE = 0, STACKOVERFLOW_WARNING = 1, STACKOVERFLOW_CRITICAL = 3 } stackOverflow;
static bool mallocFailed;
static SysUbxPkt sysPkt;
// Private functions
static void updateStats();
static void gpspSystemTask(void *parameters);
static void readFirmwareInfo();
/**
* Create the module task.
* \returns 0 on success or -1 if initialization failed
*/
int32_t GPSPSystemModStart(void)
{
// Initialize vars
stackOverflow = STACKOVERFLOW_NONE;
mallocFailed = false;
// Create system task
xTaskCreate(gpspSystemTask, (const char *)"G-Sys", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &systemTaskHandle);
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_SYSTEM);
#endif
return 0;
}
/**
* Initialize the module, called on startup.
* \returns 0 on success or -1 if initialization failed
*/
int32_t GPSPSystemModInitialize(void)
{
GPSPSystemModStart();
return 0;
}
MODULE_INITCALL(GPSPSystemModInitialize, 0);
/**
* System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS
*/
static void gpspSystemTask(__attribute__((unused)) void *parameters)
{
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_SYSTEM);
#endif
/* create all modules thread */
MODULE_TASKCREATE_ALL;
if (mallocFailed) {
// Nothing to do, this condition needs to be trapped during development.
while (true) {
;
}
}
#if defined(PIOS_INCLUDE_IAP)
PIOS_IAP_WriteBootCount(0);
#endif
/* Right now there is no configuration and uart speed is fixed at 57600.
* TODO:
* 1) add a tiny ubx parser on gps side to intercept CFG-RINV and use that for config storage;
* 2) second ubx parser on uart side that intercept custom configuration message and flash commands.
*/
PIOS_COM_ChangeBaud(pios_com_main_id, GPS_MODULE_DEFAULT_BAUDRATE);
setupGPS();
uint32_t ledTimer = 0;
static TickType_t lastUpdate;
readFirmwareInfo();
while (1) {
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_SYSTEM);
#endif
uint32_t ledPeriod = PIOS_DELAY_DiffuS(ledTimer) / 1000;
if (ledPeriod < HB_LED_BLINK_ON_PERIOD_MS) {
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
} else {
PIOS_LED_On(PIOS_LED_HEARTBEAT);
}
if (ledPeriod > (HB_LED_BLINK_ON_PERIOD_MS + HB_LED_BLINK_OFF_PERIOD_MS)) {
ledTimer = PIOS_DELAY_GetRaw();
}
handleGPS();
handleMag();
updateStats();
vTaskDelayUntil(&lastUpdate, SYSTEM_UPDATE_PERIOD_MS * configTICK_RATE_HZ / 1000);
}
}
/**
* Called periodically to update the system stats
*/
uint16_t GetFreeIrqStackSize(void)
{
uint32_t i = 0x150;
#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK)
extern uint32_t _irq_stack_top;
extern uint32_t _irq_stack_end;
uint32_t pattern = 0x0000A5A5;
uint32_t *ptr = &_irq_stack_end;
#if 1 /* the ugly way accurate but takes more time, useful for debugging */
uint32_t stack_size = (((uint32_t)&_irq_stack_top - (uint32_t)&_irq_stack_end) & ~3) / 4;
for (i = 0; i < stack_size; i++) {
if (ptr[i] != pattern) {
i = i * 4;
break;
}
}
#else /* faster way but not accurate */
if (*(volatile uint32_t *)((uint32_t)ptr + IRQSTACK_LIMIT_CRITICAL) != pattern) {
i = IRQSTACK_LIMIT_CRITICAL - 1;
} else if (*(volatile uint32_t *)((uint32_t)ptr + IRQSTACK_LIMIT_WARNING) != pattern) {
i = IRQSTACK_LIMIT_WARNING - 1;
} else {
i = IRQSTACK_LIMIT_WARNING;
}
#endif
#endif /* if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK) */
return i;
}
/**
* Called periodically to update the system stats
*/
static void updateStats()
{
static uint32_t lastUpdate;
if (PIOS_DELAY_DiffuS(lastUpdate) < STAT_UPDATE_PERIOD_MS * 1000) {
return;
}
lastUpdate = PIOS_DELAY_GetRaw();
// Get stats and update
sysPkt.fragments.data.flightTime = xTaskGetTickCount() * portTICK_RATE_MS;
sysPkt.fragments.data.options = SYS_DATA_OPTIONS_MAG | (flash_available() ? SYS_DATA_OPTIONS_FLASH : 0);
ubx_buildPacket(&sysPkt.packet, UBX_OP_CUST_CLASS, UBX_OP_SYS, sizeof(SysData));
PIOS_COM_SendBuffer(pios_com_main_id, sysPkt.packet.binarystream, sizeof(SysUbxPkt));
}
// retrieve firmware info and fill syspkt
static void readFirmwareInfo()
{
const struct pios_board_info *bdinfo = &pios_board_info_blob;
sysPkt.fragments.data.board_revision = bdinfo->board_rev;
sysPkt.fragments.data.board_type = bdinfo->board_type;
struct fw_version_info *fwinfo = (struct fw_version_info *)(bdinfo->fw_base + bdinfo->fw_size);
memcpy(&sysPkt.fragments.data.commit_tag_name, &fwinfo->commit_tag_name, sizeof(sysPkt.fragments.data.commit_tag_name));
memcpy(&sysPkt.fragments.data.sha1sum, &fwinfo->sha1sum, sizeof(sysPkt.fragments.data.sha1sum));
}
/**
* Called by the RTOS when the CPU is idle,
*/
void vApplicationIdleHook(void)
{}
/**
* Called by the RTOS when a stack overflow is detected.
*/
#define DEBUG_STACK_OVERFLOW 0
void vApplicationStackOverflowHook(__attribute__((unused)) xTaskHandle *pxTask,
__attribute__((unused)) signed portCHAR *pcTaskName)
{
stackOverflow = STACKOVERFLOW_CRITICAL;
#if DEBUG_STACK_OVERFLOW
static volatile bool wait_here = true;
while (wait_here) {
;
}
wait_here = true;
#endif
}
/**
* Called by the RTOS when a malloc call fails.
*/
#define DEBUG_MALLOC_FAILURES 0
void vApplicationMallocFailedHook(void)
{
mallocFailed = true;
#if DEBUG_MALLOC_FAILURES
static volatile bool wait_here = true;
while (wait_here) {
;
}
wait_here = true;
#endif
}
/**
* @}
* @}
*/

33
flight/modules/gpsp/inc/gps9flashhandler.h Normal file
View File

@ -0,0 +1,33 @@
/**
******************************************************************************
*
* @file gps9flashhandler.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Flash handler for GPSV9
* --
* @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 GPS9FLASHHANDLER_H_
#define GPS9FLASHHANDLER_H_
#include <openpilot.h>
bool flash_available();
#endif /* GPS9FLASHHANDLER_H_ */

34
flight/modules/gpsp/inc/gps9gpshandler.h Normal file
View File

@ -0,0 +1,34 @@
/**
******************************************************************************
*
* @file gps9gpshandler.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief handler for GPSV9 onboard ubx gps module.
* --
* @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 GPS9GPSHANDLER_H_
#define GPS9GPSHANDLER_H_
#define BUFFER_SIZE 200
void handleGPS();
void setupGPS();
#endif /* GPS9GPSHANDLER_H_ */

32
flight/modules/gpsp/inc/gps9maghandler.h Normal file
View File

@ -0,0 +1,32 @@
/**magPkt
******************************************************************************
*
* @file gps9maghandler.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief brief goes here.
* --
* @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 GPS9MAGHANDLER_H
#define GPS9MAGHANDLER_H
void handleMag();
#endif

110
flight/modules/gpsp/inc/gps9protocol.h Normal file
View File

@ -0,0 +1,110 @@
/**
******************************************************************************
*
* @file gpsv9protocol.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief brief goes here.
* --
* @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 GPSV9PROTOCOL_H_
#define GPSV9PROTOCOL_H_
#include <openpilot.h>
#include <pios_struct_helper.h>
#include <pios_helpers.h>
#include <ubx_utils.h>
#define UBX_CFG_CLASS 0x06
#define UBX_CFG_PRT 0x00
#define UBX_OP_CUST_CLASS 0x99
#define UBX_OP_SYS 0x01
#define UBX_OP_MAG 0x02
#define SYS_DATA_OPTIONS_FLASH 0x01
#define SYS_DATA_OPTIONS_MAG 0x02
#define CFG_PRT_DATA_PORTID_DDC 0x00
#define CFG_PRT_DATA_TXREADI_DISABLED 0x00
#define CFG_PRT_DATA_PORTID_DDC 0x00
#define CFG_PRT_DATA_MODE_ADDR (0x42 << 1)
#define CFG_PRT_DATA_PROTO_UBX 0x01
#define CFG_PRT_DATA_PROTO_NMEA 0x02
#define CFG_PRT_DATA_PROTO_RTCM 0x04
#define CFG_PRT_DATA_FLAGS_EXTTIMEOUT 0x02
typedef struct {
int16_t X;
int16_t Y;
int16_t Z;
uint16_t status;
} __attribute__((packed)) MagData;
typedef union {
struct {
UBXHeader_t header;
MagData data;
UBXFooter_t footer;
} __attribute__((packed)) fragments;
UBXPacket_t packet;
} MagUbxPkt;
typedef struct {
uint32_t flightTime;
uint16_t options;
uint8_t board_type;
uint8_t board_revision;
uint8_t commit_tag_name[26];
uint8_t sha1sum[8];
} __attribute__((packed)) SysData;
typedef union {
struct {
UBXHeader_t header;
SysData data;
UBXFooter_t footer;
} fragments;
UBXPacket_t packet;
} SysUbxPkt;
typedef struct {
uint8_t portID;
uint8_t reserved0;
uint16_t txReady;
uint32_t mode;
uint32_t reserved3;
uint16_t inProtoMask;
uint16_t outProtoMask;
uint16_t flags;
uint16_t reserved5;
} __attribute__((packed)) CfgPrtData;
typedef union {
struct {
UBXHeader_t header;
CfgPrtData data;
UBXFooter_t footer;
} fragments;
UBXPacket_t packet;
} CfgPrtPkt;
#endif /* GPSV9PROTOCOL_H_ */

41
flight/modules/gpsp/inc/gpsdsysmod.h Normal file
View File

@ -0,0 +1,41 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup SystemModule GPSV9 System Module
* @{
*
* @file gpsdsysmod.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief System module
*
* @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 GPSSYSTEMMOD_H
#define GPSSYSTEMMOD_H
#include <openpilot.h>
#include <pios_struct_helper.h>
#include <pios_helpers.h>
#include <ubx_utils.h>
#define GPS_MODULE_DEFAULT_BAUDRATE 57600
int32_t GPSPSystemModInitialize(void);
#endif // GPSSYSTEMMOD_H

6
flight/pios/common/libraries/FreeRTOS/Source/portable/GCC/ARM_CM0/portmacro.h
View File

@ -107,6 +107,12 @@ typedef unsigned long UBaseType_t;
#define portSTACK_GROWTH ( -1 )
#define portTICK_PERIOD_MS ( ( TickType_t ) 1000 / configTICK_RATE_HZ )
#define portBYTE_ALIGNMENT 8
#define portBYTE_HEAP_ALIGNMENT 4 // this value is used to allocate heap
// Following define allow to keep a 8 bytes alignment for stack and other RTOS structures
// while using 4 bytes alignment for the remaining heap allocations to save ram
extern void *pvPortMallocGeneric( size_t xWantedSize, size_t alignment);
#define pvPortMallocAligned( x, puxStackBuffer ) ( ( ( puxStackBuffer ) == NULL ) ? ( pvPortMallocGeneric( ( x ) , portBYTE_ALIGNMENT) ) : ( puxStackBuffer ) )
/*-----------------------------------------------------------*/

34
flight/pios/common/pios_com.c
View File

@ -167,10 +167,11 @@ out_fail:
return -1;
}
#if defined(PIOS_INCLUDE_FREERTOS)
static void PIOS_COM_UnblockRx(struct pios_com_dev *com_dev, bool *need_yield)
{
#if defined(PIOS_INCLUDE_FREERTOS)
static signed portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(com_dev->rx_sem, &xHigherPriorityTaskWoken);
if (xHigherPriorityTaskWoken != pdFALSE) {
@ -178,15 +179,19 @@ static void PIOS_COM_UnblockRx(struct pios_com_dev *com_dev, bool *need_yield)
} else {
*need_yield = false;
}
#else
*need_yield = false;
#endif
}
#else
static void PIOS_COM_UnblockRx(__attribute__((unused)) struct pios_com_dev *com_dev, bool *need_yield)
{
*need_yield = false;
}
#endif
#if defined(PIOS_INCLUDE_FREERTOS)
static void PIOS_COM_UnblockTx(struct pios_com_dev *com_dev, bool *need_yield)
{
#if defined(PIOS_INCLUDE_FREERTOS)
static signed portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(com_dev->tx_sem, &xHigherPriorityTaskWoken);
if (xHigherPriorityTaskWoken != pdFALSE) {
@ -194,10 +199,14 @@ static void PIOS_COM_UnblockTx(struct pios_com_dev *com_dev, bool *need_yield)
} else {
*need_yield = false;
}
#else
*need_yield = false;
#endif
}
#else
static void PIOS_COM_UnblockTx(__attribute__((unused)) struct pios_com_dev *com_dev, bool *need_yield)
{
*need_yield = false;
}
#endif
static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *need_yield)
{
@ -207,9 +216,12 @@ static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t *buf, uint16_t b
PIOS_Assert(valid);
PIOS_Assert(com_dev->has_rx);
uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->rx, buf, buf_len);
uint16_t bytes_into_fifo;
if (buf_len == 1) {
bytes_into_fifo = fifoBuf_putByte(&com_dev->rx, buf[0]);
} else {
bytes_into_fifo = fifoBuf_putData(&com_dev->rx, buf, buf_len);
}
if (bytes_into_fifo > 0) {
/* Data has been added to the buffer */
PIOS_COM_UnblockRx(com_dev, need_yield);

24
flight/pios/common/pios_com_msg.c
View File

@ -176,6 +176,30 @@ uint16_t PIOS_COM_MSG_Receive(uint32_t com_id, uint8_t *msg, uint16_t msg_len)
return 0;
}
/**
* Change the port speed without re-initializing
* \param[in] port COM port
* \param[in] baud Requested baud rate
* \return -1 if port not available
* \return 0 on success
*/
int32_t PIOS_COM_MSG_ChangeBaud(uint32_t com_id, uint32_t baud)
{
struct pios_com_msg_dev *com_dev = (struct pios_com_msg_dev *)com_id;
if (!com_dev) {
/* Undefined COM port for this board (see pios_board.c) */
return -1;
}
/* Invoke the driver function if it exists */
if (com_dev->driver->set_baud) {
com_dev->driver->set_baud(com_dev->lower_id, baud);
}
return 0;
}
#endif /* PIOS_INCLUDE_COM_MSG */
/**

122
flight/pios/common/pios_debuglog.c
View File

@ -49,6 +49,9 @@ static xSemaphoreHandle mutex = 0;
#endif
static bool logging_enabled = false;
#define MAX_CONSECUTIVE_FAILS_COUNT 10
static bool log_is_full = false;
static uint8_t fails_count = 0;
static uint16_t flightnum = 0;
static uint16_t lognum = 0;
static DebugLogEntryData *buffer = 0;
@ -56,8 +59,16 @@ static DebugLogEntryData *buffer = 0;
static DebugLogEntryData staticbuffer;
#endif
/* Private Function Prototypes */
#define LOG_ENTRY_MAX_DATA_SIZE (sizeof(((DebugLogEntryData *)0)->Data))
#define LOG_ENTRY_HEADER_SIZE (sizeof(DebugLogEntryData) - LOG_ENTRY_MAX_DATA_SIZE)
// build the obj_id as a DEBUGLOGENTRY ID with least significant byte zeroed and filled with flight number
#define LOG_GET_FLIGHT_OBJID(x) ((DEBUGLOGENTRY_OBJID & ~0xFF) | (x & 0xFF))
static uint32_t used_buffer_space = 0;
/* Private Function Prototypes */
static void enqueue_data(uint32_t objid, uint16_t instid, size_t size, uint8_t *data);
static bool write_current_buffer();
/**
* @brief Initialize the log facility
*/
@ -77,7 +88,10 @@ void PIOS_DEBUGLOG_Initialize()
mutexlock();
lognum = 0;
flightnum = 0;
while (PIOS_FLASHFS_ObjLoad(pios_user_fs_id, flightnum * 256, lognum, (uint8_t *)buffer, sizeof(DebugLogEntryData)) == 0) {
fails_count = 0;
used_buffer_space = 0;
log_is_full = false;
while (PIOS_FLASHFS_ObjLoad(pios_user_fs_id, LOG_GET_FLIGHT_OBJID(flightnum), lognum, (uint8_t *)buffer, sizeof(DebugLogEntryData)) == 0) {
flightnum++;
}
mutexunlock();
@ -90,6 +104,11 @@ void PIOS_DEBUGLOG_Initialize()
*/
void PIOS_DEBUGLOG_Enable(uint8_t enabled)
{
// increase the flight num as soon as logging is disabled
if (logging_enabled && !enabled) {
flightnum++;
lognum = 0;
}
logging_enabled = enabled;
}
@ -103,30 +122,13 @@ void PIOS_DEBUGLOG_Enable(uint8_t enabled)
*/
void PIOS_DEBUGLOG_UAVObject(uint32_t objid, uint16_t instid, size_t size, uint8_t *data)
{
if (!logging_enabled || !buffer) {
if (!logging_enabled || !buffer || log_is_full) {
return;
}
mutexlock();
buffer->Flight = flightnum;
#if defined(PIOS_INCLUDE_FREERTOS)
buffer->FlightTime = xTaskGetTickCount() * portTICK_RATE_MS;
#else
buffer->FlightTime = 0;
#endif
buffer->Entry = lognum;
buffer->Type = DEBUGLOGENTRY_TYPE_UAVOBJECT;
buffer->ObjectID = objid;
buffer->InstanceID = instid;
if (size > sizeof(buffer->Data)) {
size = sizeof(buffer->Data);
}
buffer->Size = size;
memset(buffer->Data, 0xff, sizeof(buffer->Data));
memcpy(buffer->Data, data, size);
if (PIOS_FLASHFS_ObjSave(pios_user_fs_id, flightnum * 256, lognum, (uint8_t *)buffer, sizeof(DebugLogEntryData)) == 0) {
lognum++;
}
enqueue_data(objid, instid, size, data);
mutexunlock();
}
/**
@ -137,27 +139,30 @@ void PIOS_DEBUGLOG_UAVObject(uint32_t objid, uint16_t instid, size_t size, uint8
*/
void PIOS_DEBUGLOG_Printf(char *format, ...)
{
if (!logging_enabled || !buffer) {
if (!logging_enabled || !buffer || log_is_full) {
return;
}
va_list args;
va_start(args, format);
mutexlock();
// flush any pending buffer before writing debug text
if (used_buffer_space) {
write_current_buffer();
}
memset(buffer->Data, 0xff, sizeof(buffer->Data));
vsnprintf((char *)buffer->Data, sizeof(buffer->Data), (char *)format, args);
buffer->Flight = flightnum;
#if defined(PIOS_INCLUDE_FREERTOS)
buffer->FlightTime = xTaskGetTickCount() * portTICK_RATE_MS;
#else
buffer->FlightTime = 0;
#endif
buffer->FlightTime = PIOS_DELAY_GetuS();
buffer->Entry = lognum;
buffer->Type = DEBUGLOGENTRY_TYPE_TEXT;
buffer->ObjectID = 0;
buffer->InstanceID = 0;
buffer->Size = strlen((const char *)buffer->Data);
if (PIOS_FLASHFS_ObjSave(pios_user_fs_id, flightnum * 256, lognum, (uint8_t *)buffer, sizeof(DebugLogEntryData)) == 0) {
if (PIOS_FLASHFS_ObjSave(pios_user_fs_id, LOG_GET_FLIGHT_OBJID(flightnum), lognum, (uint8_t *)buffer, sizeof(DebugLogEntryData)) == 0) {
lognum++;
}
mutexunlock();
@ -179,7 +184,7 @@ void PIOS_DEBUGLOG_Printf(char *format, ...)
int32_t PIOS_DEBUGLOG_Read(void *mybuffer, uint16_t flight, uint16_t inst)
{
PIOS_Assert(mybuffer);
return PIOS_FLASHFS_ObjLoad(pios_user_fs_id, flight * 256, inst, (uint8_t *)mybuffer, sizeof(DebugLogEntryData));
return PIOS_FLASHFS_ObjLoad(pios_user_fs_id, LOG_GET_FLIGHT_OBJID(flight), inst, (uint8_t *)mybuffer, sizeof(DebugLogEntryData));
}
/**
@ -216,9 +221,66 @@ void PIOS_DEBUGLOG_Format(void)
PIOS_FLASHFS_Format(pios_user_fs_id);
lognum = 0;
flightnum = 0;
log_is_full = false;
fails_count = 0;
used_buffer_space = 0;
mutexunlock();
}
void enqueue_data(uint32_t objid, uint16_t instid, size_t size, uint8_t *data)
{
DebugLogEntryData *entry;
// start a new block
if (!used_buffer_space) {
entry = buffer;
memset(buffer->Data, 0xff, sizeof(buffer->Data));
used_buffer_space += size;
} else {
// if an instance is being filled and there is enough space, does enqueues new data.
if (used_buffer_space + size + LOG_ENTRY_HEADER_SIZE > LOG_ENTRY_MAX_DATA_SIZE) {
buffer->Type = DEBUGLOGENTRY_TYPE_MULTIPLEUAVOBJECTS;
if (!write_current_buffer()) {
return;
}
entry = buffer;
memset(buffer->Data, 0xff, sizeof(buffer->Data));
used_buffer_space += size;
} else {
entry = (DebugLogEntryData *)&buffer->Data[used_buffer_space];
used_buffer_space += size + LOG_ENTRY_HEADER_SIZE;
}
}
entry->Flight = flightnum;
entry->FlightTime = PIOS_DELAY_GetuS();
entry->Entry = lognum;
entry->Type = DEBUGLOGENTRY_TYPE_UAVOBJECT;
entry->ObjectID = objid;
entry->InstanceID = instid;
if (size > sizeof(buffer->Data)) {
size = sizeof(buffer->Data);
}
entry->Size = size;
memcpy(entry->Data, data, size);
}
bool write_current_buffer()
{
// not enough space, write the block and start a new one
if (PIOS_FLASHFS_ObjSave(pios_user_fs_id, LOG_GET_FLIGHT_OBJID(flightnum), lognum, (uint8_t *)buffer, sizeof(DebugLogEntryData)) == 0) {
lognum++;
fails_count = 0;
used_buffer_space = 0;
} else {
if (fails_count++ > MAX_CONSECUTIVE_FAILS_COUNT) {
log_is_full = true;
}
return false;
}
return true;
}
/**
* @}
* @}

50
flight/pios/common/pios_flash_jedec.c
View File

@ -75,13 +75,16 @@ struct jedec_flash_dev {
enum pios_jedec_dev_magic magic;
};
#define FLASH_FAST_PRESCALER PIOS_SPI_PRESCALER_2
#define FLASH_PRESCALER PIOS_SPI_PRESCALER_4
// ! Private functions
static int32_t PIOS_Flash_Jedec_Validate(struct jedec_flash_dev *flash_dev);
static struct jedec_flash_dev *PIOS_Flash_Jedec_alloc(void);
static int32_t PIOS_Flash_Jedec_ReadID(struct jedec_flash_dev *flash_dev);
static int32_t PIOS_Flash_Jedec_ReadStatus(struct jedec_flash_dev *flash_dev);
static int32_t PIOS_Flash_Jedec_ClaimBus(struct jedec_flash_dev *flash_dev);
static int32_t PIOS_Flash_Jedec_ClaimBus(struct jedec_flash_dev *flash_dev, bool fast);
static int32_t PIOS_Flash_Jedec_ReleaseBus(struct jedec_flash_dev *flash_dev);
static int32_t PIOS_Flash_Jedec_WriteEnable(struct jedec_flash_dev *flash_dev);
static int32_t PIOS_Flash_Jedec_Busy(struct jedec_flash_dev *flash_dev);
@ -166,12 +169,12 @@ int32_t PIOS_Flash_Jedec_Init(uintptr_t *flash_id, uint32_t spi_id, uint32_t sla
* @brief Claim the SPI bus for flash use and assert CS pin
* @return 0 for sucess, -1 for failure to get semaphore
*/
static int32_t PIOS_Flash_Jedec_ClaimBus(struct jedec_flash_dev *flash_dev)
static int32_t PIOS_Flash_Jedec_ClaimBus(struct jedec_flash_dev *flash_dev, bool fast)
{
if (PIOS_SPI_ClaimBus(flash_dev->spi_id) < 0) {
return -1;
}
PIOS_SPI_SetClockSpeed(flash_dev->spi_id, fast ? FLASH_FAST_PRESCALER : FLASH_PRESCALER);
PIOS_SPI_RC_PinSet(flash_dev->spi_id, flash_dev->slave_num, 0);
flash_dev->claimed = true;
@ -209,7 +212,7 @@ static int32_t PIOS_Flash_Jedec_Busy(struct jedec_flash_dev *flash_dev)
*/
static int32_t PIOS_Flash_Jedec_WriteEnable(struct jedec_flash_dev *flash_dev)
{
if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) {
if (PIOS_Flash_Jedec_ClaimBus(flash_dev, true) != 0) {
return -1;
}
@ -226,7 +229,7 @@ static int32_t PIOS_Flash_Jedec_WriteEnable(struct jedec_flash_dev *flash_dev)
*/
static int32_t PIOS_Flash_Jedec_ReadStatus(struct jedec_flash_dev *flash_dev)
{
if (PIOS_Flash_Jedec_ClaimBus(flash_dev) < 0) {
if (PIOS_Flash_Jedec_ClaimBus(flash_dev, true) < 0) {
return -1;
}
@ -247,7 +250,7 @@ static int32_t PIOS_Flash_Jedec_ReadStatus(struct jedec_flash_dev *flash_dev)
*/
static int32_t PIOS_Flash_Jedec_ReadID(struct jedec_flash_dev *flash_dev)
{
if (PIOS_Flash_Jedec_ClaimBus(flash_dev) < 0) {
if (PIOS_Flash_Jedec_ClaimBus(flash_dev, true) < 0) {
return -2;
}
@ -354,7 +357,7 @@ static int32_t PIOS_Flash_Jedec_EraseSector(uintptr_t flash_id, uint32_t addr)
return ret;
}
if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) {
if (PIOS_Flash_Jedec_ClaimBus(flash_dev, true) != 0) {
return -1;
}
@ -368,7 +371,7 @@ static int32_t PIOS_Flash_Jedec_EraseSector(uintptr_t flash_id, uint32_t addr)
// Keep polling when bus is busy too
while (PIOS_Flash_Jedec_Busy(flash_dev) != 0) {
#if defined(FLASH_FREERTOS)
vTaskDelay(1);
vTaskDelay(2);
#endif
}
@ -394,7 +397,7 @@ static int32_t PIOS_Flash_Jedec_EraseChip(uintptr_t flash_id)
return ret;
}
if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) {
if (PIOS_Flash_Jedec_ClaimBus(flash_dev, true) != 0) {
return -1;
}
@ -455,16 +458,14 @@ static int32_t PIOS_Flash_Jedec_WriteData(uintptr_t flash_id, uint32_t addr, uin
if (((addr & 0xff) + len) > 0x100) {
return -3;
}
if ((ret = PIOS_Flash_Jedec_WriteEnable(flash_dev)) != 0) {
return ret;
}
/* Execute write page command and clock in address. Keep CS asserted */
if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) {
if (PIOS_Flash_Jedec_ClaimBus(flash_dev, true) != 0) {
return -1;
}
if (PIOS_SPI_TransferBlock(flash_dev->spi_id, out, NULL, sizeof(out), NULL) < 0) {
PIOS_Flash_Jedec_ReleaseBus(flash_dev);
return -1;
@ -486,7 +487,7 @@ static int32_t PIOS_Flash_Jedec_WriteData(uintptr_t flash_id, uint32_t addr, uin
#else
// Query status this way to prevent accel chip locking us out
if (PIOS_Flash_Jedec_ClaimBus(flash_dev) < 0) {
if (PIOS_Flash_Jedec_ClaimBus(flash_dev, true) < 0) {
return -1;
}
@ -536,13 +537,12 @@ static int32_t PIOS_Flash_Jedec_WriteChunks(uintptr_t flash_id, uint32_t addr, s
if (((addr & 0xff) + len) > 0x100) {
return -3;
}
if ((ret = PIOS_Flash_Jedec_WriteEnable(flash_dev)) != 0) {
return ret;
}
/* Execute write page command and clock in address. Keep CS asserted */
if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) {
if (PIOS_Flash_Jedec_ClaimBus(flash_dev, true) != 0) {
return -1;
}
@ -582,18 +582,30 @@ static int32_t PIOS_Flash_Jedec_ReadData(uintptr_t flash_id, uint32_t addr, uint
if (PIOS_Flash_Jedec_Validate(flash_dev) != 0) {
return -1;
}
if (PIOS_Flash_Jedec_ClaimBus(flash_dev) == -1) {
bool fast_read = flash_dev->cfg->fast_read != 0;
if (PIOS_Flash_Jedec_ClaimBus(flash_dev, fast_read) == -1) {
return -1;
}
/* Execute read command and clock in address. Keep CS asserted */
if (!fast_read) {
uint8_t out[] = { JEDEC_READ_DATA, (addr >> 16) & 0xff, (addr >> 8) & 0xff, addr & 0xff };
if (PIOS_SPI_TransferBlock(flash_dev->spi_id, out, NULL, sizeof(out), NULL) < 0) {
PIOS_Flash_Jedec_ReleaseBus(flash_dev);
return -2;
}
} else {
uint8_t cmdlen = flash_dev->cfg->fast_read_dummy_bytes + 4;
uint8_t out[cmdlen];
memset(out, 0x0, cmdlen);
out[0] = flash_dev->cfg->fast_read;
out[1] = (addr >> 16) & 0xff;
out[2] = (addr >> 8) & 0xff;
out[3] = addr & 0xff;
if (PIOS_SPI_TransferBlock(flash_dev->spi_id, out, NULL, cmdlen, NULL) < 0) {
PIOS_Flash_Jedec_ReleaseBus(flash_dev);
return -2;
}
}
/* Copy the transfer data to the buffer */
if (PIOS_SPI_TransferBlock(flash_dev->spi_id, NULL, data, len, NULL) < 0) {

425
flight/pios/common/pios_hmc5883.c
View File

@ -1,425 +0,0 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_HMC5883 HMC5883 Functions
* @brief Deals with the hardware interface to the magnetometers
* @{
* @file pios_hmc5883.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief HMC5883 Magnetic Sensor Functions from AHRS
* @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"
#ifdef PIOS_INCLUDE_HMC5883
/* Global Variables */
/* Local Types */
/* Local Variables */
volatile bool pios_hmc5883_data_ready;
static int32_t PIOS_HMC5883_Config(const struct pios_hmc5883_cfg *cfg);
static int32_t PIOS_HMC5883_Read(uint8_t address, uint8_t *buffer, uint8_t len);
static int32_t PIOS_HMC5883_Write(uint8_t address, uint8_t buffer);
static const struct pios_hmc5883_cfg *dev_cfg;
/**
* @brief Initialize the HMC5883 magnetometer sensor.
* @return none
*/
void PIOS_HMC5883_Init(const struct pios_hmc5883_cfg *cfg)
{
dev_cfg = cfg; // store config before enabling interrupt
#ifdef PIOS_HMC5883_HAS_GPIOS
PIOS_EXTI_Init(cfg->exti_cfg);
#endif
int32_t val = PIOS_HMC5883_Config(cfg);
PIOS_Assert(val == 0);
pios_hmc5883_data_ready = false;
}
/**
* @brief Initialize the HMC5883 magnetometer sensor
* \return none
* \param[in] PIOS_HMC5883_ConfigTypeDef struct to be used to configure sensor.
*
* CTRL_REGA: Control Register A
* Read Write
* Default value: 0x10
* 7:5 0 These bits must be cleared for correct operation.
* 4:2 DO2-DO0: Data Output Rate Bits
* DO2 | DO1 | DO0 | Minimum Data Output Rate (Hz)
* ------------------------------------------------------
* 0 | 0 | 0 | 0.75
* 0 | 0 | 1 | 1.5
* 0 | 1 | 0 | 3
* 0 | 1 | 1 | 7.5
* 1 | 0 | 0 | 15 (default)
* 1 | 0 | 1 | 30
* 1 | 1 | 0 | 75
* 1 | 1 | 1 | Not Used
* 1:0 MS1-MS0: Measurement Configuration Bits
* MS1 | MS0 | MODE
* ------------------------------
* 0 | 0 | Normal
* 0 | 1 | Positive Bias
* 1 | 0 | Negative Bias
* 1 | 1 | Not Used
*
* CTRL_REGB: Control RegisterB
* Read Write
* Default value: 0x20
* 7:5 GN2-GN0: Gain Configuration Bits.
* GN2 | GN1 | GN0 | Mag Input | Gain | Output Range
* | | | Range[Ga] | [LSB/mGa] |
* ------------------------------------------------------
* 0 | 0 | 0 | ±0.88Ga | 1370 | 0xF800–0x07FF (-2048:2047)
* 0 | 0 | 1 | ±1.3Ga (def) | 1090 | 0xF800–0x07FF (-2048:2047)
* 0 | 1 | 0 | ±1.9Ga | 820 | 0xF800–0x07FF (-2048:2047)
* 0 | 1 | 1 | ±2.5Ga | 660 | 0xF800–0x07FF (-2048:2047)
* 1 | 0 | 0 | ±4.0Ga | 440 | 0xF800–0x07FF (-2048:2047)
* 1 | 0 | 1 | ±4.7Ga | 390 | 0xF800–0x07FF (-2048:2047)
* 1 | 1 | 0 | ±5.6Ga | 330 | 0xF800–0x07FF (-2048:2047)
* 1 | 1 | 1 | ±8.1Ga | 230 | 0xF800–0x07FF (-2048:2047)
* |Not recommended|
*
* 4:0 CRB4-CRB: 0 This bit must be cleared for correct operation.
*
* _MODE_REG: Mode Register
* Read Write
* Default value: 0x02
* 7:2 0 These bits must be cleared for correct operation.
* 1:0 MD1-MD0: Mode Select Bits
* MS1 | MS0 | MODE
* ------------------------------
* 0 | 0 | Continuous-Conversion Mode.
* 0 | 1 | Single-Conversion Mode
* 1 | 0 | Negative Bias
* 1 | 1 | Sleep Mode
*/
static uint8_t CTRLB = 0x00;
static int32_t PIOS_HMC5883_Config(const struct pios_hmc5883_cfg *cfg)
{
uint8_t CTRLA = 0x00;
uint8_t MODE = 0x00;
CTRLB = 0;
CTRLA |= (uint8_t)(cfg->M_ODR | cfg->Meas_Conf);
CTRLB |= (uint8_t)(cfg->Gain);
MODE |= (uint8_t)(cfg->Mode);
// CRTL_REGA
if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_A, CTRLA) != 0) {
return -1;
}
// CRTL_REGB
if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_B, CTRLB) != 0) {
return -1;
}
// Mode register
if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, MODE) != 0) {
return -1;
}
return 0;
}
/**
* @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
* \return 0 for success or -1 for failure
*/
int32_t PIOS_HMC5883_ReadMag(int16_t out[3])
{
pios_hmc5883_data_ready = false;
uint8_t buffer[6];
int32_t temp;
int32_t sensitivity;
if (PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_XMSB_REG, buffer, 6) != 0) {
return -1;
}
switch (CTRLB & 0xE0) {
case 0x00:
sensitivity = PIOS_HMC5883_Sensitivity_0_88Ga;
break;
case 0x20:
sensitivity = PIOS_HMC5883_Sensitivity_1_3Ga;
break;
case 0x40:
sensitivity = PIOS_HMC5883_Sensitivity_1_9Ga;
break;
case 0x60:
sensitivity = PIOS_HMC5883_Sensitivity_2_5Ga;
break;
case 0x80:
sensitivity = PIOS_HMC5883_Sensitivity_4_0Ga;
break;
case 0xA0:
sensitivity = PIOS_HMC5883_Sensitivity_4_7Ga;
break;
case 0xC0:
sensitivity = PIOS_HMC5883_Sensitivity_5_6Ga;
break;
case 0xE0:
sensitivity = PIOS_HMC5883_Sensitivity_8_1Ga;
break;
default:
PIOS_Assert(0);
}
for (int i = 0; i < 3; i++) {
temp = ((int16_t)((uint16_t)buffer[2 * i] << 8)
+ buffer[2 * i + 1]) * 1000 / sensitivity;
out[i] = temp;
}
// Data reads out as X,Z,Y
temp = out[2];
out[2] = out[1];
out[1] = temp;
// This should not be necessary but for some reason it is coming out of continuous conversion mode
PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, PIOS_HMC5883_MODE_CONTINUOUS);
return 0;
}
/**
* @brief Read the identification bytes from the HMC5883 sensor
* \param[out] uint8_t array of size 4 to store HMC5883 ID.
* \return 0 if successful, -1 if not
*/
uint8_t PIOS_HMC5883_ReadID(uint8_t out[4])
{
uint8_t retval = PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_IDA_REG, out, 3);
out[3] = '\0';
return retval;
}
/**
* @brief Tells whether new magnetometer readings are available
* \return true if new data is available
* \return false if new data is not available
*/
bool PIOS_HMC5883_NewDataAvailable(void)
{
return pios_hmc5883_data_ready;
}
/**
* @brief Reads one or more bytes into a buffer
* \param[in] address HMC5883 register address (depends on size)
* \param[out] buffer destination buffer
* \param[in] len number of bytes which should be read
* \return 0 if operation was successful
* \return -1 if error during I2C transfer
* \return -2 if unable to claim i2c device
*/
static int32_t PIOS_HMC5883_Read(uint8_t address, uint8_t *buffer, uint8_t len)
{
uint8_t addr_buffer[] = {
address,
};
const struct pios_i2c_txn txn_list[] = {
{
.info = __func__,
.addr = PIOS_HMC5883_I2C_ADDR,
.rw = PIOS_I2C_TXN_WRITE,
.len = sizeof(addr_buffer),
.buf = addr_buffer,
}
,
{
.info = __func__,
.addr = PIOS_HMC5883_I2C_ADDR,
.rw = PIOS_I2C_TXN_READ,
.len = len,
.buf = buffer,
}
};
return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list));
}
/**
* @brief Writes one or more bytes to the HMC5883
* \param[in] address Register address
* \param[in] buffer source buffer
* \return 0 if operation was successful
* \return -1 if error during I2C transfer
* \return -2 if unable to claim i2c device
*/
static int32_t PIOS_HMC5883_Write(uint8_t address, uint8_t buffer)
{
uint8_t data[] = {
address,
buffer,
};
const struct pios_i2c_txn txn_list[] = {
{
.info = __func__,
.addr = PIOS_HMC5883_I2C_ADDR,
.rw = PIOS_I2C_TXN_WRITE,
.len = sizeof(data),
.buf = data,
}
,
};
;
return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list));
}
/**
* @brief Run self-test operation. Do not call this during operational use!!
* \return 0 if success, -1 if test failed
*/
int32_t PIOS_HMC5883_Test(void)
{
int32_t failed = 0;
uint8_t registers[3] = { 0, 0, 0 };
uint8_t status;
uint8_t ctrl_a_read;
uint8_t ctrl_b_read;
uint8_t mode_read;
int16_t values[3];
/* Verify that ID matches (HMC5883 ID is null-terminated ASCII string "H43") */
char id[4];
PIOS_HMC5883_ReadID((uint8_t *)id);
if ((id[0] != 'H') || (id[1] != '4') || (id[2] != '3')) { // Expect H43
return -1;
}
/* Backup existing configuration */
if (PIOS_HMC5883_Read(PIOS_HMC5883_CONFIG_REG_A, registers, 3) != 0) {
return -1;
}
/* Stop the device and read out last value */
PIOS_DELAY_WaitmS(10);
if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, PIOS_HMC5883_MODE_IDLE) != 0) {
return -1;
}
if (PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_STATUS_REG, &status, 1) != 0) {
return -1;
}
if (PIOS_HMC5883_ReadMag(values) != 0) {
return -1;
}
/*
* Put HMC5883 into self test mode
* This is done by placing measurement config into positive (0x01) or negative (0x10) bias
* and then placing the mode register into single-measurement mode. This causes the HMC5883
* to create an artificial magnetic field of ~1.1 Gauss.
*
* If gain were PIOS_HMC5883_GAIN_2_5, for example, X and Y will read around +766 LSB
* (1.16 Ga * 660 LSB/Ga) and Z would read around +713 LSB (1.08 Ga * 660 LSB/Ga)
*
* Changing measurement config back to PIOS_HMC5883_MEASCONF_NORMAL will leave self-test mode.
*/
PIOS_DELAY_WaitmS(10);
if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_A, PIOS_HMC5883_MEASCONF_BIAS_POS | PIOS_HMC5883_ODR_15) != 0) {
return -1;
}
PIOS_DELAY_WaitmS(10);
if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_B, PIOS_HMC5883_GAIN_8_1) != 0) {
return -1;
}
PIOS_DELAY_WaitmS(10);
if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, PIOS_HMC5883_MODE_SINGLE) != 0) {
return -1;
}
/* Must wait for value to be updated */
PIOS_DELAY_WaitmS(200);
if (PIOS_HMC5883_ReadMag(values) != 0) {
return -1;
}
/*
if(abs(values[0] - 766) > 20)
failed |= 1;
if(abs(values[1] - 766) > 20)
failed |= 1;
if(abs(values[2] - 713) > 20)
failed |= 1;
*/
PIOS_HMC5883_Read(PIOS_HMC5883_CONFIG_REG_A, &ctrl_a_read, 1);
PIOS_HMC5883_Read(PIOS_HMC5883_CONFIG_REG_B, &ctrl_b_read, 1);
PIOS_HMC5883_Read(PIOS_HMC5883_MODE_REG, &mode_read, 1);
PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_STATUS_REG, &status, 1);
/* Restore backup configuration */
PIOS_DELAY_WaitmS(10);
if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_A, registers[0]) != 0) {
return -1;
}
PIOS_DELAY_WaitmS(10);
if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_B, registers[1]) != 0) {
return -1;
}
PIOS_DELAY_WaitmS(10);
if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, registers[2]) != 0) {
return -1;
}
return failed;
}
/**
* @brief IRQ Handler
*/
bool PIOS_HMC5883_IRQHandler(void)
{
pios_hmc5883_data_ready = true;
return false;
}
#endif /* PIOS_INCLUDE_HMC5883 */
/**
* @}
* @}
*/

558
flight/pios/common/pios_hmc5x83.c Normal file
View File

@ -0,0 +1,558 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_HMC5x83 HMC5x83 Functions
* @brief Deals with the hardware interface to the magnetometers
* @{
* @file pios_hmc5x83.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief HMC5x83 Magnetic Sensor Functions from AHRS
* @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"
#include <pios_hmc5x83.h>
#include <pios_mem.h>
#ifdef PIOS_INCLUDE_HMC5X83
#define PIOS_HMC5X83_MAGIC 0x4d783833
/* Global Variables */
/* Local Types */
typedef struct {
uint32_t magic;
const struct pios_hmc5x83_cfg *cfg;
uint32_t port_id;
uint8_t slave_num;
uint8_t CTRLB;
volatile bool data_ready;
} pios_hmc5x83_dev_data_t;
static int32_t PIOS_HMC5x83_Config(pios_hmc5x83_dev_data_t *dev);
/**
* Allocate the device setting structure
* @return pios_hmc5x83_dev_data_t pointer to newly created structure
*/
pios_hmc5x83_dev_data_t *dev_alloc()
{
pios_hmc5x83_dev_data_t *dev = (pios_hmc5x83_dev_data_t *)pios_malloc(sizeof(pios_hmc5x83_dev_data_t));
PIOS_DEBUG_Assert(dev);
memset(dev, 0x00, sizeof(pios_hmc5x83_dev_data_t));
dev->magic = PIOS_HMC5X83_MAGIC;
return dev;
}
/**
* Validate a pios_hmc5x83_dev_t handler and return the related pios_hmc5x83_dev_data_t pointer
* @param dev device handler
* @return the device data structure
*/
pios_hmc5x83_dev_data_t *dev_validate(pios_hmc5x83_dev_t dev)
{
pios_hmc5x83_dev_data_t *dev_data = (pios_hmc5x83_dev_data_t *)dev;
PIOS_DEBUG_Assert(dev_data->magic == PIOS_HMC5X83_MAGIC);
return dev_data;
}
/**
* @brief Initialize the HMC5x83 magnetometer sensor.
* @return none
*/
pios_hmc5x83_dev_t PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_t port_id, uint8_t slave_num)
{
pios_hmc5x83_dev_data_t *dev = dev_alloc();
dev->cfg = cfg; // store config before enabling interrupt
dev->port_id = port_id;
dev->slave_num = slave_num;
#ifdef PIOS_HMC5X83_HAS_GPIOS
PIOS_EXTI_Init(cfg->exti_cfg);
#endif
int32_t val = PIOS_HMC5x83_Config(dev);
PIOS_Assert(val == 0);
dev->data_ready = false;
return (pios_hmc5x83_dev_t)dev;
}
/**
* @brief Initialize the HMC5x83 magnetometer sensor
* \return none
* \param[in] pios_hmc5x83_dev_data_t device config to be used.
* \param[in] PIOS_HMC5x83_ConfigTypeDef struct to be used to configure sensor.
*
* CTRL_REGA: Control Register A
* Read Write
* Default value: 0x10
* 7:5 0 These bits must be cleared for correct operation.
* 4:2 DO2-DO0: Data Output Rate Bits
* DO2 | DO1 | DO0 | Minimum Data Output Rate (Hz)
* ------------------------------------------------------
* 0 | 0 | 0 | 0.75
* 0 | 0 | 1 | 1.5
* 0 | 1 | 0 | 3
* 0 | 1 | 1 | 7.5
* 1 | 0 | 0 | 15 (default)
* 1 | 0 | 1 | 30
* 1 | 1 | 0 | 75
* 1 | 1 | 1 | Not Used
* 1:0 MS1-MS0: Measurement Configuration Bits
* MS1 | MS0 | MODE
* ------------------------------
* 0 | 0 | Normal
* 0 | 1 | Positive Bias
* 1 | 0 | Negative Bias
* 1 | 1 | Not Used
*
* CTRL_REGB: Control RegisterB
* Read Write
* Default value: 0x20
* 7:5 GN2-GN0: Gain Configuration Bits.
* GN2 | GN1 | GN0 | Mag Input | Gain | Output Range
* | | | Range[Ga] | [LSB/mGa] |
* ------------------------------------------------------
* 0 | 0 | 0 | ±0.88Ga | 1370 | 0xF800–0x07FF (-2048:2047)
* 0 | 0 | 1 | ±1.3Ga (def) | 1090 | 0xF800–0x07FF (-2048:2047)
* 0 | 1 | 0 | ±1.9Ga | 820 | 0xF800–0x07FF (-2048:2047)
* 0 | 1 | 1 | ±2.5Ga | 660 | 0xF800–0x07FF (-2048:2047)
* 1 | 0 | 0 | ±4.0Ga | 440 | 0xF800–0x07FF (-2048:2047)
* 1 | 0 | 1 | ±4.7Ga | 390 | 0xF800–0x07FF (-2048:2047)
* 1 | 1 | 0 | ±5.6Ga | 330 | 0xF800–0x07FF (-2048:2047)
* 1 | 1 | 1 | ±8.1Ga | 230 | 0xF800–0x07FF (-2048:2047)
* |Not recommended|
*
* 4:0 CRB4-CRB: 0 This bit must be cleared for correct operation.
*
* _MODE_REG: Mode Register
* Read Write
* Default value: 0x02
* 7:2 0 These bits must be cleared for correct operation.
* 1:0 MD1-MD0: Mode Select Bits
* MS1 | MS0 | MODE
* ------------------------------
* 0 | 0 | Continuous-Conversion Mode.
* 0 | 1 | Single-Conversion Mode
* 1 | 0 | Negative Bias
* 1 | 1 | Sleep Mode
*/
static int32_t PIOS_HMC5x83_Config(pios_hmc5x83_dev_data_t *dev)
{
uint8_t CTRLA = 0x00;
uint8_t MODE = 0x00;
const struct pios_hmc5x83_cfg *cfg = dev->cfg;
dev->CTRLB = 0;
CTRLA |= (uint8_t)(cfg->M_ODR | cfg->Meas_Conf);
CTRLA |= cfg->TempCompensation ? PIOS_HMC5x83_CTRLA_TEMP : 0;
dev->CTRLB |= (uint8_t)(cfg->Gain);
MODE |= (uint8_t)(cfg->Mode);
// CRTL_REGA
if (cfg->Driver->Write((pios_hmc5x83_dev_t)dev, PIOS_HMC5x83_CONFIG_REG_A, CTRLA) != 0) {
return -1;
}
// CRTL_REGB
if (cfg->Driver->Write((pios_hmc5x83_dev_t)dev, PIOS_HMC5x83_CONFIG_REG_B, dev->CTRLB) != 0) {
return -1;
}
// Mode register
if (cfg->Driver->Write((pios_hmc5x83_dev_t)dev, PIOS_HMC5x83_MODE_REG, MODE) != 0) {
return -1;
}
return 0;
}
/**
* @brief Read current X, Z, Y values (in that order)
* \param[in] dev device handler
* \param[out] int16_t array of size 3 to store X, Z, and Y magnetometer readings
* \return 0 for success or -1 for failure
*/
int32_t PIOS_HMC5x83_ReadMag(pios_hmc5x83_dev_t handler, int16_t out[3])
{
pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
dev->data_ready = false;
uint8_t buffer[6];
int32_t temp;
int32_t sensitivity;
if (dev->cfg->Driver->Read(handler, PIOS_HMC5x83_DATAOUT_XMSB_REG, buffer, 6) != 0) {
return -1;
}
switch (dev->CTRLB & 0xE0) {
case 0x00:
sensitivity = PIOS_HMC5x83_Sensitivity_0_88Ga;
break;
case 0x20:
sensitivity = PIOS_HMC5x83_Sensitivity_1_3Ga;
break;
case 0x40:
sensitivity = PIOS_HMC5x83_Sensitivity_1_9Ga;
break;
case 0x60:
sensitivity = PIOS_HMC5x83_Sensitivity_2_5Ga;
break;
case 0x80:
sensitivity = PIOS_HMC5x83_Sensitivity_4_0Ga;
break;
case 0xA0:
sensitivity = PIOS_HMC5x83_Sensitivity_4_7Ga;
break;
case 0xC0:
sensitivity = PIOS_HMC5x83_Sensitivity_5_6Ga;
break;
case 0xE0:
sensitivity = PIOS_HMC5x83_Sensitivity_8_1Ga;
break;
default:
PIOS_Assert(0);
}
for (int i = 0; i < 3; i++) {
temp = ((int16_t)((uint16_t)buffer[2 * i] << 8)
+ buffer[2 * i + 1]) * 1000 / sensitivity;
out[i] = temp;
}
// Data reads out as X,Z,Y
temp = out[2];
out[2] = out[1];
out[1] = temp;
// This should not be necessary but for some reason it is coming out of continuous conversion mode
dev->cfg->Driver->Write(handler, PIOS_HMC5x83_MODE_REG, PIOS_HMC5x83_MODE_CONTINUOUS);
return 0;
}
/**
* @brief Read the identification bytes from the HMC5x83 sensor
* \param[out] uint8_t array of size 4 to store HMC5x83 ID.
* \return 0 if successful, -1 if not
*/
uint8_t PIOS_HMC5x83_ReadID(pios_hmc5x83_dev_t handler, uint8_t out[4])
{
pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
uint8_t retval = dev->cfg->Driver->Read(handler, PIOS_HMC5x83_DATAOUT_IDA_REG, out, 3);
out[3] = '\0';
return retval;
}
/**
* @brief Tells whether new magnetometer readings are available
* \return true if new data is available
* \return false if new data is not available
*/
bool PIOS_HMC5x83_NewDataAvailable(pios_hmc5x83_dev_t handler)
{
pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
return dev->data_ready;
}
/**
* @brief Run self-test operation. Do not call this during operational use!!
* \return 0 if success, -1 if test failed
*/
int32_t PIOS_HMC5x83_Test(pios_hmc5x83_dev_t handler)
{
int32_t failed = 0;
uint8_t registers[3] = { 0, 0, 0 };
uint8_t status;
uint8_t ctrl_a_read;
uint8_t ctrl_b_read;
uint8_t mode_read;
int16_t values[3];
pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
/* Verify that ID matches (HMC5x83 ID is null-terminated ASCII string "H43") */
char id[4];
PIOS_HMC5x83_ReadID(handler, (uint8_t *)id);
if ((id[0] != 'H') || (id[1] != '4') || (id[2] != '3')) { // Expect H43
return -1;
}
/* Backup existing configuration */
if (dev->cfg->Driver->Read(handler, PIOS_HMC5x83_CONFIG_REG_A, registers, 3) != 0) {
return -1;
}
/* Stop the device and read out last value */
PIOS_DELAY_WaitmS(10);
if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_MODE_REG, PIOS_HMC5x83_MODE_IDLE) != 0) {
return -1;
}
if (dev->cfg->Driver->Read(handler, PIOS_HMC5x83_DATAOUT_STATUS_REG, &status, 1) != 0) {
return -1;
}
if (PIOS_HMC5x83_ReadMag(handler, values) != 0) {
return -1;
}
/*
* Put HMC5x83 into self test mode
* This is done by placing measurement config into positive (0x01) or negative (0x10) bias
* and then placing the mode register into single-measurement mode. This causes the HMC5x83
* to create an artificial magnetic field of ~1.1 Gauss.
*
* If gain were PIOS_HMC5x83_GAIN_2_5, for example, X and Y will read around +766 LSB
* (1.16 Ga * 660 LSB/Ga) and Z would read around +713 LSB (1.08 Ga * 660 LSB/Ga)
*
* Changing measurement config back to PIOS_HMC5x83_MEASCONF_NORMAL will leave self-test mode.
*/
PIOS_DELAY_WaitmS(10);
if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_CONFIG_REG_A, PIOS_HMC5x83_MEASCONF_BIAS_POS | PIOS_HMC5x83_ODR_15) != 0) {
return -1;
}
PIOS_DELAY_WaitmS(10);
if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_CONFIG_REG_B, PIOS_HMC5x83_GAIN_8_1) != 0) {
return -1;
}
PIOS_DELAY_WaitmS(10);
if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_MODE_REG, PIOS_HMC5x83_MODE_SINGLE) != 0) {
return -1;
}
/* Must wait for value to be updated */
PIOS_DELAY_WaitmS(200);
if (PIOS_HMC5x83_ReadMag(handler, values) != 0) {
return -1;
}
dev->cfg->Driver->Read(handler, PIOS_HMC5x83_CONFIG_REG_A, &ctrl_a_read, 1);
dev->cfg->Driver->Read(handler, PIOS_HMC5x83_CONFIG_REG_B, &ctrl_b_read, 1);
dev->cfg->Driver->Read(handler, PIOS_HMC5x83_MODE_REG, &mode_read, 1);
dev->cfg->Driver->Read(handler, PIOS_HMC5x83_DATAOUT_STATUS_REG, &status, 1);
/* Restore backup configuration */
PIOS_DELAY_WaitmS(10);
if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_CONFIG_REG_A, registers[0]) != 0) {
return -1;
}
PIOS_DELAY_WaitmS(10);
if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_CONFIG_REG_B, registers[1]) != 0) {
return -1;
}
PIOS_DELAY_WaitmS(10);
if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_MODE_REG, registers[2]) != 0) {
return -1;
}
return failed;
}
/**
* @brief IRQ Handler
*/
bool PIOS_HMC5x83_IRQHandler(pios_hmc5x83_dev_t handler)
{
pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
dev->data_ready = true;
return false;
}
#ifdef PIOS_INCLUDE_SPI
int32_t PIOS_HMC5x83_SPI_Read(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t *buffer, uint8_t len);
int32_t PIOS_HMC5x83_SPI_Write(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t buffer);
const struct pios_hmc5x83_io_driver PIOS_HMC5x83_SPI_DRIVER = {
.Read = PIOS_HMC5x83_SPI_Read,
.Write = PIOS_HMC5x83_SPI_Write,
};
static int32_t pios_hmc5x83_spi_claim_bus(pios_hmc5x83_dev_data_t *dev)
{
if (PIOS_SPI_ClaimBus(dev->port_id) < 0) {
return -1;
}
PIOS_SPI_SetClockSpeed(dev->port_id, SPI_BaudRatePrescaler_16);
PIOS_SPI_RC_PinSet(dev->port_id, dev->slave_num, 0);
return 0;
}
static void pios_hmc5x83_spi_release_bus(pios_hmc5x83_dev_data_t *dev)
{
PIOS_SPI_RC_PinSet(dev->port_id, dev->slave_num, 1);
PIOS_SPI_ReleaseBus(dev->port_id);
}
/**
* @brief Reads one or more bytes into a buffer
* \param[in] address HMC5x83 register address (depends on size)
* \param[out] buffer destination buffer
* \param[in] len number of bytes which should be read
* \return 0 if operation was successful
* \return -1 if error during I2C transfer
* \return -2 if unable to claim i2c device
*/
int32_t PIOS_HMC5x83_SPI_Read(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t *buffer, uint8_t len)
{
pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
if (pios_hmc5x83_spi_claim_bus(dev) < 0) {
return -1;
}
memset(buffer, 0xA5, len);
PIOS_SPI_TransferByte(dev->port_id, address | PIOS_HMC5x83_SPI_AUTOINCR_FLAG | PIOS_HMC5x83_SPI_READ_FLAG);
// buffer[0] = address | PIOS_HMC5x83_SPI_AUTOINCR_FLAG | PIOS_HMC5x83_SPI_READ_FLAG;
/* Copy the transfer data to the buffer */
if (PIOS_SPI_TransferBlock(dev->port_id, NULL, buffer, len, NULL) < 0) {
pios_hmc5x83_spi_release_bus(dev);
return -3;
}
pios_hmc5x83_spi_release_bus(dev);
return 0;
}
/**
* @brief Writes one or more bytes to the HMC5x83
* \param[in] address Register address
* \param[in] buffer source buffer
* \return 0 if operation was successful
* \return -1 if error during I2C transfer
* \return -2 if unable to claim spi device
*/
int32_t PIOS_HMC5x83_SPI_Write(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t buffer)
{
pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
if (pios_hmc5x83_spi_claim_bus(dev) < 0) {
return -1;
}
uint8_t data[] = {
address | PIOS_HMC5x83_SPI_AUTOINCR_FLAG,
buffer,
};
if (PIOS_SPI_TransferBlock(dev->port_id, data, NULL, sizeof(data), NULL) < 0) {
pios_hmc5x83_spi_release_bus(dev);
return -2;
}
pios_hmc5x83_spi_release_bus(dev);
return 0;
}
#endif /* PIOS_INCLUDE_SPI */
#ifdef PIOS_INCLUDE_I2C
int32_t PIOS_HMC5x83_I2C_Read(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t *buffer, uint8_t len);
int32_t PIOS_HMC5x83_I2C_Write(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t buffer);
const struct pios_hmc5x83_io_driver PIOS_HMC5x83_I2C_DRIVER = {
.Read = PIOS_HMC5x83_I2C_Read,
.Write = PIOS_HMC5x83_I2C_Write,
};
/**
* @brief Reads one or more bytes into a buffer
* \param[in] address HMC5x83 register address (depends on size)
* \param[out] buffer destination buffer
* \param[in] len number of bytes which should be read
* \return 0 if operation was successful
* \return -1 if error during I2C transfer
* \return -2 if unable to claim i2c device
*/
int32_t PIOS_HMC5x83_I2C_Read(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t *buffer, uint8_t len)
{
pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
uint8_t addr_buffer[] = {
address,
};
const struct pios_i2c_txn txn_list[] = {
{
.info = __func__,
.addr = PIOS_HMC5x83_I2C_ADDR,
.rw = PIOS_I2C_TXN_WRITE,
.len = sizeof(addr_buffer),
.buf = addr_buffer,
}
,
{
.info = __func__,
.addr = PIOS_HMC5x83_I2C_ADDR,
.rw = PIOS_I2C_TXN_READ,
.len = len,
.buf = buffer,
}
};
return PIOS_I2C_Transfer(dev->port_id, txn_list, NELEMENTS(txn_list));
}
/**
* @brief Writes one or more bytes to the HMC5x83
* \param[in] address Register address
* \param[in] buffer source buffer
* \return 0 if operation was successful
* \return -1 if error during I2C transfer
* \return -2 if unable to claim i2c device
*/
int32_t PIOS_HMC5x83_I2C_Write(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t buffer)
{
pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
uint8_t data[] = {
address,
buffer,
};
const struct pios_i2c_txn txn_list[] = {
{
.info = __func__,
.addr = PIOS_HMC5x83_I2C_ADDR,
.rw = PIOS_I2C_TXN_WRITE,
.len = sizeof(data),
.buf = data,
}
,
};
;
return PIOS_I2C_Transfer(dev->port_id, txn_list, NELEMENTS(txn_list));
}
#endif /* PIOS_INCLUDE_I2C */
#endif /* PIOS_INCLUDE_HMC5x83 */
/**
* @}
* @}
*/

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

@ -33,15 +33,13 @@
#ifdef PIOS_INCLUDE_MPU6000
#include "fifo_buffer.h"
#include <pios_constants.h>
/* Global Variables */
enum pios_mpu6000_dev_magic {
PIOS_MPU6000_DEV_MAGIC = 0x9da9b3ed,
};
#define PIOS_MPU6000_MAX_DOWNSAMPLE 2
struct mpu6000_dev {
uint32_t spi_id;
uint32_t slave_num;
@ -53,29 +51,58 @@ 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;
uint8_t Gyro_X_l;
uint8_t Gyro_Y_h;
uint8_t Gyro_Y_l;
uint8_t Gyro_Z_h;
uint8_t Gyro_Z_l;
} data;
} mpu6000_data_t;
#define GET_SENSOR_DATA(mpudataptr, sensor) (mpudataptr.data.sensor##_h << 8 | mpudataptr.data.sensor##_l)
// ! Global structure for this device device
static struct mpu6000_dev *dev;
volatile bool mpu6000_configured = false;
static mpu6000_data_t mpu6000_data;
// ! Private functions
static struct mpu6000_dev *PIOS_MPU6000_alloc(void);
static struct mpu6000_dev *PIOS_MPU6000_alloc(const struct pios_mpu6000_cfg *cfg);
static int32_t PIOS_MPU6000_Validate(struct mpu6000_dev *dev);
static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const *cfg);
static int32_t PIOS_MPU6000_SetReg(uint8_t address, uint8_t buffer);
static int32_t PIOS_MPU6000_GetReg(uint8_t address);
#define GRAV 9.81f
#ifdef PIOS_MPU6000_ACCEL
#define PIOS_MPU6000_SAMPLES_BYTES 14
#else
#define PIOS_MPU6000_SAMPLES_BYTES 8
#endif
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);
/**
* @brief Allocate a new device
*/
static struct mpu6000_dev *PIOS_MPU6000_alloc(void)
static struct mpu6000_dev *PIOS_MPU6000_alloc(const struct pios_mpu6000_cfg *cfg)
{
struct mpu6000_dev *mpu6000_dev;
@ -86,7 +113,7 @@ static struct mpu6000_dev *PIOS_MPU6000_alloc(void)
mpu6000_dev->magic = PIOS_MPU6000_DEV_MAGIC;
mpu6000_dev->queue = xQueueCreate(PIOS_MPU6000_MAX_DOWNSAMPLE, sizeof(struct pios_mpu6000_data));
mpu6000_dev->queue = xQueueCreate(cfg->max_downsample + 1, sizeof(struct pios_mpu6000_data));
if (mpu6000_dev->queue == NULL) {
vPortFree(mpu6000_dev);
return NULL;
@ -119,7 +146,7 @@ static int32_t PIOS_MPU6000_Validate(struct mpu6000_dev *vdev)
*/
int32_t PIOS_MPU6000_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_mpu6000_cfg *cfg)
{
dev = PIOS_MPU6000_alloc();
dev = PIOS_MPU6000_alloc(cfg);
if (dev == NULL) {
return -1;
}
@ -129,9 +156,7 @@ int32_t PIOS_MPU6000_Init(uint32_t spi_id, uint32_t slave_num, const struct pios
dev->cfg = cfg;
/* Configure the MPU6000 Sensor */
PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_256);
PIOS_MPU6000_Config(cfg);
PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_16);
/* Set up EXTI line */
PIOS_EXTI_Init(cfg->exti_cfg);
@ -234,11 +259,6 @@ int32_t PIOS_MPU6000_ConfigureRanges(
return -1;
}
// TODO: check that changing the SPI clock speed is safe
// to do here given that interrupts are enabled and the bus has
// not been claimed/is not claimed during this call
PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_256);
// update filter settings
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_DLPF_CFG_REG, filterSetting) != 0) {
;
@ -267,7 +287,6 @@ int32_t PIOS_MPU6000_ConfigureRanges(
dev->accel_range = accelRange;
#endif
PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_16);
return 0;
}
@ -275,7 +294,7 @@ int32_t PIOS_MPU6000_ConfigureRanges(
* @brief Claim the SPI bus for the accel communications and select this chip
* @return 0 if successful, -1 for invalid device, -2 if unable to claim bus
*/
static int32_t PIOS_MPU6000_ClaimBus()
static int32_t PIOS_MPU6000_ClaimBus(bool fast_spi)
{
if (PIOS_MPU6000_Validate(dev) != 0) {
return -1;
@ -283,17 +302,28 @@ static int32_t PIOS_MPU6000_ClaimBus()
if (PIOS_SPI_ClaimBus(dev->spi_id) != 0) {
return -2;
}
PIOS_MPU6000_SetSpeed(fast_spi);
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
return 0;
}
static void PIOS_MPU6000_SetSpeed(const bool fast)
{
if (fast) {
PIOS_SPI_SetClockSpeed(dev->spi_id, dev->cfg->fast_prescaler);
} else {
PIOS_SPI_SetClockSpeed(dev->spi_id, dev->cfg->std_prescaler);
}
}
/**
* @brief Claim the SPI bus for the accel communications and select this chip
* @return 0 if successful, -1 for invalid device, -2 if unable to claim bus
* @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_ClaimBusISR(bool *woken)
static int32_t PIOS_MPU6000_ClaimBusISR(bool *woken, bool fast_spi)
{
if (PIOS_MPU6000_Validate(dev) != 0) {
return -1;
@ -301,6 +331,7 @@ static int32_t PIOS_MPU6000_ClaimBusISR(bool *woken)
if (PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) {
return -2;
}
PIOS_MPU6000_SetSpeed(fast_spi);
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
return 0;
}
@ -342,7 +373,7 @@ static int32_t PIOS_MPU6000_GetReg(uint8_t reg)
{
uint8_t data;
if (PIOS_MPU6000_ClaimBus() != 0) {
if (PIOS_MPU6000_ClaimBus(false) != 0) {
return -1;
}
@ -363,7 +394,7 @@ static int32_t PIOS_MPU6000_GetReg(uint8_t reg)
*/
static int32_t PIOS_MPU6000_SetReg(uint8_t reg, uint8_t data)
{
if (PIOS_MPU6000_ClaimBus() != 0) {
if (PIOS_MPU6000_ClaimBus(false) != 0) {
return -1;
}
@ -393,7 +424,7 @@ int32_t PIOS_MPU6000_ReadGyros(struct pios_mpu6000_data *data)
uint8_t buf[7] = { PIOS_MPU6000_GYRO_X_OUT_MSB | 0x80, 0, 0, 0, 0, 0, 0 };
uint8_t rec[7];
if (PIOS_MPU6000_ClaimBus() != 0) {
if (PIOS_MPU6000_ClaimBus(true) != 0) {
return -1;
}
@ -460,16 +491,16 @@ float PIOS_MPU6000_GetAccelScale()
{
switch (dev->accel_range) {
case PIOS_MPU6000_ACCEL_2G:
return GRAV / 16384.0f;
return PIOS_CONST_MKS_GRAV_ACCEL_F / 16384.0f;
case PIOS_MPU6000_ACCEL_4G:
return GRAV / 8192.0f;
return PIOS_CONST_MKS_GRAV_ACCEL_F / 8192.0f;
case PIOS_MPU6000_ACCEL_8G:
return GRAV / 4096.0f;
return PIOS_CONST_MKS_GRAV_ACCEL_F / 4096.0f;
case PIOS_MPU6000_ACCEL_16G:
return GRAV / 2048.0f;
return PIOS_CONST_MKS_GRAV_ACCEL_F / 2048.0f;
}
return 0;
}
@ -504,7 +535,7 @@ 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) != 0) {
if (PIOS_MPU6000_ClaimBusISR(woken, false) != 0) {
return -1;
}
PIOS_SPI_TransferByte(dev->spi_id, (0x80 | PIOS_MPU6000_INT_STATUS_REG));
@ -525,29 +556,16 @@ static int32_t PIOS_MPU6000_ResetFifoISR(bool *woken)
{
int32_t result = 0;
/* Register writes must be at < 1MHz SPI clock.
* Speed can only be changed when SPI bus semaphore is held, but
* device chip select must not be enabled, so we use the direct
* SPI bus claim call here */
if (PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) {
if (PIOS_MPU6000_ClaimBusISR(woken, false) != 0) {
return -1;
}
/* Reduce SPI clock speed. */
PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_256);
/* Enable chip select */
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
/* 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;
}
/* Disable chip select. */
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1);
/* Increase SPI clock speed. */
PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_16);
/* Release the SPI bus semaphore. */
PIOS_SPI_ReleaseBusISR(dev->spi_id, woken);
PIOS_MPU6000_ReleaseBusISR(woken);
return result;
}
@ -562,7 +580,7 @@ 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) != 0) {
if (PIOS_MPU6000_ClaimBusISR(woken, false) != 0) {
return -1;
}
@ -604,6 +622,97 @@ bool PIOS_MPU6000_IRQHandler(void)
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);
}
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()
{
// 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
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
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
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
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);
BaseType_t higherPriorityTaskWoken;
xQueueSendToBackFromISR(dev->queue, (void *)&data, &higherPriorityTaskWoken);
return higherPriorityTaskWoken == pdTRUE;
}
static bool PIOS_MPU6000_ReadSensor(bool *woken)
{
const uint8_t mpu6000_send_buf[1 + PIOS_MPU6000_SAMPLES_BYTES] = { PIOS_MPU6000_SENSOR_FIRST_REG | 0x80 };
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;
}
static bool PIOS_MPU6000_ReadFifo(bool *woken)
{
/* 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
@ -611,128 +720,57 @@ bool PIOS_MPU6000_IRQHandler(void)
* any read clears in the status register (PIOS_MPU6000_INT_CLR_ANYRD set in
* interrupt config register) */
int32_t result;
if ((result = PIOS_MPU6000_GetInterruptStatusRegISR(&woken)) < 0) {
return woken;
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);
PIOS_MPU6000_ResetFifoISR(woken);
/* Return and wait for the next new sample. */
return woken;
return false;
}
/* Usual case - FIFO has not overflowed. */
mpu6000_count = PIOS_MPU6000_FifoDepthISR(&woken);
mpu6000_count = PIOS_MPU6000_FifoDepthISR(woken);
if (mpu6000_count < PIOS_MPU6000_SAMPLES_BYTES) {
return woken;
return false;
}
if (PIOS_MPU6000_ClaimBusISR(&woken) != 0) {
return woken;
if (PIOS_MPU6000_ClaimBusISR(woken, true) != 0) {
return false;
}
static uint8_t mpu6000_send_buf[1 + PIOS_MPU6000_SAMPLES_BYTES] = { PIOS_MPU6000_FIFO_REG | 0x80 };
static uint8_t mpu6000_rec_buf[1 + PIOS_MPU6000_SAMPLES_BYTES];
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_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) {
PIOS_MPU6000_ReleaseBusISR(&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 woken;
return false;
}
PIOS_MPU6000_ReleaseBusISR(&woken);
static struct pios_mpu6000_data data;
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) != 0) {
return woken;
if (PIOS_MPU6000_ClaimBusISR(woken, true) != 0) {
return false;
}
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);
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 woken;
return false;
}
PIOS_MPU6000_ReleaseBusISR(&woken);
PIOS_MPU6000_ReleaseBusISR(woken);
}
// 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
#if defined(PIOS_MPU6000_ACCEL)
// Currently we only support rotations on top so switch X/Y accordingly
switch (dev->cfg->orientation) {
case PIOS_MPU6000_TOP_0DEG:
data.accel_y = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]; // chip X
data.accel_x = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; // chip Y
data.gyro_y = mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]; // chip X
data.gyro_x = mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]; // chip Y
break;
case PIOS_MPU6000_TOP_90DEG:
// -1 to bring it back to -32768 +32767 range
data.accel_y = -1 - (mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); // chip Y
data.accel_x = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]; // chip X
data.gyro_y = -1 - (mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]); // chip Y
data.gyro_x = mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]; // chip X
break;
case PIOS_MPU6000_TOP_180DEG:
data.accel_y = -1 - (mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]); // chip X
data.accel_x = -1 - (mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); // chip Y
data.gyro_y = -1 - (mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]); // chip X
data.gyro_x = -1 - (mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]); // chip Y
break;
case PIOS_MPU6000_TOP_270DEG:
data.accel_y = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; // chip Y
data.accel_x = -1 - (mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]); // chip X
data.gyro_y = mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]; // chip Y
data.gyro_x = -1 - (mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]); // chip X
break;
}
data.gyro_z = -1 - (mpu6000_rec_buf[13] << 8 | mpu6000_rec_buf[14]);
data.accel_z = -1 - (mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]);
data.temperature = mpu6000_rec_buf[7] << 8 | mpu6000_rec_buf[8];
#else /* if defined(PIOS_MPU6000_ACCEL) */
data.gyro_x = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4];
data.gyro_y = mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6];
switch (dev->cfg->orientation) {
case PIOS_MPU6000_TOP_0DEG:
data.gyro_y = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4];
data.gyro_x = mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6];
break;
case PIOS_MPU6000_TOP_90DEG:
data.gyro_y = -1 - (mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]); // chip Y
data.gyro_x = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; // chip X
break;
case PIOS_MPU6000_TOP_180DEG:
data.gyro_y = -1 - (mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]);
data.gyro_x = -1 - (mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]);
break;
case PIOS_MPU6000_TOP_270DEG:
data.gyro_y = mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]; // chip Y
data.gyro_x = -1 - (mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); // chip X
break;
}
data.gyro_z = -1 - (mpu6000_rec_buf[7] << 8 | mpu6000_rec_buf[8]);
data.temperature = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2];
#endif /* if defined(PIOS_MPU6000_ACCEL) */
signed portBASE_TYPE higherPriorityTaskWoken;
xQueueSendToBackFromISR(dev->queue, (void *)&data, &higherPriorityTaskWoken);
mpu6000_irq++;
mpu6000_time_us = PIOS_DELAY_DiffuS(timeval);
return woken || higherPriorityTaskWoken == pdTRUE;
return true;
}
#endif /* PIOS_INCLUDE_MPU6000 */
/**

36
flight/pios/common/pios_notify.c
View File

@ -28,7 +28,8 @@
static volatile pios_notify_notification currentNotification = NOTIFY_NONE;
static volatile pios_notify_priority currentPriority;
static volatile ExtLedNotification_t extLedNotification;
static volatile bool newNotification;
void PIOS_NOTIFY_StartNotification(pios_notify_notification notification, pios_notify_priority priority)
{
@ -47,3 +48,36 @@ pios_notify_notification PIOS_NOTIFY_GetActiveNotification(bool clear)
}
return ret;
}
/*
* Play a sequence on the default external led. Sequences with priority higher than NOTIFY_PRIORITY_LOW
* are repeated only once if repeat = -1
* @param sequence Sequence to be played
* @param priority Priority of the sequence being played
*/
void PIOS_NOTIFICATION_Default_Ext_Led_Play(const LedSequence_t *sequence, pios_notify_priority priority)
{
// alert and alarms are repeated if condition persists. bacground notification instead are set once, so try to prevent loosing any update
if (newNotification && priority != NOTIFY_PRIORITY_BACKGROUND) {
// prevent overwriting higher priority or background notifications
if (extLedNotification.priority == NOTIFY_PRIORITY_BACKGROUND || extLedNotification.priority > priority) {
return;
}
}
extLedNotification.priority = priority;
extLedNotification.sequence = *sequence;
newNotification = true;
}
ExtLedNotification_t *PIOS_NOTIFY_GetNewExtLedSequence(bool clear)
{
if (!newNotification) {
return 0;
}
if (clear) {
newNotification = false;
}
return (ExtLedNotification_t *)&extLedNotification;
}

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