From 8aa6297597789beeeff1d6426842883e87cb4ece Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 8 Jul 2012 15:59:08 +0200 Subject: [PATCH 01/62] [state interface] added new state interface --- sw/airborne/state.c | 1093 ++++++++++++++++++++++++++++++++++++++++++ sw/airborne/state.h | 1102 +++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 2195 insertions(+) create mode 100644 sw/airborne/state.c create mode 100644 sw/airborne/state.h diff --git a/sw/airborne/state.c b/sw/airborne/state.c new file mode 100644 index 0000000000..ec07740829 --- /dev/null +++ b/sw/airborne/state.c @@ -0,0 +1,1093 @@ +/* + * Copyright (C) 2011-2012 Felix Ruess + * + * This file is part of paparazzi. + * + * paparazzi 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 2, or (at your option) + * any later version. + * + * paparazzi 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 paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file state.c + * + * General inteface for the main vehicle states. + * + * This is the more detailed description of this file. + * + * @author Felix Ruess + */ + +#include "state.h" + +struct State state; + +/** + * @addtogroup StateGroup + * @{ + */ + +void stateInit(void) { + state.pos_status = 0; + state.speed_status = 0; + state.accel_status = 0; + state.att_status = 0; + state.rate_status = 0; + state.wind_air_status = 0; + state.ned_initialized_i = FALSE; + state.ned_initialized_f = FALSE; +} + + +/******************************************************************************* + * * + * transformation functions for the POSITION representations * + * * + ******************************************************************************/ +/** @addtogroup PosGroup + * @{ */ + +void stateCalcPositionEcef_i(void) { + if (bit_is_set(state.pos_status, POS_ECEF_I)) + return; + + if (bit_is_set(state.pos_status, POS_ECEF_F)) { + ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f); + } + else if (bit_is_set(state.pos_status, POS_NED_I)) { + //TODO check if resolution is good enough + ecef_of_ned_point_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i); + } + else if (bit_is_set(state.pos_status, POS_NED_F)) { + /* transform ned_f to ecef_f, set status bit, then convert to int */ + ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f); + SetBit(state.pos_status, POS_ECEF_F); + ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f); + } + else if (bit_is_set(state.pos_status, POS_LLA_F)) { + /* transform lla_f to ecef_f, set status bit, then convert to int */ + ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f); + SetBit(state.pos_status, POS_ECEF_F); + ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f); + } + else if (bit_is_set(state.pos_status, POS_LLA_I)) { + ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); + } + else { + /* could not get this representation, set errno */ + //struct EcefCoor_i _ecef_zero = {0}; + //return _ecef_zero; + } + /* set bit to indicate this representation is computed */ + SetBit(state.pos_status, POS_ECEF_I); +} + +void stateCalcPositionNed_i(void) { + if (bit_is_set(state.pos_status, POS_NED_I)) + return; + + int errno = 0; + if (state.ned_initialized_i) { + if (bit_is_set(state.pos_status, POS_NED_F)) { + NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); + } + else if (bit_is_set(state.pos_status, POS_ECEF_I)) { + ned_of_ecef_point_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i); + } + else if (bit_is_set(state.pos_status, POS_ECEF_F)) { + /* transform ecef_f -> ned_f, set status bit, then convert to int */ + ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f); + SetBit(state.pos_status, POS_NED_F); + NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); + } + else if (bit_is_set(state.pos_status, POS_LLA_F)) { + /* transform lla_f -> ecef_f -> ned_f, set status bits, then convert to int */ + ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f); + SetBit(state.pos_status, POS_ECEF_F); + ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f); + SetBit(state.pos_status, POS_NED_F); + NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); + } + else if (bit_is_set(state.pos_status, POS_LLA_I)) { + ned_of_lla_point_i(&state.ned_pos_i, &state.ned_origin_i, &state.lla_pos_i); + } + else { /* could not get this representation, set errno */ + errno = 1; + } + } else { /* ned coordinate system not initialized, set errno */ + errno = 2; + } + if (errno) { + //struct NedCoor_i _ned_zero = {0}; + //return _ned_zero; + } + /* set bit to indicate this representation is computed */ + SetBit(state.pos_status, POS_NED_I); +} + +void stateCalcPositionEnu_i(void) { + if (bit_is_set(state.pos_status, POS_ENU_I)) + return; + + int errno = 0; + if (state.ned_initialized_i) { + if (bit_is_set(state.pos_status, POS_NED_I)) { + INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i); + } + else if (bit_is_set(state.pos_status, POS_ENU_F)) { + ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f); + } + else if (bit_is_set(state.pos_status, POS_NED_F)) { + NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); + SetBit(state.pos_status, POS_NED_I); + INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i); + } + else if (bit_is_set(state.pos_status, POS_ECEF_I)) { + enu_of_ecef_point_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i); + } + else if (bit_is_set(state.pos_status, POS_ECEF_F)) { + /* transform ecef_f -> enu_f, set status bit, then convert to int */ + enu_of_ecef_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.ecef_pos_f); + SetBit(state.pos_status, POS_ENU_F); + ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f); + } + else if (bit_is_set(state.pos_status, POS_LLA_F)) { + /* transform lla_f -> ecef_f -> enu_f, set status bits, then convert to int */ + ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f); + SetBit(state.pos_status, POS_ECEF_F); + enu_of_ecef_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.ecef_pos_f); + SetBit(state.pos_status, POS_ENU_F); + ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f); + } + else if (bit_is_set(state.pos_status, POS_LLA_I)) { + enu_of_lla_point_i(&state.enu_pos_i, &state.ned_origin_i, &state.lla_pos_i); + } + else { /* could not get this representation, set errno */ + errno = 1; + } + } else { /* ned coordinate system not initialized, set errno */ + errno = 2; + } + if (errno) { + //struct EnuCoor_i _enu_zero = {0}; + //return _enu_zero; + } + /* set bit to indicate this representation is computed */ + SetBit(state.pos_status, POS_ENU_I); +} + +void stateCalcPositionLla_i(void) { + if (bit_is_set(state.pos_status, POS_LLA_I)) + return; + + if (bit_is_set(state.pos_status, POS_LLA_F)) { + LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f); + } + else if (bit_is_set(state.pos_status, POS_ECEF_I)) { + lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i); + } + else if (bit_is_set(state.pos_status, POS_ECEF_F)) { + /* transform ecef_f -> lla_f, set status bit, then convert to int */ + lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f); + SetBit(state.pos_status, POS_LLA_F); + LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f); + } + else if (bit_is_set(state.pos_status, POS_NED_F)) { + /* transform ned_f -> ecef_f -> lla_f -> lla_i, set status bits */ + ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f); + SetBit(state.pos_status, POS_ECEF_F); + lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f); + SetBit(state.pos_status, POS_LLA_F); + LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f); + } + else if (bit_is_set(state.pos_status, POS_NED_I)) { + /* transform ned_i -> ecef_i -> lla_i, set status bits */ + //TODO check if resolution is enough + ecef_of_ned_point_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i); + SetBit(state.pos_status, POS_ECEF_I); + lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i); /* uses double version internally */ + } + else { + /* could not get this representation, set errno */ + //struct LlaCoor_i _lla_zero = {0}; + //return _lla_zero; + } + /* set bit to indicate this representation is computed */ + SetBit(state.pos_status, POS_LLA_I); +} + +void stateCalcPositionUtm_f(void) { + if (bit_is_set(state.pos_status, POS_UTM_F)) + return; + + if (bit_is_set(state.pos_status, POS_LLA_F)) { + utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f); + } + else if (bit_is_set(state.pos_status, POS_LLA_I)) { + /* transform lla_i -> lla_f -> utm_f, set status bits */ + LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_f); + SetBit(state.pos_status, POS_LLA_F); + utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f); + } + else { + /* could not get this representation, set errno */ + //struct EcefCoor_f _ecef_zero = {0.0f}; + //return _ecef_zero; + } + /* set bit to indicate this representation is computed */ + SetBit(state.pos_status, POS_UTM_F); +} + +void stateCalcPositionEcef_f(void) { + if (bit_is_set(state.pos_status, POS_ECEF_F)) + return; + + if (bit_is_set(state.pos_status, POS_ECEF_I)) { + ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i); + } + else if (bit_is_set(state.pos_status, POS_NED_F)) { + ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f); + } + else if (bit_is_set(state.pos_status, POS_NED_I)) { + /* transform ned_i -> ecef_i -> ecef_f, set status bits */ + //TODO check if resolution is good enough + ecef_of_ned_point_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i); + SetBit(state.pos_status, POS_ECEF_F); + ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i); + } + else if (bit_is_set(state.pos_status, POS_LLA_F)) { + ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f); + } + else if (bit_is_set(state.pos_status, POS_LLA_I)) { + LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i); + SetBit(state.pos_status, POS_LLA_F); + ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f); + } + else { + /* could not get this representation, set errno */ + //struct EcefCoor_f _ecef_zero = {0.0f}; + //return _ecef_zero; + } + /* set bit to indicate this representation is computed */ + SetBit(state.pos_status, POS_ECEF_F); +} + +void stateCalcPositionNed_f(void) { + if (bit_is_set(state.pos_status, POS_NED_F)) + return; + + int errno = 0; + if (state.ned_initialized_f) { + if (bit_is_set(state.pos_status, POS_NED_I)) { + NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i); + } + else if (bit_is_set(state.pos_status, POS_ECEF_F)) { + ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f); + } + else if (bit_is_set(state.pos_status, POS_ECEF_I)) { + /* transform ecef_i -> ned_i -> ned_f, set status bits */ + ned_of_ecef_point_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i); + SetBit(state.pos_status, POS_NED_I); + NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i); + } + else if (bit_is_set(state.pos_status, POS_LLA_F)) { + ned_of_lla_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.lla_pos_f); + } + else if (bit_is_set(state.pos_status, POS_LLA_I)) { + /* transform lla_i -> ecef_i -> ned_i -> ned_f, set status bits */ + ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */ + SetBit(state.pos_status, POS_ECEF_I); + ned_of_ecef_point_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i); + SetBit(state.pos_status, POS_NED_I); + NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i); + } + else { /* could not get this representation, set errno */ + errno = 1; + } + } else { /* ned coordinate system not initialized, set errno */ + errno = 2; + } + if (errno) { + //struct NedCoor_f _ned_zero = {0.0f}; + //return _ned_zero; + } + /* set bit to indicate this representation is computed */ + SetBit(state.pos_status, POS_NED_F); +} + +void stateCalcPositionEnu_f(void) { + if (bit_is_set(state.pos_status, POS_ENU_F)) + return; + + int errno = 0; + if (state.ned_initialized_f) { + if (bit_is_set(state.pos_status, POS_NED_F)) { + VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f); + } + else if (bit_is_set(state.pos_status, POS_ENU_I)) { + ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i); + } + else if (bit_is_set(state.pos_status, POS_NED_I)) { + NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i); + SetBit(state.pos_status, POS_NED_F); + VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f); + } + else if (bit_is_set(state.pos_status, POS_ECEF_F)) { + enu_of_ecef_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.ecef_pos_f); + } + else if (bit_is_set(state.pos_status, POS_ECEF_I)) { + /* transform ecef_i -> enu_i -> enu_f, set status bits */ + enu_of_ecef_point_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i); + SetBit(state.pos_status, POS_ENU_I); + ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i); + } + else if (bit_is_set(state.pos_status, POS_LLA_F)) { + enu_of_lla_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.lla_pos_f); + } + else if (bit_is_set(state.pos_status, POS_LLA_I)) { + /* transform lla_i -> ecef_i -> enu_i -> enu_f, set status bits */ + ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */ + SetBit(state.pos_status, POS_ECEF_I); + enu_of_ecef_point_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i); + SetBit(state.pos_status, POS_ENU_I); + ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i); + } + else { /* could not get this representation, set errno */ + errno = 1; + } + } else { /* ned coordinate system not initialized, set errno */ + errno = 2; + } + if (errno) { + //struct EnuCoor_f _enu_zero = {0.0f}; + //return _enu_zero; + } + /* set bit to indicate this representation is computed */ + SetBit(state.pos_status, POS_ENU_F); +} + +void stateCalcPositionLla_f(void) { + if (bit_is_set(state.pos_status, POS_LLA_F)) + return; + + if (bit_is_set(state.pos_status, POS_LLA_I)) { + LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_f); + } + else if (bit_is_set(state.pos_status, POS_ECEF_F)) { + lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f); + } + else if (bit_is_set(state.pos_status, POS_ECEF_I)) { + /* transform ecef_i -> ecef_f -> lla_f, set status bits */ + ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i); + SetBit(state.pos_status, POS_ECEF_F); + lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f); + } + else if (bit_is_set(state.pos_status, POS_NED_F)) { + /* transform ned_f -> ecef_f -> lla_f, set status bits */ + ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f); + SetBit(state.pos_status, POS_ECEF_F); + lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f); + } + else if (bit_is_set(state.pos_status, POS_NED_I)) { + /* transform ned_i -> ned_f -> ecef_f -> lla_f, set status bits */ + NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i); + SetBit(state.pos_status, POS_NED_F); + ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f); + SetBit(state.pos_status, POS_ECEF_F); + lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f); + } + else { + /* could not get this representation, set errno */ + //struct LlaCoor_f _lla_zero = {0.0}; + //return _lla_zero; + } + /* set bit to indicate this representation is computed */ + SetBit(state.pos_status, POS_LLA_F); +} +/** @}*/ + + + + + +/****************************************************************************** + * * + * Transformation functions for the SPEED representations * + * * + *****************************************************************************/ +/** @addtogroup SpeedGroup + * @{ */ +/************************ Set functions ****************************/ + +void stateCalcSpeedNed_i(void) { + if (bit_is_set(state.speed_status, SPEED_NED_I)) + return; + + int errno = 0; + if (state.ned_initialized_i) { + if (bit_is_set(state.speed_status, SPEED_NED_F)) { + SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f); + } + else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) { + ned_of_ecef_vect_i(&state.ned_speed_i, &state.ned_origin_i, &state.ecef_speed_i); + } + else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) { + /* transform ecef_f -> ecef_i -> ned_i , set status bits */ + SPEEDS_BFP_OF_REAL(state.ecef_speed_i, state.ecef_speed_f); + SetBit(state.speed_status, SPEED_ECEF_I); + ned_of_ecef_vect_i(&state.ned_speed_i, &state.ned_origin_i, &state.ecef_speed_i); + } + else { /* could not get this representation, set errno */ + errno = 1; + } + } else { /* ned coordinate system not initialized, set errno */ + errno = 2; + } + if (errno) { + //struct NedCoor_i _ned_zero = {0}; + //return _ned_zero; + } + /* set bit to indicate this representation is computed */ + SetBit(state.speed_status, SPEED_NED_I); +} + +void stateCalcSpeedEnu_i(void) { + if (bit_is_set(state.speed_status, SPEED_ENU_I)) + return; + + int errno = 0; + if (state.ned_initialized_i) { + if (bit_is_set(state.speed_status, SPEED_NED_I)) { + INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i); + } + if (bit_is_set(state.speed_status, SPEED_ENU_F)) { + ENU_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f); + } + else if (bit_is_set(state.speed_status, SPEED_NED_F)) { + SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f); + SetBit(state.pos_status, SPEED_NED_I); + INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i); + } + else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) { + enu_of_ecef_vect_i(&state.enu_speed_i, &state.ned_origin_i, &state.ecef_speed_i); + } + else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) { + /* transform ecef_f -> ecef_i -> enu_i , set status bits */ + SPEEDS_BFP_OF_REAL(state.ecef_speed_i, state.ecef_speed_f); + SetBit(state.speed_status, SPEED_ECEF_I); + enu_of_ecef_vect_i(&state.enu_speed_i, &state.ned_origin_i, &state.ecef_speed_i); + } + else { /* could not get this representation, set errno */ + errno = 1; + } + } else { /* ned coordinate system not initialized, set errno */ + errno = 2; + } + if (errno) { + //struct EnuCoor_i _enu_zero = {0}; + //return _enu_zero; + } + /* set bit to indicate this representation is computed */ + SetBit(state.speed_status, SPEED_ENU_I); +} + +void stateCalcSpeedEcef_i(void) { + if (bit_is_set(state.speed_status, SPEED_ECEF_I)) + return; + + if (bit_is_set(state.speed_status, SPEED_ECEF_F)) { + SPEEDS_BFP_OF_REAL(state.ecef_speed_i, state.ecef_speed_f); + } + else if (bit_is_set(state.speed_status, SPEED_NED_I)) { + ecef_of_ned_vect_i(&state.ecef_speed_i, &state.ned_origin_i, &state.ned_speed_i); + } + else if (bit_is_set(state.speed_status, SPEED_NED_F)) { + /* transform ned_f -> ned_i -> ecef_i , set status bits */ + SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f); + SetBit(state.speed_status, SPEED_NED_I); + ecef_of_ned_vect_i(&state.ecef_speed_i, &state.ned_origin_i, &state.ned_speed_i); + } + else { + /* could not get this representation, set errno */ + //struct EcefCoor_i _ecef_zero = {0}; + //return _ecef_zero; + } + /* set bit to indicate this representation is computed */ + SetBit(state.speed_status, SPEED_ECEF_I); +} + +void stateCalcHorizontalSpeedNorm_i(void) { //TODO + if (bit_is_set(state.speed_status, SPEED_HNORM_I)) + return; + + if (bit_is_set(state.speed_status, SPEED_HNORM_F)){ + state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f); + } + else if (bit_is_set(state.speed_status, SPEED_NED_I)) { + //TODO consider INT32_SPEED_FRAC + //INT32_VECT2_NORM(state.h_speed_norm_i, state.ned_speed_i); + } + else if (bit_is_set(state.speed_status, SPEED_NED_F)) { + FLOAT_VECT2_NORM(state.h_speed_norm_f, state.ned_speed_f); + SetBit(state.speed_status, SPEED_HNORM_F); + state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f); + } + else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) { + /* transform ecef speed to ned, set status bit, then compute norm */ + //foo + //TODO consider INT32_SPEED_FRAC + //INT32_VECT2_NORM(state.h_speed_norm_i, state.ned_speed_i); + } + else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) { + ned_of_ecef_vect_f(&state.ned_speed_f, &state.ned_origin_f, &state.ecef_speed_f); + SetBit(state.speed_status, SPEED_NED_F); + FLOAT_VECT2_NORM(state.h_speed_norm_f, state.ned_speed_f); + SetBit(state.speed_status, SPEED_HNORM_F); + state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f); + } + else { + //int32_t _norm_zero = 0; + //return _norm_zero; + } + /* set bit to indicate this representation is computed */ + SetBit(state.speed_status, SPEED_HNORM_I); + + //return state.h_speed_norm_i; +} + +void stateCalcHorizontalSpeedDir_i(void) { //TODO + if (bit_is_set(state.speed_status, SPEED_HDIR_I)) + return; + + if (bit_is_set(state.speed_status, SPEED_HDIR_F)){ + state.h_speed_dir_i = SPEED_BFP_OF_REAL(state.h_speed_dir_f); + } + else if (bit_is_set(state.speed_status, SPEED_NED_I)) { + //TODO + } + /* set bit to indicate this representation is computed */ + SetBit(state.speed_status, SPEED_HDIR_I); +} + +void stateCalcSpeedNed_f(void) { + if (bit_is_set(state.speed_status, SPEED_NED_F)) + return; + + int errno = 0; + if (state.ned_initialized_f) { + if (bit_is_set(state.speed_status, SPEED_NED_I)) { + SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i); + } + else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) { + ned_of_ecef_vect_f(&state.ned_speed_f, &state.ned_origin_f, &state.ecef_speed_f); + } + else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) { + /* transform ecef_i -> ecef_f -> ned_f , set status bits */ + SPEEDS_FLOAT_OF_BFP(state.ecef_speed_f, state.ecef_speed_i); + SetBit(state.speed_status, SPEED_ECEF_F); + ned_of_ecef_vect_f(&state.ned_speed_f, &state.ned_origin_f, &state.ecef_speed_f); + } + else { /* could not get this representation, set errno */ + errno = 1; + } + } else { /* ned coordinate system not initialized, set errno */ + errno = 2; + } + if (errno) { + //struct NedCoor_f _ned_zero = {0.0f}; + //return _ned_zero; + } + /* set bit to indicate this representation is computed */ + SetBit(state.speed_status, SPEED_NED_F); +} + +void stateCalcSpeedEnu_f(void) { + if (bit_is_set(state.speed_status, SPEED_ENU_F)) + return; + + int errno = 0; + if (state.ned_initialized_f) { + if (bit_is_set(state.speed_status, SPEED_NED_F)) { + VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f); + } + if (bit_is_set(state.speed_status, SPEED_ENU_I)) { + ENU_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i); + } + else if (bit_is_set(state.speed_status, SPEED_NED_I)) { + SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i); + SetBit(state.pos_status, SPEED_NED_F); + VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f); + } + else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) { + enu_of_ecef_vect_f(&state.enu_speed_f, &state.ned_origin_f, &state.ecef_speed_f); + } + else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) { + /* transform ecef_I -> ecef_f -> enu_f , set status bits */ + SPEEDS_FLOAT_OF_BFP(state.ecef_speed_f, state.ecef_speed_i); + SetBit(state.speed_status, SPEED_ECEF_F); + enu_of_ecef_vect_f(&state.enu_speed_f, &state.ned_origin_f, &state.ecef_speed_f); + } + else { /* could not get this representation, set errno */ + errno = 1; + } + } else { /* ned coordinate system not initialized, set errno */ + errno = 2; + } + if (errno) { + //struct EnuCoor_f _enu_zero = {0}; + //return _enu_zero; + } + /* set bit to indicate this representation is computed */ + SetBit(state.speed_status, SPEED_ENU_F); +} + +void stateCalcSpeedEcef_f(void) { + if (bit_is_set(state.speed_status, SPEED_ECEF_F)) + return; + + if (bit_is_set(state.speed_status, SPEED_ECEF_I)) { + SPEEDS_FLOAT_OF_BFP(state.ecef_speed_f, state.ned_speed_i); + } + else if (bit_is_set(state.speed_status, SPEED_NED_F)) { + ecef_of_ned_vect_f(&state.ecef_speed_f, &state.ned_origin_f, &state.ned_speed_f); + } + else if (bit_is_set(state.speed_status, SPEED_NED_I)) { + /* transform ned_f -> ned_i -> ecef_i , set status bits */ + SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i); + SetBit(state.speed_status, SPEED_NED_F); + ecef_of_ned_vect_f(&state.ecef_speed_f, &state.ned_origin_f, &state.ned_speed_f); + } + else { + /* could not get this representation, set errno */ + //struct EcefCoor_f _ecef_zero = {0.0f}; + //return _ecef_zero; + } + /* set bit to indicate this representation is computed */ + SetBit(state.speed_status, SPEED_ECEF_F); +} + +void stateCalcHorizontalSpeedNorm_f(void) { //TODO + if (bit_is_set(state.speed_status, SPEED_HNORM_F)) + return; + + if (bit_is_set(state.speed_status, SPEED_HNORM_I)){ + state.h_speed_norm_f = SPEED_FLOAT_OF_BFP(state.h_speed_norm_i); + } else if (bit_is_set(state.speed_status, SPEED_NED_F)) { + FLOAT_VECT2_NORM(state.h_speed_norm_f, state.ned_speed_f); + } + /* set bit to indicate this representation is computed */ + SetBit(state.speed_status, SPEED_HNORM_F); +} + +void stateCalcHorizontalSpeedDir_f(void) { //TODO + if (bit_is_set(state.speed_status, SPEED_HDIR_F)) + return; + + if (bit_is_set(state.speed_status, SPEED_HDIR_I)){ + state.h_speed_dir_f = SPEED_FLOAT_OF_BFP(state.h_speed_dir_i); + } else if (bit_is_set(state.speed_status, SPEED_NED_F)) { + //foo + } + /* set bit to indicate this representation is computed */ + SetBit(state.speed_status, SPEED_HDIR_F); +} +/** @}*/ + + + +/****************************************************************************** + * * + * Transformation functions for the ACCELERATION representations * + * * + *****************************************************************************/ +/** @addtogroup AccelGroup + * @{ */ + +void stateCalcAccelNed_i(void) { + if (bit_is_set(state.accel_status, ACCEL_NED_I)) + return; + + int errno = 0; + if (state.ned_initialized_i) { + if (bit_is_set(state.accel_status, ACCEL_NED_F)) { + ACCELS_BFP_OF_REAL(state.ned_accel_i, state.ned_accel_f); + } + else if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) { + ned_of_ecef_vect_i(&state.ned_accel_i, &state.ned_origin_i, &state.ecef_accel_i); + } + else if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) { + /* transform ecef_f -> ecef_i -> ned_i , set status bits */ + ACCELS_BFP_OF_REAL(state.ecef_accel_i, state.ecef_accel_f); + SetBit(state.accel_status, ACCEL_ECEF_I); + ned_of_ecef_vect_i(&state.ned_accel_i, &state.ned_origin_i, &state.ecef_accel_i); + } + else { /* could not get this representation, set errno */ + errno = 1; + } + } else { /* ned coordinate system not initialized, set errno */ + errno = 2; + } + if (errno) { + //struct NedCoor_i _ned_zero = {0}; + //return _ned_zero; + } + /* set bit to indicate this representation is computed */ + SetBit(state.accel_status, ACCEL_NED_I); +} + +void stateCalcAccelEcef_i(void) { + if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) + return; + + if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) { + ACCELS_BFP_OF_REAL(state.ecef_accel_i, state.ecef_accel_f); + } + else if (bit_is_set(state.accel_status, ACCEL_NED_I)) { + ecef_of_ned_vect_i(&state.ecef_accel_i, &state.ned_origin_i, &state.ned_accel_i); + } + else if (bit_is_set(state.accel_status, ACCEL_NED_F)) { + /* transform ned_f -> ned_i -> ecef_i , set status bits */ + ACCELS_BFP_OF_REAL(state.ned_accel_i, state.ned_accel_f); + SetBit(state.accel_status, ACCEL_NED_I); + ecef_of_ned_vect_i(&state.ecef_accel_i, &state.ned_origin_i, &state.ned_accel_i); + } + else { + /* could not get this representation, set errno */ + //struct EcefCoor_i _ecef_zero = {0}; + //return _ecef_zero; + } + /* set bit to indicate this representation is computed */ + SetBit(state.accel_status, ACCEL_ECEF_I); +} + +void stateCalcAccelNed_f(void) { + if (bit_is_set(state.accel_status, ACCEL_NED_F)) + return; + + int errno = 0; + if (state.ned_initialized_f) { + if (bit_is_set(state.accel_status, ACCEL_NED_I)) { + ACCELS_FLOAT_OF_BFP(state.ned_accel_f, state.ned_accel_i); + } + else if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) { + ned_of_ecef_vect_f(&state.ned_accel_f, &state.ned_origin_f, &state.ecef_accel_f); + } + else if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) { + /* transform ecef_i -> ecef_f -> ned_f , set status bits */ + ACCELS_FLOAT_OF_BFP(state.ecef_accel_f, state.ecef_accel_i); + SetBit(state.accel_status, ACCEL_ECEF_F); + ned_of_ecef_vect_f(&state.ned_accel_f, &state.ned_origin_f, &state.ecef_accel_f); + } + else { /* could not get this representation, set errno */ + errno = 1; + } + } else { /* ned coordinate system not initialized, set errno */ + errno = 2; + } + if (errno) { + //struct NedCoor_f _ned_zero = {0.0f}; + //return _ned_zero; + } + /* set bit to indicate this representation is computed */ + SetBit(state.accel_status, ACCEL_NED_F); +} + +void stateCalcAccelEcef_f(void) { + if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) + return; + + if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) { + ACCELS_FLOAT_OF_BFP(state.ecef_accel_f, state.ned_accel_i); + } + else if (bit_is_set(state.accel_status, ACCEL_NED_F)) { + ecef_of_ned_vect_f(&state.ecef_accel_f, &state.ned_origin_f, &state.ned_accel_f); + } + else if (bit_is_set(state.accel_status, ACCEL_NED_I)) { + /* transform ned_f -> ned_i -> ecef_i , set status bits */ + ACCELS_FLOAT_OF_BFP(state.ned_accel_f, state.ned_accel_i); + SetBit(state.accel_status, ACCEL_NED_F); + ecef_of_ned_vect_f(&state.ecef_accel_f, &state.ned_origin_f, &state.ned_accel_f); + } + else { + /* could not get this representation, set errno */ + //struct EcefCoor_f _ecef_zero = {0.0f}; + //return _ecef_zero; + } + /* set bit to indicate this representation is computed */ + SetBit(state.accel_status, ACCEL_ECEF_F); +} +/** @}*/ + + + +/****************************************************************************** + * * + * Transformation functions for the ATTITUDE representations * + * * + *****************************************************************************/ +/** @addtogroup AttGroup + * @{ */ + +void stateCalcNedToBodyQuat_i(void) { + if (bit_is_set(state.att_status, ATT_QUAT_I)) + return; + + if (bit_is_set(state.att_status, ATT_QUAT_F)) { + QUAT_BFP_OF_REAL(state.ned_to_body_quat_i, state.ned_to_body_quat_f); + } + else if (bit_is_set(state.att_status, ATT_RMAT_I)) { + INT32_QUAT_OF_RMAT(state.ned_to_body_quat_i, state.ned_to_body_rmat_i); + } + else if (bit_is_set(state.att_status, ATT_EULER_I)) { + INT32_QUAT_OF_EULERS(state.ned_to_body_quat_i, state.ned_to_body_eulers_i); + } + else if (bit_is_set(state.att_status, ATT_RMAT_F)) { + RMAT_BFP_OF_REAL(state.ned_to_body_rmat_i, state.ned_to_body_rmat_f); + SetBit(state.att_status, ATT_RMAT_I); + INT32_QUAT_OF_RMAT(state.ned_to_body_quat_i, state.ned_to_body_rmat_i); + } + else if (bit_is_set(state.att_status, ATT_EULER_F)) { + EULERS_BFP_OF_REAL(state.ned_to_body_eulers_i, state.ned_to_body_eulers_f); + SetBit(state.att_status, ATT_EULER_I); + INT32_QUAT_OF_EULERS(state.ned_to_body_quat_i, state.ned_to_body_eulers_i); + } + /* set bit to indicate this representation is computed */ + SetBit(state.att_status, ATT_QUAT_I); +} + +void stateCalcNedToBodyRMat_i(void) { + if (bit_is_set(state.att_status, ATT_RMAT_I)) + return; + + if (bit_is_set(state.att_status, ATT_RMAT_F)) { + RMAT_BFP_OF_REAL(state.ned_to_body_rmat_i, state.ned_to_body_rmat_f); + } + else if (bit_is_set(state.att_status, ATT_QUAT_I)) { + INT32_RMAT_OF_QUAT(state.ned_to_body_rmat_i, state.ned_to_body_quat_i); + } + else if (bit_is_set(state.att_status, ATT_EULER_I)) { + INT32_RMAT_OF_EULERS(state.ned_to_body_rmat_i, state.ned_to_body_eulers_i); + } + else if (bit_is_set(state.att_status, ATT_QUAT_F)) { + QUAT_BFP_OF_REAL(state.ned_to_body_quat_i, state.ned_to_body_quat_f); + SetBit(state.att_status, ATT_QUAT_I); + INT32_RMAT_OF_QUAT(state.ned_to_body_rmat_i, state.ned_to_body_quat_i); + } + else if (bit_is_set(state.att_status, ATT_EULER_F)) { + EULERS_BFP_OF_REAL(state.ned_to_body_eulers_i, state.ned_to_body_eulers_f); + SetBit(state.att_status, ATT_EULER_I); + INT32_RMAT_OF_EULERS(state.ned_to_body_rmat_i, state.ned_to_body_eulers_i); + } + /* set bit to indicate this representation is computed */ + SetBit(state.att_status, ATT_RMAT_I); +} + +void stateCalcNedToBodyEulers_i(void) { + if (bit_is_set(state.att_status, ATT_EULER_I)) + return; + + if (bit_is_set(state.att_status, ATT_EULER_F)) { + EULERS_BFP_OF_REAL(state.ned_to_body_eulers_i, state.ned_to_body_eulers_f); + } + else if (bit_is_set(state.att_status, ATT_RMAT_I)) { + INT32_EULERS_OF_RMAT(state.ned_to_body_eulers_i, state.ned_to_body_rmat_i); + } + else if (bit_is_set(state.att_status, ATT_QUAT_I)) { + INT32_EULERS_OF_QUAT(state.ned_to_body_eulers_i, state.ned_to_body_quat_i); + } + else if (bit_is_set(state.att_status, ATT_RMAT_F)) { + RMAT_BFP_OF_REAL(state.ned_to_body_rmat_i, state.ned_to_body_rmat_f); + SetBit(state.att_status, ATT_RMAT_I); + INT32_EULERS_OF_RMAT(state.ned_to_body_eulers_i, state.ned_to_body_rmat_i); + } + else if (bit_is_set(state.att_status, ATT_QUAT_F)) { + QUAT_BFP_OF_REAL(state.ned_to_body_quat_i, state.ned_to_body_quat_f); + SetBit(state.att_status, ATT_QUAT_I); + INT32_EULERS_OF_QUAT(state.ned_to_body_eulers_i, state.ned_to_body_quat_i); + } + /* set bit to indicate this representation is computed */ + SetBit(state.att_status, ATT_EULER_I); +} + +void stateCalcNedToBodyQuat_f(void) { + if (bit_is_set(state.att_status, ATT_QUAT_F)) + return; + + if (bit_is_set(state.att_status, ATT_QUAT_I)) { + QUAT_FLOAT_OF_BFP(state.ned_to_body_quat_f, state.ned_to_body_quat_i); + } + else if (bit_is_set(state.att_status, ATT_RMAT_F)) { + FLOAT_QUAT_OF_RMAT(state.ned_to_body_quat_f, state.ned_to_body_rmat_f); + } + else if (bit_is_set(state.att_status, ATT_EULER_F)) { + FLOAT_QUAT_OF_EULERS(state.ned_to_body_quat_f, state.ned_to_body_eulers_f); + } + else if (bit_is_set(state.att_status, ATT_RMAT_I)) { + RMAT_FLOAT_OF_BFP(state.ned_to_body_rmat_f, state.ned_to_body_rmat_i); + SetBit(state.att_status, ATT_RMAT_F); + FLOAT_QUAT_OF_RMAT(state.ned_to_body_quat_f, state.ned_to_body_rmat_f); + } + else if (bit_is_set(state.att_status, ATT_EULER_I)) { + EULERS_FLOAT_OF_BFP(state.ned_to_body_eulers_f, state.ned_to_body_eulers_i); + SetBit(state.att_status, ATT_EULER_F); + FLOAT_QUAT_OF_EULERS(state.ned_to_body_quat_f, state.ned_to_body_eulers_f); + } + /* set bit to indicate this representation is computed */ + SetBit(state.att_status, ATT_QUAT_F); +} + +void stateCalcNedToBodyRMat_f(void) { + if (bit_is_set(state.att_status, ATT_RMAT_F)) + return; + + if (bit_is_set(state.att_status, ATT_RMAT_I)) { + RMAT_FLOAT_OF_BFP(state.ned_to_body_rmat_f, state.ned_to_body_rmat_i); + } + else if (bit_is_set(state.att_status, ATT_QUAT_F)) { + FLOAT_RMAT_OF_QUAT(state.ned_to_body_rmat_i, state.ned_to_body_quat_i); + } + else if (bit_is_set(state.att_status, ATT_EULER_F)) { + FLOAT_RMAT_OF_EULERS(state.ned_to_body_rmat_i, state.ned_to_body_eulers_i); + } + else if (bit_is_set(state.att_status, ATT_QUAT_I)) { + QUAT_FLOAT_OF_BFP(state.ned_to_body_quat_f, state.ned_to_body_quat_i); + SetBit(state.att_status, ATT_QUAT_F); + FLOAT_RMAT_OF_QUAT(state.ned_to_body_rmat_f, state.ned_to_body_quat_f); + } + else if (bit_is_set(state.att_status, ATT_EULER_I)) { + EULERS_FLOAT_OF_BFP(state.ned_to_body_eulers_f, state.ned_to_body_eulers_i); + SetBit(state.att_status, ATT_EULER_F); + FLOAT_RMAT_OF_EULERS(state.ned_to_body_rmat_f, state.ned_to_body_eulers_f); + } + /* set bit to indicate this representation is computed */ + SetBit(state.att_status, ATT_RMAT_F); +} + +void stateCalcNedToBodyEulers_f(void) { + if (bit_is_set(state.att_status, ATT_EULER_F)) + return; + + if (bit_is_set(state.att_status, ATT_EULER_I)) { + EULERS_FLOAT_OF_BFP(state.ned_to_body_eulers_f, state.ned_to_body_eulers_i); + } + else if (bit_is_set(state.att_status, ATT_RMAT_F)) { + FLOAT_EULERS_OF_RMAT(state.ned_to_body_eulers_f, state.ned_to_body_rmat_f); + } + else if (bit_is_set(state.att_status, ATT_QUAT_F)) { + FLOAT_EULERS_OF_QUAT(state.ned_to_body_eulers_f, state.ned_to_body_quat_f); + } + else if (bit_is_set(state.att_status, ATT_RMAT_I)) { + RMAT_FLOAT_OF_BFP(state.ned_to_body_rmat_f, state.ned_to_body_rmat_i); + SetBit(state.att_status, ATT_RMAT_F); + FLOAT_EULERS_OF_RMAT(state.ned_to_body_eulers_i, state.ned_to_body_rmat_i); + } + else if (bit_is_set(state.att_status, ATT_QUAT_I)) { + QUAT_FLOAT_OF_BFP(state.ned_to_body_quat_f, state.ned_to_body_quat_i); + SetBit(state.att_status, ATT_QUAT_F); + FLOAT_EULERS_OF_QUAT(state.ned_to_body_eulers_f, state.ned_to_body_quat_f); + } + /* set bit to indicate this representation is computed */ + SetBit(state.att_status, ATT_EULER_F); +} +/** @}*/ + + +/****************************************************************************** + * * + * Transformation functions for the ANGULAR RATE representations * + * * + *****************************************************************************/ +/** @addtogroup RateGroup + * @{ */ + +void stateCalcBodyRates_i(void) { + if (bit_is_set(state.rate_status, RATE_I)) + return; + + if (bit_is_set(state.rate_status, RATE_F)) { + RATES_BFP_OF_REAL(state.body_rates_i, state.body_rates_f); + } + /* set bit to indicate this representation is computed */ + SetBit(state.rate_status, RATE_I); +} + +void stateCalcBodyRates_f(void) { + if (bit_is_set(state.rate_status, RATE_F)) + return; + + if (bit_is_set(state.rate_status, RATE_I)) { + RATES_FLOAT_OF_BFP(state.body_rates_f, state.body_rates_i); + } + /* set bit to indicate this representation is computed */ + SetBit(state.rate_status, RATE_F); +} + +/** @}*/ + + +/****************************************************************************** + * * + * Transformation functions for the WIND- AND AIRSPEED representations * + * * + *****************************************************************************/ +/** @addtogroup WindAirGroup + * @{ */ + +void stateCalcHorizontalWindspeed_i(void) { + if (bit_is_set(state.wind_air_status, WINDSPEED_I)) + return; + + if (bit_is_set(state.wind_air_status, WINDSPEED_F)) { + state.h_windspeed_i.x = SPEED_BFP_OF_REAL(state.h_windspeed_f.x); + state.h_windspeed_i.y = SPEED_BFP_OF_REAL(state.h_windspeed_f.y); + } + /* set bit to indicate this representation is computed */ + SetBit(state.rate_status, WINDSPEED_I); +} + +void stateCalcAirspeed_i(void) { + if (bit_is_set(state.wind_air_status, AIRSPEED_I)) + return; + + if (bit_is_set(state.wind_air_status, AIRSPEED_F)) { + state.airspeed_i = SPEED_BFP_OF_REAL(state.airspeed_f); + } + /* set bit to indicate this representation is computed */ + SetBit(state.wind_air_status, AIRSPEED_I); +} + +void stateCalcHorizontalWindspeed_f(void) { + if (bit_is_set(state.wind_air_status, WINDSPEED_F)) + return; + + if (bit_is_set(state.wind_air_status, WINDSPEED_I)) { + state.h_windspeed_f.x = SPEED_FLOAT_OF_BFP(state.h_windspeed_i.x); + state.h_windspeed_f.x = SPEED_FLOAT_OF_BFP(state.h_windspeed_i.y); + } + /* set bit to indicate this representation is computed */ + SetBit(state.rate_status, WINDSPEED_F); +} + +void stateCalcAirspeed_f(void) { + if (bit_is_set(state.wind_air_status, AIRSPEED_F)) + return; + + if (bit_is_set(state.wind_air_status, AIRSPEED_I)) { + state.airspeed_f = SPEED_FLOAT_OF_BFP(state.airspeed_i); + } + /* set bit to indicate this representation is computed */ + SetBit(state.wind_air_status, AIRSPEED_F); +} +/** @}*/ + +/** @}*/ diff --git a/sw/airborne/state.h b/sw/airborne/state.h new file mode 100644 index 0000000000..69ba0ce498 --- /dev/null +++ b/sw/airborne/state.h @@ -0,0 +1,1102 @@ +/* + * Copyright (C) 2011-2012 Felix Ruess + * + * This file is part of paparazzi. + * + * paparazzi 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 2, or (at your option) + * any later version. + * + * paparazzi 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 paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file state.h + * + * API to get/set the generic vehicle states. + * + * This general state interface holds all the most important vehicle states like + * position, velocity, attitude, etc. It handles coordinate system and + * fixed-/floating-point conversion on the fly when needed. + * + * You can set e.g. the position in any coordinate system you wish: + * stateSetPositionNed_i() to set the position in fixed-point NED coordinates. + * If you need to read the position somewhere else in a different representation, + * call: stateGetPositionLla_f() and only then the LLA float position representation + * is calculated on the fly and returned. It's also only calculated once, + * until a new position is set which invalidates all the other representations again. + * + * @author Felix Ruess + */ + +#ifndef STATE_H +#define STATE_H + +#include "math/pprz_algebra_int.h" +#include "math/pprz_algebra_float.h" +#include "math/pprz_geodetic_int.h" +#include "math/pprz_geodetic_float.h" + +#include "std.h" +#include + +/** + * @defgroup StateGroup State interface + * @{ + */ + +/** + * @defgroup PosGroup Position representations + * @{ + */ +#define POS_ECEF_I 0 +#define POS_NED_I 1 +#define POS_ENU_I 2 +#define POS_LLA_I 3 +#define POS_UTM_I 4 +#define POS_ECEF_F 5 +#define POS_NED_F 6 +#define POS_ENU_F 7 +#define POS_LLA_F 8 +#define POS_UTM_F 9 +/**@}*/ + +/** + * @defgroup SpeedGroup Speed representations + * @{ + */ +#define SPEED_ECEF_I 0 +#define SPEED_NED_I 1 +#define SPEED_ENU_I 2 +#define SPEED_HNORM_I 3 +#define SPEED_HDIR_I 4 +#define SPEED_ECEF_F 5 +#define SPEED_NED_F 6 +#define SPEED_ENU_F 7 +#define SPEED_HNORM_F 8 +#define SPEED_HDIR_F 9 +/**@}*/ + +/** + * @defgroup AccelGroup Acceleration representations + * @{ + */ +#define ACCEL_ECEF_I 0 +#define ACCEL_NED_I 1 +#define ACCEL_ECEF_F 2 +#define ACCEL_NED_F 3 +/**@}*/ + +/** + * @defgroup AttGroup Attitude representations + * @{ + */ +#define ATT_QUAT_I 0 +#define ATT_EULER_I 1 +#define ATT_RMAT_I 2 +#define ATT_QUAT_F 3 +#define ATT_EULER_F 4 +#define ATT_RMAT_F 5 +/**@}*/ + +/** + * @defgroup RateGroup Angular rate representations + * @{ + */ +#define RATE_I 0 +#define RATE_F 1 +/**@}*/ + +/** + * @defgroup WindAirGroup Wind- and airspeed representations + * @{ + */ +#define WINDSPEED_I 0 +#define AIRSPEED_I 1 +#define WINDSPEED_F 2 +#define AIRSPEED_F 3 +/**@}*/ + + +/** + * Structure holding vehicle state data. + */ +struct State { + + /** @addtogroup PosGroup + * @{ */ + + /** + * Holds the status bits for all position representations. + * When the corresponding bit is set the representation + * is already computed. + */ + uint16_t pos_status; + + /** + * Position in EarthCenteredEarthFixed coordinates. + * Units: centimeters + */ + struct EcefCoor_i ecef_pos_i; + + /** + * Position in Latitude, Longitude and Altitude. + * Units lat,lon: radians*1e7 + * Units alt: centimeters above MSL + */ + struct LlaCoor_i lla_pos_i; + + /** + * Definition of the local (flat earth) coordinate system. + * Defines the origin of the local NorthEastDown coordinate system + * in ECEF (EarthCenteredEarthFixed) and LLA (LatitudeLongitudeAlt) + * coordinates and the roation matrix from ECEF to local frame. + * (int version) + */ + struct LtpDef_i ned_origin_i; + + /** + * true if local int coordinate frame is initialsed + */ + bool_t ned_initialized_i; + + /** + * Position in North East Down coordinates. + * with respect to ned_origin_i (flat earth) + * Units: m in BFP with INT32_POS_FRAC + */ + struct NedCoor_i ned_pos_i; + + /** + * Position in East North Up coordinates. + * with respect to ned_origin_i (flat earth) + * Units: m in BFP with INT32_POS_FRAC + */ + struct EnuCoor_i enu_pos_i; + + /** + * Position in UTM coordinates. + * Units x,y: meters. + * Units z: meters above MSL + */ + struct UtmCoor_f utm_pos_f; + + /** + * Altitude above ground level. + * Unit: meters + */ + float alt_agl_f; + + /** + * Position in Latitude, Longitude and Altitude. + * Units lat,lon: radians + * Units alt: meters above MSL + */ + struct LlaCoor_f lla_pos_f; + + /** + * Position in EarthCenteredEarthFixed coordinates. + * Units: meters + */ + struct EcefCoor_f ecef_pos_f; + + /** + * Definition of the local (flat earth) coordinate system. + * Defines the origin of the local NorthEastDown coordinate system + * in ECEF (EarthCenteredEarthFixed) and LLA (LatitudeLongitudeAlt) + * coordinates and the roation matrix from ECEF to local frame. + * (float version) + */ + struct LtpDef_f ned_origin_f; + + /// True if local float coordinate frame is initialsed + bool_t ned_initialized_f; + + /** + * Position in North East Down coordinates. + * with respect to ned_origin_i (flat earth) + * Units: meters + */ + struct NedCoor_f ned_pos_f; + + /** + * Position in East North Up coordinates. + * with respect to ned_origin_i (flat earth) + * Units: meters + */ + struct EnuCoor_f enu_pos_f; + /** @}*/ + + + /** @addtogroup SpeedGroup + * @{ */ + /** + * Holds the status bits for all ground speed representations. + * When the corresponding bit is one the representation + * is already computed. + */ + uint16_t speed_status; + + /** + * Velocity in EarthCenteredEarthFixed coordinates. + * Units: m/s in BFP with #INT32_SPEED_FRAC + */ + struct EcefCoor_i ecef_speed_i; + + /** + * Velocity in North East Down coordinates. + * Units: m/s in BFP with #INT32_SPEED_FRAC + */ + struct NedCoor_i ned_speed_i; + + /** + * Velocity in East North Up coordinates. + * Units: m/s in BFP with #INT32_SPEED_FRAC + */ + struct EnuCoor_i enu_speed_i; + + /** + * Norm of horizontal ground speed. + * Unit: m/s in BFP with #INT32_SPEED_FRAC + */ + int32_t h_speed_norm_i; + + /** + * Direction of horizontal ground speed. + * Unit: rad in BFP with #INT32_ANGLE_FRAC + * (clockwise, zero=north) + */ + int32_t h_speed_dir_i; + + /** + * Velocity in EarthCenteredEarthFixed coordinates. + * Units: m/s + */ + struct EcefCoor_f ecef_speed_f; + + /** + * @brief speed in North East Down coordinates + * @details Units: m/s */ + struct NedCoor_f ned_speed_f; + + /** + * Velocity in East North Up coordinates. + * Units: m/s + */ + struct EnuCoor_f enu_speed_f; + + /** + * Norm of horizontal ground speed. + * Unit: m/s + */ + float h_speed_norm_f; + + /** + * Direction of horizontal ground speed. + * Unit: rad (clockwise, zero=north) + */ + float h_speed_dir_f; + /** @}*/ + + + /** @addtogroup AccelGroup + * @{ */ + /** + * Holds the status bits for all acceleration representations. + * When the corresponding bit is one the representation + * is already computed. + */ + uint8_t accel_status; + + /** + * Acceleration in North East Down coordinates. + * Units: m/s^2 in BFP with #INT32_ACCEL_FRAC + */ + struct NedCoor_i ned_accel_i; + + /** + * Acceleration in EarthCenteredEarthFixed coordinates. + * Units: m/s^2 in BFP with INT32_ACCEL_FRAC + */ + struct EcefCoor_i ecef_accel_i; + + /** + * Acceleration in North East Down coordinates. + * Units: m/s^2 + */ + struct NedCoor_f ned_accel_f; + + /** + * Acceleration in EarthCenteredEarthFixed coordinates. + * Units: m/s^2 + */ + struct EcefCoor_f ecef_accel_f; + /** @}*/ + + + /** @addtogroup AttGroup + * @{ */ + /** + * Holds the status bits for all attitude representations. + * When the corresponding bit is set, the representation + * is already computed. + */ + uint8_t att_status; + + /** + * Attitude quaternion. + * Specifies rotation from local NED frame to body frame. + * Units: #INT32_QUAT_FRAC + * + * @code + * struct Int32Vect3 body_accel; + * INT32_QUAT_VMULT(body_accel, *stateGetNedToBodyQuat_i(), *stateGetAccelNed_i()); + * @endcode + */ + struct Int32Quat ned_to_body_quat_i; + + /** + * Attitude in zyx euler angles. + * Specifies rotation from local NED frame to body frame. + * Units: rad in BFP with #INT32_ANGLE_FRAC + */ + struct Int32Eulers ned_to_body_eulers_i; + + /** + * Attitude rotation matrix. + * Specifies rotation from local NED frame to body frame. + * Units: rad in BFP with #INT32_TRIG_FRAC + */ + struct Int32RMat ned_to_body_rmat_i; + + /** + * Attitude as quaternion. + * Specifies rotation from local NED frame to body frame. + * Units: unit length quaternion + * + * @code + * struct FloatVect3 body_accel; + * FLOAT_QUAT_VMULT(body_accel, *stateGetNedToBodyQuat_f(), *stateGetAccelNed_f()); + * @endcode + */ + struct FloatQuat ned_to_body_quat_f; + + /** + * Attitude in zyx euler angles. + * Specifies rotation from local NED frame to body frame. + * Units: rad + */ + struct FloatEulers ned_to_body_eulers_f; + + /** + * Attitude rotation matrix. + * Specifies rotation from local NED frame to body frame. + * Units: rad + */ + struct FloatRMat ned_to_body_rmat_f; + /** @}*/ + + + /** @addtogroup RateGroup + * @{ */ + /** + * Holds the status bits for all angular rate representations. + * When the corresponding bit is one the representation + * is already computed. + */ + uint8_t rate_status; + + /** + * Angular rates in body frame. + * Units: rad/s^2 in BFP with #INT32_RATE_FRAC + */ + struct Int32Rates body_rates_i; + + /** + * Angular rates in body frame. + * Units: rad/s^2 + */ + struct FloatRates body_rates_f; + /** @}*/ + + + /** @addtogroup WindAirGroup + * @{ */ + /** + * Holds the status bits for all wind- and airspeed representations. + * When the corresponding bit is one the representation + * is already computed. + */ + uint8_t wind_air_status; + + /** + * Horizontal windspeed in north/east. + * Units: m/s in BFP with #INT32_SPEED_FRAC + */ + struct Int32Vect2 h_windspeed_i; + + /** + * Norm of horizontal ground speed. + * @details Unit: m/s in BFP with #INT32_SPEED_FRAC + */ + int32_t airspeed_i; + + /** + * Horizontal windspeed. + * Units: m/s with x=north, y=east + */ + struct FloatVect2 h_windspeed_f; + + /** + * Norm of horizontal ground speed. + * Unit: m/s + */ + float airspeed_f; + /** @}*/ + +}; + +extern struct State state; + +extern void stateInit(void); + +/// Set the local (flat earth) coordinate frame origin (int). +static inline void stateSetLocalOrigin_i(struct LtpDef_i* ltp_def) { + LTP_DEF_COPY(state.ned_origin_i, *ltp_def); + state.ned_initialized_i = TRUE; + ECEF_FLOAT_OF_BFP(state.ned_origin_f.ecef, state.ned_origin_i.ecef); + LLA_FLOAT_OF_BFP(state.ned_origin_f.lla, state.ned_origin_i.lla); + RMAT_FLOAT_OF_BFP(state.ned_origin_f.ltp_of_ecef, state.ned_origin_i.ltp_of_ecef); + state.ned_origin_f.hmsl = M_OF_MM(state.ned_origin_f.hmsl); + state.ned_initialized_f = TRUE; + + /* clear bits for all local frame representations */ + ClearBit(state.pos_status, POS_NED_I); + ClearBit(state.pos_status, POS_ENU_I); + ClearBit(state.pos_status, POS_NED_F); + ClearBit(state.pos_status, POS_ENU_F); + ClearBit(state.speed_status, SPEED_NED_I); + ClearBit(state.speed_status, SPEED_ENU_I); + ClearBit(state.speed_status, SPEED_NED_F); + ClearBit(state.speed_status, SPEED_ENU_F); + ClearBit(state.accel_status, ACCEL_NED_I); + ClearBit(state.accel_status, ACCEL_NED_F); +} + + +/******************************************************************************* + * * + * Set and Get functions for the POSITION representations * + * * + ******************************************************************************/ +/** @addtogroup PosGroup + * @{ */ + +/************* declaration of transformation functions ************/ +extern void stateCalcPositionEcef_i(void); +extern void stateCalcPositionNed_i(void); +extern void stateCalcPositionEnu_i(void); +extern void stateCalcPositionLla_i(void); +extern void stateCalcPositionUtm_f(void); +extern void stateCalcPositionEcef_f(void); +extern void stateCalcPositionNed_f(void); +extern void stateCalcPositionEnu_f(void); +extern void stateCalcPositionLla_f(void); + +/************************ Set functions ****************************/ + +/// Set position from ECEF coordinates (int). +static inline void stateSetPositionEcef_i(struct EcefCoor_i* ecef_pos) { + INT32_VECT3_COPY(state.ecef_pos_i, *ecef_pos); + /* clear bits for all position representations and only set the new one */ + state.pos_status = (1 << POS_ECEF_I); +} + +/// Set position from local NED coordinates (int). +static inline void stateSetPositionNed_i(struct NedCoor_i* ned_pos) { + INT32_VECT3_COPY(state.ned_pos_i, *ned_pos); + /* clear bits for all position representations and only set the new one */ + state.pos_status = (1 << POS_NED_I); +} + +/// Set position from local ENU coordinates (int). +static inline void stateSetPositionEnu_i(struct EnuCoor_i* enu_pos) { + INT32_VECT3_COPY(state.enu_pos_i, *enu_pos); + /* clear bits for all position representations and only set the new one */ + state.pos_status = (1 << POS_ENU_I); +} + +/// Set position from LLA coordinates (int). +static inline void stateSetPositionLla_i(struct LlaCoor_i* lla_pos) { + LLA_COPY(state.lla_pos_i, *lla_pos); + /* clear bits for all position representations and only set the new one */ + state.pos_status = (1 << POS_LLA_I); +} + +/// Set position from UTM coordinates (float). +static inline void stateSetPositionUtm_f(struct UtmCoor_f* utm_pos) { + memcpy(&state.utm_pos_f, utm_pos, sizeof(struct UtmCoor_f)); + /* clear bits for all position representations and only set the new one */ + state.pos_status = (1 << POS_UTM_F); +} + +/// Set position from ECEF coordinates (float). +static inline void stateSetPositionEcef_f(struct EcefCoor_f* ecef_pos) { + VECT3_COPY(state.ecef_pos_f, *ecef_pos); + /* clear bits for all position representations and only set the new one */ + state.pos_status = (1 << POS_ECEF_F); +} + +/// Set position from local NED coordinates (float). +static inline void stateSetPositionNed_f(struct NedCoor_f* ned_pos) { + VECT3_COPY(state.ned_pos_f, *ned_pos); + /* clear bits for all position representations and only set the new one */ + state.pos_status = (1 << POS_NED_F); +} + +/// Set position from local ENU coordinates (float). +static inline void stateSetPositionEnu_f(struct EnuCoor_f* enu_pos) { + VECT3_COPY(state.enu_pos_f, *enu_pos); + /* clear bits for all position representations and only set the new one */ + state.pos_status = (1 << POS_ENU_F); +} + +/// Set position from LLA coordinates (float). +static inline void stateSetPositionLla_f(struct LlaCoor_f* lla_pos) { + LLA_COPY(state.lla_pos_f, *lla_pos); + /* clear bits for all position representations and only set the new one */ + state.pos_status = (1 << POS_LLA_F); +} + +/************************ Get functions ****************************/ + +/// Get position in ECEF coordinates (int). +static inline struct EcefCoor_i* stateGetPositionEcef_i(void) { + if (!bit_is_set(state.pos_status, POS_ECEF_I)) + stateCalcPositionEcef_i(); + return &state.ecef_pos_i; +} + +/// Get position in local NED coordinates (int). +static inline struct NedCoor_i* stateGetPositionNed_i(void) { + if (!bit_is_set(state.pos_status, POS_NED_I)) + stateCalcPositionNed_i(); + return &state.ned_pos_i; +} + +/// Get position in local ENU coordinates (int). +static inline struct EnuCoor_i* stateGetPositionEnu_i(void) { + if (!bit_is_set(state.pos_status, POS_ENU_I)) + stateCalcPositionEnu_i(); + return &state.enu_pos_i; +} + +/// Get position in LLA coordinates (int). +static inline struct LlaCoor_i* stateGetPositionLla_i(void) { + if (!bit_is_set(state.pos_status, POS_LLA_I)) + stateCalcPositionLla_i(); + return &state.lla_pos_i; +} + +/// Get position in UTM coordinates (float). +static inline struct UtmCoor_f* stateGetPositionUtm_f(void) { + if (!bit_is_set(state.pos_status, POS_UTM_F)) + stateCalcPositionUtm_f(); + return &state.utm_pos_f; +} + +/// Get position in ECEF coordinates (float). +static inline struct EcefCoor_f* stateGetPositionEcef_f(void) { + if (!bit_is_set(state.pos_status, POS_ECEF_F)) + stateCalcPositionEcef_f(); + return &state.ecef_pos_f; +} + +/// Get position in local NED coordinates (float). +static inline struct NedCoor_f* stateGetPositionNed_f(void) { + if (!bit_is_set(state.pos_status, POS_NED_F)) + stateCalcPositionNed_f(); + return &state.ned_pos_f; +} + +/// Get position in local ENU coordinates (float). +static inline struct EnuCoor_f* stateGetPositionEnu_f(void) { + if (!bit_is_set(state.pos_status, POS_ENU_F)) + stateCalcPositionEnu_f(); + return &state.enu_pos_f; +} + +/// Get position in LLA coordinates (float). +static inline struct LlaCoor_f* stateGetPositionLla_f(void) { + if (!bit_is_set(state.pos_status, POS_LLA_F)) + stateCalcPositionLla_f(); + return &state.lla_pos_f; +} + +/** @}*/ + + + +/****************************************************************************** + * * + * Set and Get functions for the SPEED representations * + * * + *****************************************************************************/ +/** @addtogroup SpeedGroup + * @{ */ + +/************* declaration of transformation functions ************/ +extern void stateCalcSpeedNed_i(void); +extern void stateCalcSpeedEnu_i(void); +extern void stateCalcSpeedEcef_i(void); +extern void stateCalcHorizontalSpeedNorm_i(void); +extern void stateCalcHorizontalSpeedDir_i(void); +extern void stateCalcSpeedNed_f(void); +extern void stateCalcSpeedEnu_f(void); +extern void stateCalcSpeedEcef_f(void); +extern void stateCalcHorizontalSpeedNorm_f(void); +extern void stateCalcHorizontalSpeedDir_f(void); + +/************************ Set functions ****************************/ + +/// Set ground speed in local NED coordinates (int). +static inline void stateSetSpeedNed_i(struct NedCoor_i* ned_speed) { + INT32_VECT3_COPY(state.ned_speed_i, *ned_speed); + /* clear bits for all speed representations and only set the new one */ + state.speed_status = (1 << SPEED_NED_I); +} + +/// Set ground speed in local ENU coordinates (int). +static inline void stateSetSpeedEnu_i(struct EnuCoor_i* enu_speed) { + INT32_VECT3_COPY(state.enu_speed_i, *enu_speed); + /* clear bits for all speed representations and only set the new one */ + state.speed_status = (1 << SPEED_ENU_I); +} + +/// Set ground speed in ECEF coordinates (int). +static inline void stateSetSpeedEcef_i(struct EcefCoor_i* ecef_speed) { + INT32_VECT3_COPY(state.ecef_speed_i, *ecef_speed); + /* clear bits for all speed representations and only set the new one */ + state.speed_status = (1 << SPEED_ECEF_I); +} + +/// Set ground speed in local NED coordinates (float). +static inline void stateSetSpeedNed_f(struct NedCoor_f* ned_speed) { + VECT3_COPY(state.ned_speed_f, *ned_speed); + /* clear bits for all speed representations and only set the new one */ + state.speed_status = (1 << SPEED_NED_F); +} + +/// Set ground speed in local ENU coordinates (float). +static inline void stateSetSpeedEnu_f(struct EnuCoor_f* enu_speed) { + VECT3_COPY(state.enu_speed_f, *enu_speed); + /* clear bits for all speed representations and only set the new one */ + state.speed_status = (1 << SPEED_ENU_F); +} + +/// Set ground speed in ECEF coordinates (float). +static inline void stateSetSpeedEcef_f(struct EcefCoor_f* ecef_speed) { + VECT3_COPY(state.ecef_speed_f, *ecef_speed); + /* clear bits for all speed representations and only set the new one */ + state.speed_status = (1 << SPEED_ECEF_F); +} + +/************************ Get functions ****************************/ + +/// Get ground speed in local NED coordinates (int). +static inline struct NedCoor_i* stateGetSpeedNed_i(void) { + if (!bit_is_set(state.speed_status, SPEED_NED_I)) + stateCalcSpeedNed_i(); + return &state.ned_speed_i; +} + +/// Get ground speed in local ENU coordinates (int). +static inline struct EnuCoor_i* stateGetSpeedEnu_i(void) { + if (!bit_is_set(state.speed_status, SPEED_ENU_I)) + stateCalcSpeedEnu_i(); + return &state.enu_speed_i; +} + +/// Get ground speed in ECEF coordinates (int). +static inline struct EcefCoor_i* stateGetSpeedEcef_i(void) { + if (!bit_is_set(state.speed_status, SPEED_ECEF_I)) + stateCalcSpeedEcef_i(); + return &state.ecef_speed_i; +} + +/// Get norm of horizontal ground speed (int). +static inline int32_t* stateGetHorizontalSpeedNorm_i(void) { + if (!bit_is_set(state.speed_status, SPEED_HNORM_I)) + stateCalcHorizontalSpeedNorm_i(); + return &state.h_speed_norm_i; +} + +/// Get dir of horizontal ground speed (int). +static inline int32_t* stateGetHorizontalSpeedDir_i(void) { + if (!bit_is_set(state.speed_status, SPEED_HDIR_I)) + stateCalcHorizontalSpeedDir_i(); + return &state.h_speed_dir_i; +} + +/// Get ground speed in local NED coordinates (float). +static inline struct NedCoor_f* stateGetSpeedNed_f(void) { + if (!bit_is_set(state.speed_status, SPEED_NED_F)) + stateCalcSpeedNed_f(); + return &state.ned_speed_f; +} + +/// Get ground speed in local ENU coordinates (float). +static inline struct EnuCoor_f* stateGetSpeedEnu_f(void) { + if (!bit_is_set(state.speed_status, SPEED_ENU_F)) + stateCalcSpeedEnu_f(); + return &state.enu_speed_f; +} + +/// Get ground speed in ECEF coordinates (float). +static inline struct EcefCoor_f* stateGetSpeedEcef_f(void) { + if (!bit_is_set(state.speed_status, SPEED_ECEF_F)) + stateCalcSpeedEcef_f(); + return &state.ecef_speed_f; +} + +/// Get norm of horizontal ground speed (float). +static inline float* stateGetHorizontalSpeedNorm_f(void) { + if (!bit_is_set(state.speed_status, SPEED_HNORM_F)) + stateCalcHorizontalSpeedNorm_f(); + return &state.h_speed_norm_f; +} + +/// Get dir of horizontal ground speed (float). +static inline float* stateGetHorizontalSpeedDir_f(void) { + if (!bit_is_set(state.speed_status, SPEED_HDIR_F)) + stateCalcHorizontalSpeedDir_f(); + return &state.h_speed_dir_f; +} +/** @}*/ + + + +/****************************************************************************** + * * + * Set and Get functions for the ACCELERATION representations * + * * + *****************************************************************************/ +/** @addtogroup AccelGroup + * @{ */ + +/************* declaration of transformation functions ************/ +extern void stateCalcAccelNed_i(void); +extern void stateCalcAccelEcef_i(void); +extern void stateCalcAccelNed_f(void); +extern void stateCalcAccelEcef_f(void); + +/************************ Set functions ****************************/ + +/// Set acceleration in NED coordinates (int). +static inline void stateSetAccelNed_i(struct NedCoor_i* ned_accel) { + INT32_VECT3_COPY(state.ned_accel_i, *ned_accel); + /* clear bits for all accel representations and only set the new one */ + state.accel_status = (1 << ACCEL_NED_I); +} + +/// Set acceleration in ECEF coordinates (int). +static inline void stateSetAccelEcef_i(struct EcefCoor_i* ecef_accel) { + INT32_VECT3_COPY(state.ecef_accel_i, *ecef_accel); + /* clear bits for all accel representations and only set the new one */ + state.accel_status = (1 << ACCEL_ECEF_I); +} + +/// Set acceleration in NED coordinates (float). +static inline void stateSetAccelNed_f(struct NedCoor_f* ned_accel) { + VECT3_COPY(state.ned_accel_f, *ned_accel); + /* clear bits for all accel representations and only set the new one */ + state.accel_status = (1 << ACCEL_NED_F); +} + +/// Set acceleration in ECEF coordinates (float). +static inline void stateSetAccelEcef_f(struct EcefCoor_f* ecef_accel) { + VECT3_COPY(state.ecef_accel_f, *ecef_accel); + /* clear bits for all accel representations and only set the new one */ + state.accel_status = (1 << ACCEL_ECEF_F); +} + +/************************ Get functions ****************************/ + +/// Get acceleration in NED coordinates (int). +static inline struct NedCoor_i* stateGetAccelNed_i(void) { + if (!bit_is_set(state.accel_status, ACCEL_NED_I)) + stateCalcAccelNed_i(); + return &state.ned_accel_i; +} + +/// Get acceleration in ECEF coordinates (int). +static inline struct EcefCoor_i* stateGetAccelEcef_i(void) { + if (!bit_is_set(state.accel_status, ACCEL_ECEF_I)) + stateCalcAccelEcef_i(); + return &state.ecef_accel_i; +} + +/// Get acceleration in NED coordinates (float). +static inline struct NedCoor_f* stateGetAccelNed_f(void) { + if (!bit_is_set(state.accel_status, ACCEL_NED_F)) + stateCalcAccelNed_f(); + return &state.ned_accel_f; +} + +/// Get acceleration in ECEF coordinates (float). +static inline struct EcefCoor_f* stateGetAccelEcef_f(void) { + if (!bit_is_set(state.accel_status, ACCEL_ECEF_F)) + stateCalcAccelEcef_f(); + return &state.ecef_accel_f; +} +/** @}*/ + + + +/****************************************************************************** + * * + * Set and Get functions for the ATTITUDE representations * + * * + *****************************************************************************/ +/** @addtogroup AttGroup + * @{ */ + +/************* declaration of transformation functions ************/ +extern void stateCalcNedToBodyQuat_i(void); +extern void stateCalcNedToBodyRMat_i(void); +extern void stateCalcNedToBodyEulers_i(void); +extern void stateCalcNedToBodyQuat_f(void); +extern void stateCalcNedToBodyRMat_f(void); +extern void stateCalcNedToBodyEulers_f(void); + +/************************ Set functions ****************************/ + +/// Set vehicle body attitude from quaternion (int). +static inline void stateSetNedToBodyQuat_i(struct Int32Quat* ned_to_body_quat) { + QUAT_COPY(state.ned_to_body_quat_i, *ned_to_body_quat); + /* clear bits for all attitude representations and only set the new one */ + state.att_status = (1 << ATT_QUAT_I); +} + +/// Set vehicle body attitude from rotation matrix (int). +static inline void stateSetNedToBodyRMat_i(struct Int32RMat* ned_to_body_rmat) { + RMAT_COPY(state.ned_to_body_rmat_i, *ned_to_body_rmat); + /* clear bits for all attitude representations and only set the new one */ + state.att_status = (1 << ATT_RMAT_I); +} + +/// Set vehicle body attitude from euler angles (int). +static inline void stateSetNedToBodyEulers_i(struct Int32Eulers* ned_to_body_eulers) { + EULERS_COPY(state.ned_to_body_eulers_i, *ned_to_body_eulers); + /* clear bits for all attitude representations and only set the new one */ + state.att_status = (1 << ATT_EULER_I); +} + +/// Set vehicle body attitude from quaternion (float). +static inline void stateSetNedToBodyQuat_f(struct FloatQuat* ned_to_body_quat) { + QUAT_COPY(state.ned_to_body_quat_f, *ned_to_body_quat); + /* clear bits for all attitude representations and only set the new one */ + state.att_status = (1 << ATT_QUAT_F); +} + +/// Set vehicle body attitude from rotation matrix (float). +static inline void stateSetNedToBodyRMat_f(struct FloatRMat* ned_to_body_rmat) { + RMAT_COPY(state.ned_to_body_rmat_f, *ned_to_body_rmat); + /* clear bits for all attitude representations and only set the new one */ + state.att_status = (1 << ATT_RMAT_F); +} + +/// Set vehicle body attitude from euler angles (float). +static inline void stateSetNedToBodyEulers_f(struct FloatEulers* ned_to_body_eulers) { + EULERS_COPY(state.ned_to_body_eulers_f, *ned_to_body_eulers); + /* clear bits for all attitude representations and only set the new one */ + state.att_status = (1 << ATT_EULER_F); +} + +/************************ Get functions ****************************/ + +/// Get vehicle body attitude quaternion (int). +static inline struct Int32Quat* stateGetNedToBodyQuat_i(void) { + if (!bit_is_set(state.att_status, ATT_QUAT_I)) + stateCalcNedToBodyQuat_i(); + return &state.ned_to_body_quat_i; +} + +/// Get vehicle body attitude rotation matrix (int). +static inline struct Int32RMat* stateGetNedToBodyRMat_i(void) { + if (!bit_is_set(state.att_status, ATT_RMAT_I)) + stateCalcNedToBodyRMat_i(); + return &state.ned_to_body_rmat_i; +} + +/// Get vehicle body attitude euler angles (int). +static inline struct Int32Eulers* stateGetNedToBodyEulers_i(void) { + if (!bit_is_set(state.att_status, ATT_EULER_I)) + stateCalcNedToBodyEulers_i(); + return &state.ned_to_body_eulers_i; +} + +/// Get vehicle body attitude quaternion (float). +static inline struct FloatQuat* stateGetNedToBodyQuat_f(void) { + if (!bit_is_set(state.att_status, ATT_QUAT_F)) + stateCalcNedToBodyQuat_f(); + return &state.ned_to_body_quat_f; +} + +/// Get vehicle body attitude rotation matrix (float). +static inline struct FloatRMat* stateGetNedToBodyRMat_f(void) { + if (!bit_is_set(state.att_status, ATT_RMAT_F)) + stateCalcNedToBodyRMat_f(); + return &state.ned_to_body_rmat_f; +} + +/// Get vehicle body attitude euler angles (float). +static inline struct FloatEulers* stateGetNedToBodyEulers_f(void) { + if (!bit_is_set(state.att_status, ATT_EULER_F)) + stateCalcNedToBodyEulers_f(); + return &state.ned_to_body_eulers_f; +} +/** @}*/ + + + +/****************************************************************************** + * * + * Set and Get functions for the ANGULAR RATE representations * + * * + *****************************************************************************/ +/** @addtogroup RateGroup + * @{ */ + +/************* declaration of transformation functions ************/ +extern void stateCalcBodyRates_i(void); +extern void stateCalcBodyRates_f(void); + +/************************ Set functions ****************************/ + +/// Set vehicle body angular rate (int). +static inline void stateSetBodyRates_i(struct Int32Rates* body_rate) { + RATES_COPY(state.body_rates_i, *body_rate); + /* clear bits for all attitude representations and only set the new one */ + state.rate_status = (1 << RATE_I); +} + +/// Set vehicle body angular rate (float). +static inline void stateSetBodyRates_f(struct FloatRates* body_rate) { + RATES_COPY(state.body_rates_f, *body_rate); + /* clear bits for all attitude representations and only set the new one */ + state.rate_status = (1 << RATE_F); +} + +/************************ Get functions ****************************/ + +/// Get vehicle body angular rate (int). +static inline struct Int32Rates* stateGetBodyRates_i(void) { + if (!bit_is_set(state.rate_status, RATE_I)) + stateCalcBodyRates_i(); + return &state.body_rates_i; +} + +/// Get vehicle body angular rate (float). +static inline struct FloatRates* stateGetBodyRates_f(void) { + if (!bit_is_set(state.rate_status, RATE_F)) + stateCalcBodyRates_f(); + return &state.body_rates_f; +} + +/** @}*/ + + + +/****************************************************************************** + * * + * Set and Get functions for the WIND- AND AIRSPEED representations * + * * + *****************************************************************************/ +/** @addtogroup WindAirGroup + * @{ */ + +/************* declaration of transformation functions ************/ +extern void stateCalcHorizontalWindspeed_i(void); +extern void stateCalcAirspeed_i(void); +extern void stateCalcHorizontalWindspeed_f(void); +extern void stateCalcAirspeed_f(void); + +/************************ Set functions ****************************/ + +/// Set horizontal windspeed (int). +static inline void stateSetHorizontalWindspeed_i(struct Int32Vect2* h_windspeed) { + VECT2_COPY(state.h_windspeed_i, *h_windspeed); + /* clear bits for all windspeed representations and only set the new one */ + ClearBit(state.wind_air_status, WINDSPEED_F); + SetBit(state.wind_air_status, WINDSPEED_I); +} + +/// Set airspeed (int). +static inline void stateSetAirspeed_i(int32_t* airspeed) { + state.airspeed_i = *airspeed; + /* clear bits for all windspeed representations and only set the new one */ + ClearBit(state.wind_air_status, AIRSPEED_F); + SetBit(state.wind_air_status, AIRSPEED_I); +} + +/// Set horizontal windspeed (float). +static inline void stateSetHorizontalWindspeed_f(struct FloatVect2* h_windspeed) { + VECT2_COPY(state.h_windspeed_f, *h_windspeed); + /* clear bits for all windspeed representations and only set the new one */ + ClearBit(state.wind_air_status, WINDSPEED_I); + SetBit(state.wind_air_status, WINDSPEED_F); +} + +/// Set airspeed (float). +static inline void stateSetAirspeed_f(float* airspeed) { + state.airspeed_f = *airspeed; + /* clear bits for all windspeed representations and only set the new one */ + ClearBit(state.wind_air_status, AIRSPEED_I); + SetBit(state.wind_air_status, AIRSPEED_F); +} + +/************************ Get functions ****************************/ + +/// Get horizontal windspeed (int). +static inline struct Int32Vect2* stateGetHorizontalWindspeed_i(void) { + if (!bit_is_set(state.wind_air_status, WINDSPEED_I)) + stateCalcHorizontalWindspeed_i(); + return &state.h_windspeed_i; +} + +/// Get airspeed (int). +static inline int32_t* stateGetAirspeed_i(void) { + if (!bit_is_set(state.wind_air_status, AIRSPEED_I)) + stateCalcAirspeed_i(); + return &state.airspeed_i; +} + +/// Get horizontal windspeed (float). +static inline struct FloatVect2* stateGetHorizontalWindspeed_f(void) { + if (!bit_is_set(state.wind_air_status, WINDSPEED_F)) + stateCalcHorizontalWindspeed_f(); + return &state.h_windspeed_f; +} + +/// Get airspeed (float). +static inline float* stateGetAirspeed_f(void) { + if (!bit_is_set(state.wind_air_status, AIRSPEED_F)) + stateCalcAirspeed_f(); + return &state.airspeed_f; +} + +/** @}*/ + +/** @}*/ + + +#endif /* STATE_H */ From 7bc4ba3dea856d6e0a4fb4cfb117c8885aad43f4 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 8 Jul 2012 16:01:51 +0200 Subject: [PATCH 02/62] [math] some more math macros --- sw/airborne/math/pprz_algebra.h | 12 ++++++++++++ sw/airborne/math/pprz_algebra_float.h | 4 ++++ sw/airborne/math/pprz_geodetic.h | 7 +++++++ sw/airborne/math/pprz_geodetic_int.h | 12 +++++++----- 4 files changed, 30 insertions(+), 5 deletions(-) diff --git a/sw/airborne/math/pprz_algebra.h b/sw/airborne/math/pprz_algebra.h index 9bc9b54c8a..f9d2dbc7eb 100644 --- a/sw/airborne/math/pprz_algebra.h +++ b/sw/airborne/math/pprz_algebra.h @@ -628,6 +628,18 @@ (_ri).r = RATE_BFP_OF_REAL((_rf).r); \ } +#define SPEEDS_FLOAT_OF_BFP(_ef, _ei) { \ + (_ef).x = SPEED_FLOAT_OF_BFP((_ei).x); \ + (_ef).y = SPEED_FLOAT_OF_BFP((_ei).y); \ + (_ef).z = SPEED_FLOAT_OF_BFP((_ei).z); \ + } + +#define SPEEDS_BFP_OF_REAL(_ef, _ei) { \ + (_ef).x = SPEED_BFP_OF_REAL((_ei).x); \ + (_ef).y = SPEED_BFP_OF_REAL((_ei).y); \ + (_ef).z = SPEED_BFP_OF_REAL((_ei).z); \ + } + #define ACCELS_FLOAT_OF_BFP(_ef, _ei) { \ (_ef).x = ACCEL_FLOAT_OF_BFP((_ei).x); \ (_ef).y = ACCEL_FLOAT_OF_BFP((_ei).y); \ diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h index e02194a019..e48aca10dd 100644 --- a/sw/airborne/math/pprz_algebra_float.h +++ b/sw/airborne/math/pprz_algebra_float.h @@ -123,6 +123,10 @@ struct FloatRates { /* _vo = _vi * _s */ #define FLOAT_VECT2_SMUL(_vo, _vi, _s) VECT2_SMUL(_vo, _vi, _s) +#define FLOAT_VECT2_NORM(n, v) { \ + n = sqrtf((v).x*(v).x + (v).y*(v).y); \ + } + /* * Dimension 3 Vectors diff --git a/sw/airborne/math/pprz_geodetic.h b/sw/airborne/math/pprz_geodetic.h index 08f0a84ed7..72b62df929 100644 --- a/sw/airborne/math/pprz_geodetic.h +++ b/sw/airborne/math/pprz_geodetic.h @@ -20,4 +20,11 @@ (_pos1).alt = (_pos2).alt; \ } +#define LTP_DEF_COPY(_def1,_def2){ \ + LLA_COPY((_def1).lla, (_def2).lla); \ + VECT3_COPY((_def1).ecef, (_def2).ecef); \ + RMAT_COPY((_def1).ltp_of_ecef, (_def2).ltp_of_ecef); \ + (_def1).hmsl = (_def2).hmsl; \ + } + #endif /* PPRZ_GEODETIC_H */ diff --git a/sw/airborne/math/pprz_geodetic_int.h b/sw/airborne/math/pprz_geodetic_int.h index 7813bff7a7..149acb1ad7 100644 --- a/sw/airborne/math/pprz_geodetic_int.h +++ b/sw/airborne/math/pprz_geodetic_int.h @@ -123,13 +123,15 @@ extern void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, st #define EM7RAD_OF_RAD(_r) ((_r)*1e7) #define RAD_OF_EM7RAD(_r) ((_r)/1e7) -#define INT32_VECT3_ENU_OF_NED(_o, _i) { \ - (_o).x = (_i).y; \ - (_o).y = (_i).x; \ - (_o).z = -(_i).z; \ +#define VECT3_ENU_OF_NED(_o, _i) { \ + (_o).x = (_i).y; \ + (_o).y = (_i).x; \ + (_o).z = -(_i).z; \ } -#define INT32_VECT3_NED_OF_ENU(_o, _i) INT32_VECT3_ENU_OF_NED(_o,_i) +#define VECT3_NED_OF_ENU(_o, _i) VECT3_ENU_OF_NED(_o,_i) +#define INT32_VECT3_NED_OF_ENU(_o, _i) VECT3_ENU_OF_NED(_o,_i) +#define INT32_VECT3_ENU_OF_NED(_o, _i) VECT3_ENU_OF_NED(_o,_i) #define ECEF_BFP_OF_REAL(_o, _i) { \ (_o).x = (int32_t)CM_OF_M((_i).x); \ From fc4bb0ffbb288a68566ad8d826d29b0e75fbc28c Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 8 Jul 2012 16:04:38 +0200 Subject: [PATCH 03/62] [state interface] use state interface in rotorcraft firmware --- conf/firmwares/rotorcraft.makefile | 2 ++ conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile | 2 ++ sw/airborne/firmwares/rotorcraft/main.c | 4 ++++ 3 files changed, 8 insertions(+) diff --git a/conf/firmwares/rotorcraft.makefile b/conf/firmwares/rotorcraft.makefile index f2b4c6229a..77bb62acdc 100644 --- a/conf/firmwares/rotorcraft.makefile +++ b/conf/firmwares/rotorcraft.makefile @@ -198,6 +198,8 @@ endif ap.srcs += $(SRC_FIRMWARE)/autopilot.c +ap.srcs += state.c + ap.srcs += $(SRC_FIRMWARE)/stabilization.c ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_none.c ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_rate.c diff --git a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile index 03c68288a4..b884d3b903 100644 --- a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile +++ b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile @@ -107,6 +107,8 @@ nps.srcs += subsystems/electrical.c nps.srcs += $(SRC_FIRMWARE)/autopilot.c +nps.srcs += state.c + # # in makefile section of airframe xml # include $(CFG_BOOZ)/subsystems/booz2_ahrs_lkf.makefile diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index ed193506ff..26fe749ae5 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -59,6 +59,8 @@ #include "subsystems/ahrs.h" #include "subsystems/ins.h" +#include "state.h" + #include "firmwares/rotorcraft/main.h" #ifdef SITL @@ -100,6 +102,8 @@ STATIC_INLINE void main_init( void ) { electrical_init(); + stateInit(); + actuators_init(); radio_control_init(); From 366271644a7c5dab1ab90500d2deb5c622f1903d Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 8 Jul 2012 16:06:30 +0200 Subject: [PATCH 04/62] [state interface] use state interface in ahrs int_cmpl_euler --- sw/airborne/subsystems/ahrs.h | 6 ++++++ sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c | 3 +++ 2 files changed, 9 insertions(+) diff --git a/sw/airborne/subsystems/ahrs.h b/sw/airborne/subsystems/ahrs.h index 3c2a29dbbe..6985698c2c 100644 --- a/sw/airborne/subsystems/ahrs.h +++ b/sw/airborne/subsystems/ahrs.h @@ -29,6 +29,7 @@ #include "std.h" #include "math/pprz_algebra_int.h" #include "math/pprz_algebra_float.h" +#include "state.h" #define AHRS_UNINIT 0 #define AHRS_RUNNING 1 @@ -98,6 +99,11 @@ extern float ahrs_mag_offset; EULERS_BFP_OF_REAL(ahrs.ltp_to_imu_euler, ahrs_float.ltp_to_imu_euler); \ RMAT_BFP_OF_REAL(ahrs.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_rmat); \ RATES_BFP_OF_REAL(ahrs.imu_rate, ahrs_float.imu_rate); \ + +/* copy attitude to state interface */ +#define AHRS_BODY_TO_STATE() { \ + stateSetNedToBodyQuat_i(&ahrs.ltp_to_body_quat); \ + stateSetBodyRates_i(&ahrs.body_rate); \ } /** AHRS initialization. Called at startup. diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c index 4718311da1..4ec9a24337 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c @@ -291,9 +291,12 @@ __attribute__ ((always_inline)) static inline void compute_body_orientation(void /* compute body rates */ INT32_RMAT_TRANSP_RATEMULT(ahrs.body_rate, imu.body_to_imu_rmat, ahrs.imu_rate); + AHRS_BODY_TO_STATE(); + } + #ifdef AHRS_UPDATE_FW_ESTIMATOR // TODO use ahrs result directly #include "estimator.h" From 46680314bbca62548970a89629607a238ab12ae3 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 8 Jul 2012 16:10:39 +0200 Subject: [PATCH 05/62] [state interface] removed ins_enu_* representations from ins interface, use state interface in navigation instead --- conf/flight_plans/rotorcraft_basic.xml | 2 +- sw/airborne/firmwares/rotorcraft/navigation.c | 14 +++++++------ sw/airborne/firmwares/rotorcraft/navigation.h | 8 ++++---- sw/airborne/modules/cam_control/booz_cam.c | 4 ++-- sw/airborne/subsystems/ins.c | 20 +++++-------------- sw/airborne/subsystems/ins.h | 13 ++++++++---- 6 files changed, 29 insertions(+), 32 deletions(-) diff --git a/conf/flight_plans/rotorcraft_basic.xml b/conf/flight_plans/rotorcraft_basic.xml index 7719308239..e9798d0de1 100644 --- a/conf/flight_plans/rotorcraft_basic.xml +++ b/conf/flight_plans/rotorcraft_basic.xml @@ -34,7 +34,7 @@ - + diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c index 52535bda4b..64e6f5916f 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/sw/airborne/firmwares/rotorcraft/navigation.c @@ -28,6 +28,7 @@ #include "pprz_debug.h" #include "subsystems/gps.h" #include "subsystems/ins.h" +#include "state.h" #include "firmwares/rotorcraft/autopilot.h" #include "generated/modules.h" @@ -106,7 +107,7 @@ void nav_run(void) { /* compute a vector to the waypoint */ struct Int32Vect2 path_to_waypoint; - VECT2_DIFF(path_to_waypoint, navigation_target, ins_enu_pos); + VECT2_DIFF(path_to_waypoint, navigation_target, *stateGetPositionEnu_i()); /* saturate it */ VECT2_STRIM(path_to_waypoint, -(1<<15), (1<<15)); @@ -122,7 +123,7 @@ void nav_run(void) { struct Int32Vect2 path_to_carrot; VECT2_SMUL(path_to_carrot, path_to_waypoint, CARROT_DIST); VECT2_SDIV(path_to_carrot, path_to_carrot, dist_to_waypoint); - VECT2_SUM(navigation_carrot, path_to_carrot, ins_enu_pos); + VECT2_SUM(navigation_carrot, path_to_carrot, *stateGetPositionEnu_i()); } #else // if H_REF is used, CARROT_DIST is not used @@ -138,7 +139,7 @@ void nav_circle(uint8_t wp_center, int32_t radius) { } else { struct Int32Vect2 pos_diff; - VECT2_DIFF(pos_diff, ins_enu_pos,waypoints[wp_center]); + VECT2_DIFF(pos_diff, *stateGetPositionEnu_i(), waypoints[wp_center]); // go back to half metric precision or values are too large //INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC/2); // store last qdr @@ -182,7 +183,7 @@ void nav_circle(uint8_t wp_center, int32_t radius) { void nav_route(uint8_t wp_start, uint8_t wp_end) { struct Int32Vect2 wp_diff,pos_diff; VECT2_DIFF(wp_diff, waypoints[wp_end],waypoints[wp_start]); - VECT2_DIFF(pos_diff, ins_enu_pos,waypoints[wp_start]); + VECT2_DIFF(pos_diff, *stateGetPositionEnu_i(), waypoints[wp_start]); // go back to metric precision or values are too large INT32_VECT2_RSHIFT(wp_diff,wp_diff,INT32_POS_FRAC); INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC); @@ -218,7 +219,7 @@ bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx) { int32_t dist_to_point; struct Int32Vect2 diff; static uint8_t time_at_wp = 0; - VECT2_DIFF(diff, waypoints[wp_idx], ins_enu_pos); + VECT2_DIFF(diff, waypoints[wp_idx], *stateGetPositionEnu_i()); INT32_VECT2_RSHIFT(diff,diff,INT32_POS_FRAC); INT32_VECT2_NORM(dist_to_point, diff); //printf("dist %d | %d %d\n", dist_to_point,diff.x,diff.y); @@ -258,13 +259,14 @@ unit_t nav_reset_alt( void ) { #if USE_GPS ins_ltp_def.lla.alt = gps.lla_pos.alt; ins_ltp_def.hmsl = gps.hmsl; + stateSetLocalOrigin_i(&ins_ltp_def); #endif return 0; } void nav_init_stage( void ) { - INT32_VECT3_COPY(nav_last_point, ins_enu_pos); + INT32_VECT3_COPY(nav_last_point, *stateGetPositionEnu_i()); stage_time = 0; nav_circle_radians = 0; horizontal_mode = HORIZONTAL_MODE_WAYPOINT; diff --git a/sw/airborne/firmwares/rotorcraft/navigation.h b/sw/airborne/firmwares/rotorcraft/navigation.h index a1a68e391e..25beea6324 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.h +++ b/sw/airborne/firmwares/rotorcraft/navigation.h @@ -83,7 +83,7 @@ void nav_home(void); #define NavSetGroundReferenceHere() ({ nav_reset_reference(); FALSE; }) #define NavSetAltitudeReferenceHere() ({ nav_reset_alt(); FALSE; }) -#define NavSetWaypointHere(_wp) ({ VECT2_COPY(waypoints[_wp], ins_enu_pos); FALSE; }) +#define NavSetWaypointHere(_wp) ({ VECT2_COPY(waypoints[_wp], *stateGetPositionEnu_i()); FALSE; }) #define NavCopyWaypoint(_wp1, _wp2) ({ VECT2_COPY(waypoints[_wp1], waypoints[_wp2]); FALSE; }) #define WaypointX(_wp) POS_FLOAT_OF_BFP(waypoints[_wp].x) @@ -183,9 +183,9 @@ bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx); } -#define GetPosX() POS_FLOAT_OF_BFP(ins_enu_pos.x) -#define GetPosY() POS_FLOAT_OF_BFP(ins_enu_pos.y) -#define GetPosAlt() (POS_FLOAT_OF_BFP(ins_enu_pos.z+ground_alt)) +#define GetPosX() (stateGetPositionEnu_f()->x) +#define GetPosY() (stateGetPositionEnu_f()->y) +#define GetPosAlt() (stateGetPositionEnu_f()->z+ground_alt) extern void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int16_t heading_rate_sp ); diff --git a/sw/airborne/modules/cam_control/booz_cam.c b/sw/airborne/modules/cam_control/booz_cam.c index 2082a831d3..ce1db6ffcf 100644 --- a/sw/airborne/modules/cam_control/booz_cam.c +++ b/sw/airborne/modules/cam_control/booz_cam.c @@ -121,14 +121,14 @@ void booz_cam_periodic(void) { #ifdef WP_CAM { struct Int32Vect2 diff; - VECT2_DIFF(diff, waypoints[WP_CAM], ins_enu_pos); + VECT2_DIFF(diff, waypoints[WP_CAM], *stateGetPositionEnu_i()); INT32_VECT2_RSHIFT(diff,diff,INT32_POS_FRAC); INT32_ATAN2(booz_cam_pan,diff.x,diff.y); nav_heading = booz_cam_pan; #ifdef BOOZ_CAM_USE_TILT_ANGLES int32_t dist, height; INT32_VECT2_NORM(dist, diff); - height = (waypoints[WP_CAM].z - ins_enu_pos.z) >> INT32_POS_FRAC; + height = (waypoints[WP_CAM].z - stateGetPositionEnu_i()->z) >> INT32_POS_FRAC; INT32_ATAN2(booz_cam_tilt, height, dist); Bound(booz_cam_tilt, CAM_TA_MIN, CAM_TA_MAX); booz_cam_tilt_pwm = BOOZ_CAM_TILT_MIN + D_TILT * (booz_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN); diff --git a/sw/airborne/subsystems/ins.c b/sw/airborne/subsystems/ins.c index 3ee24f568f..08b5735ed3 100644 --- a/sw/airborne/subsystems/ins.c +++ b/sw/airborne/subsystems/ins.c @@ -77,9 +77,6 @@ bool_t ins_vf_realign; struct NedCoor_i ins_ltp_pos; struct NedCoor_i ins_ltp_speed; struct NedCoor_i ins_ltp_accel; -struct EnuCoor_i ins_enu_pos; -struct EnuCoor_i ins_enu_speed; -struct EnuCoor_i ins_enu_accel; void ins_init() { @@ -98,6 +95,7 @@ void ins_init() { ltp_def_from_ecef_i(&ins_ltp_def, &ecef_nav0); ins_ltp_def.hmsl = NAV_ALT0; + stateSetLocalOrigin_i(&ins_ltp_def); #else ins_ltp_initialised = FALSE; #endif @@ -116,9 +114,6 @@ void ins_init() { INT32_VECT3_ZERO(ins_ltp_pos); INT32_VECT3_ZERO(ins_ltp_speed); INT32_VECT3_ZERO(ins_ltp_accel); - INT32_VECT3_ZERO(ins_enu_pos); - INT32_VECT3_ZERO(ins_enu_speed); - INT32_VECT3_ZERO(ins_enu_accel); } void ins_periodic( void ) { @@ -170,9 +165,7 @@ void ins_propagate() { ins_ltp_accel.y = accel_meas_ltp.y; #endif /* USE_HFF */ - INT32_VECT3_ENU_OF_NED(ins_enu_pos, ins_ltp_pos); - INT32_VECT3_ENU_OF_NED(ins_enu_speed, ins_ltp_speed); - INT32_VECT3_ENU_OF_NED(ins_enu_accel, ins_ltp_accel); + INS_NED_TO_STATE(); } void ins_update_baro() { @@ -192,9 +185,6 @@ void ins_update_baro() { ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot); ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot); ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z); - ins_enu_pos.z = -ins_ltp_pos.z; - ins_enu_speed.z = -ins_ltp_speed.z; - ins_enu_accel.z = -ins_ltp_accel.z; } else { /* not realigning, so normal update with baro measurement */ ins_baro_alt = ((baro.absolute - ins_qfe) * INS_BARO_SENS_NUM)/INS_BARO_SENS_DEN; @@ -202,6 +192,7 @@ void ins_update_baro() { vff_update(alt_float); } } + INS_NED_TO_STATE(); #endif } @@ -214,6 +205,7 @@ void ins_update_gps(void) { ins_ltp_def.lla.alt = gps.lla_pos.alt; ins_ltp_def.hmsl = gps.hmsl; ins_ltp_initialised = TRUE; + stateSetLocalOrigin_i(&ins_ltp_def); } ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &gps.ecef_pos); ned_of_ecef_vect_i(&ins_gps_speed_cm_s_ned, &ins_ltp_def, &gps.ecef_vel); @@ -258,9 +250,7 @@ void ins_update_gps(void) { #endif #endif /* hff not used */ - INT32_VECT3_ENU_OF_NED(ins_enu_pos, ins_ltp_pos); - INT32_VECT3_ENU_OF_NED(ins_enu_speed, ins_ltp_speed); - INT32_VECT3_ENU_OF_NED(ins_enu_accel, ins_ltp_accel); + INS_NED_TO_STATE(); } #endif /* USE_GPS */ } diff --git a/sw/airborne/subsystems/ins.h b/sw/airborne/subsystems/ins.h index 83bb470096..e949d7f554 100644 --- a/sw/airborne/subsystems/ins.h +++ b/sw/airborne/subsystems/ins.h @@ -26,6 +26,8 @@ #include "math/pprz_geodetic_int.h" #include "math/pprz_algebra_float.h" +#include "state.h" + /* gps transformed to LTP-NED */ extern struct LtpDef_i ins_ltp_def; extern bool_t ins_ltp_initialised; @@ -47,10 +49,6 @@ extern int32_t ins_sonar_offset; extern struct NedCoor_i ins_ltp_pos; extern struct NedCoor_i ins_ltp_speed; extern struct NedCoor_i ins_ltp_accel; -/* output LTP ENU */ -extern struct EnuCoor_i ins_enu_pos; -extern struct EnuCoor_i ins_enu_speed; -extern struct EnuCoor_i ins_enu_accel; #if USE_HFF /* horizontal gps transformed to NED in meters as float */ extern struct FloatVect2 ins_gps_pos_m_ned; @@ -69,5 +67,12 @@ extern void ins_update_baro( void ); extern void ins_update_gps( void ); extern void ins_update_sonar( void ); +/* copy position and speed to state interface */ +#define INS_NED_TO_STATE() { \ + stateSetPositionNed_i(&ins_ltp_pos); \ + stateSetSpeedNed_i(&ins_ltp_speed); \ + stateSetAccelNed_i(&ins_ltp_accel); \ + } + #endif /* INS_H */ From f5054fcb6bb909e4d5c020079f178416dac93c32 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 8 Jul 2012 16:18:47 +0200 Subject: [PATCH 06/62] [rotorcraft] use new state interface instead of ins in guidance --- .../firmwares/rotorcraft/guidance/guidance_h.c | 10 +++++----- .../firmwares/rotorcraft/guidance/guidance_v.c | 17 +++++++++-------- 2 files changed, 14 insertions(+), 13 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index df5a66fcde..286235e0b8 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -266,12 +266,12 @@ static inline void guidance_h_update_reference(bool_t use_ref) { static inline void guidance_h_traj_run(bool_t in_flight) { /* compute position error */ - VECT2_DIFF(guidance_h_pos_err, guidance_h_pos_ref, ins_ltp_pos); + VECT2_DIFF(guidance_h_pos_err, guidance_h_pos_ref, *stateGetPositionNed_i()); /* saturate it */ VECT2_STRIM(guidance_h_pos_err, -MAX_POS_ERR, MAX_POS_ERR); /* compute speed error */ - VECT2_DIFF(guidance_h_speed_err, guidance_h_speed_ref, ins_ltp_speed); + VECT2_DIFF(guidance_h_speed_err, guidance_h_speed_ref, *stateGetSpeedNed_i()); /* saturate it */ VECT2_STRIM(guidance_h_speed_err, -MAX_SPEED_ERR, MAX_SPEED_ERR); @@ -326,7 +326,7 @@ static inline void guidance_h_traj_run(bool_t in_flight) { static inline void guidance_h_hover_enter(void) { - VECT2_COPY(guidance_h_pos_sp, ins_ltp_pos); + VECT2_COPY(guidance_h_pos_sp, *stateGetPositionNed_i()); guidance_h_rc_sp.psi = ahrs.ltp_to_body_euler.psi; reset_psi_ref_from_body(); @@ -340,8 +340,8 @@ static inline void guidance_h_nav_enter(void) { INT32_VECT2_NED_OF_ENU(guidance_h_pos_sp, navigation_carrot); struct Int32Vect2 pos,speed,zero; INT_VECT2_ZERO(zero); - VECT2_COPY(pos, ins_ltp_pos); - VECT2_COPY(speed, ins_ltp_speed); + VECT2_COPY(pos, *stateGetPositionNed_i()); + VECT2_COPY(speed, *stateGetSpeedNed_i()); GuidanceHSetRef(pos, speed, zero); /* reset psi reference, set psi setpoint to current psi */ diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index ab0a4c90f0..b0d20ee477 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -35,7 +35,8 @@ // #include "booz_fms.h" FIXME #include "firmwares/rotorcraft/navigation.h" -#include "subsystems/ins.h" +#include "state.h" + #include "math/pprz_algebra_int.h" #include "generated/airframe.h" @@ -133,9 +134,9 @@ void guidance_v_mode_changed(uint8_t new_mode) { switch (new_mode) { case GUIDANCE_V_MODE_HOVER: - guidance_v_z_sp = ins_ltp_pos.z; // set current altitude as setpoint + guidance_v_z_sp = stateGetPositionNed_i()->z; // set current altitude as setpoint guidance_v_z_sum_err = 0; - GuidanceVSetRef(ins_ltp_pos.z, 0, 0); + GuidanceVSetRef(stateGetPositionNed_i()->z, 0, 0); break; case GUIDANCE_V_MODE_RC_CLIMB: @@ -143,7 +144,7 @@ void guidance_v_mode_changed(uint8_t new_mode) { guidance_v_zd_sp = 0; case GUIDANCE_V_MODE_NAV: guidance_v_z_sum_err = 0; - GuidanceVSetRef(ins_ltp_pos.z, ins_ltp_speed.z, 0); + GuidanceVSetRef(stateGetPositionNed_i()->z, stateGetSpeedNed_i()->z, 0); break; default: @@ -167,13 +168,13 @@ void guidance_v_run(bool_t in_flight) { // FIXME... SATURATIONS NOT TAKEN INTO ACCOUNT // AKA SUPERVISION and co if (in_flight) { - gv_adapt_run(ins_ltp_accel.z, stabilization_cmd[COMMAND_THRUST], guidance_v_zd_ref); + gv_adapt_run(stateGetAccelNed_i()->z, stabilization_cmd[COMMAND_THRUST], guidance_v_zd_ref); } switch (guidance_v_mode) { case GUIDANCE_V_MODE_RC_DIRECT: - guidance_v_z_sp = ins_ltp_pos.z; // for display only + guidance_v_z_sp = stateGetPositionNed_i()->z; // for display only stabilization_cmd[COMMAND_THRUST] = guidance_v_rc_delta_t; break; @@ -261,9 +262,9 @@ __attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flig guidance_v_zd_ref = gv_zd_ref<<(INT32_SPEED_FRAC - GV_ZD_REF_FRAC); guidance_v_zdd_ref = gv_zdd_ref<<(INT32_ACCEL_FRAC - GV_ZDD_REF_FRAC); /* compute the error to our reference */ - int32_t err_z = guidance_v_z_ref - ins_ltp_pos.z; + int32_t err_z = guidance_v_z_ref - stateGetPositionNed_i()->z; Bound(err_z, GUIDANCE_V_MIN_ERR_Z, GUIDANCE_V_MAX_ERR_Z); - int32_t err_zd = guidance_v_zd_ref - ins_ltp_speed.z; + int32_t err_zd = guidance_v_zd_ref - stateGetSpeedNed_i()->z; Bound(err_zd, GUIDANCE_V_MIN_ERR_ZD, GUIDANCE_V_MAX_ERR_ZD); if (in_flight) { From ff0ac15f84d16e8edd6c9cc1174768fde5c09440 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 8 Jul 2012 16:21:32 +0200 Subject: [PATCH 07/62] [rotorcraft] use new state interface in telemetry --- sw/airborne/firmwares/rotorcraft/telemetry.h | 40 ++++++++++---------- 1 file changed, 21 insertions(+), 19 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index ace0a8d03d..3964154646 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -35,6 +35,8 @@ #include "subsystems/radio_control.h" #endif +#include "state.h" + #include "firmwares/rotorcraft/autopilot.h" #include "firmwares/rotorcraft/guidance.h" @@ -439,10 +441,10 @@ &ahrs.ltp_to_imu_quat.qx, \ &ahrs.ltp_to_imu_quat.qy, \ &ahrs.ltp_to_imu_quat.qz, \ - &ahrs.ltp_to_body_quat.qi, \ - &ahrs.ltp_to_body_quat.qx, \ - &ahrs.ltp_to_body_quat.qy, \ - &ahrs.ltp_to_body_quat.qz); \ + &(stateGetNedToBodyQuat_i()->qi), \ + &(stateGetNedToBodyQuat_i()->qx), \ + &(stateGetNedToBodyQuat_i()->qy), \ + &(stateGetNedToBodyQuat_i()->qz)); \ } #define PERIODIC_SEND_AHRS_EULER_INT(_trans, _dev) { \ @@ -635,21 +637,21 @@ #define PERIODIC_SEND_ROTORCRAFT_FP(_trans, _dev) { \ int32_t carrot_up = -guidance_v_z_sp; \ DOWNLINK_SEND_ROTORCRAFT_FP( _trans, _dev, \ - &ins_enu_pos.x, \ - &ins_enu_pos.y, \ - &ins_enu_pos.z, \ - &ins_enu_speed.x, \ - &ins_enu_speed.y, \ - &ins_enu_speed.z, \ - &ahrs.ltp_to_body_euler.phi, \ - &ahrs.ltp_to_body_euler.theta, \ - &ahrs.ltp_to_body_euler.psi, \ - &guidance_h_pos_sp.y, \ - &guidance_h_pos_sp.x, \ - &carrot_up, \ - &guidance_h_command_body.psi, \ - &stabilization_cmd[COMMAND_THRUST], \ - &autopilot_flight_time); \ + &(stateGetPositionEnu_i()->x), \ + &(stateGetPositionEnu_i()->y), \ + &(stateGetPositionEnu_i()->z), \ + &(stateGetSpeedEnu_i()->x), \ + &(stateGetSpeedEnu_i()->y), \ + &(stateGetSpeedEnu_i()->z), \ + &(stateGetNedToBodyEulers_i()->phi), \ + &(stateGetNedToBodyEulers_i()->theta), \ + &(stateGetNedToBodyEulers_i()->psi), \ + &guidance_h_pos_sp.y, \ + &guidance_h_pos_sp.x, \ + &carrot_up, \ + &guidance_h_command_body.psi, \ + &stabilization_cmd[COMMAND_THRUST], \ + &autopilot_flight_time); \ } #if USE_GPS From 474642b27d3b6d7a15a6523f6781b48a3e9de1e5 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 19 Jul 2012 23:37:13 +0200 Subject: [PATCH 08/62] [state interface] add validity test function and multiple set functions --- sw/airborne/state.h | 148 +++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 147 insertions(+), 1 deletion(-) diff --git a/sw/airborne/state.h b/sw/airborne/state.h index 69ba0ce498..5551ed30cb 100644 --- a/sw/airborne/state.h +++ b/sw/airborne/state.h @@ -67,6 +67,8 @@ #define POS_ENU_F 7 #define POS_LLA_F 8 #define POS_UTM_F 9 +#define POS_LOCAL_COORD ((1< Date: Thu, 19 Jul 2012 23:43:58 +0200 Subject: [PATCH 09/62] [state interface] Reminder: no copy/paste... --- sw/airborne/state.h | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/sw/airborne/state.h b/sw/airborne/state.h index 5551ed30cb..3aa7555f44 100644 --- a/sw/airborne/state.h +++ b/sw/airborne/state.h @@ -515,12 +515,12 @@ extern void stateCalcPositionLla_f(void); /*********************** validity test functions ******************/ /// Test if local coordinates are valid. -static inline bool_t stateIsLocalCoordinateValid() { +static inline bool_t stateIsLocalCoordinateValid(void) { return (state.ned_initialized_i && (state.pos_status &= ~(POS_LOCAL_COORD))); } /// Test if global coordinates are valid. -static inline bool_t stateIsGlobalCoordinateValid() { +static inline bool_t stateIsGlobalCoordinateValid(void) { return ((state.pos_status &= ~(POS_GLOBAL_COORD)) || stateIsLocalCoordinateValid()); } @@ -616,7 +616,7 @@ static inline void stateSetPositionLla_f(struct LlaCoor_f* lla_pos) { } /// Set multiple position coordinates (float). -static inline void stateSetPosition_i( +static inline void stateSetPosition_f( struct EcefCoor_f* ecef_pos, struct NedCoor_f* ned_pos, struct EnuCoor_f* enu_pos, @@ -759,7 +759,7 @@ static inline void stateSetSpeedEcef_i(struct EcefCoor_i* ecef_speed) { } /// Set multiple speed coordinates (int). -static inline void stateSetPosition_i( +static inline void stateSetSpeed_i( struct EcefCoor_i* ecef_speed, struct NedCoor_i* ned_speed, struct EnuCoor_i* enu_speed) { @@ -801,7 +801,7 @@ static inline void stateSetSpeedEcef_f(struct EcefCoor_f* ecef_speed) { } /// Set multiple speed coordinates (float). -static inline void stateSetPosition_f( +static inline void stateSetSpeed_f( struct EcefCoor_f* ecef_speed, struct NedCoor_f* ned_speed, struct EnuCoor_f* enu_speed) { @@ -913,7 +913,7 @@ extern void stateCalcAccelEcef_f(void); /*********************** validity test functions ******************/ /// Test if accelerations are valid. -static inline bool_t stateIsAccelValid() { +static inline bool_t stateIsAccelValid(void) { return (state.accel_status); } @@ -999,7 +999,7 @@ extern void stateCalcNedToBodyEulers_f(void); /*********************** validity test functions ******************/ /// Test if attitudes are valid. -static inline bool_t stateIsAttitudeValid() { +static inline bool_t stateIsAttitudeValid(void) { return (state.att_status); } @@ -1109,7 +1109,7 @@ extern void stateCalcBodyRates_f(void); /*********************** validity test functions ******************/ /// Test if rates are valid. -static inline bool_t stateIsRateValid() { +static inline bool_t stateIsRateValid(void) { return (state.rate_status); } @@ -1167,12 +1167,12 @@ extern void stateCalcAirspeed_f(void); /************************ validity test function *******************/ /// test if wind speed is available. -static inline void stateIsWindspeedValid() { +static inline bool_t stateIsWindspeedValid(void) { return (state.wind_air_status &= ~((1< Date: Fri, 20 Jul 2012 08:53:21 +0200 Subject: [PATCH 10/62] [state interface] check pointers for NULL instead of 0 --- sw/airborne/state.h | 30 +++++++++++++++--------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/sw/airborne/state.h b/sw/airborne/state.h index 3aa7555f44..4076442d73 100644 --- a/sw/airborne/state.h +++ b/sw/airborne/state.h @@ -562,19 +562,19 @@ static inline void stateSetPosition_i( struct LlaCoor_i* lla_pos) { /* clear all status bit */ state.pos_status = 0; - if (ecef_pos != 0) { + if (ecef_pos != NULL) { INT32_VECT3_COPY(state.ecef_pos_i, *ecef_pos); state.pos_status |= (1 << POS_ECEF_I); } - if (ned_pos != 0) { + if (ned_pos != NULL) { INT32_VECT3_COPY(state.ned_pos_i, *ned_pos); state.pos_status |= (1 << POS_NED_I); } - if (enu_pos != 0) { + if (enu_pos != NULL) { INT32_VECT3_COPY(state.enu_pos_i, *enu_pos); state.pos_status |= (1 << POS_ENU_I); } - if (lla_pos != 0) { + if (lla_pos != NULL) { LLA_COPY(state.lla_pos_i, *lla_pos); state.pos_status |= (1 << POS_LLA_I); } @@ -624,23 +624,23 @@ static inline void stateSetPosition_f( struct UtmCoor_f* utm_pos) { /* clear all status bit */ state.pos_status = 0; - if (ecef_pos != 0) { + if (ecef_pos != NULL) { VECT3_COPY(state.ecef_pos_f, *ecef_pos); state.pos_status |= (1 << POS_ECEF_F); } - if (ned_pos != 0) { + if (ned_pos != NULL) { VECT3_COPY(state.ned_pos_f, *ned_pos); state.pos_status |= (1 << POS_NED_F); } - if (enu_pos != 0) { + if (enu_pos != NULL) { VECT3_COPY(state.enu_pos_f, *enu_pos); state.pos_status |= (1 << POS_ENU_F); } - if (lla_pos != 0) { + if (lla_pos != NULL) { LLA_COPY(state.lla_pos_f, *lla_pos); state.pos_status |= (1 << POS_LLA_F); } - if (utm_pos != 0) { + if (utm_pos != NULL) { memcpy(&state.utm_pos_f, utm_pos, sizeof(struct UtmCoor_f)); state.pos_status |= (1 << POS_UTM_F); } @@ -765,15 +765,15 @@ static inline void stateSetSpeed_i( struct EnuCoor_i* enu_speed) { /* clear all status bit */ state.speed_status = 0; - if (ecef_speed != 0) { + if (ecef_speed != NULL) { INT32_VECT3_COPY(state.ecef_speed_i, *ecef_speed); state.speed_status |= (1 << SPEED_ECEF_I); } - if (ned_speed != 0) { + if (ned_speed != NULL) { INT32_VECT3_COPY(state.ned_speed_i, *ned_speed); state.speed_status |= (1 << SPEED_NED_I); } - if (enu_speed != 0) { + if (enu_speed != NULL) { INT32_VECT3_COPY(state.enu_speed_i, *enu_speed); state.speed_status |= (1 << SPEED_ENU_I); } @@ -807,15 +807,15 @@ static inline void stateSetSpeed_f( struct EnuCoor_f* enu_speed) { /* clear all status bit */ state.speed_status = 0; - if (ecef_speed != 0) { + if (ecef_speed != NULL) { VECT3_COPY(state.ecef_speed_f, *ecef_speed); state.speed_status |= (1 << SPEED_ECEF_F); } - if (ned_speed != 0) { + if (ned_speed != NULL) { VECT3_COPY(state.ned_speed_f, *ned_speed); state.speed_status |= (1 << SPEED_NED_F); } - if (enu_speed != 0) { + if (enu_speed != NULL) { VECT3_COPY(state.enu_speed_f, *enu_speed); state.speed_status |= (1 << SPEED_ENU_F); } From a6219ccebdf830168c6e3173667765b7ff763159 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Mon, 23 Jul 2012 11:16:00 +0200 Subject: [PATCH 11/62] [ahrs] fix macro in ahrs.h --- sw/airborne/subsystems/ahrs.h | 1 + 1 file changed, 1 insertion(+) diff --git a/sw/airborne/subsystems/ahrs.h b/sw/airborne/subsystems/ahrs.h index 6985698c2c..1eafd751e6 100644 --- a/sw/airborne/subsystems/ahrs.h +++ b/sw/airborne/subsystems/ahrs.h @@ -99,6 +99,7 @@ extern float ahrs_mag_offset; EULERS_BFP_OF_REAL(ahrs.ltp_to_imu_euler, ahrs_float.ltp_to_imu_euler); \ RMAT_BFP_OF_REAL(ahrs.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_rmat); \ RATES_BFP_OF_REAL(ahrs.imu_rate, ahrs_float.imu_rate); \ +} /* copy attitude to state interface */ #define AHRS_BODY_TO_STATE() { \ From 767370e701614e00c61bce94a0957454b9601f9d Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Mon, 23 Jul 2012 16:33:59 +0200 Subject: [PATCH 12/62] [state interface] convert euler and quat int_cmpl ahrs filters --- .../subsystems/ahrs/ahrs_int_cmpl_euler.c | 37 +++------ .../subsystems/ahrs/ahrs_int_cmpl_quat.c | 83 +++++++------------ 2 files changed, 40 insertions(+), 80 deletions(-) diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c index 4ec9a24337..dc0ed746b4 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c @@ -21,6 +21,7 @@ #include "ahrs_int_cmpl_euler.h" +#include "state.h" #include "subsystems/imu.h" #include "subsystems/ahrs/ahrs_aligner.h" #include "math/pprz_trig_int.h" @@ -37,8 +38,7 @@ struct AhrsIntCmplEuler ahrs_impl; static inline void get_phi_theta_measurement_fom_accel(int32_t* phi_meas, int32_t* theta_meas, struct Int32Vect3 accel); static inline void get_psi_measurement_from_mag(int32_t* psi_meas, int32_t phi_est, int32_t theta_est, struct Int32Vect3 mag); -static inline void compute_imu_quat_and_rmat_from_euler(void); -static inline void compute_body_orientation(void); +static inline void set_body_state_from_euler(void); #define F_UPDATE 512 @@ -86,9 +86,7 @@ void ahrs_align(void) { /* Compute LTP to IMU eulers */ EULERS_SDIV(ahrs.ltp_to_imu_euler, ahrs_impl.hi_res_euler, F_UPDATE); - compute_imu_quat_and_rmat_from_euler(); - - compute_body_orientation(); + set_body_state_from_euler(); RATES_COPY( ahrs_impl.gyro_bias, ahrs_aligner.lp_gyro); ahrs.status = AHRS_RUNNING; @@ -191,9 +189,7 @@ void ahrs_propagate(void) { /* Compute LTP to IMU eulers */ EULERS_SDIV(ahrs.ltp_to_imu_euler, ahrs_impl.hi_res_euler, F_UPDATE); - compute_imu_quat_and_rmat_from_euler(); - - compute_body_orientation(); + set_body_state_from_euler(); } @@ -269,34 +265,23 @@ __attribute__ ((always_inline)) static inline void get_psi_measurement_from_mag( } -/* Compute ltp to imu rotation in quaternion and rotation matrice representation - from the euler angle representation */ -__attribute__ ((always_inline)) static inline void compute_imu_quat_and_rmat_from_euler(void) { - - /* Compute LTP to IMU quaternion */ - INT32_QUAT_OF_EULERS(ahrs.ltp_to_imu_quat, ahrs.ltp_to_imu_euler); +/* Rotate angles and rates from imu to body frame and set state */ +__attribute__ ((always_inline)) static inline void set_body_state_from_euler(void) { /* Compute LTP to IMU rotation matrix */ INT32_RMAT_OF_EULERS(ahrs.ltp_to_imu_rmat, ahrs.ltp_to_imu_euler); - -} - -__attribute__ ((always_inline)) static inline void compute_body_orientation(void) { - - /* Compute LTP to BODY quaternion */ - INT32_QUAT_COMP_INV(ahrs.ltp_to_body_quat, ahrs.ltp_to_imu_quat, imu.body_to_imu_quat); /* Compute LTP to BODY rotation matrix */ INT32_RMAT_COMP_INV(ahrs.ltp_to_body_rmat, ahrs.ltp_to_imu_rmat, imu.body_to_imu_rmat); - /* compute LTP to BODY eulers */ - INT32_EULERS_OF_RMAT(ahrs.ltp_to_body_euler, ahrs.ltp_to_body_rmat); + /* Set state */ + stateSetNedToBodyRMat_i(&ahrs.ltp_to_body_rmat); + /* compute body rates */ INT32_RMAT_TRANSP_RATEMULT(ahrs.body_rate, imu.body_to_imu_rmat, ahrs.imu_rate); - - AHRS_BODY_TO_STATE(); + /* Set state */ + stateSetBodyRates_i(&ahrs.body_rate); } - #ifdef AHRS_UPDATE_FW_ESTIMATOR // TODO use ahrs result directly #include "estimator.h" diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c index 0455ebd98d..eff2aa0011 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -24,6 +24,8 @@ #include "subsystems/ahrs/ahrs_aligner.h" #include "subsystems/ahrs/ahrs_int_utils.h" +#include "state.h" + #include "subsystems/imu.h" #if USE_GPS #include "subsystems/gps.h" @@ -52,10 +54,8 @@ static inline void ahrs_update_mag_2d(void); struct AhrsIntCmpl ahrs_impl; -static inline void compute_imu_euler_and_rmat_from_quat(void); -static inline void compute_body_euler_and_rmat_from_quat(void); +static inline void set_body_state_from_quat(void); static inline void compute_imu_orientation(void); -static inline void compute_body_orientation(void); void ahrs_init(void) { @@ -105,10 +105,7 @@ void ahrs_align(void) { ahrs_impl.heading_aligned = FALSE; #endif - /* Convert initial orientation from quat to euler and rotation matrix representations. */ - compute_imu_euler_and_rmat_from_quat(); - - compute_body_orientation(); + set_body_state_from_quat(); /* Use low passed gyro value as initial bias */ RATES_COPY( ahrs_impl.gyro_bias, ahrs_aligner.lp_gyro); @@ -149,9 +146,7 @@ void ahrs_propagate(void) { INT32_QUAT_INTEGRATE_FI(ahrs.ltp_to_imu_quat, ahrs_impl.high_rez_quat, omega, AHRS_PROPAGATE_FREQUENCY); INT32_QUAT_NORMALIZE(ahrs.ltp_to_imu_quat); - compute_imu_euler_and_rmat_from_quat(); - - compute_body_orientation(); + set_body_state_from_quat(); } @@ -352,9 +347,10 @@ void ahrs_update_heading(int32_t heading) { // row 0 of ltp_to_body_rmat = body x-axis in ltp frame // we only consider x and y + struct Int32RMat* ltp_to_body_rmat = stateGetNedToBodyRMat_i(); struct Int32Vect2 expected_ltp = - { RMAT_ELMT(ahrs.ltp_to_body_rmat, 0, 0), - RMAT_ELMT(ahrs.ltp_to_body_rmat, 0, 1) }; + { RMAT_ELMT((*ltp_to_body_rmat), 0, 0), + RMAT_ELMT((*ltp_to_body_rmat), 0, 1) }; int32_t heading_x, heading_y; PPRZ_ITRIG_COS(heading_x, heading); // measured course in x-direction @@ -424,8 +420,8 @@ void ahrs_realign_heading(int32_t heading) { INT32_QUAT_COMP_NORM_SHORTEST(q, q_c, ahrs.ltp_to_body_quat); QUAT_COPY(ahrs.ltp_to_body_quat, q); - /* compute other representations in body frame */ - compute_body_euler_and_rmat_from_quat(); + /* Set state */ + stateSetNedToBodyQuat_i(&ahrs.ltp_to_body_quat); /* compute ltp to imu rotations */ compute_imu_orientation(); @@ -434,39 +430,20 @@ void ahrs_realign_heading(int32_t heading) { } -/* Compute ltp to imu rotation in euler angles and rotation matrix representation - from the quaternion representation */ -__attribute__ ((always_inline)) static inline void compute_imu_euler_and_rmat_from_quat(void) { - - /* Compute LTP to IMU euler */ - INT32_EULERS_OF_QUAT(ahrs.ltp_to_imu_euler, ahrs.ltp_to_imu_quat); - /* Compute LTP to IMU rotation matrix */ +/* Rotate angles and rates from imu to body frame and set state */ +__attribute__ ((always_inline)) static inline void set_body_state_from_quat(void) { + /* Compute LTP to IMU rotation matrix (internal use) */ INT32_RMAT_OF_QUAT(ahrs.ltp_to_imu_rmat, ahrs.ltp_to_imu_quat); -} - -/* Compute ltp to body rotation in euler angles and rotation matrix representation - from the quaternion representation */ -__attribute__ ((always_inline)) static inline void compute_body_euler_and_rmat_from_quat(void) { - - /* Compute LTP to body euler */ - INT32_EULERS_OF_QUAT(ahrs.ltp_to_body_euler, ahrs.ltp_to_body_quat); - /* Compute LTP to body rotation matrix */ - INT32_RMAT_OF_QUAT(ahrs.ltp_to_body_rmat, ahrs.ltp_to_body_quat); - -} - -__attribute__ ((always_inline)) static inline void compute_body_orientation(void) { - /* Compute LTP to BODY quaternion */ INT32_QUAT_COMP_INV(ahrs.ltp_to_body_quat, ahrs.ltp_to_imu_quat, imu.body_to_imu_quat); - /* Compute LTP to BODY rotation matrix */ - INT32_RMAT_COMP_INV(ahrs.ltp_to_body_rmat, ahrs.ltp_to_imu_rmat, imu.body_to_imu_rmat); - /* compute LTP to BODY eulers */ - INT32_EULERS_OF_RMAT(ahrs.ltp_to_body_euler, ahrs.ltp_to_body_rmat); + /* Set state */ + stateSetNedToBodyQuat_i(&ahrs.ltp_to_body_quat); + /* compute body rates */ INT32_RMAT_TRANSP_RATEMULT(ahrs.body_rate, imu.body_to_imu_rmat, ahrs.imu_rate); - + /* Set state */ + stateSetBodyRates_i(&ahrs.body_rate); } __attribute__ ((always_inline)) static inline void compute_imu_orientation(void) { @@ -474,9 +451,7 @@ __attribute__ ((always_inline)) static inline void compute_imu_orientation(void) /* Compute LTP to IMU quaternion */ INT32_QUAT_COMP(ahrs.ltp_to_imu_quat, ahrs.ltp_to_body_quat, imu.body_to_imu_quat); /* Compute LTP to IMU rotation matrix */ - INT32_RMAT_COMP(ahrs.ltp_to_imu_rmat, ahrs.ltp_to_body_rmat, imu.body_to_imu_rmat); - /* compute LTP to IMU eulers */ - INT32_EULERS_OF_RMAT(ahrs.ltp_to_imu_euler, ahrs.ltp_to_imu_rmat); + INT32_RMAT_OF_QUAT(ahrs.ltp_to_imu_rmat, ahrs.ltp_to_imu_quat); /* compute IMU rates */ INT32_RMAT_RATEMULT(ahrs.imu_rate, imu.body_to_imu_rmat, ahrs.body_rate); @@ -497,19 +472,19 @@ float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; void ahrs_update_fw_estimator(void) { - struct FloatEulers att; + struct FloatEulers* att; // export results to estimator - EULERS_FLOAT_OF_BFP(att, ahrs.ltp_to_body_euler); + att = stateGetNedToBodyEulers_f(); - estimator_phi = att.phi - ins_roll_neutral; - estimator_theta = att.theta - ins_pitch_neutral; - estimator_psi = att.psi; + estimator_phi = att->phi - ins_roll_neutral; + estimator_theta = att->theta - ins_pitch_neutral; + estimator_psi = att->psi; - struct FloatRates rates; - RATES_FLOAT_OF_BFP(rates, ahrs.body_rate); - estimator_p = rates.p; - estimator_q = rates.q; - estimator_r = rates.r; + struct FloatRates* rates; + rates = stateGetBodyRates_f(); + estimator_p = rates->p; + estimator_q = rates->q; + estimator_r = rates->r; } #endif //AHRS_UPDATE_FW_ESTIMATOR From 084ba681e3af03e24dbacf6316e4372d38d464ca Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Mon, 23 Jul 2012 18:10:50 +0200 Subject: [PATCH 13/62] [state interface] update rotorcraft ins to new state interface --- sw/airborne/subsystems/ins.c | 2 +- sw/airborne/subsystems/ins/hf_float.c | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/sw/airborne/subsystems/ins.c b/sw/airborne/subsystems/ins.c index 08b5735ed3..d69d018fd2 100644 --- a/sw/airborne/subsystems/ins.c +++ b/sw/airborne/subsystems/ins.c @@ -139,7 +139,7 @@ void ins_propagate() { struct Int32Vect3 accel_meas_body; INT32_RMAT_TRANSP_VMULT(accel_meas_body, imu.body_to_imu_rmat, imu.accel); struct Int32Vect3 accel_meas_ltp; - INT32_RMAT_TRANSP_VMULT(accel_meas_ltp, ahrs.ltp_to_body_rmat, accel_meas_body); + INT32_RMAT_TRANSP_VMULT(accel_meas_ltp, (*stateGetNedToBodyRMat_i()), accel_meas_body); #if USE_VFF float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z); diff --git a/sw/airborne/subsystems/ins/hf_float.c b/sw/airborne/subsystems/ins/hf_float.c index f9cdcc78bc..bc0adaaeaf 100644 --- a/sw/airborne/subsystems/ins/hf_float.c +++ b/sw/airborne/subsystems/ins/hf_float.c @@ -25,7 +25,7 @@ #include "subsystems/ins/hf_float.h" #include "subsystems/ins.h" #include "subsystems/imu.h" -#include "subsystems/ahrs.h" +#include "state.h" #include "subsystems/gps.h" #include @@ -440,7 +440,8 @@ void b2_hff_propagate(void) { /* compute float ltp mean acceleration */ b2_hff_compute_accel_body_mean(HFF_PRESCALER); struct Int32Vect3 mean_accel_ltp; - INT32_RMAT_TRANSP_VMULT(mean_accel_ltp, ahrs.ltp_to_body_rmat, acc_body_mean); + struct Int32RMat* ltp_to_body_rmat = stateGetNedToBodyRMat_i(); + INT32_RMAT_TRANSP_VMULT(mean_accel_ltp, (*ltp_to_body_rmat), acc_body_mean); b2_hff_xdd_meas = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.x); b2_hff_ydd_meas = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.y); #ifdef GPS_LAG From 5450ba7774ec1d502050956a9ddba59ce4ef842a Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 24 Jul 2012 11:14:28 +0200 Subject: [PATCH 14/62] [stab rate] convert stabilization in rate mode to new state interface --- .../firmwares/rotorcraft/stabilization/stabilization_rate.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c index 79c2328e3a..b1f39edc4f 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c @@ -28,7 +28,7 @@ #include "firmwares/rotorcraft/stabilization.h" -#include "subsystems/ahrs.h" +#include "state.h" #include "subsystems/imu.h" #include "subsystems/radio_control.h" @@ -193,7 +193,8 @@ void stabilization_rate_run(bool_t in_flight) { OFFSET_AND_ROUND(stabilization_rate_ref.q, (REF_FRAC - INT32_RATE_FRAC)), OFFSET_AND_ROUND(stabilization_rate_ref.r, (REF_FRAC - INT32_RATE_FRAC)) }; struct Int32Rates _error; - RATES_DIFF(_error, _ref_scaled, ahrs.body_rate); + struct Int32Rates* body_rate = stateGetBodyRates_i(); + RATES_DIFF(_error, _ref_scaled, (*body_rate)); if (in_flight) { /* update integrator */ RATES_ADD(stabilization_rate_sum_err, _error); From c57e6c704ec832392eaa46a5c8b8d8381b8c4c4a Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 24 Jul 2012 11:15:12 +0200 Subject: [PATCH 15/62] [stab att] converte stab attitude euler to new state interface --- .../stabilization/stabilization_attitude_euler_int.c | 10 ++++++---- .../stabilization/stabilization_attitude_rc_setpoint.h | 4 ++-- .../stabilization/stabilization_attitude_ref_int.h | 4 ++-- 3 files changed, 10 insertions(+), 8 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c index a0d42477d3..c204263705 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c @@ -20,7 +20,7 @@ */ #include "firmwares/rotorcraft/stabilization.h" -#include "subsystems/ahrs.h" +#include "state.h" #include "subsystems/radio_control.h" #include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h" @@ -87,7 +87,7 @@ void stabilization_attitude_read_rc(bool_t in_flight) { void stabilization_attitude_enter(void) { - stab_att_sp_euler.psi = ahrs.ltp_to_body_euler.psi; + stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi; reset_psi_ref_from_body(); INT_EULERS_ZERO( stabilization_att_sum_err ); @@ -120,7 +120,8 @@ void stabilization_attitude_run(bool_t in_flight) { OFFSET_AND_ROUND(stab_att_ref_euler.theta, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)), OFFSET_AND_ROUND(stab_att_ref_euler.psi, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)) }; struct Int32Eulers att_err; - EULERS_DIFF(att_err, att_ref_scaled, ahrs.ltp_to_body_euler); + struct Int32Eulers* ltp_to_body_euler = stateGetNedToBodyEulers_i(); + EULERS_DIFF(att_err, att_ref_scaled, (*ltp_to_body_euler)); INT32_ANGLE_NORMALIZE(att_err.psi); if (in_flight) { @@ -138,7 +139,8 @@ void stabilization_attitude_run(bool_t in_flight) { OFFSET_AND_ROUND(stab_att_ref_rate.q, (REF_RATE_FRAC - INT32_RATE_FRAC)), OFFSET_AND_ROUND(stab_att_ref_rate.r, (REF_RATE_FRAC - INT32_RATE_FRAC)) }; struct Int32Rates rate_err; - RATES_DIFF(rate_err, rate_ref_scaled, ahrs.body_rate); + struct Int32Rates* body_rate = stateGetBodyRates_i(); + RATES_DIFF(rate_err, rate_ref_scaled, (*body_rate)); /* PID */ stabilization_att_fb_cmd[COMMAND_ROLL] = diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h index 03ce96df9b..f1a5107ca0 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h @@ -32,7 +32,7 @@ #include "math/pprz_algebra_float.h" #include "subsystems/radio_control.h" -#include "subsystems/ahrs.h" +#include "state.h" #ifdef STABILISATION_ATTITUDE_TYPE_INT #define SP_MAX_PHI (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI) @@ -80,7 +80,7 @@ static inline void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eu } } else { /* if not flying, use current yaw as setpoint */ - sp->psi = ahrs.ltp_to_body_euler.psi; + sp->psi = stateGetNedToBodyEulers_i()->psi; } } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h index 8026b06828..5be7bae51e 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h @@ -29,7 +29,7 @@ #include "math/pprz_algebra_int.h" -#include "subsystems/ahrs.h" +#include "state.h" extern struct Int32Eulers stab_att_sp_euler; ///< with #INT32_ANGLE_FRAC extern struct Int32Quat stab_att_sp_quat; ///< with #INT32_QUAT_FRAC @@ -58,7 +58,7 @@ extern struct Int32RefModel stab_att_ref_model; static inline void reset_psi_ref_from_body(void) { - stab_att_ref_euler.psi = ahrs.ltp_to_body_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); + stab_att_ref_euler.psi = stateGetNedToBodyEulers_i()->psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); stab_att_ref_rate.r = 0; stab_att_ref_accel.r = 0; } From d5983a4e820cf91f689c7001711e5c8c6a1508b6 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 24 Jul 2012 11:22:27 +0200 Subject: [PATCH 16/62] [stab att] convert stab attitude euler float --- .../stabilization_attitude_euler_float.c | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c index 6139806670..4ca9c4aded 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c @@ -24,7 +24,7 @@ #include "firmwares/rotorcraft/stabilization.h" #include "math/pprz_algebra_float.h" -#include "subsystems/ahrs.h" +#include "state.h" #include "subsystems/radio_control.h" #include "generated/airframe.h" @@ -111,10 +111,9 @@ void stabilization_attitude_run(bool_t in_flight) { /* Compute feedback */ /* attitude error */ - struct FloatEulers att_float; - EULERS_FLOAT_OF_BFP(att_float, ahrs.ltp_to_body_euler); + struct FloatEulers att_float* = stateGetNedToBodyEulers_f(); struct FloatEulers att_err; - EULERS_DIFF(att_err, stab_att_ref_euler, att_float); + EULERS_DIFF(att_err, stab_att_ref_euler, *att_float); FLOAT_ANGLE_NORMALIZE(att_err.psi); if (in_flight) { @@ -127,10 +126,9 @@ void stabilization_attitude_run(bool_t in_flight) { } /* rate error */ - struct FloatRates rate_float; - RATES_FLOAT_OF_BFP(rate_float, ahrs.body_rate); + struct FloatRates* rate_float = stateGetBodyRates_f(); struct FloatRates rate_err; - RATES_DIFF(rate_err, stab_att_ref_rate, rate_float); + RATES_DIFF(rate_err, stab_att_ref_rate, *rate_float); /* PID */ From c633d347b8ff2c9097d76945d3fd81aa50d2f922 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 24 Jul 2012 11:34:32 +0200 Subject: [PATCH 17/62] [stab att] convert attitude ref float --- .../stabilization/stabilization_attitude_ref_euler_float.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h index bd3a6d2a49..c45c8292c2 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h @@ -59,7 +59,7 @@ } \ } \ else { /* if not flying, use current yaw as setpoint */ \ - _sp.psi = ANGLE_FLOAT_OF_BFP(ahrs.ltp_to_body_euler.psi); \ + _sp.psi = stateGetNedToBodyEulers_f()->psi; \ } \ } From b2b006ec6fd486f55b609c10b86cdf80512d7a85 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 24 Jul 2012 11:40:28 +0200 Subject: [PATCH 18/62] [stab att] convert stab attitude quat to new state interface --- .../rotorcraft/stabilization/quat_setpoint_int.c | 6 +++--- .../stabilization/stabilization_attitude_quat_int.c | 12 +++++++----- .../stabilization_attitude_ref_quat_int.c | 1 - 3 files changed, 10 insertions(+), 9 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint_int.c index 173fe3454f..ac14499dcf 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint_int.c @@ -9,7 +9,7 @@ * */ -#include "subsystems/ahrs.h" +#include "state.h" #include "stabilization/stabilization_attitude_ref_quat_int.h" #include "stabilization/quat_setpoint_int.h" @@ -94,7 +94,7 @@ void stabilization_attitude_read_rc_absolute(bool_t in_flight) { // update setpoint by rotating by incremental yaw command INT32_QUAT_COMP_NORM_SHORTEST(stab_att_sp_quat, prev_sp_quat, sticks_quat); } else { /* if not flying, use current body position + pitch/yaw from sticks to compose setpoint */ - reset_sp_quat(RATE_BFP_OF_REAL(yaw * YAW_COEF), RATE_BFP_OF_REAL(pitch * PITCH_COEF), &ahrs.ltp_to_body_quat); + reset_sp_quat(RATE_BFP_OF_REAL(yaw * YAW_COEF), RATE_BFP_OF_REAL(pitch * PITCH_COEF), stateGetNedToBodyQuat_i()); } // update euler setpoints for telemetry @@ -104,5 +104,5 @@ void stabilization_attitude_read_rc_absolute(bool_t in_flight) { void stabilization_attitude_sp_enter(void) { // reset setpoint to "hover" - reset_sp_quat(0., 0., &ahrs.ltp_to_body_quat); + reset_sp_quat(0., 0., stateGetNedToBodyQuat_i()); } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c index 1aec849712..d2f14e8c0e 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c @@ -33,7 +33,7 @@ #include #include "math/pprz_algebra_float.h" #include "math/pprz_algebra_int.h" -#include "subsystems/ahrs.h" +#include "state.h" #include "generated/airframe.h" struct Int32AttitudeGains stabilization_gains = { @@ -80,7 +80,7 @@ void stabilization_attitude_enter(void) { #if !USE_SETPOINTS_WITH_TRANSITIONS /* reset psi setpoint to current psi angle */ - stab_att_sp_euler.psi = ahrs.ltp_to_body_euler.psi; + stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi; #endif stabilization_attitude_ref_enter(); @@ -135,14 +135,16 @@ void stabilization_attitude_run(bool_t enable_integrator) { /* attitude error */ struct Int32Quat att_err; - INT32_QUAT_INV_COMP(att_err, ahrs.ltp_to_body_quat, stab_att_ref_quat); + struct Int32Quat* att_quat = stateGetNedToBodyQuat_i(); + INT32_QUAT_INV_COMP(att_err, *att_quat, stab_att_ref_quat); /* wrap it in the shortest direction */ INT32_QUAT_WRAP_SHORTEST(att_err); INT32_QUAT_NORMALIZE(att_err); /* rate error */ struct Int32Rates rate_err; - RATES_DIFF(rate_err, stab_att_ref_rate, ahrs.body_rate); + struct Int32Rates* body_rate = stateGetBodyRates_i(); + RATES_DIFF(rate_err, stab_att_ref_rate, *body_rate); /* integrated error */ if (enable_integrator) { @@ -194,7 +196,7 @@ void stabilization_attitude_read_rc(bool_t in_flight) { /* get current heading */ const struct FloatVect3 zaxis = {0., 0., 1.}; struct FloatQuat q_yaw; - FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw, zaxis, ANGLE_FLOAT_OF_BFP(ahrs.ltp_to_body_euler.psi)); + FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw, zaxis, ANGLE_FLOAT_OF_BFP(stateGetNedToBodyEulers_i()->psi)); /* apply roll and pitch commands with respect to current heading */ struct FloatQuat q_sp; diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c index c49529e168..7352be5239 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c @@ -26,7 +26,6 @@ #include "generated/airframe.h" #include "firmwares/rotorcraft/stabilization.h" -#include "subsystems/ahrs.h" #include "stabilization_attitude_ref_int.h" From d7313aae2a42f27a0c9fbebd8ea7c08c256f6561 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 24 Jul 2012 13:29:10 +0200 Subject: [PATCH 19/62] [stab att] convert attitude quat float body_rate_d is computed by the controller now, but it is not divided by the period yet, so the gain may change for different frequencies. it was not really computed before anyway... I doubt someone was really using this. --- .../stabilization_attitude_quat_float.c | 17 +++++++++++++---- .../stabilization_attitude_ref_quat_float.c | 4 ++-- 2 files changed, 15 insertions(+), 6 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c index 011a486ae1..71d64b961b 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c @@ -30,7 +30,7 @@ #include #include "math/pprz_algebra_float.h" #include "math/pprz_algebra_int.h" -#include "subsystems/ahrs.h" +#include "state.h" #include "generated/airframe.h" #include "stabilization_attitude_float.h" #include "stabilization_attitude_rc_setpoint.h" @@ -40,6 +40,8 @@ struct FloatAttitudeGains stabilization_gains[STABILIZATION_ATTITUDE_FLOAT_GAIN_ struct FloatQuat stabilization_att_sum_err_quat; struct FloatEulers stabilization_att_sum_err_eulers; +struct FloatRates last_body_rate; + float stabilization_att_fb_cmd[COMMANDS_NB]; float stabilization_att_ff_cmd[COMMANDS_NB]; @@ -101,6 +103,7 @@ void stabilization_attitude_init(void) { FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat ); FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers ); + FLOAT_RAYES_ZERO( last_body_rate ); } void stabilization_attitude_gain_schedule(uint8_t idx) @@ -197,13 +200,19 @@ void stabilization_attitude_run(bool_t enable_integrator) { /* attitude error */ struct FloatQuat att_err; - FLOAT_QUAT_INV_COMP(att_err, ahrs_float.ltp_to_body_quat, stab_att_ref_quat); + struct FloatQuat* att_quat = stateGetNedToBodyQuat_f(); + FLOAT_QUAT_INV_COMP(att_err, *att_quat, stab_att_ref_quat); /* wrap it in the shortest direction */ FLOAT_QUAT_WRAP_SHORTEST(att_err); /* rate error */ struct FloatRates rate_err; - RATES_DIFF(rate_err, stab_att_ref_rate, ahrs_float.body_rate); + struct FloatRates* body_rate = stateGetBodyRates_f(); + RATES_DIFF(rate_err, stab_att_ref_rate, *body_rate); + /* rate_d error */ + struct FloatRates body_rate_d; + RATES_DIFF(body_rate_d, *body_rate, last_body_rate); + RATES_COPY(last_body_rate, *body_rate); /* integrated error */ if (enable_integrator) { @@ -225,7 +234,7 @@ void stabilization_attitude_run(bool_t enable_integrator) { attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains[gain_idx], &stab_att_ref_accel); - attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains[gain_idx], &att_err, &rate_err, &ahrs_float.body_rate_d, &stabilization_att_sum_err_quat); + attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains[gain_idx], &att_err, &rate_err, body_rate_d, &stabilization_att_sum_err_quat); // FIXME: this is very dangerous! only works if this really includes all commands for (int i = COMMAND_ROLL; i <= COMMAND_YAW_SURFACE; i++) { diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c index f31f758b4c..09a2891acc 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c @@ -28,7 +28,7 @@ #include "generated/airframe.h" #include "firmwares/rotorcraft/stabilization.h" -#include "subsystems/ahrs.h" +#include "state.h" #include "stabilization_attitude_ref_float.h" @@ -55,7 +55,7 @@ static const float omega_r[] = STABILIZATION_ATTITUDE_FLOAT_REF_OMEGA_R; static const float zeta_r[] = STABILIZATION_ATTITUDE_FLOAT_REF_ZETA_R; static void reset_psi_ref_from_body(void) { - stab_att_ref_euler.psi = ahrs_float.ltp_to_body_euler.psi; + stab_att_ref_euler.psi = stateGetNedToBodyEulers_f()->psi; stab_att_ref_rate.r = 0; stab_att_ref_accel.r = 0; } From 472ac96fbf2e21a3686ab0363e3f5f19c7f7cd01 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 24 Jul 2012 14:48:54 +0200 Subject: [PATCH 20/62] [guidance] convert rotorcraft guidance to new state interface --- .../firmwares/rotorcraft/guidance/guidance_h.c | 15 ++++++++------- .../firmwares/rotorcraft/guidance/guidance_v.c | 6 +++--- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index 286235e0b8..c7189d4918 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -30,10 +30,10 @@ #include "firmwares/rotorcraft/stabilization.h" #include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h" -#include "subsystems/ahrs.h" -#include "subsystems/ins.h" #include "firmwares/rotorcraft/navigation.h" +#include "state.h" + #include "generated/airframe.h" uint8_t guidance_h_mode; @@ -209,7 +209,7 @@ void guidance_h_run(bool_t in_flight) { stab_att_sp_euler.phi = nav_roll; stab_att_sp_euler.theta = nav_pitch; /* FIXME: heading can't be set via attitude block yet, use current heading for now */ - stab_att_sp_euler.psi = ahrs.ltp_to_body_euler.psi; + stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi; #ifdef STABILISATION_ATTITUDE_TYPE_QUAT INT32_QUAT_OF_EULERS(stab_att_sp_quat, stab_att_sp_euler); INT32_QUAT_WRAP_SHORTEST(stab_att_sp_quat); @@ -300,8 +300,9 @@ static inline void guidance_h_traj_run(bool_t in_flight) { /* Rotate to body frame */ int32_t s_psi, c_psi; - PPRZ_ITRIG_SIN(s_psi, ahrs.ltp_to_body_euler.psi); - PPRZ_ITRIG_COS(c_psi, ahrs.ltp_to_body_euler.psi); + int32_t psi = stateGetNedToBodyEulers_i()->psi; + PPRZ_ITRIG_SIN(s_psi, psi); + PPRZ_ITRIG_COS(c_psi, psi); // Restore angle ref resolution after rotation guidance_h_command_body.phi = @@ -328,7 +329,7 @@ static inline void guidance_h_hover_enter(void) { VECT2_COPY(guidance_h_pos_sp, *stateGetPositionNed_i()); - guidance_h_rc_sp.psi = ahrs.ltp_to_body_euler.psi; + guidance_h_rc_sp.psi = stateGetNedToBodyEulers_i()->psi; reset_psi_ref_from_body(); INT_VECT2_ZERO(guidance_h_pos_err_sum); @@ -346,7 +347,7 @@ static inline void guidance_h_nav_enter(void) { /* reset psi reference, set psi setpoint to current psi */ reset_psi_ref_from_body(); - nav_heading = ahrs.ltp_to_body_euler.psi; + nav_heading = stateGetNedToBodyEulers_i()->psi; INT_VECT2_ZERO(guidance_h_pos_err_sum); diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index 44ab30602b..7664543650 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -31,7 +31,6 @@ #include "subsystems/radio_control.h" #include "firmwares/rotorcraft/stabilization.h" -#include "subsystems/ahrs.h" // #include "booz_fms.h" FIXME #include "firmwares/rotorcraft/navigation.h" @@ -285,8 +284,9 @@ __attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flig guidance_v_ff_cmd = g_m_zdd / inv_m; int32_t cphi,ctheta,cphitheta; - PPRZ_ITRIG_COS(cphi, ahrs.ltp_to_body_euler.phi); - PPRZ_ITRIG_COS(ctheta, ahrs.ltp_to_body_euler.theta); + struct Int32Eulers* att_euler = stateGetNedToBodyEulers_i(); + PPRZ_ITRIG_COS(cphi, att_euler->phi); + PPRZ_ITRIG_COS(ctheta, att_euler->theta); cphitheta = (cphi * ctheta) >> INT32_TRIG_FRAC; if (cphitheta < MAX_BANK_COEF) cphitheta = MAX_BANK_COEF; /* feed forward command */ From 2cea989197a5de4a204f81bfad3a7a146c69a23e Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 24 Jul 2012 15:03:44 +0200 Subject: [PATCH 21/62] [ground detect] convert to new state interface --- sw/airborne/firmwares/rotorcraft/autopilot.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.h b/sw/airborne/firmwares/rotorcraft/autopilot.h index 6c2f4cb13f..d3fbfb1df3 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.h +++ b/sw/airborne/firmwares/rotorcraft/autopilot.h @@ -114,8 +114,9 @@ extern uint16_t autopilot_flight_time; #endif static inline void DetectGroundEvent(void) { if (autopilot_mode == AP_MODE_FAILSAFE || autopilot_detect_ground_once) { - if (ins_ltp_accel.z < -THRESHOLD_GROUND_DETECT || - ins_ltp_accel.z > THRESHOLD_GROUND_DETECT) { + struct NedCoor_i* accel = stateGetAccelNed_i(); + if (accel->z < -THRESHOLD_GROUND_DETECT || + accel->z > THRESHOLD_GROUND_DETECT) { autopilot_detect_ground = TRUE; autopilot_detect_ground_once = FALSE; } From d054afb5577d07b788663ad8c15758abdae08b7d Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 24 Jul 2012 15:18:33 +0200 Subject: [PATCH 22/62] [cam control] use state interface in rotorcraft cam control --- sw/airborne/modules/cam_control/booz_cam.c | 5 ++--- sw/airborne/modules/cam_control/cam_track.c | 17 +++++++++++------ 2 files changed, 13 insertions(+), 9 deletions(-) diff --git a/sw/airborne/modules/cam_control/booz_cam.c b/sw/airborne/modules/cam_control/booz_cam.c index 20594d2ea3..ddd5dd3451 100644 --- a/sw/airborne/modules/cam_control/booz_cam.c +++ b/sw/airborne/modules/cam_control/booz_cam.c @@ -24,9 +24,8 @@ #include "cam_control/booz_cam.h" #include "modules/core/booz_pwm_arch.h" -#include "subsystems/ahrs.h" +#include "state.h" #include "firmwares/rotorcraft/navigation.h" -#include "subsystems/ins.h" #include "generated/flight_plan.h" #include "std.h" @@ -103,7 +102,7 @@ void booz_cam_periodic(void) { booz_cam_tilt_pwm = BOOZ_CAM_TILT_NEUTRAL; #endif #ifdef BOOZ_CAM_USE_PAN - booz_cam_pan = ahrs.ltp_to_body_euler.psi; + booz_cam_pan = stateGetNedToBodyEulers_i()->psi; #endif break; case BOOZ_CAM_MODE_MANUAL: diff --git a/sw/airborne/modules/cam_control/cam_track.c b/sw/airborne/modules/cam_control/cam_track.c index 78167c838a..055123659b 100644 --- a/sw/airborne/modules/cam_control/cam_track.c +++ b/sw/airborne/modules/cam_control/cam_track.c @@ -25,7 +25,7 @@ #include "cam_track.h" #include "subsystems/ins.h" -#include "subsystems/ahrs.h" +#include "state.h" #if USE_HFF #include "subsystems/ins/hf_float.h" @@ -72,7 +72,8 @@ void track_periodic_task(void) { cmd_msg[c++] = 'A'; cmd_msg[c++] = ' '; - float phi = ANGLE_FLOAT_OF_BFP(ahrs.ltp_to_body_euler.phi); + struct FloatEulers* att = stateGetNedToBodyEulers_f(); + float phi = att->phi; if (phi > 0) cmd_msg[c++] = ' '; else { cmd_msg[c++] = '-'; phi = -phi; } cmd_msg[c++] = '0' + ((unsigned int) phi % 10); @@ -81,7 +82,7 @@ void track_periodic_task(void) { cmd_msg[c++] = '0' + ((unsigned int) (1000*phi) % 10); cmd_msg[c++] = '0' + ((unsigned int) (10000*phi) % 10); cmd_msg[c++] = ' '; - float theta = ANGLE_FLOAT_OF_BFP(ahrs.ltp_to_body_euler.theta); + float theta = att->theta; if (theta > 0) cmd_msg[c++] = ' '; else { cmd_msg[c++] = '-'; theta = -theta; } cmd_msg[c++] = '0' + ((unsigned int) theta % 10); @@ -90,7 +91,7 @@ void track_periodic_task(void) { cmd_msg[c++] = '0' + ((unsigned int) (1000*theta) % 10); cmd_msg[c++] = '0' + ((unsigned int) (10000*theta) % 10); cmd_msg[c++] = ' '; - float psi = ANGLE_FLOAT_OF_BFP(ahrs.ltp_to_body_euler.psi); + float psi = att->psi; if (psi > 0) cmd_msg[c++] = ' '; else { cmd_msg[c++] = '-'; psi = -psi; } cmd_msg[c++] = '0' + ((unsigned int) psi % 10); @@ -99,7 +100,7 @@ void track_periodic_task(void) { cmd_msg[c++] = '0' + ((unsigned int) (1000*psi) % 10); cmd_msg[c++] = '0' + ((unsigned int) (10000*psi) % 10); cmd_msg[c++] = ' '; - float alt = -POS_FLOAT_OF_BFP(ins_ltp_pos.z); + float alt = stateGetPositionEnu_f()->z; //alt = 0.40; if (alt > 0) cmd_msg[c++] = ' '; else { cmd_msg[c++] = '-'; alt = -alt; } @@ -133,7 +134,7 @@ void track_event(void) { pos.y = -target_pos_ned.y; ins_realign_h(pos, zero); } - const stuct FlotVect2 measuremet_noise = { 10.0, 10.0); + const stuct FlotVect2 measuremet_noise = { 10.0, 10.0 }; b2_hff_update_pos(-target_pos_ned, measurement_noise); ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot); ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot); @@ -141,6 +142,8 @@ void track_event(void) { ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot); ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x); ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y); + + INS_NED_TO_STATE(); #else // store pos in ins ins_ltp_pos.x = -(POS_BFP_OF_REAL(target_pos_ned.x)); @@ -149,6 +152,8 @@ void track_event(void) { // TODO get delta T // store last pos VECT3_COPY(last_pos_ned, target_pos_ned); + + stateSetPositionNed_i(&ins_ltp_pos); #endif b2_hff_lost_counter = 0; From 315b732a49715e4eb4ab572b0a9a5700b09ed36f Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 25 Jul 2012 14:12:46 +0200 Subject: [PATCH 23/62] [state interface] add Utm reference point --- sw/airborne/math/pprz_geodetic.h | 13 +++ sw/airborne/state.c | 186 +++++++++++++++++++++++++++++-- sw/airborne/state.h | 31 +++++- 3 files changed, 220 insertions(+), 10 deletions(-) diff --git a/sw/airborne/math/pprz_geodetic.h b/sw/airborne/math/pprz_geodetic.h index 72b62df929..717cf1a86c 100644 --- a/sw/airborne/math/pprz_geodetic.h +++ b/sw/airborne/math/pprz_geodetic.h @@ -27,4 +27,17 @@ (_def1).hmsl = (_def2).hmsl; \ } +#define ENU_OF_UTM_DIFF(_pos, _utm1, _utm2) { \ + (_pos).x = (_utm1).east - (_utm2).east; \ + (_pos).y = (_utm1).north - (_utm2).north; \ + (_pos).z = (_utm1).alt - (_utm2).alt; \ +} + +#define NED_OF_UTM_DIFF(_pos, _utm1, _utm2) { \ + (_pos).x = (_utm1).north - (_utm2).north; \ + (_pos).y = (_utm1).east - (_utm2).east; \ + (_pos).z = -(_utm1).alt + (_utm2).alt; \ +} + + #endif /* PPRZ_GEODETIC_H */ diff --git a/sw/airborne/state.c b/sw/airborne/state.c index ec07740829..0e2a6959ec 100644 --- a/sw/airborne/state.c +++ b/sw/airborne/state.c @@ -46,6 +46,7 @@ void stateInit(void) { state.wind_air_status = 0; state.ned_initialized_i = FALSE; state.ned_initialized_f = FALSE; + state.utm_initialized_f = FALSE; } @@ -101,6 +102,14 @@ void stateCalcPositionNed_i(void) { if (bit_is_set(state.pos_status, POS_NED_F)) { NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); } + else if (bit_is_set(state.pos_status, POS_ENU_I)) { + INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i); + } + else if (bit_is_set(state.pos_status, POS_ENU_F)) { + ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f); + SetBit(state.pos_status, POS_ENU_I); + INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i); + } else if (bit_is_set(state.pos_status, POS_ECEF_I)) { ned_of_ecef_point_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i); } @@ -124,8 +133,49 @@ void stateCalcPositionNed_i(void) { else { /* could not get this representation, set errno */ errno = 1; } - } else { /* ned coordinate system not initialized, set errno */ - errno = 2; + } + else if (state.utm_initialized_f) { + if (bit_is_set(state.pos_status, POS_NED_F)) { + NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); + } + else if (bit_is_set(state.pos_status, POS_ENU_I)) { + INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i); + } + else if (bit_is_set(state.pos_status, POS_ENU_F)) { + ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f); + SetBit(state.pos_status, POS_ENU_I); + INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i); + } + else if (bit_is_set(state.pos_status, POS_UTM_F)) { + /* transform utm_f -> ned_f -> ned_i, set status bits */ + NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f); + SetBit(state.pos_status, POS_NED_F); + NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); + } + else if (bit_is_set(state.pos_status, POS_LLA_F)) { + /* transform lla_f -> utm_f -> ned_f -> ned_i, set status bits */ + utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f); + SetBit(state.pos_status, POS_UTM_F); + NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f); + SetBit(state.pos_status, POS_NED_F); + NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); + } + else if (bit_is_set(state.pos_status, POS_LLA_I)) { + /* transform lla_i -> lla_f -> utm_f -> ned_f -> ned_i, set status bits */ + LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i); + SetBit(state.pos_status, POS_LLA_F); + utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f); + SetBit(state.pos_status, POS_UTM_F); + NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f); + SetBit(state.pos_status, POS_NED_F); + NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); + } + else { /* could not get this representation, set errno */ + errno = 2; + } + } + else { /* ned coordinate system not initialized, set errno */ + errno = 3; } if (errno) { //struct NedCoor_i _ned_zero = {0}; @@ -175,8 +225,49 @@ void stateCalcPositionEnu_i(void) { else { /* could not get this representation, set errno */ errno = 1; } - } else { /* ned coordinate system not initialized, set errno */ - errno = 2; + } + else if (state.utm_initialized_f) { + if (bit_is_set(state.pos_status, POS_ENU_F)) { + ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f); + } + else if (bit_is_set(state.pos_status, POS_NED_I)) { + INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i); + } + else if (bit_is_set(state.pos_status, POS_NED_F)) { + NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); + SetBit(state.pos_status, POS_NED_I); + INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i); + } + else if (bit_is_set(state.pos_status, POS_UTM_F)) { + /* transform utm_f -> enu_f -> enu_i , set status bits */ + ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f); + SetBit(state.pos_status, POS_ENU_F); + ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f); + } + else if (bit_is_set(state.pos_status, POS_LLA_F)) { + /* transform lla_f -> utm_f -> enu_f -> enu_i , set status bits */ + utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f); + SetBit(state.pos_status, POS_UTM_F); + ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f); + SetBit(state.pos_status, POS_ENU_F); + ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f); + } + else if (bit_is_set(state.pos_status, POS_LLA_I)) { + /* transform lla_i -> lla_f -> utm_f -> enu_f -> enu_i , set status bits */ + LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i); + SetBit(state.pos_status, POS_LLA_F); + utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f); + SetBit(state.pos_status, POS_UTM_F); + ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f); + SetBit(state.pos_status, POS_ENU_F); + ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f); + } + else { /* could not get this representation, set errno */ + errno = 2; + } + } + else { /* ned coordinate system not initialized, set errno */ + errno = 3; } if (errno) { //struct EnuCoor_i _enu_zero = {0}; @@ -217,6 +308,12 @@ void stateCalcPositionLla_i(void) { SetBit(state.pos_status, POS_ECEF_I); lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i); /* uses double version internally */ } + else if (bit_is_set(state.pos_status, POS_UTM_F)) { + /* transform utm_f -> lla_f -> lla_i, set status bits */ + lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f); + SetBit(state.pos_status, POS_LLA_F); + LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f); + } else { /* could not get this representation, set errno */ //struct LlaCoor_i _lla_zero = {0}; @@ -235,7 +332,7 @@ void stateCalcPositionUtm_f(void) { } else if (bit_is_set(state.pos_status, POS_LLA_I)) { /* transform lla_i -> lla_f -> utm_f, set status bits */ - LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_f); + LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i); SetBit(state.pos_status, POS_LLA_F); utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f); } @@ -314,8 +411,42 @@ void stateCalcPositionNed_f(void) { else { /* could not get this representation, set errno */ errno = 1; } - } else { /* ned coordinate system not initialized, set errno */ - errno = 2; + } + else if (state.utm_initialized_f) { + if (bit_is_set(state.pos_status, POS_NED_I)) { + NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i); + } + else if (bit_is_set(state.pos_status, POS_ENU_I)) { + ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i); + SetBit(state.pos_status, POS_ENU_F); + VECT3_NED_OF_ENU(state.ned_pos_f, state.enu_pos_f); + } + else if (bit_is_set(state.pos_status, POS_ENU_F)) { + VECT3_NED_OF_ENU(state.ned_pos_f, state.enu_pos_f); + } + else if (bit_is_set(state.pos_status, POS_UTM_F)) { + NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f); + } + else if (bit_is_set(state.pos_status, POS_LLA_F)) { + /* transform lla_f -> utm_f -> ned, set status bits */ + utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f); + SetBit(state.pos_status, POS_UTM_F); + NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f); + } + else if (bit_is_set(state.pos_status, POS_LLA_I)) { + /* transform lla_i -> lla_f -> utm_f -> ned, set status bits */ + LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i); + SetBit(state.pos_status, POS_LLA_F); + utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f); + SetBit(state.pos_status, POS_UTM_F); + NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f); + } + else { /* could not get this representation, set errno */ + errno = 2; + } + } + else { /* ned coordinate system not initialized, set errno */ + errno = 3; } if (errno) { //struct NedCoor_f _ned_zero = {0.0f}; @@ -365,8 +496,42 @@ void stateCalcPositionEnu_f(void) { else { /* could not get this representation, set errno */ errno = 1; } - } else { /* ned coordinate system not initialized, set errno */ - errno = 2; + } + else if (state.utm_initialized_f) { + if (bit_is_set(state.pos_status, POS_ENU_I)) { + ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i); + } + else if (bit_is_set(state.pos_status, POS_NED_F)) { + VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f); + } + else if (bit_is_set(state.pos_status, POS_NED_I)) { + NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i); + SetBit(state.pos_status, POS_NED_F); + VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f); + } + else if (bit_is_set(state.pos_status, POS_UTM_F)) { + ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f); + } + else if (bit_is_set(state.pos_status, POS_LLA_F)) { + /* transform lla_f -> utm_f -> enu, set status bits */ + utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f); + SetBit(state.pos_status, POS_UTM_F); + ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f); + } + else if (bit_is_set(state.pos_status, POS_LLA_I)) { + /* transform lla_i -> lla_f -> utm_f -> enu, set status bits */ + LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i); + SetBit(state.pos_status, POS_LLA_F); + utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f); + SetBit(state.pos_status, POS_UTM_F); + ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f); + } + else { /* could not get this representation, set errno */ + errno = 2; + } + } + else { /* ned coordinate system not initialized, set errno */ + errno = 3; } if (errno) { //struct EnuCoor_f _enu_zero = {0.0f}; @@ -406,6 +571,9 @@ void stateCalcPositionLla_f(void) { SetBit(state.pos_status, POS_ECEF_F); lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f); } + else if (bit_is_set(state.pos_status, POS_UTM_F)) { + lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f); + } else { /* could not get this representation, set errno */ //struct LlaCoor_f _lla_zero = {0.0}; diff --git a/sw/airborne/state.h b/sw/airborne/state.h index 4076442d73..a7bc524ff4 100644 --- a/sw/airborne/state.h +++ b/sw/airborne/state.h @@ -222,6 +222,18 @@ struct State { /// True if local float coordinate frame is initialsed bool_t ned_initialized_f; + /** + * Definition of the origin of Utm coordinate system. + * Defines the origin of the local NorthEastDown coordinate system + * in UTM coordinates, used as a reference when ned_origin is not + * initialized. + * (float version) + */ + struct UtmCoor_f utm_origin_f; + + /// True if utm origin (float) coordinate frame is initialsed + bool_t utm_initialized_f; + /** * Position in North East Down coordinates. * with respect to ned_origin_i (flat earth) @@ -493,6 +505,23 @@ static inline void stateSetLocalOrigin_i(struct LtpDef_i* ltp_def) { ClearBit(state.accel_status, ACCEL_NED_F); } +/// Set the local (flat earth) coordinate frame origin from UTM (float). +static inline void stateSetLocalUtmOrigin_f(struct UtmCoor_f* utm_def) { + memcpy(&state.utm_origin_f, utm_def, sizeof(struct UtmCoor_f)); + state.utm_initialized_f = TRUE; + + /* clear bits for all local frame representations */ + ClearBit(state.pos_status, POS_NED_I); + ClearBit(state.pos_status, POS_ENU_I); + ClearBit(state.pos_status, POS_NED_F); + ClearBit(state.pos_status, POS_ENU_F); + ClearBit(state.speed_status, SPEED_NED_I); + ClearBit(state.speed_status, SPEED_ENU_I); + ClearBit(state.speed_status, SPEED_NED_F); + ClearBit(state.speed_status, SPEED_ENU_F); + ClearBit(state.accel_status, ACCEL_NED_I); + ClearBit(state.accel_status, ACCEL_NED_F); +} /******************************************************************************* * * * Set and Get functions for the POSITION representations * @@ -516,7 +545,7 @@ extern void stateCalcPositionLla_f(void); /// Test if local coordinates are valid. static inline bool_t stateIsLocalCoordinateValid(void) { - return (state.ned_initialized_i && (state.pos_status &= ~(POS_LOCAL_COORD))); + return ((state.ned_initialized_i || state.utm_initialized_f) && (state.pos_status &= ~(POS_LOCAL_COORD))); } /// Test if global coordinates are valid. From 242c6e20019a991df11a07b5636c56ae83be4d01 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 25 Jul 2012 17:03:04 +0200 Subject: [PATCH 24/62] [state interface] add some missing conversions --- sw/airborne/state.c | 166 ++++++++++++++++++++++++++++++++++++++++---- 1 file changed, 152 insertions(+), 14 deletions(-) diff --git a/sw/airborne/state.c b/sw/airborne/state.c index 0e2a6959ec..401ce04c58 100644 --- a/sw/airborne/state.c +++ b/sw/airborne/state.c @@ -606,6 +606,14 @@ void stateCalcSpeedNed_i(void) { if (bit_is_set(state.speed_status, SPEED_NED_F)) { SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f); } + else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { + INT32_VECT3_NED_OF_ENU(state.ned_speed_i, state.enu_speed_i); + } + else if (bit_is_set(state.speed_status, SPEED_ENU_F)) { + SPEEDS_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f); + SetBit(state.speed_status, SPEED_ENU_I); + INT32_VECT3_NED_OF_ENU(state.ned_speed_i, state.enu_speed_i); + } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) { ned_of_ecef_vect_i(&state.ned_speed_i, &state.ned_origin_i, &state.ecef_speed_i); } @@ -618,8 +626,25 @@ void stateCalcSpeedNed_i(void) { else { /* could not get this representation, set errno */ errno = 1; } - } else { /* ned coordinate system not initialized, set errno */ - errno = 2; + } + else if (state.utm_initialized_f) { + if (bit_is_set(state.speed_status, SPEED_NED_F)) { + SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f); + } + else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { + INT32_VECT3_NED_OF_ENU(state.ned_speed_i, state.enu_speed_i); + } + else if (bit_is_set(state.speed_status, SPEED_ENU_F)) { + SPEEDS_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f); + SetBit(state.speed_status, SPEED_ENU_I); + INT32_VECT3_NED_OF_ENU(state.ned_speed_i, state.enu_speed_i); + } + else { /* could not get this representation, set errno */ + errno = 2; + } + } + else { /* ned coordinate system not initialized, set errno */ + errno = 3; } if (errno) { //struct NedCoor_i _ned_zero = {0}; @@ -658,8 +683,25 @@ void stateCalcSpeedEnu_i(void) { else { /* could not get this representation, set errno */ errno = 1; } - } else { /* ned coordinate system not initialized, set errno */ - errno = 2; + } + else if (state.utm_initialized_f) { + if (bit_is_set(state.speed_status, SPEED_NED_I)) { + INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i); + } + if (bit_is_set(state.speed_status, SPEED_ENU_F)) { + ENU_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f); + } + else if (bit_is_set(state.speed_status, SPEED_NED_F)) { + SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f); + SetBit(state.pos_status, SPEED_NED_I); + INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i); + } + else { /* could not get this representation, set errno */ + errno = 2; + } + } + else { /* ned coordinate system not initialized, set errno */ + errno = 3; } if (errno) { //struct EnuCoor_i _enu_zero = {0}; @@ -703,13 +745,22 @@ void stateCalcHorizontalSpeedNorm_i(void) { //TODO } else if (bit_is_set(state.speed_status, SPEED_NED_I)) { //TODO consider INT32_SPEED_FRAC - //INT32_VECT2_NORM(state.h_speed_norm_i, state.ned_speed_i); + INT32_VECT2_NORM(state.h_speed_norm_i, state.ned_speed_i); } else if (bit_is_set(state.speed_status, SPEED_NED_F)) { FLOAT_VECT2_NORM(state.h_speed_norm_f, state.ned_speed_f); SetBit(state.speed_status, SPEED_HNORM_F); state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f); } + else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { + //TODO consider INT32_SPEED_FRAC + INT32_VECT2_NORM(state.h_speed_norm_i, state.enu_speed_i); + } + else if (bit_is_set(state.speed_status, SPEED_ENU_F)) { + FLOAT_VECT2_NORM(state.h_speed_norm_f, state.enu_speed_f); + SetBit(state.speed_status, SPEED_HNORM_F); + state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f); + } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) { /* transform ecef speed to ned, set status bit, then compute norm */ //foo @@ -741,7 +792,24 @@ void stateCalcHorizontalSpeedDir_i(void) { //TODO state.h_speed_dir_i = SPEED_BFP_OF_REAL(state.h_speed_dir_f); } else if (bit_is_set(state.speed_status, SPEED_NED_I)) { - //TODO + INT32_ATAN2(state.h_speed_dir_i, state.ned_speed_i.y, state.ned_speed_i.x); + INT32_COURSE_NORMALIZE(state.h_speed_dir_i); + } + else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { + INT32_ATAN2(state.h_speed_dir_i, state.enu_speed_i.x, state.enu_speed_i.y); + INT32_COURSE_NORMALIZE(state.h_speed_dir_i); + } + else if (bit_is_set(state.speed_status, SPEED_NED_F)) { + SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f); + SetBit(state.speed_status, SPEED_NED_I); + INT32_ATAN2(state.h_speed_dir_i, state.ned_speed_i.y, state.ned_speed_i.x); + INT32_COURSE_NORMALIZE(state.h_speed_dir_i); + } + else if (bit_is_set(state.speed_status, SPEED_ENU_F)) { + SPEEDS_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f); + SetBit(state.speed_status, SPEED_ENU_I); + INT32_ATAN2(state.h_speed_dir_i, state.enu_speed_i.x, state.enu_speed_i.y); + INT32_COURSE_NORMALIZE(state.h_speed_dir_i); } /* set bit to indicate this representation is computed */ SetBit(state.speed_status, SPEED_HDIR_I); @@ -756,6 +824,14 @@ void stateCalcSpeedNed_f(void) { if (bit_is_set(state.speed_status, SPEED_NED_I)) { SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i); } + else if (bit_is_set(state.speed_status, SPEED_ENU_F)) { + VECT3_NED_OF_ENU(state.ned_speed_f, state.enu_speed_f); + } + else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { + SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i); + SetBit(state.speed_status, SPEED_ENU_F); + VECT3_NED_OF_ENU(state.ned_speed_f, state.enu_speed_f); + } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) { ned_of_ecef_vect_f(&state.ned_speed_f, &state.ned_origin_f, &state.ecef_speed_f); } @@ -768,8 +844,25 @@ void stateCalcSpeedNed_f(void) { else { /* could not get this representation, set errno */ errno = 1; } - } else { /* ned coordinate system not initialized, set errno */ - errno = 2; + } + else if (state.utm_initialized_f) { + if (bit_is_set(state.speed_status, SPEED_NED_I)) { + SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i); + } + else if (bit_is_set(state.speed_status, SPEED_ENU_F)) { + VECT3_NED_OF_ENU(state.ned_speed_f, state.enu_speed_f); + } + else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { + SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i); + SetBit(state.speed_status, SPEED_ENU_F); + VECT3_NED_OF_ENU(state.ned_speed_f, state.enu_speed_f); + } + else { /* could not get this representation, set errno */ + errno = 2; + } + } + else { /* ned coordinate system not initialized, set errno */ + errno = 3; } if (errno) { //struct NedCoor_f _ned_zero = {0.0f}; @@ -788,7 +881,7 @@ void stateCalcSpeedEnu_f(void) { if (bit_is_set(state.speed_status, SPEED_NED_F)) { VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f); } - if (bit_is_set(state.speed_status, SPEED_ENU_I)) { + else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { ENU_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i); } else if (bit_is_set(state.speed_status, SPEED_NED_I)) { @@ -808,8 +901,25 @@ void stateCalcSpeedEnu_f(void) { else { /* could not get this representation, set errno */ errno = 1; } - } else { /* ned coordinate system not initialized, set errno */ - errno = 2; + } + else if (state.utm_initialized_f) { + if (bit_is_set(state.speed_status, SPEED_NED_F)) { + VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f); + } + else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { + ENU_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i); + } + else if (bit_is_set(state.speed_status, SPEED_NED_I)) { + SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i); + SetBit(state.pos_status, SPEED_NED_F); + VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f); + } + else { /* could not get this representation, set errno */ + errno = 2; + } + } + else { /* ned coordinate system not initialized, set errno */ + errno = 3; } if (errno) { //struct EnuCoor_f _enu_zero = {0}; @@ -850,9 +960,23 @@ void stateCalcHorizontalSpeedNorm_f(void) { //TODO if (bit_is_set(state.speed_status, SPEED_HNORM_I)){ state.h_speed_norm_f = SPEED_FLOAT_OF_BFP(state.h_speed_norm_i); - } else if (bit_is_set(state.speed_status, SPEED_NED_F)) { + } + else if (bit_is_set(state.speed_status, SPEED_NED_F)) { FLOAT_VECT2_NORM(state.h_speed_norm_f, state.ned_speed_f); } + else if (bit_is_set(state.speed_status, SPEED_ENU_F)) { + FLOAT_VECT2_NORM(state.h_speed_norm_f, state.enu_speed_f); + } + else if (bit_is_set(state.speed_status, SPEED_NED_I)) { + SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i); + SetBit(state.speed_status, SPEED_NED_F); + FLOAT_VECT2_NORM(state.h_speed_norm_f, state.ned_speed_f); + } + else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { + SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i); + SetBit(state.speed_status, SPEED_ENU_F); + FLOAT_VECT2_NORM(state.h_speed_norm_f, state.enu_speed_f); + } /* set bit to indicate this representation is computed */ SetBit(state.speed_status, SPEED_HNORM_F); } @@ -863,8 +987,22 @@ void stateCalcHorizontalSpeedDir_f(void) { //TODO if (bit_is_set(state.speed_status, SPEED_HDIR_I)){ state.h_speed_dir_f = SPEED_FLOAT_OF_BFP(state.h_speed_dir_i); - } else if (bit_is_set(state.speed_status, SPEED_NED_F)) { - //foo + } + else if (bit_is_set(state.speed_status, SPEED_NED_F)) { + state.h_speed_dir_f = atan2f(state.ned_speed_f.y, state.ned_speed_f.x); + } + else if (bit_is_set(state.speed_status, SPEED_ENU_F)) { + state.h_speed_dir_f = atan2f(state.enu_speed_f.x, state.enu_speed_f.y); + } + else if (bit_is_set(state.speed_status, SPEED_NED_I)) { + SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i); + SetBit(state.speed_status, SPEED_NED_F); + state.h_speed_dir_f = atan2f(state.ned_speed_f.y, state.ned_speed_f.x); + } + else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { + SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i); + SetBit(state.speed_status, SPEED_ENU_F); + state.h_speed_dir_f = atan2f(state.enu_speed_f.x, state.enu_speed_f.y); } /* set bit to indicate this representation is computed */ SetBit(state.speed_status, SPEED_HDIR_F); From aa2cd517e82c67f7f40f07c23a48bad2c8050d73 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 25 Jul 2012 17:05:00 +0200 Subject: [PATCH 25/62] [state interface] fw control/guidance/nav converted Only the basic navigation routines are converted to the new state interface Some stuff are missing into the state interface: angle of attack and sideslip angle --- .../firmwares/fixedwing/guidance/guidance_v.c | 14 ++++---- .../fixedwing/guidance/guidance_v_n.c | 14 ++++---- .../stabilization/stabilization_adaptive.c | 23 +++++++------ .../stabilization/stabilization_attitude.c | 34 ++++++++++--------- sw/airborne/subsystems/nav.c | 33 ++++++++++-------- sw/airborne/subsystems/nav.h | 3 +- .../subsystems/navigation/common_nav.c | 12 ++++--- .../subsystems/navigation/common_nav.h | 5 +-- 8 files changed, 75 insertions(+), 63 deletions(-) diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c index a4fd061a31..1f591dce31 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c +++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c @@ -26,7 +26,7 @@ */ #include "firmwares/fixedwing/guidance/guidance_v.h" -#include "estimator.h" +#include "state.h" #include "subsystems/nav.h" #include "generated/airframe.h" #include "firmwares/fixedwing/autopilot.h" @@ -207,7 +207,7 @@ void v_ctl_altitude_loop( void ) { } #endif - v_ctl_altitude_error = v_ctl_altitude_setpoint - estimator_z; + v_ctl_altitude_error = v_ctl_altitude_setpoint - stateGetPositionUtm_f()->alt; v_ctl_climb_setpoint = altitude_pgain_boost * v_ctl_altitude_pgain * v_ctl_altitude_error + v_ctl_altitude_pre_climb * v_ctl_altitude_pre_climb_correction; BoundAbs(v_ctl_climb_setpoint, v_ctl_altitude_max_climb); @@ -253,7 +253,7 @@ inline static void v_ctl_climb_auto_throttle_loop(void) { static float last_err; float f_throttle = 0; - float err = estimator_z_dot - v_ctl_climb_setpoint; + float err = stateGetSpeedEnu_f()->z - v_ctl_climb_setpoint; float d_err = err - last_err; last_err = err; float controlled_throttle = v_ctl_auto_throttle_cruise_throttle @@ -326,14 +326,14 @@ inline static void v_ctl_climb_auto_throttle_loop(void) { v_ctl_climb_setpoint_last = v_ctl_climb_setpoint; // Pitch control (input: rate of climb error, output: pitch setpoint) - float err = estimator_z_dot - v_ctl_climb_setpoint; + float err = stateGetSpeedEnu_f()->z - v_ctl_climb_setpoint; v_ctl_auto_pitch_sum_err += err; BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR); v_ctl_pitch_of_vz = -v_ctl_auto_pitch_pgain * (err + v_ctl_auto_pitch_igain * v_ctl_auto_pitch_sum_err); // Ground speed control loop (input: groundspeed error, output: airspeed controlled) - float err_groundspeed = (v_ctl_auto_groundspeed_setpoint - estimator_hspeed_mod); + float err_groundspeed = (v_ctl_auto_groundspeed_setpoint - *stateGetHorizontalSpeedNorm_f()); v_ctl_auto_groundspeed_sum_err += err_groundspeed; BoundAbs(v_ctl_auto_groundspeed_sum_err, V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR); v_ctl_auto_airspeed_controlled = (err_groundspeed + v_ctl_auto_groundspeed_sum_err * v_ctl_auto_groundspeed_igain) * v_ctl_auto_groundspeed_pgain; @@ -345,7 +345,7 @@ inline static void v_ctl_climb_auto_throttle_loop(void) { } // Airspeed control loop (input: airspeed controlled, output: throttle controlled) - float err_airspeed = (v_ctl_auto_airspeed_controlled - estimator_airspeed); + float err_airspeed = (v_ctl_auto_airspeed_controlled - *stateGetAirspeed_f()); v_ctl_auto_airspeed_sum_err += err_airspeed; BoundAbs(v_ctl_auto_airspeed_sum_err, V_CTL_AUTO_AIRSPEED_MAX_SUM_ERR); controlled_throttle = (err_airspeed + v_ctl_auto_airspeed_sum_err * v_ctl_auto_airspeed_igain) * v_ctl_auto_airspeed_pgain; @@ -367,7 +367,7 @@ inline static void v_ctl_climb_auto_throttle_loop(void) { */ #ifdef V_CTL_AUTO_PITCH_PGAIN inline static void v_ctl_climb_auto_pitch_loop(void) { - float err = estimator_z_dot - v_ctl_climb_setpoint; + float err = stateGetSpeedEnu_f()->z - v_ctl_climb_setpoint; v_ctl_throttle_setpoint = nav_throttle_setpoint; v_ctl_auto_pitch_sum_err += err; BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR); diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c index d0469b53e4..5edff3ad84 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c +++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c @@ -27,7 +27,7 @@ #include "firmwares/fixedwing/guidance/guidance_v.h" #include "firmwares/fixedwing/guidance/guidance_v_n.h" -#include "estimator.h" +#include "state.h" #include "subsystems/nav.h" #include "generated/airframe.h" #include "firmwares/fixedwing/autopilot.h" @@ -185,7 +185,7 @@ void v_ctl_altitude_loop( void ) { //static float last_lead_input = 0.; // Altitude error - v_ctl_altitude_error = v_ctl_altitude_setpoint - estimator_z; + v_ctl_altitude_error = v_ctl_altitude_setpoint - stateGetPositionUtm_f()->alt; v_ctl_climb_setpoint = v_ctl_altitude_pgain * v_ctl_altitude_error + v_ctl_altitude_pre_climb; // Lead controller @@ -209,7 +209,7 @@ static inline void v_ctl_set_pitch ( void ) { v_ctl_auto_pitch_sum_err = 0; // Compute errors - float err = v_ctl_climb_setpoint - estimator_z_dot; + float err = v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z; float d_err = err - last_err; last_err = err; @@ -234,7 +234,7 @@ static inline void v_ctl_set_throttle( void ) { v_ctl_auto_throttle_sum_err = 0; // Compute errors - float err = v_ctl_climb_setpoint - estimator_z_dot; + float err = v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z; float d_err = err - last_err; last_err = err; @@ -264,7 +264,7 @@ static inline void v_ctl_set_airspeed( void ) { Bound(v_ctl_auto_airspeed_setpoint, V_CTL_AIRSPEED_MIN, V_CTL_AIRSPEED_MAX); // Compute errors - float err_vz = v_ctl_climb_setpoint - estimator_z_dot; + float err_vz = v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z; float d_err_vz = (err_vz - last_err_vz)*AIRSPEED_LOOP_PERIOD; last_err_vz = err_vz; if (v_ctl_auto_throttle_igain > 0.) { @@ -276,7 +276,7 @@ static inline void v_ctl_set_airspeed( void ) { BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR / v_ctl_auto_pitch_igain); } - float err_airspeed = v_ctl_auto_airspeed_setpoint - estimator_airspeed; + float err_airspeed = v_ctl_auto_airspeed_setpoint - *stateGetAirspeed_f(); float d_err_airspeed = (err_airspeed - last_err_as)*AIRSPEED_LOOP_PERIOD; last_err_as = err_airspeed; if (v_ctl_auto_airspeed_throttle_igain > 0.) { @@ -321,7 +321,7 @@ static inline void v_ctl_set_airspeed( void ) { static inline void v_ctl_set_groundspeed( void ) { // Ground speed control loop (input: groundspeed error, output: airspeed controlled) - float err_groundspeed = v_ctl_auto_groundspeed_setpoint - estimator_hspeed_mod; + float err_groundspeed = v_ctl_auto_groundspeed_setpoint - *stateGetHorizontalSpeedNorm_f(); v_ctl_auto_groundspeed_sum_err += err_groundspeed; BoundAbs(v_ctl_auto_groundspeed_sum_err, V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR); v_ctl_auto_airspeed_setpoint = err_groundspeed * v_ctl_auto_groundspeed_pgain + v_ctl_auto_groundspeed_sum_err * v_ctl_auto_groundspeed_igain; diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c index cf9a09ff90..502d51ccd9 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c @@ -32,7 +32,7 @@ #include "led.h" #include "firmwares/fixedwing/stabilization/stabilization_attitude.h" #include "firmwares/fixedwing/stabilization/stabilization_adaptive.h" -#include "estimator.h" +#include "state.h" #include "subsystems/nav.h" #include "generated/airframe.h" #include "firmwares/fixedwing/guidance/guidance_v.h" @@ -205,14 +205,14 @@ void h_ctl_course_loop ( void ) { static float last_err; // Ground path error - float err = h_ctl_course_setpoint - estimator_hspeed_dir; + float err = h_ctl_course_setpoint - (*stateGetHorizontalSpeedDir_f()); NormRadAngle(err); float d_err = err - last_err; last_err = err; NormRadAngle(d_err); - float speed_depend_nav = estimator_hspeed_mod/NOMINAL_AIRSPEED; + float speed_depend_nav = (*stateGetHorizontalSpeedNorm_f())/NOMINAL_AIRSPEED; Bound(speed_depend_nav, 0.66, 1.5); h_ctl_roll_setpoint = h_ctl_course_pre_bank_correction * h_ctl_course_pre_bank @@ -227,7 +227,7 @@ static inline void compute_airspeed_ratio( void ) { if (use_airspeed_ratio) { // low pass airspeed static float airspeed = 0.; - airspeed = ( 15*airspeed + estimator_airspeed ) / 16; + airspeed = ( 15*airspeed + (*stateGetAirspeed_f()) ) / 16; // compute ratio float airspeed_ratio = airspeed / NOMINAL_AIRSPEED; Bound(airspeed_ratio, AIRSPEED_RATIO_MIN, AIRSPEED_RATIO_MAX); @@ -289,13 +289,14 @@ inline static void h_ctl_roll_loop( void ) { #endif // Compute errors - float err = h_ctl_ref_roll_angle - estimator_phi; + float err = h_ctl_ref_roll_angle - stateGetNedToBodyEulers_f()->phi; + struct FloatRates* body_rate = stateGetBodyRates_f(); #ifdef SITL static float last_err = 0; - estimator_p = (err - last_err)/(1/60.); + body_rate->p = (err - last_err)/(1/60.); // FIXME should be done in ahrs sim last_err = err; #endif - float d_err = h_ctl_ref_roll_rate - estimator_p; + float d_err = h_ctl_ref_roll_rate - body_rate->p; if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) { h_ctl_roll_sum_err = 0.; @@ -329,7 +330,7 @@ inline static void loiter(void) { float pitch_trim; #if USE_AIRSPEED - if (estimator_airspeed > NOMINAL_AIRSPEED) { + if (stateGetAirspeed_f() > NOMINAL_AIRSPEED) { pitch_trim = v_ctl_pitch_dash_trim * (airspeed_ratio2-1) / ((AIRSPEED_RATIO_MAX * AIRSPEED_RATIO_MAX) - 1); } else { pitch_trim = v_ctl_pitch_loiter_trim * (airspeed_ratio2-1) / ((AIRSPEED_RATIO_MIN * AIRSPEED_RATIO_MIN) - 1); @@ -359,7 +360,7 @@ inline static void h_ctl_pitch_loop( void ) { if (h_ctl_pitch_of_roll <0.) h_ctl_pitch_of_roll = 0.; - h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_pitch_of_roll * fabs(estimator_phi); + h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_pitch_of_roll * fabs(stateGetNedToBodyEulers_f()->phi); #if USE_PITCH_TRIM loiter(); #endif @@ -386,9 +387,9 @@ inline static void h_ctl_pitch_loop( void ) { #endif // Compute errors - float err = h_ctl_ref_pitch_angle - estimator_theta; + float err = h_ctl_ref_pitch_angle - stateGetNedToBodyEulers_f()->theta; #if USE_GYRO_PITCH_RATE - float d_err = h_ctl_ref_pitch_rate - estimator_q; + float d_err = h_ctl_ref_pitch_rate - stateGetBodyRates_f()->q; #else // soft derivation float d_err = (err - last_err)/H_CTL_REF_DT - h_ctl_ref_pitch_rate; last_err = err; diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c index 313ed6cc95..d8739b8f94 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c @@ -31,7 +31,7 @@ #include "firmwares/fixedwing/stabilization/stabilization_attitude.h" #include "std.h" #include "led.h" -#include "estimator.h" +#include "state.h" #include "subsystems/nav.h" #include "generated/airframe.h" #include "firmwares/fixedwing/guidance/guidance_v.h" @@ -181,17 +181,17 @@ void h_ctl_course_loop ( void ) { static float last_err; // Ground path error - float err = estimator_hspeed_dir - h_ctl_course_setpoint; + float err = *stateGetHorizontalSpeedDir_f() - h_ctl_course_setpoint; NormRadAngle(err); #ifdef STRONG_WIND // Usefull path speed const float reference_advance = (NOMINAL_AIRSPEED / 2.); - float advance = cos(err) * estimator_hspeed_mod / reference_advance; + float advance = cos(err) * (*stateGetHorizontalSpeedNorm_f()) / reference_advance; if ( (advance < 1.) && // Path speed is small - (estimator_hspeed_mod < reference_advance) // Small path speed is due to wind (small groundspeed) + ((*stateGetHorizontalSpeedNorm_f()) < reference_advance) // Small path speed is due to wind (small groundspeed) ) { /* @@ -210,7 +210,7 @@ void h_ctl_course_loop ( void ) { */ // Heading error - float herr = estimator_psi - h_ctl_course_setpoint; //+crab); + float herr = *stateGetNedToBodyEulers_f()->psi - h_ctl_course_setpoint; //+crab); NormRadAngle(herr); if (advance < -0.5) // 90 degree turn @@ -268,7 +268,7 @@ void h_ctl_course_loop ( void ) { } #endif - float speed_depend_nav = estimator_hspeed_mod/NOMINAL_AIRSPEED; + float speed_depend_nav = (*stateGetHorizontalSpeedNorm_f())/NOMINAL_AIRSPEED; Bound(speed_depend_nav, 0.66, 1.5); float cmd = -h_ctl_course_pgain * speed_depend_nav * (err + d_err * h_ctl_course_dgain); @@ -316,14 +316,15 @@ void h_ctl_attitude_loop ( void ) { #ifdef H_CTL_ROLL_ATTITUDE_GAIN inline static void h_ctl_roll_loop( void ) { - float err = estimator_phi - h_ctl_roll_setpoint; + float err = stateGetNedToBodyEulers_f()->phi - h_ctl_roll_setpoint; + struct FloatRates* body_rate = stateGetBodyRates_f(); #ifdef SITL static float last_err = 0; - estimator_p = (err - last_err)/(1/60.); + body_rate->p = (err - last_err)/(1/60.); last_err = err; #endif float cmd = h_ctl_roll_attitude_gain * err - + h_ctl_roll_rate_gain * estimator_p + + h_ctl_roll_rate_gain * body_rate->p + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle; h_ctl_aileron_setpoint = TRIM_PPRZ(cmd); @@ -333,7 +334,7 @@ inline static void h_ctl_roll_loop( void ) { /** Computes h_ctl_aileron_setpoint from h_ctl_roll_setpoint */ inline static void h_ctl_roll_loop( void ) { - float err = estimator_phi - h_ctl_roll_setpoint; + float err = stateGetNedToBodyEulers_f()->phi - h_ctl_roll_setpoint; float cmd = h_ctl_roll_pgain * err + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle; h_ctl_aileron_setpoint = TRIM_PPRZ(cmd); @@ -357,7 +358,7 @@ inline static void h_ctl_roll_loop( void ) { #ifdef H_CTL_RATE_LOOP static inline void h_ctl_roll_rate_loop() { - float err = estimator_p - h_ctl_roll_rate_setpoint; + float err = stateGetBodyRates_f()->p - h_ctl_roll_rate_setpoint; /* I term calculation */ static float roll_rate_sum_err = 0.; @@ -419,28 +420,29 @@ inline static float loiter(void) { inline static void h_ctl_pitch_loop( void ) { static float last_err; + struct FloatEulers* att = stateGetNedToBodyEulers_f(); /* sanity check */ if (h_ctl_elevator_of_roll <0.) h_ctl_elevator_of_roll = 0.; - h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_elevator_of_roll / h_ctl_pitch_pgain * fabs(estimator_phi); + h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_elevator_of_roll / h_ctl_pitch_pgain * fabs(att->phi); float err = 0; #ifdef USE_AOA switch (h_ctl_pitch_mode){ case H_CTL_PITCH_MODE_THETA: - err = estimator_theta - h_ctl_pitch_loop_setpoint; + err = att->theta - h_ctl_pitch_loop_setpoint; break; case H_CTL_PITCH_MODE_AOA: - err = estimator_AOA - h_ctl_pitch_loop_setpoint; + err = estimator_AOA - h_ctl_pitch_loop_setpoint; //TODO AOA into state interface break; default: - err = estimator_theta - h_ctl_pitch_loop_setpoint; + err = att->theta - h_ctl_pitch_loop_setpoint; break; } #else //NO_AOA - err = estimator_theta - h_ctl_pitch_loop_setpoint; + err = att->theta - h_ctl_pitch_loop_setpoint; #endif diff --git a/sw/airborne/subsystems/nav.c b/sw/airborne/subsystems/nav.c index 9ac2d1c8f2..5d1465b41a 100644 --- a/sw/airborne/subsystems/nav.c +++ b/sw/airborne/subsystems/nav.c @@ -32,7 +32,6 @@ #include "subsystems/nav.h" #include "subsystems/gps.h" -#include "estimator.h" #include "firmwares/fixedwing/stabilization/stabilization_attitude.h" #include "firmwares/fixedwing/guidance/guidance_v.h" #include "firmwares/fixedwing/autopilot.h" @@ -90,7 +89,8 @@ bool_t nav_survey_active; int nav_mode; void nav_init_stage( void ) { - last_x = estimator_x; last_y = estimator_y; + last_x = stateGetPositionEnu_f()->x; + last_y = stateGetPositionEnu_f()->y; stage_time = 0; nav_circle_radians = 0; nav_in_circle = FALSE; @@ -107,8 +107,9 @@ void nav_init_stage( void ) { /** Navigates around (x, y). Clockwise iff radius > 0 */ void nav_circle_XY(float x, float y, float radius) { + struct EnuCoor_f* pos = stateGetPositionEnu_f(); float last_trigo_qdr = nav_circle_trigo_qdr; - nav_circle_trigo_qdr = atan2(estimator_y - y, estimator_x - x); + nav_circle_trigo_qdr = atan2(pos->y - y, pos->x - x); if (nav_in_circle) { float trigo_diff = nav_circle_trigo_qdr - last_trigo_qdr; @@ -116,7 +117,7 @@ void nav_circle_XY(float x, float y, float radius) { nav_circle_radians += trigo_diff; } - float dist2_center = DistanceSquare(estimator_x, estimator_y, x, y); + float dist2_center = DistanceSquare(pos->x, pos->y, x, y); float dist_carrot = CARROT*NOMINAL_AIRSPEED; float sign_radius = radius > 0 ? 1 : -1; @@ -129,7 +130,7 @@ void nav_circle_XY(float x, float y, float radius) { (dist2_center > Square(abs_radius + dist_carrot) || dist2_center < Square(abs_radius - dist_carrot)) ? 0 : - atan(estimator_hspeed_mod*estimator_hspeed_mod / (G*radius)); + atan((*stateGetHorizontalSpeedNorm_f())*(*stateGetHorizontalSpeedNorm_f()) / (G*radius)); float carrot_angle = dist_carrot / abs_radius; carrot_angle = Min(carrot_angle, M_PI/4); @@ -152,7 +153,7 @@ void nav_circle_XY(float x, float y, float radius) { float start_alt = waypoints[_last_wp].a; \ float diff_alt = waypoints[_wp].a - start_alt; \ float alt = start_alt + nav_leg_progress * diff_alt; \ - float pre_climb = estimator_hspeed_mod * diff_alt / nav_leg_length; \ + float pre_climb = (*stateGetHorizontalSpeedNorm_f()) * diff_alt / nav_leg_length; \ NavVerticalAltitudeMode(alt, pre_climb); \ } @@ -203,7 +204,7 @@ static inline void nav_follow(uint8_t _ac_id, float _distance, float _height); static void nav_ground_speed_loop( void ) { if (MINIMUM_AIRSPEED < nav_ground_speed_setpoint && nav_ground_speed_setpoint < MAXIMUM_AIRSPEED) { - float err = nav_ground_speed_setpoint - estimator_hspeed_mod; + float err = nav_ground_speed_setpoint - (*stateGetHorizontalSpeedNorm_f()); v_ctl_auto_throttle_cruise_throttle += nav_ground_speed_pgain*err; Bound(v_ctl_auto_throttle_cruise_throttle, V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE); } else { @@ -273,7 +274,7 @@ static inline void nav_follow(uint8_t _ac_id, float _distance, float _height) { float y = ac->north - _distance*sa; fly_to_xy(x, y); #ifdef NAV_FOLLOW_PGAIN - float s = (estimator_x-x)*ca+(estimator_y-y)*sa; + float s = (stateGetPositionEnu_f()->x - x)*ca + (stateGetPositionEnu_f()->y - y)*sa; nav_ground_speed_setpoint = ac->gspeed + NAV_FOLLOW_PGAIN*s; nav_ground_speed_loop(); #endif @@ -294,12 +295,12 @@ float fp_pitch; /* deg */ */ bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float approaching_time) { /** distance to waypoint in x */ - float pw_x = x - estimator_x; + float pw_x = x - stateGetPositionEnu_f()->x; /** distance to waypoint in y */ - float pw_y = y - estimator_y; + float pw_y = y - stateGetPositionEnu_f()->y; dist2_to_wp = pw_x*pw_x + pw_y *pw_y; - float min_dist = approaching_time * estimator_hspeed_mod; + float min_dist = approaching_time * (*stateGetHorizontalSpeedNorm_f()); if (dist2_to_wp < min_dist*min_dist) return TRUE; @@ -314,19 +315,21 @@ bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float ap */ //static inline void fly_to_xy(float x, float y) { void fly_to_xy(float x, float y) { + struct EnuCoor_f* pos = stateGetPositionEnu_f(); desired_x = x; desired_y = y; if (nav_mode == NAV_MODE_COURSE) { - h_ctl_course_setpoint = atan2(x - estimator_x, y - estimator_y); + h_ctl_course_setpoint = atan2(x - pos->x, y - pos->y); if (h_ctl_course_setpoint < 0.) h_ctl_course_setpoint += 2 * M_PI; lateral_mode = LATERAL_MODE_COURSE; } else { - float diff = atan2(x - estimator_x, y - estimator_y) - estimator_hspeed_dir; + float diff = atan2(x - pos->x, y - pos->y) - (*stateGetHorizontalSpeedDir_f()); NormRadAngle(diff); BoundAbs(diff,M_PI/2.); float s = sin(diff); - h_ctl_roll_setpoint = atan(2 * estimator_hspeed_mod*estimator_hspeed_mod * s * h_ctl_course_pgain / (CARROT * NOMINAL_AIRSPEED * 9.81) ); + float speed = *stateGetHorizontalSpeedNorm_f(); + h_ctl_roll_setpoint = atan(2 * speed * speed * s * h_ctl_course_pgain / (CARROT * NOMINAL_AIRSPEED * 9.81) ); BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint); lateral_mode = LATERAL_MODE_ROLL; } @@ -339,7 +342,7 @@ void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y) { float leg_x = wp_x - last_wp_x; float leg_y = wp_y - last_wp_y; float leg2 = Max(leg_x * leg_x + leg_y * leg_y, 1.); - nav_leg_progress = ((estimator_x - last_wp_x) * leg_x + (estimator_y - last_wp_y) * leg_y) / leg2; + nav_leg_progress = ((stateGetPositionEnu_f()->x - last_wp_x) * leg_x + (stateGetPositionEnu_f()->y - last_wp_y) * leg_y) / leg2; nav_leg_length = sqrt(leg2); /** distance of carrot (in meter) */ diff --git a/sw/airborne/subsystems/nav.h b/sw/airborne/subsystems/nav.h index 47d9625028..2a84649014 100644 --- a/sw/airborne/subsystems/nav.h +++ b/sw/airborne/subsystems/nav.h @@ -34,6 +34,7 @@ #include "std.h" #include "paparazzi.h" +#include "state.h" #include "firmwares/fixedwing/guidance/guidance_v.h" #include "subsystems/navigation/nav_survey_rectangle.h" #include "subsystems/navigation/common_flight_plan.h" @@ -129,7 +130,7 @@ extern void nav_circle_XY(float x, float y, float radius); /** True if x (in degrees) is close to the current QDR (less than 10 degrees)*/ #define NavQdrCloseTo(x) CloseDegAngles(x, NavCircleQdr()) -#define NavCourseCloseTo(x) CloseDegAngles(x, DegOfRad(estimator_hspeed_dir)) +#define NavCourseCloseTo(x) CloseDegAngles(x, DegOfRad(*stateGetHorizontalSpeedDir_f())) /*********** Navigation along a line *************************************/ extern void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y); diff --git a/sw/airborne/subsystems/navigation/common_nav.c b/sw/airborne/subsystems/navigation/common_nav.c index c2609a9a1c..34127ee69b 100644 --- a/sw/airborne/subsystems/navigation/common_nav.c +++ b/sw/airborne/subsystems/navigation/common_nav.c @@ -23,7 +23,6 @@ */ #include "subsystems/navigation/common_nav.h" -#include "estimator.h" #include "generated/flight_plan.h" #include "subsystems/gps.h" #include "math/pprz_geodetic_float.h" @@ -47,12 +46,13 @@ float max_dist_from_home = MAX_DIST_FROM_HOME; * \a too_far_from_home */ void compute_dist2_to_home(void) { - float ph_x = waypoints[WP_HOME].x - estimator_x; - float ph_y = waypoints[WP_HOME].y - estimator_y; + struct EnuCoor_f* pos = stateGetPositionEnu_f(); + float ph_x = waypoints[WP_HOME].x - pos->x; + float ph_y = waypoints[WP_HOME].y - pos->y; dist2_to_home = ph_x*ph_x + ph_y *ph_y; too_far_from_home = dist2_to_home > (MAX_DIST_FROM_HOME*MAX_DIST_FROM_HOME); #if defined InAirspace - too_far_from_home = too_far_from_home || !(InAirspace(estimator_x, estimator_y)); + too_far_from_home = too_far_from_home || !(InAirspace(pos_x, pos_y)); #endif } @@ -80,6 +80,10 @@ unit_t nav_reset_reference( void ) { nav_utm_north0 = gps.utm_pos.north/100; #endif + // reset state UTM ref + struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 }; + stateSetLocalUtmOrigin_f(&utm0); + previous_ground_alt = ground_alt; ground_alt = gps.hmsl/1000.; return 0; diff --git a/sw/airborne/subsystems/navigation/common_nav.h b/sw/airborne/subsystems/navigation/common_nav.h index d97e5c27bb..25bf528ba0 100644 --- a/sw/airborne/subsystems/navigation/common_nav.h +++ b/sw/airborne/subsystems/navigation/common_nav.h @@ -26,6 +26,7 @@ #define COMMON_NAV_H #include "std.h" +#include "state.h" #include "subsystems/navigation/common_flight_plan.h" extern float max_dist_from_home; @@ -66,8 +67,8 @@ void common_nav_periodic_task_4Hz(void); #define NavSetGroundReferenceHere() ({ nav_reset_reference(); nav_update_waypoints_alt(); FALSE; }) #define NavSetWaypointHere(_wp) ({ \ - waypoints[_wp].x = estimator_x; \ - waypoints[_wp].y = estimator_y; \ + waypoints[_wp].x = stateGetPositionEnu_f()->x; \ + waypoints[_wp].y = stateGetPositionEnu_f()->y; \ FALSE; \ }) From 81b8b6b07c9bb11aa70e5c164eb6508c3557487a Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 25 Jul 2012 17:07:22 +0200 Subject: [PATCH 26/62] [state interface] some datalink stuff converted --- sw/airborne/firmwares/fixedwing/ap_downlink.h | 21 +++++++++++++------ sw/airborne/firmwares/fixedwing/datalink.c | 7 ++++--- 2 files changed, 19 insertions(+), 9 deletions(-) diff --git a/sw/airborne/firmwares/fixedwing/ap_downlink.h b/sw/airborne/firmwares/fixedwing/ap_downlink.h index 90311b1acb..ea76821409 100644 --- a/sw/airborne/firmwares/fixedwing/ap_downlink.h +++ b/sw/airborne/firmwares/fixedwing/ap_downlink.h @@ -38,6 +38,7 @@ #include #include "generated/airframe.h" +#include "state.h" #ifndef DOWNLINK_DEVICE #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE @@ -86,7 +87,8 @@ #define PERIODIC_SEND_ATTITUDE(_trans, _dev) Downlink({ \ - DOWNLINK_SEND_ATTITUDE(_trans, _dev, &estimator_phi, &estimator_psi, &estimator_theta); \ + struct FloatEulers* att = stateGetNedToBodyEulers_f(); \ + DOWNLINK_SEND_ATTITUDE(_trans, _dev, &(att->phi), &(att->psi), &(att->theta)); \ }) @@ -181,9 +183,13 @@ #define PERIODIC_SEND_IMU(_trans, _dev) {} #endif -#define PERIODIC_SEND_ESTIMATOR(_trans, _dev) DOWNLINK_SEND_ESTIMATOR(_trans, _dev, &estimator_z, &estimator_z_dot) +#define PERIODIC_SEND_ESTIMATOR(_trans, _dev) DOWNLINK_SEND_ESTIMATOR(_trans, _dev, &(stateGetPositionUtm_f()->alt), &(stateGetSpeedEnu_f()->z)) -#define SEND_NAVIGATION(_trans, _dev) Downlink({ uint8_t _circle_count = NavCircleCount(); DOWNLINK_SEND_NAVIGATION(_trans, _dev, &nav_block, &nav_stage, &estimator_x, &estimator_y, &dist2_to_wp, &dist2_to_home, &_circle_count, &nav_oval_count);}) +#define SEND_NAVIGATION(_trans, _dev) Downlink({ \ + uint8_t _circle_count = NavCircleCount(); \ + struct EnuCoor_f* pos = stateGetPositionEnu_f(); \ + DOWNLINK_SEND_NAVIGATION(_trans, _dev, &nav_block, &nav_stage, &(pos->x), &(pos->y), &dist2_to_wp, &dist2_to_home, &_circle_count, &nav_oval_count); \ + }) #define PERIODIC_SEND_NAVIGATION(_trans, _dev) SEND_NAVIGATION(_trans, _dev) @@ -205,7 +211,7 @@ #define PERIODIC_SEND_RANGEFINDER(_trans, _dev) DOWNLINK_SEND_RANGEFINDER(_trans, _dev, &rangemeter, &ctl_grz_z_dot, &ctl_grz_z_dot_sum_err, &ctl_grz_z_dot_setpoint, &ctl_grz_z_sum_err, &ctl_grz_z_setpoint, &flying) -#define PERIODIC_SEND_TUNE_ROLL(_trans, _dev) DOWNLINK_SEND_TUNE_ROLL(_trans, _dev, &estimator_p,&estimator_phi, &h_ctl_roll_setpoint); +#define PERIODIC_SEND_TUNE_ROLL(_trans, _dev) DOWNLINK_SEND_TUNE_ROLL(_trans, _dev, &(stateGetBodyRates_f()->p), &(stateGetNedToBodyEulers_f()->phi), &h_ctl_roll_setpoint); #if USE_GPS || defined SITL #define PERIODIC_SEND_GPS_SOL(_trans, _dev) DOWNLINK_SEND_GPS_SOL(_trans, _dev, &gps.pacc, &gps.sacc, &gps.pdop, &gps.num_sv) @@ -292,9 +298,12 @@ #endif #ifdef MEASURE_AIRSPEED -#define PERIODIC_SEND_AIRSPEED(_trans, _dev) DOWNLINK_SEND_AIRSPEED (_trans, _dev, &estimator_airspeed, &estimator_airspeed, &estimator_airspeed, &estimator_airspeed) +#define PERIODIC_SEND_AIRSPEED(_trans, _dev) { \ + float* airspeed = stateGetAirspeed_f(); \ + DOWNLINK_SEND_AIRSPEED (_trans, _dev, airspeed, airspeed, airspeed, airspeed); \ +} #elif USE_AIRSPEED -#define PERIODIC_SEND_AIRSPEED(_trans, _dev) DOWNLINK_SEND_AIRSPEED (_trans, _dev, &estimator_airspeed, &v_ctl_auto_airspeed_setpoint, &v_ctl_auto_airspeed_controlled, &v_ctl_auto_groundspeed_setpoint) +#define PERIODIC_SEND_AIRSPEED(_trans, _dev) DOWNLINK_SEND_AIRSPEED (_trans, _dev, stateGetAirspeed_f(), &v_ctl_auto_airspeed_setpoint, &v_ctl_auto_airspeed_controlled, &v_ctl_auto_groundspeed_setpoint) #else #define PERIODIC_SEND_AIRSPEED(_trans, _dev) {} #endif diff --git a/sw/airborne/firmwares/fixedwing/datalink.c b/sw/airborne/firmwares/fixedwing/datalink.c index 553bb828df..2e2f305c98 100644 --- a/sw/airborne/firmwares/fixedwing/datalink.c +++ b/sw/airborne/firmwares/fixedwing/datalink.c @@ -40,7 +40,7 @@ #endif // TRAFFIC_INFO #if defined NAV || defined WIND_INFO -#include "estimator.h" +#include "state.h" #include "subsystems/nav.h" #endif @@ -137,10 +137,11 @@ void dl_parse_msg(void) { wind_east = DL_WIND_INFO_east(dl_buffer); wind_north = DL_WIND_INFO_north(dl_buffer); #if !USE_AIRSPEED - estimator_airspeed = DL_WIND_INFO_airspeed(dl_buffer); + float airspeed = DL_WIND_INFO_airspeed(dl_buffer); + stateSetAirspeed_f(&airspeed); #endif #ifdef WIND_INFO_RET - DOWNLINK_SEND_WIND_INFO_RET(DefaultChannel, DefaultDevice, &wind_east, &wind_north, &estimator_airspeed); + DOWNLINK_SEND_WIND_INFO_RET(DefaultChannel, DefaultDevice, &wind_east, &wind_north, stateGetAirspeed_f()); #endif } else #endif /** WIND_INFO */ From d5cf03f4ec3d8edc96265c891729e8e0ffe13ac7 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 25 Jul 2012 17:08:04 +0200 Subject: [PATCH 27/62] [state interface] estimation in sim is now working FW can fly in simulation using basic navigation routines Most of the ahrs filters are already feeding the interface, so it should work as well The ins_neutral_* are missing for now --- .../subsystems/fixedwing/autopilot.makefile | 1 + sw/airborne/arch/sim/sim_gps.c | 2 + sw/airborne/estimator.c | 48 ++++++++----------- sw/airborne/estimator.h | 9 ++-- sw/airborne/subsystems/ahrs/ahrs_sim.c | 22 ++++----- 5 files changed, 40 insertions(+), 42 deletions(-) diff --git a/conf/firmwares/subsystems/fixedwing/autopilot.makefile b/conf/firmwares/subsystems/fixedwing/autopilot.makefile index 7dca02d30c..2a19499303 100644 --- a/conf/firmwares/subsystems/fixedwing/autopilot.makefile +++ b/conf/firmwares/subsystems/fixedwing/autopilot.makefile @@ -164,6 +164,7 @@ ap_CFLAGS += -DAP ap_srcs += $(SRC_FIRMWARE)/main_ap.c ap_srcs += $(SRC_FIXEDWING)/estimator.c ap_srcs += $(SRC_FIRMWARE)/ap_downlink.c +ap_srcs += state.c ###################################################################### diff --git a/sw/airborne/arch/sim/sim_gps.c b/sw/airborne/arch/sim/sim_gps.c index c329f6b76e..9e51ba9330 100644 --- a/sw/airborne/arch/sim/sim_gps.c +++ b/sw/airborne/arch/sim/sim_gps.c @@ -24,6 +24,8 @@ value sim_use_gps_pos(value x, value y, value z, value c, value a, value s, valu gps.course = Double_val(c) * 1e7; gps.hmsl = Double_val(a) * 1000.; gps.gspeed = Double_val(s) * 100.; + gps.ned_vel.x = gps.gspeed * cos(Double_val(c)); + gps.ned_vel.y = gps.gspeed * sin(Double_val(c)); gps.ned_vel.z = -Double_val(cl) * 100.; gps.week = 0; // FIXME gps.tow = Double_val(t) * 1000.; diff --git a/sw/airborne/estimator.c b/sw/airborne/estimator.c index 30c3b76e23..0539844040 100644 --- a/sw/airborne/estimator.c +++ b/sw/airborne/estimator.c @@ -30,13 +30,11 @@ #include #include "estimator.h" +#include "state.h" #include "mcu_periph/uart.h" #include "ap_downlink.h" #include "subsystems/gps.h" #include "subsystems/nav.h" -#ifdef EXTRA_DOWNLINK_DEVICE -#include "core/extra_pprz_dl.h" -#endif /* position in meters */ float estimator_x; @@ -84,23 +82,15 @@ float estimator_AOA; void estimator_init( void ) { - EstimatorSetPosXY(0., 0.); + struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 }; + stateSetLocalUtmOrigin_f(&utm0); + + stateSetPositionUtm_f(&utm0); + EstimatorSetAlt(0.); - EstimatorSetAtt (0., 0., 0); - - EstimatorSetSpeedPol ( 0., 0., 0.); - - EstimatorSetRate(0., 0., 0.); - -#ifdef USE_AOA - EstimatorSetAOA( 0. ); -#endif - estimator_flight_time = 0; - // FIXME? Set initial airspeed to zero if USE_AIRSPEED ? - EstimatorSetAirspeed( NOMINAL_AIRSPEED ); } @@ -202,22 +192,26 @@ void alt_kalman(float gps_z) { #endif // ALT_KALMAN void estimator_update_state_gps( void ) { - float gps_east = gps.utm_pos.east / 100.; - float gps_north = gps.utm_pos.north / 100.; + struct UtmCoor_f utm; + utm.east = gps.utm_pos.east / 100.; + utm.north = gps.utm_pos.north / 100.; + utm.zone = nav_utm_zone0; - /* Relative position to reference */ - gps_east -= nav_utm_east0; - gps_north -= nav_utm_north0; - - EstimatorSetPosXY(gps_east, gps_north); #if !USE_BARO_BMP && !USE_BARO_ETS && !USE_BARO_MS5534A float falt = gps.hmsl / 1000.; EstimatorSetAlt(falt); #endif - float fspeed = gps.gspeed / 100.; - float fclimb = -gps.ned_vel.z / 100.; - float fcourse = gps.course / 1e7; - EstimatorSetSpeedPol(fspeed, fcourse, fclimb); + utm.alt = estimator_z; + // set position + stateSetPositionUtm_f(&utm); + + struct NedCoor_f ned_vel = { + gps.ned_vel.x / 100., + gps.ned_vel.y / 100., + gps.ned_vel.z / 100. + }; + // set velocity + stateSetSpeedNed_f(&ned_vel); // Heading estimation now in ahrs_infrared diff --git a/sw/airborne/estimator.h b/sw/airborne/estimator.h index 2d01172f43..e24a468469 100644 --- a/sw/airborne/estimator.h +++ b/sw/airborne/estimator.h @@ -32,6 +32,9 @@ #include #include "std.h" + +#include "state.h" + #ifdef BARO_MS5534A #include "baro_MS5534A.h" #endif @@ -88,9 +91,9 @@ extern void alt_kalman( float ); #endif -#define GetPosX() (estimator_x) -#define GetPosY() (estimator_y) -#define GetPosAlt() (estimator_z) +#define GetPosX() (stateGetPositionEnu_f()->x) +#define GetPosY() (stateGetPositionEnu_f()->y) +#define GetPosAlt() (stateGetPositionEnu_f()->z) #ifdef ALT_KALMAN diff --git a/sw/airborne/subsystems/ahrs/ahrs_sim.c b/sw/airborne/subsystems/ahrs/ahrs_sim.c index 3dcfcf0263..0a5232ef2f 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_sim.c +++ b/sw/airborne/subsystems/ahrs/ahrs_sim.c @@ -37,7 +37,7 @@ extern float sim_r; extern bool_t ahrs_sim_available; -void compute_body_orientation_and_rates(void); +void set_body_orientation_and_rates(void); void update_ahrs_from_sim(void) { ahrs_float.ltp_to_imu_euler.phi = sim_phi; @@ -49,13 +49,13 @@ void update_ahrs_from_sim(void) { ahrs_float.imu_rate.r = sim_r; /* set quaternion and rotation matrix representations as well */ - FLOAT_QUAT_OF_EULERS(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_imu_euler); - FLOAT_RMAT_OF_EULERS(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_euler); + //FLOAT_QUAT_OF_EULERS(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_imu_euler); + //FLOAT_RMAT_OF_EULERS(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_euler); - compute_body_orientation_and_rates(); + set_body_orientation_and_rates(); #ifdef AHRS_UPDATE_FW_ESTIMATOR - ahrs_update_fw_estimator(); + //ahrs_update_fw_estimator(); #endif } @@ -89,7 +89,7 @@ void ahrs_align(void) update_ahrs_from_sim(); /* Compute initial body orientation */ - compute_body_orientation_and_rates(); + //set_body_orientation_and_rates(); ahrs.status = AHRS_RUNNING; } @@ -114,16 +114,14 @@ void ahrs_update_gps(void) { /* - * Compute body orientation and rates from imu orientation and rates + * Set state interface */ -void compute_body_orientation_and_rates(void) { +void set_body_orientation_and_rates(void) { /* set ltp_to_body to same as ltp_to_imu, currently no difference simulated */ + stateSetNedToBodyEulers_f(&ahrs_float.ltp_to_imu_euler); + stateSetBodyRates_f(&ahrs_float.imu_rate); - QUAT_COPY(ahrs_float.ltp_to_body_quat, ahrs_float.ltp_to_imu_quat); - EULERS_COPY(ahrs_float.ltp_to_body_euler, ahrs_float.ltp_to_imu_euler); - RMAT_COPY(ahrs_float.ltp_to_body_rmat, ahrs_float.ltp_to_imu_rmat); - RATES_COPY(ahrs_float.body_rate, ahrs_float.imu_rate); } From 6603e1f0dfc2a48c4fc46ce1f634a3a2046f7367 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 26 Jul 2012 13:52:14 +0200 Subject: [PATCH 28/62] [state interface] convert all extra navigation routines --- sw/airborne/subsystems/navigation/OSAMNav.c | 54 +++++++++---------- sw/airborne/subsystems/navigation/bomb.c | 8 +-- .../subsystems/navigation/discsurvey.c | 12 ++--- sw/airborne/subsystems/navigation/gls.c | 6 +-- .../navigation/nav_survey_rectangle.c | 18 +++---- .../subsystems/navigation/poly_survey_adv.c | 4 +- sw/airborne/subsystems/navigation/snav.c | 18 +++---- sw/airborne/subsystems/navigation/spiral.c | 30 +++++------ 8 files changed, 75 insertions(+), 75 deletions(-) diff --git a/sw/airborne/subsystems/navigation/OSAMNav.c b/sw/airborne/subsystems/navigation/OSAMNav.c index 11a2f27a5c..abf28882d0 100644 --- a/sw/airborne/subsystems/navigation/OSAMNav.c +++ b/sw/airborne/subsystems/navigation/OSAMNav.c @@ -1,7 +1,7 @@ #include "subsystems/navigation/OSAMNav.h" #include "subsystems/nav.h" -#include "estimator.h" +#include "state.h" #include "autopilot.h" #include "generated/flight_plan.h" @@ -47,15 +47,15 @@ bool_t InitializeFlower(uint8_t CenterWP, uint8_t EdgeWP) Flowerradius = sqrt(EdgeCurrentX*EdgeCurrentX+EdgeCurrentY*EdgeCurrentY); - TransCurrentX = estimator_x - WaypointX(Center); - TransCurrentY = estimator_y - WaypointY(Center); + TransCurrentX = stateGetPositionEnu_f()->x - WaypointX(Center); + TransCurrentY = stateGetPositionEnu_f()->y - WaypointY(Center); DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY); FlowerTheta = atan2(TransCurrentY,TransCurrentX); Fly2X = Flowerradius*cos(FlowerTheta+3.14)+WaypointX(Center); Fly2Y = Flowerradius*sin(FlowerTheta+3.14)+WaypointY(Center); - FlyFromX = estimator_x; - FlyFromY = estimator_y; + FlyFromX = stateGetPositionEnu_f()->x; + FlyFromY = stateGetPositionEnu_f()->y; if(DistanceFromCenter > Flowerradius) CFlowerStatus = Outside; @@ -69,8 +69,8 @@ bool_t InitializeFlower(uint8_t CenterWP, uint8_t EdgeWP) bool_t FlowerNav(void) { - TransCurrentX = estimator_x - WaypointX(Center); - TransCurrentY = estimator_y - WaypointY(Center); + TransCurrentX = stateGetPositionEnu_f()->x - WaypointX(Center); + TransCurrentY = stateGetPositionEnu_f()->y - WaypointY(Center); DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY); bool_t InCircle = TRUE; @@ -92,8 +92,8 @@ bool_t FlowerNav(void) FlowerTheta = atan2(TransCurrentY,TransCurrentX); Fly2X = Flowerradius*cos(FlowerTheta+3.14)+WaypointX(Center); Fly2Y = Flowerradius*sin(FlowerTheta+3.14)+WaypointY(Center); - FlyFromX = estimator_x; - FlyFromY = estimator_y; + FlyFromX = stateGetPositionEnu_f()->x; + FlyFromY = stateGetPositionEnu_f()->y; nav_init_stage(); } break; @@ -122,8 +122,8 @@ bool_t FlowerNav(void) FlowerTheta = atan2(TransCurrentY,TransCurrentX); Fly2X = Flowerradius*cos(FlowerTheta+3.14)+WaypointX(Center); Fly2Y = Flowerradius*sin(FlowerTheta+3.14)+WaypointY(Center); - FlyFromX = estimator_x; - FlyFromY = estimator_y; + FlyFromX = stateGetPositionEnu_f()->x; + FlyFromY = stateGetPositionEnu_f()->y; nav_init_stage(); } break; @@ -180,8 +180,8 @@ bool_t InitializeBungeeTakeoff(uint8_t BungeeWP) { float ThrottleB; - initialx = estimator_x; - initialy = estimator_y; + initialx = stateGetPositionEnu_f()->x; + initialy = stateGetPositionEnu_f()->y; BungeeWaypoint = BungeeWP; @@ -233,8 +233,8 @@ bool_t InitializeBungeeTakeoff(uint8_t BungeeWP) bool_t BungeeTakeoff(void) { //Translate current position so Throttle point is (0,0) - float Currentx = estimator_x-throttlePx; - float Currenty = estimator_y-throttlePy; + float Currentx = stateGetPositionEnu_f()->x-throttlePx; + float Currenty = stateGetPositionEnu_f()->y-throttlePy; bool_t CurrentAboveLine; float ThrottleB; @@ -249,10 +249,10 @@ bool_t BungeeTakeoff(void) kill_throttle = 1; //recalculate lines if below min speed - if(estimator_hspeed_mod < Takeoff_MinSpeed) + if((*stateGetHorizontalSpeedNorm_f()) < Takeoff_MinSpeed) { - initialx = estimator_x; - initialy = estimator_y; + initialx = stateGetPositionEnu_f()->x; + initialy = stateGetPositionEnu_f()->y; //Translate initial position so that the position of the bungee is (0,0) Currentx = initialx-(WaypointX(BungeeWaypoint)); @@ -294,7 +294,7 @@ bool_t BungeeTakeoff(void) CurrentAboveLine = FALSE; //Find out if UAV has crossed the line - if(AboveLine != CurrentAboveLine && estimator_hspeed_mod > Takeoff_MinSpeed) + if(AboveLine != CurrentAboveLine && (*stateGetHorizontalSpeedNorm_f()) > Takeoff_MinSpeed) { CTakeoffStatus = Throttle; kill_throttle = 0; @@ -308,7 +308,7 @@ bool_t BungeeTakeoff(void) nav_route_xy(initialx,initialy,throttlePx,throttlePy); kill_throttle = 0; - if((estimator_z > BungeeAlt+Takeoff_Height-10) && (estimator_hspeed_mod > Takeoff_Speed)) + if((stateGetPositionEnu_f()->z > BungeeAlt+Takeoff_Height-10) && ((*stateGetHorizontalSpeedNorm_f()) > Takeoff_Speed)) { CTakeoffStatus = Finished; return FALSE; @@ -573,7 +573,7 @@ bool_t PolygonSurvey(void) //follow the circle nav_circle_XY(C.x, C.y, SurveyRadius); - if(NavQdrCloseTo(SurveyCircleQdr) && NavCircleCount() > .1 && estimator_z > waypoints[SurveyEntryWP].a-10) + if(NavQdrCloseTo(SurveyCircleQdr) && NavCircleCount() > .1 && stateGetPositionEnu_f()->z > waypoints[SurveyEntryWP].a-10) { CSurveyStatus = Sweep; nav_init_stage(); @@ -806,7 +806,7 @@ bool_t VerticalRaster(uint8_t l1, uint8_t l2, float radius, float AltSweep) { break; case LTC2: nav_circle_XY(l2_c2.x, l2_c2.y, -radius); - if (NavQdrCloseTo(DegOfRad(qdr_out_2_2)+10) && estimator_z >= (waypoints[l1].a-10)) { + if (NavQdrCloseTo(DegOfRad(qdr_out_2_2)+10) && stateGetPositionEnu_f()->z >= (waypoints[l1].a-10)) { line_status = LQC22; nav_init_stage(); } @@ -835,7 +835,7 @@ bool_t VerticalRaster(uint8_t l1, uint8_t l2, float radius, float AltSweep) { break; case LTC1: nav_circle_XY(l1_c2.x, l1_c2.y, -radius); - if (NavQdrCloseTo(DegOfRad(qdr_out_2_2 + M_PI)+10) && estimator_z >= (waypoints[l1].a-5)) { + if (NavQdrCloseTo(DegOfRad(qdr_out_2_2 + M_PI)+10) && stateGetPositionEnu_f()->z >= (waypoints[l1].a-5)) { line_status = LQC11; nav_init_stage(); } @@ -893,7 +893,7 @@ bool_t InitializeSkidLanding(uint8_t AFWP, uint8_t TDWP, float radius) TDWaypoint = TDWP; CLandingStatus = CircleDown; LandRadius = radius; - LandAppAlt = estimator_z; + LandAppAlt = stateGetPositionEnu_f()->z; FinalLandAltitude = Landing_FinalHeight; FinalLandCount = 1; waypoints[AFWaypoint].a = waypoints[TDWaypoint].a + Landing_AFHeight; @@ -942,7 +942,7 @@ bool_t SkidLanding(void) nav_circle_XY(LandCircle.x, LandCircle.y, LandRadius); - if(estimator_z < waypoints[AFWaypoint].a + 5) + if(stateGetPositionEnu_f()->z < waypoints[AFWaypoint].a + 5) { CLandingStatus = LandingWait; nav_init_stage(); @@ -1017,8 +1017,8 @@ bool_t FlightLine(uint8_t From_WP, uint8_t To_WP, float radius, float Space_Befo V.y = WaypointY(To_WP) - WaypointY(From_WP); //Record Aircraft Position - P.x = estimator_x; - P.y = estimator_y; + P.x = stateGetPositionEnu_f()->x; + P.y = stateGetPositionEnu_f()->y; //Rotate Aircraft Position so V is aligned with x axis TranslateAndRotateFromWorld(&P, atan2(V.y,V.x), WaypointX(From_WP), WaypointY(From_WP)); diff --git a/sw/airborne/subsystems/navigation/bomb.c b/sw/airborne/subsystems/navigation/bomb.c index 733109994c..a3c8d5ead0 100644 --- a/sw/airborne/subsystems/navigation/bomb.c +++ b/sw/airborne/subsystems/navigation/bomb.c @@ -22,7 +22,7 @@ * */ -#include "estimator.h" +#include "state.h" #include "subsystems/nav.h" #include "subsystems/navigation/bomb.h" #include "generated/flight_plan.h" @@ -93,12 +93,12 @@ static void integrate( uint8_t wp_target ) { /** Update the RELEASE location with the actual ground speed and altitude */ unit_t bomb_update_release( uint8_t wp_target ) { - bomb_z = estimator_z - waypoints[wp_target].a; + bomb_z = stateGetPositionEnu_f()->z - waypoints[wp_target].a; bomb_x = 0.; bomb_y = 0.; - bomb_vx = estimator_hspeed_mod * sin(estimator_hspeed_dir); - bomb_vy = estimator_hspeed_mod * cos(estimator_hspeed_dir); + bomb_vx = (*stateGetHorizontalSpeedNorm_f()) * sin((*stateGetHorizontalSpeedDir_f())); + bomb_vy = (*stateGetHorizontalSpeedNorm_f()) * cos((*stateGetHorizontalSpeedDir_f())); bomb_vz = 0.; integrate(wp_target); diff --git a/sw/airborne/subsystems/navigation/discsurvey.c b/sw/airborne/subsystems/navigation/discsurvey.c index 1a56f3b8f2..2feadf41f0 100644 --- a/sw/airborne/subsystems/navigation/discsurvey.c +++ b/sw/airborne/subsystems/navigation/discsurvey.c @@ -1,7 +1,7 @@ #include "subsystems/navigation/discsurvey.h" #include "generated/airframe.h" -#include "estimator.h" +#include "state.h" #include "std.h" #include "subsystems/nav.h" #include "generated/flight_plan.h" @@ -18,8 +18,8 @@ bool_t disc_survey_init( float grid ) { nav_survey_shift = grid; status = DOWNWIND; sign = 1; - c1.x = estimator_x; - c1.y = estimator_y; + c1.x = stateGetPositionEnu_f()->x; + c1.y = stateGetPositionEnu_f()->y; return FALSE; } @@ -36,10 +36,10 @@ bool_t disc_survey( uint8_t center, float radius) { case UTURN: nav_circle_XY(c.x, c.y, grid*sign); if (NavQdrCloseTo(DegOfRad(M_PI_2-wind_dir))) { - c1.x = estimator_x; - c1.y = estimator_y; + c1.x = stateGetPositionEnu_f()->x; + c1.y = stateGetPositionEnu_f()->y; - float d = ScalarProduct(upwind_x, upwind_y, estimator_x-WaypointX(center), estimator_y-WaypointY(center)); + float d = ScalarProduct(upwind_x, upwind_y, stateGetPositionEnu_f()->x-WaypointX(center), stateGetPositionEnu_f()->y-WaypointY(center)); if (d > radius) { status = DOWNWIND; } else { diff --git a/sw/airborne/subsystems/navigation/gls.c b/sw/airborne/subsystems/navigation/gls.c index 8f22c9548a..dc8d271496 100644 --- a/sw/airborne/subsystems/navigation/gls.c +++ b/sw/airborne/subsystems/navigation/gls.c @@ -45,7 +45,7 @@ #include "generated/airframe.h" -#include "estimator.h" +#include "state.h" #include "subsystems/navigation/gls.h" #include "subsystems/nav.h" #include "generated/flight_plan.h" @@ -137,11 +137,11 @@ bool_t gls(uint8_t _af, uint8_t _tod, uint8_t _td) { float final_y = WaypointY(_td) - WaypointY(_tod); float final2 = Max(final_x * final_x + final_y * final_y, 1.); - float nav_final_progress = ((estimator_x - WaypointX(_tod)) * final_x + (estimator_y - WaypointY(_tod)) * final_y) / final2; + float nav_final_progress = ((stateGetPositionEnu_f()->x - WaypointX(_tod)) * final_x + (stateGetPositionEnu_f()->y - WaypointY(_tod)) * final_y) / final2; Bound(nav_final_progress,-1,1); float nav_final_length = sqrt(final2); - float pre_climb = -(WaypointAlt(_tod) - WaypointAlt(_td)) / (nav_final_length / estimator_hspeed_mod); + float pre_climb = -(WaypointAlt(_tod) - WaypointAlt(_td)) / (nav_final_length / (*stateGetHorizontalSpeedNorm_f())); Bound(pre_climb, -5, 0.); float start_alt = WaypointAlt(_tod); diff --git a/sw/airborne/subsystems/navigation/nav_survey_rectangle.c b/sw/airborne/subsystems/navigation/nav_survey_rectangle.c index 6779748ce7..8698f6d937 100644 --- a/sw/airborne/subsystems/navigation/nav_survey_rectangle.c +++ b/sw/airborne/subsystems/navigation/nav_survey_rectangle.c @@ -1,5 +1,5 @@ #include "subsystems/navigation/nav_survey_rectangle.h" -#include "estimator.h" +#include "state.h" static struct point survey_from; static struct point survey_to; @@ -29,8 +29,8 @@ void nav_survey_rectangle_init(uint8_t wp1, uint8_t wp2, float grid, survey_orie survey_orientation = so; if (survey_orientation == NS) { - survey_from.x = survey_to.x = Min(Max(estimator_x, nav_survey_west+grid/2.), nav_survey_east-grid/2.); - if (estimator_y > nav_survey_north || (estimator_y > nav_survey_south && estimator_hspeed_dir > M_PI/2. && estimator_hspeed_dir < 3*M_PI/2)) { + survey_from.x = survey_to.x = Min(Max(stateGetPositionEnu_f()->x, nav_survey_west+grid/2.), nav_survey_east-grid/2.); + if (stateGetPositionEnu_f()->y > nav_survey_north || (stateGetPositionEnu_f()->y > nav_survey_south && (*stateGetHorizontalSpeedDir_f()) > M_PI/2. && (*stateGetHorizontalSpeedDir_f()) < 3*M_PI/2)) { survey_to.y = nav_survey_south; survey_from.y = nav_survey_north; } else { @@ -38,8 +38,8 @@ void nav_survey_rectangle_init(uint8_t wp1, uint8_t wp2, float grid, survey_orie survey_to.y = nav_survey_north; } } else { /* survey_orientation == WE */ - survey_from.y = survey_to.y = Min(Max(estimator_y, nav_survey_south+grid/2.), nav_survey_north-grid/2.); - if (estimator_x > nav_survey_east || (estimator_x > nav_survey_west && estimator_hspeed_dir > M_PI)) { + survey_from.y = survey_to.y = Min(Max(stateGetPositionEnu_f()->y, nav_survey_south+grid/2.), nav_survey_north-grid/2.); + if (stateGetPositionEnu_f()->x > nav_survey_east || (stateGetPositionEnu_f()->x > nav_survey_west && (*stateGetHorizontalSpeedDir_f()) > M_PI)) { survey_to.x = nav_survey_west; survey_from.x = nav_survey_east; } else { @@ -79,10 +79,10 @@ void nav_survey_rectangle(uint8_t wp1, uint8_t wp2) { } if (! survey_uturn) { /* S-N, N-S, W-E or E-W straight route */ - if ((estimator_y < nav_survey_north && SurveyGoingNorth()) || - (estimator_y > nav_survey_south && SurveyGoingSouth()) || - (estimator_x < nav_survey_east && SurveyGoingEast()) || - (estimator_x > nav_survey_west && SurveyGoingWest())) { + if ((stateGetPositionEnu_f()->y < nav_survey_north && SurveyGoingNorth()) || + (stateGetPositionEnu_f()->y > nav_survey_south && SurveyGoingSouth()) || + (stateGetPositionEnu_f()->x < nav_survey_east && SurveyGoingEast()) || + (stateGetPositionEnu_f()->x > nav_survey_west && SurveyGoingWest())) { /* Continue ... */ nav_route_xy(survey_from.x, survey_from.y, survey_to.x, survey_to.y); } else { diff --git a/sw/airborne/subsystems/navigation/poly_survey_adv.c b/sw/airborne/subsystems/navigation/poly_survey_adv.c index 73f6e74852..de25066e87 100644 --- a/sw/airborne/subsystems/navigation/poly_survey_adv.c +++ b/sw/airborne/subsystems/navigation/poly_survey_adv.c @@ -1,7 +1,7 @@ #include "poly_survey_adv.h" #include "subsystems/nav.h" -#include "estimator.h" +#include "state.h" #include "autopilot.h" #include "generated/flight_plan.h" @@ -276,7 +276,7 @@ bool_t poly_survey_adv(void) nav_circle_XY(entry_center.x, entry_center.y, -psa_min_rad); if (NavCourseCloseTo(segment_angle) && nav_approaching_xy(seg_start.x, seg_start.y, last_x, last_y, CARROT) - && fabs(estimator_z - psa_altitude) <= 20) { + && fabs(stateGetPositionEnu_f()->z - psa_altitude) <= 20) { psa_stage = SEG; NavVerticalAutoThrottleMode(0.0); nav_init_stage(); diff --git a/sw/airborne/subsystems/navigation/snav.c b/sw/airborne/subsystems/navigation/snav.c index b4a659a0e7..ffa9706238 100644 --- a/sw/airborne/subsystems/navigation/snav.c +++ b/sw/airborne/subsystems/navigation/snav.c @@ -4,7 +4,7 @@ #include #include "generated/airframe.h" #include "subsystems/navigation/snav.h" -#include "estimator.h" +#include "state.h" #include "subsystems/nav.h" #include "subsystems/gps.h" @@ -25,15 +25,15 @@ bool_t snav_init(uint8_t a, float desired_course_rad, float radius) { wp_a = a; radius = fabs(radius); - float da_x = WaypointX(wp_a) - estimator_x; - float da_y = WaypointY(wp_a) - estimator_y; + float da_x = WaypointX(wp_a) - stateGetPositionEnu_f()->x; + float da_y = WaypointY(wp_a) - stateGetPositionEnu_f()->y; /* D_CD orthogonal to current course, CD on the side of A */ - float u_x = cos(M_PI_2 - estimator_hspeed_dir); - float u_y = sin(M_PI_2 - estimator_hspeed_dir); + float u_x = cos(M_PI_2 - (*stateGetHorizontalSpeedDir_f())); + float u_y = sin(M_PI_2 - (*stateGetHorizontalSpeedDir_f())); d_radius = - Sign(u_x*da_y - u_y*da_x) * radius; - wp_cd.x = estimator_x + d_radius * u_y; - wp_cd.y = estimator_y - d_radius * u_x; + wp_cd.x = stateGetPositionEnu_f()->x + d_radius * u_y; + wp_cd.y = stateGetPositionEnu_f()->y - d_radius * u_x; wp_cd.a = WaypointAlt(wp_a); /* A_CA orthogonal to desired course, CA on the side of D */ @@ -148,12 +148,12 @@ static void compute_ground_speed(float airspeed, bool_t snav_on_time(float nominal_radius) { nominal_radius = fabs(nominal_radius); - float current_qdr = M_PI_2 - atan2(estimator_y-wp_ca.y, estimator_x-wp_ca.x); + float current_qdr = M_PI_2 - atan2(stateGetPositionEnu_f()->y-wp_ca.y, stateGetPositionEnu_f()->x-wp_ca.x); float remaining_angle = Norm2Pi(Sign(a_radius)*(qdr_a - current_qdr)); float remaining_time = snav_desired_tow - gps.tow / 1000.; /* Use the nominal airspeed if the estimated one is not realistic */ - float airspeed = estimator_airspeed; + float airspeed = *stateGetAirspeed_f(); if (airspeed < NOMINAL_AIRSPEED / 2. || airspeed > 2.* NOMINAL_AIRSPEED) airspeed = NOMINAL_AIRSPEED; diff --git a/sw/airborne/subsystems/navigation/spiral.c b/sw/airborne/subsystems/navigation/spiral.c index 7d47c57d41..d08cb2576d 100644 --- a/sw/airborne/subsystems/navigation/spiral.c +++ b/sw/airborne/subsystems/navigation/spiral.c @@ -9,7 +9,7 @@ #include "subsystems/navigation/spiral.h" #include "subsystems/nav.h" -#include "estimator.h" +#include "state.h" #include "autopilot.h" #include "generated/flight_plan.h" @@ -64,9 +64,9 @@ bool_t InitializeSpiral(uint8_t CenterWP, uint8_t EdgeWP, float StartRad, float Spiralradius = sqrt(EdgeCurrentX*EdgeCurrentX+EdgeCurrentY*EdgeCurrentY); - TransCurrentX = estimator_x - WaypointX(Center); - TransCurrentY = estimator_y - WaypointY(Center); - TransCurrentZ = estimator_z - ZPoint; + TransCurrentX = stateGetPositionEnu_f()->x - WaypointX(Center); + TransCurrentY = stateGetPositionEnu_f()->y - WaypointY(Center); + TransCurrentZ = stateGetPositionEnu_f()->z - ZPoint; DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY); // SpiralTheta = atan2(TransCurrentY,TransCurrentX); @@ -76,8 +76,8 @@ bool_t InitializeSpiral(uint8_t CenterWP, uint8_t EdgeWP, float StartRad, float // Alphalimit denotes angle, where the radius will be increased Alphalimit = 2*M_PI / Segments; //current position - FlyFromX = estimator_x; - FlyFromY = estimator_y; + FlyFromX = stateGetPositionEnu_f()->x; + FlyFromY = stateGetPositionEnu_f()->y; if(DistanceFromCenter > Spiralradius) CSpiralStatus = Outside; @@ -86,8 +86,8 @@ bool_t InitializeSpiral(uint8_t CenterWP, uint8_t EdgeWP, float StartRad, float bool_t SpiralNav(void) { - TransCurrentX = estimator_x - WaypointX(Center); - TransCurrentY = estimator_y - WaypointY(Center); + TransCurrentX = stateGetPositionEnu_f()->x - WaypointX(Center); + TransCurrentY = stateGetPositionEnu_f()->y - WaypointY(Center); DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY); float DistanceStartEstim; @@ -113,8 +113,8 @@ bool_t SpiralNav(void) // calculation needed, State switch to Circle nav_circle_XY(WaypointX(Center), WaypointY(Center), SRad); if(DistanceFromCenter >= SRad){ - LastCircleX = estimator_x; - LastCircleY = estimator_y; + LastCircleX = stateGetPositionEnu_f()->x; + LastCircleY = stateGetPositionEnu_f()->y; CSpiralStatus = Circle; // Start helix #ifdef DIGITAL_CAM @@ -130,12 +130,12 @@ bool_t SpiralNav(void) // if alphamax already reached, increase radius. //DistanceStartEstim = |Starting position angular - current positon| - DistanceStartEstim = sqrt (((LastCircleX-estimator_x)*(LastCircleX-estimator_x)) - + ((LastCircleY-estimator_y)*(LastCircleY-estimator_y))); + DistanceStartEstim = sqrt (((LastCircleX-stateGetPositionEnu_f()->x)*(LastCircleX-stateGetPositionEnu_f()->x)) + + ((LastCircleY-stateGetPositionEnu_f()->y)*(LastCircleY-stateGetPositionEnu_f()->y))); CircleAlpha = (2.0 * asin (DistanceStartEstim / (2 * SRad))); if (CircleAlpha >= Alphalimit) { - LastCircleX = estimator_x; - LastCircleY = estimator_y; + LastCircleX = stateGetPositionEnu_f()->x; + LastCircleY = stateGetPositionEnu_f()->y; CSpiralStatus = IncSpiral; } break; @@ -148,7 +148,7 @@ bool_t SpiralNav(void) #ifdef DIGITAL_CAM if (dc_cam_tracing) { // calculating Cam angle for camera alignment - TransCurrentZ = estimator_z - ZPoint; + TransCurrentZ = stateGetPositionEnu_f()->z - ZPoint; dc_cam_angle = atan(SRad/TransCurrentZ) * 180 / M_PI; } #endif From 0e9f8ed9476c66704a549ac20491f5fdb75bc616 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 26 Jul 2012 17:36:10 +0200 Subject: [PATCH 29/62] [state interface] convert modules to new state interface Nothing has been tested and a lot of code could be optimized, especially the ins modules --- .../arch/sim/modules/ins/ins_arduimu.c | 10 ++-- .../arch/sim/modules/ins/ins_arduimu_basic.c | 13 +++-- .../airborne_ant_track/airborne_ant_track.c | 12 ++--- .../modules/benchmark/flight_benchmark.c | 16 +++---- sw/airborne/modules/cam_control/cam.c | 13 +++-- sw/airborne/modules/cam_control/cam_roll.c | 4 +- sw/airborne/modules/cam_control/cam_segment.c | 1 - sw/airborne/modules/cartography/cartography.c | 48 +++++++++---------- .../modules/digital_cam/atmega_i2c_cam_ctrl.c | 1 - sw/airborne/modules/digital_cam/dc.c | 18 +++---- sw/airborne/modules/digital_cam/dc.h | 8 ++-- .../modules/digital_cam/sim_i2c_cam_ctrl.c | 2 +- sw/airborne/modules/enose/anemotaxis.c | 20 ++++---- sw/airborne/modules/enose/chemotaxis.c | 14 +++--- sw/airborne/modules/ins/fw_ins_vn100.c | 10 +++- sw/airborne/modules/ins/ins_arduimu.c | 14 +++--- sw/airborne/modules/ins/ins_arduimu_basic.c | 21 ++++---- sw/airborne/modules/ins/ins_chimu_spi.c | 16 +++++-- sw/airborne/modules/ins/ins_chimu_uart.c | 10 ++-- sw/airborne/modules/ins/ins_xsens.c | 28 +++++++++-- sw/airborne/modules/multi/formation.c | 38 +++++++-------- sw/airborne/modules/multi/potential.c | 18 +++---- sw/airborne/modules/multi/tcas.c | 20 ++++---- sw/airborne/modules/nav/nav_catapult.c | 10 ++-- sw/airborne/modules/sensors/AOA_adc.c | 1 + sw/airborne/modules/sensors/airspeed_adc.c | 6 +-- sw/airborne/modules/sensors/airspeed_amsys.c | 6 +-- sw/airborne/modules/sensors/airspeed_ets.c | 6 +-- sw/airborne/modules/sensors/baro_MS5534A.c | 1 + sw/airborne/modules/sensors/baro_amsys.c | 1 + sw/airborne/modules/sensors/baro_bmp.c | 1 + sw/airborne/modules/sensors/baro_ets.c | 1 + sw/airborne/modules/sensors/mag_hmc5843.c | 1 - .../modules/sensors/pressure_board_navarro.c | 4 +- 34 files changed, 221 insertions(+), 172 deletions(-) diff --git a/sw/airborne/arch/sim/modules/ins/ins_arduimu.c b/sw/airborne/arch/sim/modules/ins/ins_arduimu.c index 0fd1d8bace..4ff9503db8 100644 --- a/sw/airborne/arch/sim/modules/ins/ins_arduimu.c +++ b/sw/airborne/arch/sim/modules/ins/ins_arduimu.c @@ -4,7 +4,7 @@ #include "generated/airframe.h" -#include "estimator.h" +#include "state.h" // Arduimu empty implementation #include "modules/ins/ins_arduimu.h" @@ -25,8 +25,12 @@ extern float sim_theta; void ArduIMU_init( void ) {} void ArduIMU_periodic( void ) { // Feed directly the estimator - estimator_phi = sim_phi - ins_roll_neutral; - estimator_theta = sim_theta - ins_pitch_neutral; + struct FloatEulers att = { + sim_phi - ins_roll_neutral, + sim_theta - ins_pitch_neutral, + 0. + }; + stateSetNedToBodyEulers_f(&att); } void ArduIMU_periodicGPS( void ) {} void IMU_Daten_verarbeiten( void ) {} diff --git a/sw/airborne/arch/sim/modules/ins/ins_arduimu_basic.c b/sw/airborne/arch/sim/modules/ins/ins_arduimu_basic.c index 5ba78b7070..3ced3fcc49 100644 --- a/sw/airborne/arch/sim/modules/ins/ins_arduimu_basic.c +++ b/sw/airborne/arch/sim/modules/ins/ins_arduimu_basic.c @@ -27,11 +27,14 @@ extern float sim_r; void ArduIMU_init( void ) {} void ArduIMU_periodic( void ) { // Feed directly the estimator - estimator_phi = sim_phi - ins_roll_neutral; - estimator_theta = sim_theta - ins_pitch_neutral; - estimator_p = sim_p; - estimator_q = sim_q; - estimator_r = sim_r; + struct FloatEulers att = { + sim_phi - ins_roll_neutral, + sim_theta - ins_pitch_neutral, + 0. + }; + stateSetNedToBodyEulers_f(&att); + struct FloatRates rates = { sim_p, sim_q, sim_r }; + stateSetBodyRates_f(&rates); } void ArduIMU_periodicGPS( void ) {} void ArduIMU_event( void ) {} diff --git a/sw/airborne/modules/airborne_ant_track/airborne_ant_track.c b/sw/airborne/modules/airborne_ant_track/airborne_ant_track.c index 608b9b242c..c40a5cb84f 100644 --- a/sw/airborne/modules/airborne_ant_track/airborne_ant_track.c +++ b/sw/airborne/modules/airborne_ant_track/airborne_ant_track.c @@ -23,7 +23,7 @@ #include "subsystems/navigation/common_nav.h" #include "autopilot.h" #include "generated/flight_plan.h" -#include "estimator.h" +#include "state.h" #include "subsystems/navigation/traffic_info.h" #include "airborne_ant_track.h" @@ -85,9 +85,9 @@ float airborne_ant_pan_servo = 0; static MATRIX smRotation; - svPlanePosition.fx = estimator_y; - svPlanePosition.fy = estimator_x; - svPlanePosition.fz = estimator_z; + svPlanePosition.fx = stateGetPositionEnu_f()->y; + svPlanePosition.fy = stateGetPositionEnu_f()->x; + svPlanePosition.fz = stateGetPositionEnu_f()->z; Home_Position.fx = WaypointY(WP_HOME); Home_Position.fy = WaypointX(WP_HOME); @@ -97,8 +97,8 @@ float airborne_ant_pan_servo = 0; vSubtractVectors(&Home_PositionForPlane, Home_Position, svPlanePosition); /* yaw */ - smRotation.fx1 = (float)(cos(estimator_hspeed_dir)); - smRotation.fx2 = (float)(sin(estimator_hspeed_dir)); + smRotation.fx1 = (float)(cos((*stateGetHorizontalSpeedDir_f()))); + smRotation.fx2 = (float)(sin((*stateGetHorizontalSpeedDir_f()))); smRotation.fx3 = 0.; smRotation.fy1 = -smRotation.fx2; smRotation.fy2 = smRotation.fx1; diff --git a/sw/airborne/modules/benchmark/flight_benchmark.c b/sw/airborne/modules/benchmark/flight_benchmark.c index be1c0700cc..7c352bc5d1 100644 --- a/sw/airborne/modules/benchmark/flight_benchmark.c +++ b/sw/airborne/modules/benchmark/flight_benchmark.c @@ -7,7 +7,7 @@ #include "firmwares/fixedwing/guidance/guidance_v.h" -#include "estimator.h" +#include "state.h" #include "messages.h" #include "subsystems/datalink/downlink.h" #include "mcu_periph/uart.h" @@ -60,7 +60,7 @@ void flight_benchmark_periodic( void ) { if (benchm_go){ #if USE_AIRSPEED && defined(BENCHMARK_AIRSPEED) - Err_airspeed = fabs(estimator_airspeed - v_ctl_auto_airspeed_setpoint); + Err_airspeed = fabs(*stateGetAirspeed_f() - v_ctl_auto_airspeed_setpoint); if (Err_airspeed>ToleranceAispeed){ Err_airspeed = Err_airspeed-ToleranceAispeed; SquareSumErr_airspeed += (Err_airspeed * Err_airspeed); @@ -68,7 +68,7 @@ void flight_benchmark_periodic( void ) { #endif #ifdef BENCHMARK_ALTITUDE - Err_altitude = fabs(estimator_z - v_ctl_altitude_setpoint); + Err_altitude = fabs(stateGetPositionEnu_f()->z - v_ctl_altitude_setpoint); if (Err_altitude>ToleranceAltitude){ Err_altitude = Err_altitude-ToleranceAltitude; SquareSumErr_altitude += (Err_altitude * Err_altitude); @@ -79,7 +79,7 @@ void flight_benchmark_periodic( void ) { //---------------This part is a waste of memory and calculation power - but it works - feel free to optimize it ;-) ----------------- - // err_temp = waypoints[target].x - estimator_x; + // err_temp = waypoints[target].x - stateGetPositionEnu_f()->x; float deltaPlaneX = 0; float deltaPlaneY = 0; float Err_position_segment = 0; @@ -89,16 +89,16 @@ void flight_benchmark_periodic( void ) { float deltaX = nav_segment_x_2 - nav_segment_x_1; float deltaY = nav_segment_y_2 - nav_segment_y_1; float anglePath = atan2(deltaX,deltaY); - deltaPlaneX = nav_segment_x_2 - estimator_x; - deltaPlaneY = nav_segment_y_2 - estimator_y; + deltaPlaneX = nav_segment_x_2 - stateGetPositionEnu_f()->x; + deltaPlaneY = nav_segment_y_2 - stateGetPositionEnu_f()->y; float anglePlane = atan2(deltaPlaneX,deltaPlaneY); float angleDiff = fabs(anglePlane - anglePath); Err_position_segment = fabs(sin(angleDiff)*sqrt(deltaPlaneX*deltaPlaneX+deltaPlaneY*deltaPlaneY)); // } // if (nav_in_circle){ - deltaPlaneX = nav_circle_x - estimator_x; - deltaPlaneY = nav_circle_y - estimator_y; + deltaPlaneX = nav_circle_x - stateGetPositionEnu_f()->x; + deltaPlaneY = nav_circle_y - stateGetPositionEnu_f()->y; Err_position_circle = fabs(sqrt(deltaPlaneX*deltaPlaneX+deltaPlaneY*deltaPlaneY)-nav_circle_radius); // } if (Err_position_circle < Err_position_segment){ diff --git a/sw/airborne/modules/cam_control/cam.c b/sw/airborne/modules/cam_control/cam.c index 6e5c1877c8..6a738754ff 100644 --- a/sw/airborne/modules/cam_control/cam.c +++ b/sw/airborne/modules/cam_control/cam.c @@ -31,7 +31,7 @@ #include "subsystems/navigation/common_nav.h" //needed for WaypointX, WaypointY and ground_alt #include "autopilot.h" #include "generated/flight_plan.h" -#include "estimator.h" +#include "state.h" #include "subsystems/navigation/traffic_info.h" #ifdef POINT_CAM #include "point.h" @@ -234,8 +234,10 @@ void cam_target( void ) { cam_target_x, cam_target_y, cam_target_alt, &cam_pan_c, &cam_tilt_c); #else - vPoint(estimator_x, estimator_y, estimator_z, - estimator_phi, estimator_theta, estimator_hspeed_dir, + struct EnuCoor_f* pos = stateGetPositionEnu_f(); + struct FloatEulers* att = stateGetNedToBodyEulers_f(); + vPoint(pos->x, pos->y, pos->z, + att->phi, att->theta, *stateGetHorizontalSpeedDir_f(), cam_target_x, cam_target_y, cam_target_alt, &cam_pan_c, &cam_tilt_c); #endif @@ -244,12 +246,13 @@ void cam_target( void ) { /** Point straight down */ void cam_nadir( void ) { + struct EnuCoor_f* pos = stateGetPositionEnu_f(); #ifdef TEST_CAM cam_target_x = test_cam_estimator_x; cam_target_y = test_cam_estimator_y; #else - cam_target_x = estimator_x; - cam_target_y = estimator_y; + cam_target_x = pos->x; + cam_target_y = pos->y; #endif cam_target_alt = -10; cam_target(); diff --git a/sw/airborne/modules/cam_control/cam_roll.c b/sw/airborne/modules/cam_control/cam_roll.c index 39cd30bbac..dd3a86acf1 100644 --- a/sw/airborne/modules/cam_control/cam_roll.c +++ b/sw/airborne/modules/cam_control/cam_roll.c @@ -30,7 +30,7 @@ #include "subsystems/nav.h" #include "autopilot.h" #include "generated/flight_plan.h" -#include "estimator.h" +#include "state.h" #include "inter_mcu.h" #ifndef CAM_PHI_MAX @@ -62,7 +62,7 @@ void cam_init( void ) { void cam_periodic( void ) { switch (cam_roll_mode) { case MODE_STABILIZED: - phi_c = cam_roll_phi + estimator_phi; + phi_c = cam_roll_phi + stateGetNedToBodyEulers_f()->phi; break; case MODE_MANUAL: phi_c = cam_roll_phi; diff --git a/sw/airborne/modules/cam_control/cam_segment.c b/sw/airborne/modules/cam_control/cam_segment.c index 1b6d104044..67e39c69e9 100644 --- a/sw/airborne/modules/cam_control/cam_segment.c +++ b/sw/airborne/modules/cam_control/cam_segment.c @@ -30,7 +30,6 @@ #include "modules/cam_control/cam_segment.h" #include "modules/cam_control/cam.h" #include "subsystems/nav.h" -#include "estimator.h" void cam_segment_init( void ) { } diff --git a/sw/airborne/modules/cartography/cartography.c b/sw/airborne/modules/cartography/cartography.c index 1fa3aa897f..6c56ba2e4c 100644 --- a/sw/airborne/modules/cartography/cartography.c +++ b/sw/airborne/modules/cartography/cartography.c @@ -29,7 +29,7 @@ -#include "estimator.h" +#include "state.h" #include "stdio.h" #include "subsystems/nav.h" @@ -210,10 +210,10 @@ bool_t nav_survey_computefourth_corner(uint8_t wp1, uint8_t wp2, uint8_t wp3, u } //////////////////////////////////////////////////////////////////////////////////////////////// -bool_t nav_survey_ComputeProjectionOnLine(struct point pointAf,struct point pointBf,float estimator_xf,float estimator_yf,float *normAMf,float *normBMf,float *distancefromrailf); +bool_t nav_survey_ComputeProjectionOnLine(struct point pointAf,struct point pointBf,float pos_xf,float pos_yf,float *normAMf,float *normBMf,float *distancefromrailf); -bool_t nav_survey_ComputeProjectionOnLine(struct point pointAf,struct point pointBf,float estimator_xf,float estimator_yf,float *normAMf,float *normBMf,float *distancefromrailf) +bool_t nav_survey_ComputeProjectionOnLine(struct point pointAf,struct point pointBf,float pos_xf,float pos_yf,float *normAMf,float *normBMf,float *distancefromrailf) //return if the projection of the estimator on the AB line is located inside the AB interval { float a,b,c,xa,xb,xc,ya,yb,yc; @@ -234,8 +234,8 @@ bool_t nav_survey_ComputeProjectionOnLine(struct point pointAf,struct point poi xc=pointBf.x; yc=pointBf.y; - xa=estimator_xf; - ya=estimator_yf; + xa=pos_xf; + ya=pos_yf; //calcul des parametres de la droite pointAf pointBf a = yc - yb; @@ -427,8 +427,8 @@ bool_t nav_survey_losange_carto(void) PRTDEB(d,railnumberSinceBoot) - //PRTDEB(f,estimator_x) - //PRTDEB(f,estimator_y) + //PRTDEB(f,stateGetPositionEnu_f()->x) + //PRTDEB(f,stateGetPositionEnu_f()->y) //sortir du bloc si données abhérantes if (norm13<1e-15) @@ -465,15 +465,15 @@ bool_t nav_survey_losange_carto(void) //the following test can cause problem when the aircraft is quite close to the entry point, as it can turn around for infinte time - //if ( DISTXY(estimator_x,estimator_y,pointA.x,pointA.y) >DISTLIMIT) - //if ( DISTXY(estimator_x,estimator_y,pointA.x,pointA.y) >DISTLIMIT) + //if ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointA.x,pointA.y) >DISTLIMIT) + //if ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointA.x,pointA.y) >DISTLIMIT) - nav_survey_ComputeProjectionOnLine(pointA,pointB,estimator_x,estimator_y,&normAM,&normBM,&distancefromrail); + nav_survey_ComputeProjectionOnLine(pointA,pointB,stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,&normAM,&normBM,&distancefromrail); - if ((DISTXY(estimator_x,estimator_y,pointA.x,pointA.y) >2* DISTLIMIT) || (normBM<(DISTXY(pointB.x,pointB.y,pointA.x,pointA.y)))) + if ((DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointA.x,pointA.y) >2* DISTLIMIT) || (normBM<(DISTXY(pointB.x,pointB.y,pointA.x,pointA.y)))) { - nav_route_xy(estimator_x, estimator_y,pointA.x,pointA.y); + nav_route_xy(stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y,pointA.x,pointA.y); //nav_route_xy(pointB.x, pointB.y,pointA.x,pointA.y); } else @@ -519,15 +519,15 @@ bool_t nav_survey_losange_carto(void) // PRTDEB(f,pointA.y) // PRTDEB(f,pointB.x) // PRTDEB(f,pointB.y) - ProjectionInsideLimitOfRail=nav_survey_ComputeProjectionOnLine(pointA,pointB,estimator_x,estimator_y,&normAM,&normBM,&distancefromrail); + ProjectionInsideLimitOfRail=nav_survey_ComputeProjectionOnLine(pointA,pointB,stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,&normAM,&normBM,&distancefromrail); - // if ( ( DISTXY(estimator_x,estimator_y,pointB.x,pointB.y) >DISTLIMIT) && + // if ( ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointB.x,pointB.y) >DISTLIMIT) && // (normBM>(DISTXY(pointB.x,pointB.y,pointA.x,pointA.y)))) - if (! ( ( DISTXY(estimator_x,estimator_y,pointB.x,pointB.y) (DISTXY(pointB.x,pointB.y,pointA.x,pointA.y)))))) + if (! ( ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointB.x,pointB.y) (DISTXY(pointB.x,pointB.y,pointA.x,pointA.y)))))) // (normBM>(DISTXY(pointB.x,pointB.y,pointA.x,pointA.y)))) { nav_route_xy(pointA.x,pointA.y,pointB.x,pointB.y); @@ -536,7 +536,7 @@ bool_t nav_survey_losange_carto(void) //est ce que l'avion est dans la zone ou il doit prendre des images? //DEJA APPELE AVANT LE IF - // nav_survey_ComputeProjectionOnLine(pointA,pointB,estimator_x,estimator_y,&normAM,&normBM,&distancefromrail); + // nav_survey_ComputeProjectionOnLine(pointA,pointB,stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,&normAM,&normBM,&distancefromrail); if ( (normAM> distplus) && (normBM> distplus) && (distancefromrail M_PI) angle_between -= 2 * M_PI; while (angle_between < -M_PI) angle_between += 2 * M_PI; @@ -618,7 +618,7 @@ bool_t nav_survey_losange_carto(void) //if (angle_between> -10 && angle_between< 10) PRTDEBSTR(ON SE CASSE) NavCircleWaypoint(0,signforturn*tempcircleradius); - if ( ( DISTXY(estimator_x,estimator_y,pointB.x,pointB.y) -10 && angle_between< 10) ) + if ( ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointB.x,pointB.y) -10 && angle_between< 10) ) { //l'avion fait le rail suivant survey_losange_uturn=FALSE; @@ -652,9 +652,9 @@ bool_t nav_survey_losange_carto(void) course_next_rail=atan2(pointC.x-pointB.x,pointC.y-pointB.y); PRTDEB(f,course_next_rail ) - PRTDEB(f,estimator_hspeed_dir) + PRTDEB(f,(*stateGetHorizontalSpeedDir_f())) - angle_between=(course_next_rail-estimator_hspeed_dir); + angle_between=(course_next_rail-(*stateGetHorizontalSpeedDir_f())); while (angle_between > M_PI) angle_between -= 2 * M_PI; while (angle_between < -M_PI) angle_between += 2 * M_PI; @@ -663,7 +663,7 @@ bool_t nav_survey_losange_carto(void) //if (angle_between> -10 && angle_between< 10) PRTDEBSTR(ON SE CASSE) NavCircleWaypoint(0,signforturn*(-1)*tempcircleradius); - if (( DISTXY(estimator_x,estimator_y,pointB.x,pointB.y) -10 && angle_between< 10) ) + if (( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointB.x,pointB.y) -10 && angle_between< 10) ) { //l'avion fait le rail suivant survey_losange_uturn=FALSE; @@ -684,7 +684,7 @@ bool_t nav_survey_losange_carto(void) pointB.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail); pointB.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail); nav_route_xy(pointA.x,pointA.y,pointB.x,pointB.y); - if ( DISTXY(estimator_x,estimator_y,pointB.x,pointB.y) x,stateGetPositionEnu_f()->y,pointB.x,pointB.y) x,stateGetPositionEnu_f()->y,pointB.x,pointB.y) phi*10.0f); + int16_t theta = DegOfRad(stateGetNedToBodyEulers_f()->theta*10.0f); float gps_z = ((float)gps.hmsl) / 1000.0f; int16_t course = (DegOfRad(gps.course)/((int32_t)1e6)); int16_t photo_nr = -1; @@ -88,11 +88,11 @@ void dc_send_shot_position(void) uint8_t dc_info(void) { #ifdef DOWNLINK_SEND_DC_INFO - float course = DegOfRad(estimator_psi); + float course = DegOfRad(stateGetNedToBodyEulers_f()->psi); DOWNLINK_SEND_DC_INFO(DefaultChannel, DefaultDevice, &dc_autoshoot, - &estimator_x, - &estimator_y, + &stateGetPositionEnu_f()->x, + &stateGetPositionEnu_f()->y, &course, &dc_buffer, &dc_gps_dist, @@ -115,7 +115,7 @@ uint8_t dc_circle(float interval, float start) { dc_circle_interval = fmodf(fmaxf(interval, 1.), 360.); if(start == DC_IGNORE) { - start = DegOfRad(estimator_psi); + start = DegOfRad(stateGetNedToBodyEulers_f()->psi); } dc_circle_start_angle = fmodf(start, 360.); @@ -136,8 +136,8 @@ uint8_t dc_survey(float interval, float x, float y) { dc_gps_dist = interval; if (x == DC_IGNORE && y == DC_IGNORE) { - dc_gps_x = estimator_x; - dc_gps_y = estimator_y; + dc_gps_x = stateGetPositionEnu_f()->x; + dc_gps_y = stateGetPositionEnu_f()->y; } else if (y == DC_IGNORE) { dc_gps_x = waypoints[(uint8_t)x].x; dc_gps_y = waypoints[(uint8_t)x].y; diff --git a/sw/airborne/modules/digital_cam/dc.h b/sw/airborne/modules/digital_cam/dc.h index 55532659b6..2ae73b2c6c 100644 --- a/sw/airborne/modules/digital_cam/dc.h +++ b/sw/airborne/modules/digital_cam/dc.h @@ -40,7 +40,7 @@ #include "float.h" #include "std.h" #include "led.h" -#include "estimator.h" +#include "state.h" #include "subsystems/nav.h" #include "generated/airframe.h" #include "subsystems/gps.h" @@ -252,7 +252,7 @@ static inline void dc_periodic_4Hz( void ) break; case DC_AUTOSHOOT_CIRCLE: { - float course = DegOfRad(estimator_psi) - dc_circle_start_angle; + float course = DegOfRad(stateGetNedToBodyEulers_f()->psi) - dc_circle_start_angle; if (course < 0.) course += 360.; float current_block = floorf(course/dc_circle_interval); @@ -272,8 +272,8 @@ static inline void dc_periodic_4Hz( void ) break; case DC_AUTOSHOOT_SURVEY : { - float dist_x = dc_gps_x - estimator_x; - float dist_y = dc_gps_y - estimator_y; + float dist_x = dc_gps_x - stateGetPositionEnu_f()->x; + float dist_y = dc_gps_y - stateGetPositionEnu_f()->y; if (dc_probing) { if (dist_x*dist_x + dist_y*dist_y < dc_gps_dist*dc_gps_dist) { diff --git a/sw/airborne/modules/digital_cam/sim_i2c_cam_ctrl.c b/sw/airborne/modules/digital_cam/sim_i2c_cam_ctrl.c index d577dfb4b1..1902a84af8 100644 --- a/sw/airborne/modules/digital_cam/sim_i2c_cam_ctrl.c +++ b/sw/airborne/modules/digital_cam/sim_i2c_cam_ctrl.c @@ -36,7 +36,7 @@ #include "mcu_periph/uart.h" #include "messages.h" #include "subsystems/datalink/downlink.h" -#include "estimator.h" +#include "state.h" void atmega_i2c_cam_ctrl_init(void) diff --git a/sw/airborne/modules/enose/anemotaxis.c b/sw/airborne/modules/enose/anemotaxis.c index 055816bb84..2a25214ec5 100644 --- a/sw/airborne/modules/enose/anemotaxis.c +++ b/sw/airborne/modules/enose/anemotaxis.c @@ -1,6 +1,6 @@ #include "modules/enose/anemotaxis.h" #include "generated/airframe.h" -#include "estimator.h" +#include "state.h" #include "std.h" #include "subsystems/nav.h" #include "generated/flight_plan.h" @@ -13,8 +13,8 @@ static int8_t sign; static struct point last_plume; static void last_plume_was_here( void ) { - last_plume.x = estimator_x; - last_plume.y = estimator_y; + last_plume.x = stateGetPositionEnu_f()->x; + last_plume.y = stateGetPositionEnu_f()->y; } bool_t nav_anemotaxis_downwind( uint8_t c, float radius ) { @@ -28,8 +28,8 @@ bool_t nav_anemotaxis_init( uint8_t c ) { status = UTURN; sign = 1; float wind_dir = atan2(wind_north, wind_east); - waypoints[c].x = estimator_x + DEFAULT_CIRCLE_RADIUS*cos(wind_dir+M_PI); - waypoints[c].y = estimator_y + DEFAULT_CIRCLE_RADIUS*sin(wind_dir+M_PI); + waypoints[c].x = stateGetPositionEnu_f()->x + DEFAULT_CIRCLE_RADIUS*cos(wind_dir+M_PI); + waypoints[c].y = stateGetPositionEnu_f()->y + DEFAULT_CIRCLE_RADIUS*sin(wind_dir+M_PI); last_plume_was_here(); return FALSE; } @@ -37,8 +37,8 @@ bool_t nav_anemotaxis_init( uint8_t c ) { bool_t nav_anemotaxis( uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume ) { if (chemo_sensor) { last_plume_was_here(); - waypoints[plume].x = estimator_x; - waypoints[plume].y = estimator_y; + waypoints[plume].x = stateGetPositionEnu_f()->x; + waypoints[plume].y = stateGetPositionEnu_f()->y; // DownlinkSendWp(plume); } @@ -57,7 +57,7 @@ bool_t nav_anemotaxis( uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume ) { waypoints[c1].x = waypoints[c].x + DEFAULT_CIRCLE_RADIUS*upwind_x; waypoints[c1].y = waypoints[c].y + DEFAULT_CIRCLE_RADIUS*upwind_y; - float width = Max(2*ScalarProduct(upwind_x, upwind_y, estimator_x-last_plume.x, estimator_y-last_plume.y), DEFAULT_CIRCLE_RADIUS); + float width = Max(2*ScalarProduct(upwind_x, upwind_y, stateGetPositionEnu_f()->x-last_plume.x, stateGetPositionEnu_f()->y-last_plume.y), DEFAULT_CIRCLE_RADIUS); waypoints[c2].x = waypoints[c1].x - width*crosswind_x*sign; waypoints[c2].y = waypoints[c1].y - width*crosswind_y*sign; @@ -84,8 +84,8 @@ bool_t nav_anemotaxis( uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume ) { } if (chemo_sensor) { - waypoints[c].x = estimator_x + DEFAULT_CIRCLE_RADIUS*upwind_x; - waypoints[c].y = estimator_y + DEFAULT_CIRCLE_RADIUS*upwind_y; + waypoints[c].x = stateGetPositionEnu_f()->x + DEFAULT_CIRCLE_RADIUS*upwind_x; + waypoints[c].y = stateGetPositionEnu_f()->y + DEFAULT_CIRCLE_RADIUS*upwind_y; // DownlinkSendWp(c); diff --git a/sw/airborne/modules/enose/chemotaxis.c b/sw/airborne/modules/enose/chemotaxis.c index bbb6504da2..6433f930d6 100644 --- a/sw/airborne/modules/enose/chemotaxis.c +++ b/sw/airborne/modules/enose/chemotaxis.c @@ -1,6 +1,6 @@ #include "modules/enose/chemotaxis.h" #include "generated/airframe.h" -#include "estimator.h" +#include "state.h" #include "std.h" #include "subsystems/nav.h" #include "generated/flight_plan.h" @@ -28,14 +28,14 @@ bool_t nav_chemotaxis( uint8_t c, uint8_t plume ) { if (chemo_sensor > last_plume_value) { /* Move the circle in this direction */ - float x = estimator_x - waypoints[plume].x; - float y = estimator_y - waypoints[plume].y; + float x = stateGetPositionEnu_f()->x - waypoints[plume].x; + float y = stateGetPositionEnu_f()->y - waypoints[plume].y; waypoints[c].x = waypoints[plume].x + ALPHA * x; waypoints[c].y = waypoints[plume].y + ALPHA * y; // DownlinkSendWp(c); /* Turn in the right direction */ - float dir_x = cos(M_PI_2 - estimator_hspeed_dir); - float dir_y = sin(M_PI_2 - estimator_hspeed_dir); + float dir_x = cos(M_PI_2 - (*stateGetHorizontalSpeedDir_f())); + float dir_y = sin(M_PI_2 - (*stateGetHorizontalSpeedDir_f())); float pvect = dir_x*y - dir_y*x; sign = (pvect > 0 ? -1 : 1); /* Reduce the radius */ @@ -43,8 +43,8 @@ bool_t nav_chemotaxis( uint8_t c, uint8_t plume ) { /* Store this plume */ - waypoints[plume].x = estimator_x; - waypoints[plume].y = estimator_y; + waypoints[plume].x = stateGetPositionEnu_f()->x; + waypoints[plume].y = stateGetPositionEnu_f()->y; // DownlinkSendWp(plume); last_plume_value = chemo_sensor; } diff --git a/sw/airborne/modules/ins/fw_ins_vn100.c b/sw/airborne/modules/ins/fw_ins_vn100.c index c5f01c08d3..fe595cbfe2 100644 --- a/sw/airborne/modules/ins/fw_ins_vn100.c +++ b/sw/airborne/modules/ins/fw_ins_vn100.c @@ -28,7 +28,7 @@ #include "modules/ins/ins_vn100.h" #include "mcu_periph/spi.h" -#include "estimator.h" +#include "state.h" #include "generated/airframe.h" #ifndef INS_YAW_NEUTRAL_DEFAULT @@ -113,7 +113,13 @@ void ins_event_task( void ) { #ifndef INS_VN100_READ_ONLY // Update estimator // FIXME Use a proper rotation matrix here - EstimatorSetAtt((ins_eulers.phi - ins_roll_neutral), ins_eulers.psi, (ins_eulers.theta - ins_pitch_neutral)); + struct FloatEulers att = { + ins_eulers.phi - ins_roll_neutral, + ins_eulers.theta - ins_pitch_neutral, + ins_eulers.psi + }; + stateSetNedToBodyEulers_f(&att); + stateSetBodyRates(&ins_rates); #endif //uint8_t s = 4+VN100_REG_QMR_SIZE; //DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice,s,spi_buffer_input); diff --git a/sw/airborne/modules/ins/ins_arduimu.c b/sw/airborne/modules/ins/ins_arduimu.c index 5f574befd2..5790c891ee 100644 --- a/sw/airborne/modules/ins/ins_arduimu.c +++ b/sw/airborne/modules/ins/ins_arduimu.c @@ -11,7 +11,7 @@ Autoren@ZHAW: schmiemi #include "mcu_periph/i2c.h" // test -#include "estimator.h" +#include "state.h" // für das Senden von GPS-Daten an den ArduIMU #ifndef UBX @@ -209,12 +209,12 @@ void IMU_Daten_verarbeiten( void ) { ArduIMU_data[5] = (float) (recievedData[5] / (float)100); // test - estimator_phi = (float)ArduIMU_data[0]*0.01745329252 - ins_roll_neutral; - estimator_theta = (float)ArduIMU_data[1]*0.01745329252 - ins_pitch_neutral; + struct FloatEulers att; + att.phi = (float)ArduIMU_data[0]*0.01745329252 - ins_roll_neutral; + att.theta = (float)ArduIMU_data[1]*0.01745329252 - ins_pitch_neutral; + att.psi = 0.; imu_daten_angefordert = FALSE; + stateSetNedToBodyEulers_f(&att); - { - float psi=0; - RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &estimator_phi, &estimator_theta, &psi)); - } + RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &att->phi, &att->theta, &att->psi)); } diff --git a/sw/airborne/modules/ins/ins_arduimu_basic.c b/sw/airborne/modules/ins/ins_arduimu_basic.c index 5285492d6a..441452936d 100644 --- a/sw/airborne/modules/ins/ins_arduimu_basic.c +++ b/sw/airborne/modules/ins/ins_arduimu_basic.c @@ -28,7 +28,7 @@ #include "mcu_periph/i2c.h" // Estimator interface -#include "estimator.h" +#include "state.h" // GPS data for ArduIMU #include "subsystems/gps.h" @@ -112,14 +112,15 @@ void ArduIMU_periodicGPS( void ) { // Test for high acceleration: // - low speed // - high thrust - if (estimator_hspeed_mod < HIGH_ACCEL_LOW_SPEED && ap_state->commands[COMMAND_THROTTLE] > HIGH_ACCEL_HIGH_THRUST && !high_accel_done) { + float speed = *stateGetHorizontalSpeedNorm_f(); + if (speed < HIGH_ACCEL_LOW_SPEED && ap_state->commands[COMMAND_THROTTLE] > HIGH_ACCEL_HIGH_THRUST && !high_accel_done) { high_accel_flag = TRUE; } else { high_accel_flag = FALSE; - if (estimator_hspeed_mod > HIGH_ACCEL_LOW_SPEED && !high_accel_done) { + if (speed > HIGH_ACCEL_LOW_SPEED && !high_accel_done) { high_accel_done = TRUE; // After takeoff, don't use high accel before landing (GS small, Throttle small) } - if (estimator_hspeed_mod < HIGH_ACCEL_HIGH_THRUST_RESUME && ap_state->commands[COMMAND_THROTTLE] < HIGH_ACCEL_HIGH_THRUST_RESUME) { + if (speed < HIGH_ACCEL_HIGH_THRUST_RESUME && ap_state->commands[COMMAND_THROTTLE] < HIGH_ACCEL_HIGH_THRUST_RESUME) { high_accel_done = FALSE; // Activate high accel after landing } } @@ -174,8 +175,8 @@ void ArduIMU_event( void ) { recievedData[8] = (ardu_ins_trans.buf[17]<<8) | ardu_ins_trans.buf[16]; // Update ArduIMU data - arduimu_eulers.phi = ANGLE_FLOAT_OF_BFP(recievedData[0]); - arduimu_eulers.theta = ANGLE_FLOAT_OF_BFP(recievedData[1]); + arduimu_eulers.phi = ANGLE_FLOAT_OF_BFP(recievedData[0]) - ins_roll_neutral; + arduimu_eulers.theta = ANGLE_FLOAT_OF_BFP(recievedData[1]) - ins_pitch_neutral; arduimu_eulers.psi = ANGLE_FLOAT_OF_BFP(recievedData[2]); arduimu_rates.p = RATE_FLOAT_OF_BFP(recievedData[3]); arduimu_rates.q = RATE_FLOAT_OF_BFP(recievedData[4]); @@ -185,11 +186,9 @@ void ArduIMU_event( void ) { arduimu_accel.z = ACCEL_FLOAT_OF_BFP(recievedData[8]); // Update estimator - estimator_phi = arduimu_eulers.phi - ins_roll_neutral; - estimator_theta = arduimu_eulers.theta - ins_pitch_neutral; - estimator_p = arduimu_rates.p; - estimator_q = arduimu_rates.q; - estimator_r = arduimu_rates.r; + stateSetNedToBodyEulers_f(&arduimu_eulers); + stateSetBodyRates_f(&arduimu_rates); + stateSetAccelNed_f(&((struct NedCoor_f)arduimu_accel)); ardu_ins_trans.status = I2CTransDone; #ifdef ARDUIMU_SYNC_SEND diff --git a/sw/airborne/modules/ins/ins_chimu_spi.c b/sw/airborne/modules/ins/ins_chimu_spi.c index 07ba9c35e5..8674e1e039 100644 --- a/sw/airborne/modules/ins/ins_chimu_spi.c +++ b/sw/airborne/modules/ins/ins_chimu_spi.c @@ -10,7 +10,7 @@ #include "mcu_periph/spi_slave_hs_arch.h" // Output -#include "estimator.h" +#include "state.h" // For centripedal corrections #include "subsystems/gps.h" @@ -81,8 +81,18 @@ void parse_ins_msg( void ) CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI; } - EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta); - EstimatorSetRate(CHIMU_DATA.m_sensor.rate[0],CHIMU_DATA.m_attrates.euler.theta,0.); // FIXME rate r + struct FloatEulers att = { + CHIMU_DATA.m_attitude.euler.phi, + CHIMU_DATA.m_attitude.euler.theta, + CHIMU_DATA.m_attitude.euler.psi + }; + stateSetNedToBodyEulers_f(&att); + struct FloatRates rates = { + CHIMU_DATA.m_sensor.rate[0], + CHIMU_DATA.m_attrates.euler.theta, + 0. + }; // FIXME rate r + stateSetBodyRates_f(&rates); } else if(CHIMU_DATA.m_MsgID==0x02) { diff --git a/sw/airborne/modules/ins/ins_chimu_uart.c b/sw/airborne/modules/ins/ins_chimu_uart.c index baa3af3779..7a05dde04f 100644 --- a/sw/airborne/modules/ins/ins_chimu_uart.c +++ b/sw/airborne/modules/ins/ins_chimu_uart.c @@ -10,7 +10,7 @@ // Output -#include "estimator.h" +#include "state.h" // For centripedal corrections #include "subsystems/gps.h" @@ -82,8 +82,12 @@ void parse_ins_msg( void ) CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI; } - EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta); - //EstimatorSetRate(ins_p,ins_q,ins_r); + struct FloatEulers att = { + CHIMU_DATA.m_attitude.euler.phi, + CHIMU_DATA.m_attitude.euler.theta, + CHIMU_DATA.m_attitude.euler.psi + }; + stateSetNedToBodyEulers_f(&att); DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_attitude.euler.phi, &CHIMU_DATA.m_attitude.euler.theta, &CHIMU_DATA.m_attitude.euler.psi); diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c index bfe3790d9d..343ce1cef6 100644 --- a/sw/airborne/modules/ins/ins_xsens.c +++ b/sw/airborne/modules/ins/ins_xsens.c @@ -306,17 +306,35 @@ void ins_periodic_task( void ) { } #if USE_INS -#include "estimator.h" +#include "state.h" static inline void update_fw_estimator(void) { // Send to Estimator (Control) #ifdef XSENS_BACKWARDS - EstimatorSetAtt((-ins_phi+ins_roll_neutral), (ins_psi + RadOfDeg(180)), (-ins_theta+ins_pitch_neutral)); - EstimatorSetRate(-ins_p,-ins_q, ins_r); + struct FloatEulers att = { + -ins_phi+ins_roll_neutral, + -ins_theta+ins_pitch_neutral, + ins_psi + RadOfDeg(180) + }; + struct FloatRates rates = { + -ins_p, + -ins_q, + ins_r + }; #else - EstimatorSetAtt(ins_phi+ins_roll_neutral, ins_psi, ins_theta+ins_pitch_neutral); - EstimatorSetRate(ins_p, ins_q, ins_r); + struct FloatEulers att = { + ins_phi+ins_roll_neutral, + ins_theta+ins_pitch_neutral, + ins_psi + }; + struct FloatRates rates = { + ins_p, + ins_q, + ins_r + }; #endif + stateSetNedToBodyEulers_f(&att); + stateSetBodyRates_f(&rates); } #endif /* USE_INS */ diff --git a/sw/airborne/modules/multi/formation.c b/sw/airborne/modules/multi/formation.c index 0377137ffd..300ea47b8e 100644 --- a/sw/airborne/modules/multi/formation.c +++ b/sw/airborne/modules/multi/formation.c @@ -12,7 +12,7 @@ #include "subsystems/datalink/downlink.h" #include "multi/formation.h" -#include "estimator.h" +#include "state.h" #include "firmwares/fixedwing/stabilization/stabilization_attitude.h" #include "firmwares/fixedwing/guidance/guidance_v.h" #include "autopilot.h" @@ -128,21 +128,21 @@ int formation_flight(void) { static uint8_t _1Hz = 0; uint8_t nb = 0, i; - float ch = cosf(estimator_hspeed_dir); - float sh = sinf(estimator_hspeed_dir); + float ch = cosf((*stateGetHorizontalSpeedDir_f())); + float sh = sinf((*stateGetHorizontalSpeedDir_f())); form_n = 0.; form_e = 0.; form_a = 0.; - form_speed = estimator_hspeed_mod; - form_speed_n = estimator_hspeed_mod * ch; - form_speed_e = estimator_hspeed_mod * sh; + form_speed = (*stateGetHorizontalSpeedNorm_f()); + form_speed_n = (*stateGetHorizontalSpeedNorm_f()) * ch; + form_speed_e = (*stateGetHorizontalSpeedNorm_f()) * sh; if (AC_ID == leader_id) { - estimator_x += formation[the_acs_id[AC_ID]].east; - estimator_y += formation[the_acs_id[AC_ID]].north; + stateGetPositionEnu_f()->x += formation[the_acs_id[AC_ID]].east; + stateGetPositionEnu_f()->y += formation[the_acs_id[AC_ID]].north; } // set info for this AC - SetAcInfo(AC_ID, estimator_x, estimator_y, estimator_hspeed_dir, estimator_z, estimator_hspeed_mod, estimator_z_dot, gps.tow); + SetAcInfo(AC_ID, stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, (*stateGetHorizontalSpeedDir_f()), stateGetPositionEnu_f()->z, (*stateGetHorizontalSpeedNorm_f()), stateGetSpeedEnu_f()->z, gps.tow); // broadcast info uint8_t ac_id = AC_ID; @@ -188,12 +188,12 @@ int formation_flight(void) { } else formation[i].status = ACTIVE; // compute control if AC is ACTIVE and around the same altitude (maybe not so usefull) - if (formation[i].status == ACTIVE && fabs(estimator_z - ac->alt) < form_prox && ac->alt > 0) { - form_e += (ac->east + ac->gspeed*sinf(ac->course)*delta_t - estimator_x) + if (formation[i].status == ACTIVE && fabs(stateGetPositionEnu_f()->z - ac->alt) < form_prox && ac->alt > 0) { + form_e += (ac->east + ac->gspeed*sinf(ac->course)*delta_t - stateGetPositionEnu_f()->x) - (form[i].east - form[the_acs_id[AC_ID]].east); - form_n += (ac->north + ac->gspeed*cosf(ac->course)*delta_t - estimator_y) + form_n += (ac->north + ac->gspeed*cosf(ac->course)*delta_t - stateGetPositionEnu_f()->y) - (form[i].north - form[the_acs_id[AC_ID]].north); - form_a += (ac->alt - estimator_z) - (formation[i].alt - formation[the_acs_id[AC_ID]].alt); + form_a += (ac->alt - stateGetPositionEnu_f()->z) - (formation[i].alt - formation[the_acs_id[AC_ID]].alt); form_speed += ac->gspeed; //form_speed_e += ac->gspeed * sinf(ac->course); //form_speed_n += ac->gspeed * cosf(ac->course); @@ -204,9 +204,9 @@ int formation_flight(void) { form_n /= _nb; form_e /= _nb; form_a /= _nb; - form_speed = form_speed / (nb+1) - estimator_hspeed_mod; - //form_speed_e = form_speed_e / (nb+1) - estimator_hspeed_mod * sh; - //form_speed_n = form_speed_n / (nb+1) - estimator_hspeed_mod * ch; + form_speed = form_speed / (nb+1) - (*stateGetHorizontalSpeedNorm_f()); + //form_speed_e = form_speed_e / (nb+1) - (*stateGetHorizontalSpeedNorm_f()) * sh; + //form_speed_n = form_speed_n / (nb+1) - (*stateGetHorizontalSpeedNorm_f()) * ch; // set commands NavVerticalAutoThrottleMode(0.); @@ -230,7 +230,7 @@ int formation_flight(void) { desired_y = leader->north + dy; // lateral correction //float diff_heading = asin((dx*ch - dy*sh) / sqrt(dx*dx + dy*dy)); - //float diff_course = leader->course - estimator_hspeed_dir; + //float diff_course = leader->course - (*stateGetHorizontalSpeedDir_f()); //NormRadAngle(diff_course); //h_ctl_roll_setpoint += coef_form_course * diff_course; //h_ctl_roll_setpoint += coef_form_course * diff_heading; @@ -249,8 +249,8 @@ int formation_flight(void) { void formation_pre_call(void) { if (leader_id == AC_ID) { - estimator_x -= formation[the_acs_id[AC_ID]].east; - estimator_y -= formation[the_acs_id[AC_ID]].north; + stateGetPositionEnu_f()->x -= formation[the_acs_id[AC_ID]].east; + stateGetPositionEnu_f()->y -= formation[the_acs_id[AC_ID]].north; } } diff --git a/sw/airborne/modules/multi/potential.c b/sw/airborne/modules/multi/potential.c index fe82759935..fa0e09aa43 100644 --- a/sw/airborne/modules/multi/potential.c +++ b/sw/airborne/modules/multi/potential.c @@ -12,7 +12,7 @@ #include "dl_protocol.h" #include "potential.h" -#include "estimator.h" +#include "state.h" #include "firmwares/fixedwing/stabilization/stabilization_attitude.h" #include "firmwares/fixedwing/guidance/guidance_v.h" #include "autopilot.h" @@ -59,8 +59,8 @@ int potential_task(void) { uint8_t i; - float ch = cosf(estimator_hspeed_dir); - float sh = sinf(estimator_hspeed_dir); + float ch = cosf((*stateGetHorizontalSpeedDir_f())); + float sh = sinf((*stateGetHorizontalSpeedDir_f())); potential_force.east = 0.; potential_force.north = 0.; potential_force.alt = 0.; @@ -76,17 +76,17 @@ int potential_task(void) { else { float sha = sinf(ac->course); float cha = cosf(ac->course); - float de = ac->east + sha*delta_t - estimator_x; + float de = ac->east + sha*delta_t - stateGetPositionEnu_f()->x; if (de > FORCE_MAX_DIST || de < -FORCE_MAX_DIST) continue; - float dn = ac->north + cha*delta_t - estimator_y; + float dn = ac->north + cha*delta_t - stateGetPositionEnu_f()->y; if (dn > FORCE_MAX_DIST || dn < -FORCE_MAX_DIST) continue; - float da = ac->alt + ac->climb*delta_t - estimator_z; + float da = ac->alt + ac->climb*delta_t - stateGetPositionEnu_f()->z; if (da > FORCE_MAX_DIST || da < -FORCE_MAX_DIST) continue; float dist = sqrtf(de*de + dn*dn + da*da); if (dist == 0.) continue; - float dve = estimator_hspeed_mod * sh - ac->gspeed * sha; - float dvn = estimator_hspeed_mod * ch - ac->gspeed * cha; - float dva = estimator_z_dot - the_acs[i].climb; + float dve = (*stateGetHorizontalSpeedNorm_f()) * sh - ac->gspeed * sha; + float dvn = (*stateGetHorizontalSpeedNorm_f()) * ch - ac->gspeed * cha; + float dva = stateGetSpeedEnu_f()->z - the_acs[i].climb; float scal = dve*de + dvn*dn + dva*da; if (scal < 0.) continue; // No risk of collision float d3 = dist * dist * dist; diff --git a/sw/airborne/modules/multi/tcas.c b/sw/airborne/modules/multi/tcas.c index 518d1c4c02..48f834d458 100644 --- a/sw/airborne/modules/multi/tcas.c +++ b/sw/airborne/modules/multi/tcas.c @@ -29,7 +29,7 @@ #include "multi/tcas.h" #include "generated/airframe.h" -#include "estimator.h" +#include "state.h" #include "subsystems/nav.h" #include "subsystems/gps.h" #include "generated/flight_plan.h" @@ -88,7 +88,7 @@ void tcas_init( void ) { static inline enum tcas_resolve tcas_test_direction(uint8_t id) { struct ac_info_ * ac = get_ac_info(id); - float dz = ac->alt - estimator_z; + float dz = ac->alt - stateGetPositionEnu_f()->z; if (dz > tcas_alim/2) return RA_DESCEND; else if (dz < -tcas_alim/2) return RA_CLIMB; else // AC with the smallest ID descend @@ -102,7 +102,7 @@ static inline enum tcas_resolve tcas_test_direction(uint8_t id) { /* conflicts detection and monitoring */ void tcas_periodic_task_1Hz( void ) { // no TCAS under security_height - if (estimator_z < GROUND_ALT + SECURITY_HEIGHT) { + if (stateGetPositionEnu_f()->z < GROUND_ALT + SECURITY_HEIGHT) { uint8_t i; for (i = 0; i < NB_ACS; i++) tcas_acs_status[i].status = TCAS_NO_ALARM; return; @@ -111,8 +111,8 @@ void tcas_periodic_task_1Hz( void ) { float tau_min = tcas_tau_ta; uint8_t ac_id_close = AC_ID; uint8_t i; - float vx = estimator_hspeed_mod * sinf(estimator_hspeed_dir); - float vy = estimator_hspeed_mod * cosf(estimator_hspeed_dir); + float vx = (*stateGetHorizontalSpeedNorm_f()) * sinf((*stateGetHorizontalSpeedDir_f())); + float vy = (*stateGetHorizontalSpeedNorm_f()) * cosf((*stateGetHorizontalSpeedDir_f())); for (i = 2; i < NB_ACS; i++) { if (the_acs[i].ac_id == 0) continue; // no AC data uint32_t dt = gps.tow - the_acs[i].itow; @@ -121,12 +121,12 @@ void tcas_periodic_task_1Hz( void ) { continue; } if (dt > TCAS_DT_MAX) continue; // lost com but keep current status - float dx = the_acs[i].east - estimator_x; - float dy = the_acs[i].north - estimator_y; - float dz = the_acs[i].alt - estimator_z; + float dx = the_acs[i].east - stateGetPositionEnu_f()->x; + float dy = the_acs[i].north - stateGetPositionEnu_f()->y; + float dz = the_acs[i].alt - stateGetPositionEnu_f()->z; float dvx = vx - the_acs[i].gspeed * sinf(the_acs[i].course); float dvy = vy - the_acs[i].gspeed * cosf(the_acs[i].course); - float dvz = estimator_z_dot - the_acs[i].climb; + float dvz = stateGetSpeedEnu_f()->z - the_acs[i].climb; float scal = dvx*dx + dvy*dy + dvz*dz; float ddh = dx*dx + dy*dy; float ddv = dz*dz; @@ -219,7 +219,7 @@ void tcas_periodic_task_1Hz( void ) { /* altitude control loop */ void tcas_periodic_task_4Hz( void ) { // set alt setpoint - if (estimator_z > GROUND_ALT + SECURITY_HEIGHT && tcas_status == TCAS_RA) { + if (stateGetPositionEnu_f()->z > GROUND_ALT + SECURITY_HEIGHT && tcas_status == TCAS_RA) { struct ac_info_ * ac = get_ac_info(tcas_ac_RA); switch (tcas_resolve) { case RA_CLIMB : diff --git a/sw/airborne/modules/nav/nav_catapult.c b/sw/airborne/modules/nav/nav_catapult.c index 21cf2f68a9..1819b81c84 100644 --- a/sw/airborne/modules/nav/nav_catapult.c +++ b/sw/airborne/modules/nav/nav_catapult.c @@ -37,7 +37,7 @@ #include "generated/airframe.h" -#include "estimator.h" +#include "state.h" #include "ap_downlink.h" #include "modules/nav/nav_catapult.h" #include "subsystems/nav.h" @@ -166,8 +166,8 @@ bool_t nav_catapult(uint8_t _to, uint8_t _climb) WaypointY(_to) = GetPosY(); WaypointAlt(_to) = GetPosAlt(); - nav_catapult_x = estimator_x; - nav_catapult_y = estimator_y; + nav_catapult_x = stateGetPositionEnu_f()->x; + nav_catapult_y = stateGetPositionEnu_f()->y; } // No Roll, Climb Pitch, Full Power @@ -189,8 +189,8 @@ bool_t nav_catapult(uint8_t _to, uint8_t _climb) // Store Heading, move Climb nav_catapult_launch = 0xffff; - float dir_x = estimator_x - nav_catapult_x; - float dir_y = estimator_y - nav_catapult_y; + float dir_x = stateGetPositionEnu_f()->x - nav_catapult_x; + float dir_y = stateGetPositionEnu_f()->y - nav_catapult_y; float dir_L = sqrt(dir_x * dir_x + dir_y * dir_y); diff --git a/sw/airborne/modules/sensors/AOA_adc.c b/sw/airborne/modules/sensors/AOA_adc.c index 38b38e583f..ee9b58f832 100644 --- a/sw/airborne/modules/sensors/AOA_adc.c +++ b/sw/airborne/modules/sensors/AOA_adc.c @@ -29,6 +29,7 @@ #include BOARD_CONFIG #include "generated/airframe.h" #include "estimator.h" +#include "state.h" #include "std.h" //Messages #include "mcu_periph/uart.h" diff --git a/sw/airborne/modules/sensors/airspeed_adc.c b/sw/airborne/modules/sensors/airspeed_adc.c index 776a15ae91..f461d310bf 100644 --- a/sw/airborne/modules/sensors/airspeed_adc.c +++ b/sw/airborne/modules/sensors/airspeed_adc.c @@ -24,7 +24,7 @@ #include "mcu_periph/adc.h" #include BOARD_CONFIG #include "generated/airframe.h" -#include "estimator.h" +#include "state.h" uint16_t adc_airspeed_val; @@ -59,10 +59,10 @@ void airspeed_adc_update( void ) { #else float airspeed = AIRSPEED_SCALE * (adc_airspeed_val - AIRSPEED_BIAS); #endif - EstimatorSetAirspeed(airspeed); + stateSetAirspeed_f(&airspeed); #else // SITL extern float sim_air_speed; - EstimatorSetAirspeed(sim_air_speed); + stateSetAirspeed_f(&sim_air_speed); adc_airspeed_val = 0; #endif //SITL } diff --git a/sw/airborne/modules/sensors/airspeed_amsys.c b/sw/airborne/modules/sensors/airspeed_amsys.c index adb8f80a91..2c37dec03f 100644 --- a/sw/airborne/modules/sensors/airspeed_amsys.c +++ b/sw/airborne/modules/sensors/airspeed_amsys.c @@ -23,7 +23,7 @@ */ #include "sensors/airspeed_amsys.h" -#include "estimator.h" +#include "state.h" #include "mcu_periph/i2c.h" #include "mcu_periph/uart.h" #include "messages.h" @@ -106,7 +106,7 @@ void airspeed_amsys_read_periodic( void ) { #else // SITL extern float sim_air_speed; - EstimatorSetAirspeed(sim_air_speed); + stateSetAirspeed_f(&sim_air_speed); #endif //SITL } @@ -146,7 +146,7 @@ void airspeed_amsys_read_event( void ) { airspeed_old = airspeed_amsys; #if USE_AIRSPEED - EstimatorSetAirspeed(airspeed_amsys); + stateSetAirspeed_f(&airspeed_amsys); #endif #ifdef SENSOR_SYNC_SEND DOWNLINK_SEND_AMSYS_AIRSPEED(DefaultChannel, DefaultDevice, &airspeed_amsys_raw, &pressure_amsys, &airspeed_tmp, &airspeed_amsys, &airspeed_temperature); diff --git a/sw/airborne/modules/sensors/airspeed_ets.c b/sw/airborne/modules/sensors/airspeed_ets.c index 227526ad9a..5d06b47d33 100644 --- a/sw/airborne/modules/sensors/airspeed_ets.c +++ b/sw/airborne/modules/sensors/airspeed_ets.c @@ -35,7 +35,7 @@ * */ #include "sensors/airspeed_ets.h" -#include "estimator.h" +#include "state.h" #include "mcu_periph/i2c.h" #include "mcu_periph/uart.h" #include "messages.h" @@ -109,7 +109,7 @@ void airspeed_ets_read_periodic( void ) { I2CReceive(AIRSPEED_ETS_I2C_DEV, airspeed_ets_i2c_trans, AIRSPEED_ETS_ADDR, 2); #else // SITL extern float sim_air_speed; - EstimatorSetAirspeed(sim_air_speed); + stateSetAirspeed_f(&sim_air_speed); #endif //SITL } @@ -167,7 +167,7 @@ void airspeed_ets_read_event( void ) { airspeed_ets += airspeed_ets_buffer[n]; airspeed_ets = airspeed_ets / (float)AIRSPEED_ETS_NBSAMPLES_AVRG; #if USE_AIRSPEED - EstimatorSetAirspeed(airspeed_ets); + stateSetAirspeed_f(&airspeed_ets); #endif #ifdef SENSOR_SYNC_SEND DOWNLINK_SEND_AIRSPEED_ETS(DefaultChannel, DefaultDevice, &airspeed_ets_raw, &airspeed_ets_offset, &airspeed_ets); diff --git a/sw/airborne/modules/sensors/baro_MS5534A.c b/sw/airborne/modules/sensors/baro_MS5534A.c index e89f8519ea..eaa752b7d2 100644 --- a/sw/airborne/modules/sensors/baro_MS5534A.c +++ b/sw/airborne/modules/sensors/baro_MS5534A.c @@ -36,6 +36,7 @@ #endif #include "subsystems/nav.h" #include "estimator.h" +#include "state.h" bool_t baro_MS5534A_do_reset; uint32_t baro_MS5534A_pressure; diff --git a/sw/airborne/modules/sensors/baro_amsys.c b/sw/airborne/modules/sensors/baro_amsys.c index 8e244f37e6..d69f663219 100644 --- a/sw/airborne/modules/sensors/baro_amsys.c +++ b/sw/airborne/modules/sensors/baro_amsys.c @@ -26,6 +26,7 @@ #include "sensors/baro_amsys.h" #include "mcu_periph/i2c.h" #include "estimator.h" +#include "state.h" #include #include "generated/flight_plan.h" // for ground alt diff --git a/sw/airborne/modules/sensors/baro_bmp.c b/sw/airborne/modules/sensors/baro_bmp.c index 4bb12a8b34..2e1af7ad70 100644 --- a/sw/airborne/modules/sensors/baro_bmp.c +++ b/sw/airborne/modules/sensors/baro_bmp.c @@ -38,6 +38,7 @@ #include "messages.h" #include "subsystems/datalink/downlink.h" #include "estimator.h" +#include "state.h" #include "subsystems/nav.h" #ifdef SITL diff --git a/sw/airborne/modules/sensors/baro_ets.c b/sw/airborne/modules/sensors/baro_ets.c index 4f9a355c0c..4380d76756 100644 --- a/sw/airborne/modules/sensors/baro_ets.c +++ b/sw/airborne/modules/sensors/baro_ets.c @@ -38,6 +38,7 @@ #include "sensors/baro_ets.h" #include "mcu_periph/i2c.h" #include "estimator.h" +#include "state.h" #include #include "subsystems/nav.h" diff --git a/sw/airborne/modules/sensors/mag_hmc5843.c b/sw/airborne/modules/sensors/mag_hmc5843.c index 07a7b8e5d7..d503cba8bd 100644 --- a/sw/airborne/modules/sensors/mag_hmc5843.c +++ b/sw/airborne/modules/sensors/mag_hmc5843.c @@ -17,7 +17,6 @@ * Boston, MA 02111-1307, USA. * */ -#include "estimator.h" #include "mcu_periph/i2c.h" #include "mcu_periph/uart.h" #include "messages.h" diff --git a/sw/airborne/modules/sensors/pressure_board_navarro.c b/sw/airborne/modules/sensors/pressure_board_navarro.c index 09fd922c2a..a407e14889 100644 --- a/sw/airborne/modules/sensors/pressure_board_navarro.c +++ b/sw/airborne/modules/sensors/pressure_board_navarro.c @@ -26,7 +26,7 @@ #include "pressure_board_navarro.h" -#include "estimator.h" +#include "state.h" /* Default I2C device on tiny is i2c0 */ @@ -140,7 +140,7 @@ void pbn_read_event( void ) { pbn_airspeed = (airspeed_filter*pbn_airspeed + tmp_airspeed) / (airspeed_filter + 1.); #if USE_AIRSPEED - EstimatorSetAirspeed(pbn_airspeed); + stateSetAirspeed_f(&pbn_airspeed); #endif //alt_kalman(pbn_altitude); From c8a7fa77aa6bf1966e9a530c4f3ede2bf9fc0d97 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 26 Jul 2012 17:39:10 +0200 Subject: [PATCH 30/62] [fix] a missing header --- sw/airborne/arch/sim/modules/ins/ins_arduimu_basic.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/airborne/arch/sim/modules/ins/ins_arduimu_basic.c b/sw/airborne/arch/sim/modules/ins/ins_arduimu_basic.c index 3ced3fcc49..6b130d49f2 100644 --- a/sw/airborne/arch/sim/modules/ins/ins_arduimu_basic.c +++ b/sw/airborne/arch/sim/modules/ins/ins_arduimu_basic.c @@ -4,7 +4,7 @@ #include "generated/airframe.h" -#include "estimator.h" +#include "state.h" // Arduimu empty implementation #include "modules/ins/ins_arduimu_basic.h" From a7a7fcf249d4a1a2852b81dc3db7d72743c604da Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 26 Jul 2012 18:11:27 +0200 Subject: [PATCH 31/62] [state interface] more ahrs converted to new state interface --- sw/airborne/subsystems/ahrs/ahrs_float_dcm.c | 42 +++++++++---------- sw/airborne/subsystems/ahrs/ahrs_infrared.c | 13 ++---- sw/airborne/subsystems/sensors/infrared_i2c.c | 1 - 3 files changed, 23 insertions(+), 33 deletions(-) diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c index a2b8a7de23..92caf543d6 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c @@ -29,6 +29,8 @@ #include "subsystems/ahrs/ahrs_float_dcm_algebra.h" #include "math/pprz_algebra_float.h" +#include "state.h" + #if USE_GPS #include "subsystems/gps.h" #endif @@ -48,7 +50,6 @@ #ifdef AHRS_UPDATE_FW_ESTIMATOR // FIXME this is still needed for fixedwing integration -#include "estimator.h" // remotely settable #ifndef INS_ROLL_NEUTRAL_DEFAULT #define INS_ROLL_NEUTRAL_DEFAULT 0 @@ -88,7 +89,7 @@ float MAG_Heading_Y = 0; #endif static inline void compute_ahrs_representations(void); -static inline void compute_body_orientation_and_rates(void); +static inline void set_body_orientation_and_rates(void); static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat); void Normalize(void); @@ -169,8 +170,8 @@ void ahrs_align(void) /* set filter dcm */ set_dcm_matrix_from_rmat(&ahrs_float.ltp_to_imu_rmat); - /* Compute initial body orientation */ - compute_body_orientation_and_rates(); + /* Set initial body orientation */ + set_body_orientation_and_rates(); /* use averaged gyro as initial value for bias */ struct Int32Rates bias0; @@ -400,14 +401,15 @@ void Drift_correction(void) // Test for high acceleration: // - low speed // - high thrust - if (estimator_hspeed_mod < HIGH_ACCEL_LOW_SPEED && ap_state->commands[COMMAND_THROTTLE] > HIGH_ACCEL_HIGH_THRUST && !high_accel_done) { + float speed = *stateGetHorizontalSpeedNorm_f(); + if (speed < HIGH_ACCEL_LOW_SPEED && ap_state->commands[COMMAND_THROTTLE] > HIGH_ACCEL_HIGH_THRUST && !high_accel_done) { high_accel_flag = TRUE; } else { high_accel_flag = FALSE; - if (estimator_hspeed_mod > HIGH_ACCEL_LOW_SPEED && !high_accel_done) { + if (speed > HIGH_ACCEL_LOW_SPEED && !high_accel_done) { high_accel_done = TRUE; // After takeoff, don't use high accel before landing (GS small, Throttle small) } - if (estimator_hspeed_mod < HIGH_ACCEL_HIGH_THRUST_RESUME && ap_state->commands[COMMAND_THROTTLE] < HIGH_ACCEL_HIGH_THRUST_RESUME) { + if (speed < HIGH_ACCEL_HIGH_THRUST_RESUME && ap_state->commands[COMMAND_THROTTLE] < HIGH_ACCEL_HIGH_THRUST_RESUME) { high_accel_done = FALSE; // Activate high accel after landing } } @@ -515,14 +517,19 @@ void Matrix_update(void) /* * Compute body orientation and rates from imu orientation and rates */ -static inline void compute_body_orientation_and_rates(void) { +static inline void set_body_orientation_and_rates(void) { + + FLOAT_RMAT_TRANSP_RATEMULT(ahrs_float.body_rate, ahrs_impl.body_to_imu_rmat, ahrs_float.imu_rate); + stateSetBodyRates_f(&ahrs_float.body_rate); - FLOAT_QUAT_COMP_INV(ahrs_float.ltp_to_body_quat, - ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat); FLOAT_RMAT_COMP_INV(ahrs_float.ltp_to_body_rmat, ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat); FLOAT_EULERS_OF_RMAT(ahrs_float.ltp_to_body_euler, ahrs_float.ltp_to_body_rmat); - FLOAT_RMAT_TRANSP_RATEMULT(ahrs_float.body_rate, ahrs_impl.body_to_imu_rmat, ahrs_float.imu_rate); +#ifdef AHRS_UPDATE_FW_ESTIMATOR + ahrs_float.ltp_to_body_euler.phi -= ins_roll_neutral; + ahrs_float.ltp_to_body_euler.theta -= ins_pitch_neutral; +#endif + stateSetNedToBodyEulers_f(&ahrs_float.ltp_to_body_euler); } @@ -538,11 +545,7 @@ static inline void compute_ahrs_representations(void) { ahrs_float.ltp_to_imu_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ #endif - /* set quaternion and rotation matrix representations as well */ - FLOAT_QUAT_OF_EULERS(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_imu_euler); - FLOAT_RMAT_OF_EULERS(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_euler); - - compute_body_orientation_and_rates(); + set_body_orientation_and_rates(); /* RunOnceEvery(6,DOWNLINK_SEND_RMAT_DEBUG(DefaultChannel, DefaultDevice, @@ -565,12 +568,5 @@ static inline void compute_ahrs_representations(void) { #ifdef AHRS_UPDATE_FW_ESTIMATOR void ahrs_update_fw_estimator( void ) { // export results to estimator - estimator_phi = ahrs_float.ltp_to_body_euler.phi - ins_roll_neutral; - estimator_theta = ahrs_float.ltp_to_body_euler.theta - ins_pitch_neutral; - estimator_psi = ahrs_float.ltp_to_body_euler.psi; - - estimator_p = ahrs_float.body_rate.p; - estimator_q = ahrs_float.body_rate.q; - estimator_r = ahrs_float.body_rate.r; } #endif //AHRS_UPDATE_FW_ESTIMATOR diff --git a/sw/airborne/subsystems/ahrs/ahrs_infrared.c b/sw/airborne/subsystems/ahrs/ahrs_infrared.c index 064cf7dcf7..c95c966a3e 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_infrared.c +++ b/sw/airborne/subsystems/ahrs/ahrs_infrared.c @@ -25,7 +25,8 @@ #include "subsystems/imu.h" #include "subsystems/gps.h" -#include "estimator.h" +#include "state.h" +#include "estimator.h" // for wind FIXME use state interface @@ -114,13 +115,7 @@ void ahrs_update_infrared(void) { void ahrs_update_fw_estimator(void) { // export results to estimator - - estimator_phi = ahrs_float.ltp_to_body_euler.phi; - estimator_theta = ahrs_float.ltp_to_body_euler.theta; - estimator_psi = ahrs_float.ltp_to_body_euler.psi; - - estimator_p = ahrs_float.body_rate.p; - estimator_q = ahrs_float.body_rate.q; - estimator_r = ahrs_float.body_rate.r; + stateSetNedToBodyEulers_f(&ahrs_float.ltp_to_body_euler); + stateSetBodyRates_f(&ahrs_float.body_rate); } diff --git a/sw/airborne/subsystems/sensors/infrared_i2c.c b/sw/airborne/subsystems/sensors/infrared_i2c.c index 2067fd0a2e..206816cf56 100644 --- a/sw/airborne/subsystems/sensors/infrared_i2c.c +++ b/sw/airborne/subsystems/sensors/infrared_i2c.c @@ -21,7 +21,6 @@ */ #include "sensors/infrared_i2c.h" -#include "estimator.h" // IR I2C definitions #define IR_HOR_I2C_ADDR (0x6C << 1) From b37808aeeadec25ac7a65ffa9fe8142935e2df46 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Mon, 20 Aug 2012 15:53:14 +0200 Subject: [PATCH 32/62] [ahrs] update float_dcm to not use ahrs structure state interface is directly filled instead --- sw/airborne/subsystems/ahrs/ahrs_float_dcm.c | 112 +++++++++---------- sw/airborne/subsystems/ahrs/ahrs_float_dcm.h | 14 +-- 2 files changed, 55 insertions(+), 71 deletions(-) diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c index cb1349d7a4..480c6490a4 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c @@ -50,7 +50,6 @@ #endif -#ifdef AHRS_UPDATE_FW_ESTIMATOR // FIXME this is still needed for fixedwing integration // remotely settable #ifndef INS_ROLL_NEUTRAL_DEFAULT @@ -61,7 +60,6 @@ #endif float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; -#endif /* AHRS_UPDATE_FW_ESTIMATOR */ struct AhrsFloatDCM ahrs_impl; @@ -136,23 +134,14 @@ void ahrs_init(void) { */ struct FloatEulers body_to_imu_euler = {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI}; - FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler); FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler); - /* set ltp_to_body to zero */ - FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat); - FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler); - FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat); - FLOAT_RATES_ZERO(ahrs_float.body_rate); + EULERS_COPY(ahrs_impl.ltp_to_imu_euler, body_to_imu_euler); - /* set ltp_to_imu so that body is zero */ - QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat); - RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat); - EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler); - FLOAT_RATES_ZERO(ahrs_float.imu_rate); + FLOAT_RATES_ZERO(ahrs_impl.imu_rate); /* set inital filter dcm */ - set_dcm_matrix_from_rmat(&ahrs_float.ltp_to_imu_rmat); + set_dcm_matrix_from_rmat(&ahrs_impl.body_to_imu_rmat); #if USE_HIGH_ACCEL_FLAG high_accel_done = FALSE; @@ -169,14 +158,14 @@ void ahrs_init(void) { void ahrs_align(void) { /* Compute an initial orientation using euler angles */ - ahrs_float_get_euler_from_accel_mag(&ahrs_float.ltp_to_imu_euler, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); + ahrs_float_get_euler_from_accel_mag(&ahrs_impl.ltp_to_imu_euler, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); /* Convert initial orientation in quaternion and rotation matrice representations. */ - FLOAT_QUAT_OF_EULERS(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_imu_euler); - FLOAT_RMAT_OF_QUAT(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_quat); + struct FloatRMat ltp_to_imu_rmat; + FLOAT_RMAT_OF_EULERS(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_euler); /* set filter dcm */ - set_dcm_matrix_from_rmat(&ahrs_float.ltp_to_imu_rmat); + set_dcm_matrix_from_rmat(<p_to_imu_rmat); /* Set initial body orientation */ set_body_orientation_and_rates(); @@ -197,21 +186,21 @@ void ahrs_propagate(void) RATES_FLOAT_OF_BFP(gyro_float, imu.gyro); /* unbias rate measurement */ - RATES_DIFF(ahrs_float.imu_rate, gyro_float, ahrs_impl.gyro_bias); + RATES_DIFF(ahrs_impl.imu_rate, gyro_float, ahrs_impl.gyro_bias); /* Uncouple Motions */ #ifdef IMU_GYRO_P_Q float dp=0,dq=0,dr=0; - dp += ahrs_float.imu_rate.q * IMU_GYRO_P_Q; - dp += ahrs_float.imu_rate.r * IMU_GYRO_P_R; - dq += ahrs_float.imu_rate.p * IMU_GYRO_Q_P; - dq += ahrs_float.imu_rate.r * IMU_GYRO_Q_R; - dr += ahrs_float.imu_rate.p * IMU_GYRO_R_P; - dr += ahrs_float.imu_rate.q * IMU_GYRO_R_Q; + dp += ahrs_impl.imu_rate.q * IMU_GYRO_P_Q; + dp += ahrs_impl.imu_rate.r * IMU_GYRO_P_R; + dq += ahrs_impl.imu_rate.p * IMU_GYRO_Q_P; + dq += ahrs_impl.imu_rate.r * IMU_GYRO_Q_R; + dr += ahrs_impl.imu_rate.p * IMU_GYRO_R_P; + dr += ahrs_impl.imu_rate.q * IMU_GYRO_R_Q; - ahrs_float.imu_rate.p += dp; - ahrs_float.imu_rate.q += dq; - ahrs_float.imu_rate.r += dr; + ahrs_impl.imu_rate.p += dp; + ahrs_impl.imu_rate.q += dq; + ahrs_impl.imu_rate.r += dr; #endif Matrix_update(); @@ -287,10 +276,10 @@ void ahrs_update_mag(void) float cos_pitch; float sin_pitch; - cos_roll = cos(ahrs_float.ltp_to_imu_euler.phi); - sin_roll = sin(ahrs_float.ltp_to_imu_euler.phi); - cos_pitch = cos(ahrs_float.ltp_to_imu_euler.theta); - sin_pitch = sin(ahrs_float.ltp_to_imu_euler.theta); + cos_roll = cosf(ahrs_impl.ltp_to_imu_euler.phi); + sin_roll = sinf(ahrs_impl.ltp_to_imu_euler.phi); + cos_pitch = cosf(ahrs_impl.ltp_to_imu_euler.theta); + sin_pitch = sinf(ahrs_impl.ltp_to_imu_euler.theta); // Pitch&Roll Compensation: @@ -517,7 +506,7 @@ void Drift_correction(void) void Matrix_update(void) { - Vector_Add(&Omega[0], &ahrs_float.imu_rate.p, &Omega_I[0]); //adding proportional term + Vector_Add(&Omega[0], &ahrs_impl.imu_rate.p, &Omega_I[0]); //adding proportional term Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term #if OUTPUTMODE==1 // With corrected data (drift correction) @@ -532,13 +521,13 @@ void Matrix_update(void) Update_Matrix[2][2]=0; #else // Uncorrected data (no drift correction) Update_Matrix[0][0]=0; - Update_Matrix[0][1]=-G_Dt*ahrs_float.imu_rate.r;//-z - Update_Matrix[0][2]=G_Dt*ahrs_float.imu_rate.q;//y - Update_Matrix[1][0]=G_Dt*ahrs_float.imu_rate.r;//z + Update_Matrix[0][1]=-G_Dt*ahrs_impl.imu_rate.r;//-z + Update_Matrix[0][2]=G_Dt*ahrs_impl.imu_rate.q;//y + Update_Matrix[1][0]=G_Dt*ahrs_impl.imu_rate.r;//z Update_Matrix[1][1]=0; - Update_Matrix[1][2]=-G_Dt*ahrs_float.imu_rate.p; - Update_Matrix[2][0]=-G_Dt*ahrs_float.imu_rate.q; - Update_Matrix[2][1]=G_Dt*ahrs_float.imu_rate.p; + Update_Matrix[1][2]=-G_Dt*ahrs_impl.imu_rate.p; + Update_Matrix[2][0]=-G_Dt*ahrs_impl.imu_rate.q; + Update_Matrix[2][1]=G_Dt*ahrs_impl.imu_rate.p; Update_Matrix[2][2]=0; #endif @@ -558,30 +547,36 @@ void Matrix_update(void) */ static inline void set_body_orientation_and_rates(void) { - FLOAT_RMAT_TRANSP_RATEMULT(ahrs_float.body_rate, ahrs_impl.body_to_imu_rmat, ahrs_float.imu_rate); - stateSetBodyRates_f(&ahrs_float.body_rate); + struct FloatRates body_rate; + FLOAT_RMAT_TRANSP_RATEMULT(body_rate, ahrs_impl.body_to_imu_rmat, ahrs_impl.imu_rate); + stateSetBodyRates_f(&body_rate); - FLOAT_RMAT_COMP_INV(ahrs_float.ltp_to_body_rmat, - ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat); - FLOAT_EULERS_OF_RMAT(ahrs_float.ltp_to_body_euler, ahrs_float.ltp_to_body_rmat); -#ifdef AHRS_UPDATE_FW_ESTIMATOR - ahrs_float.ltp_to_body_euler.phi -= ins_roll_neutral; - ahrs_float.ltp_to_body_euler.theta -= ins_pitch_neutral; -#endif - stateSetNedToBodyEulers_f(&ahrs_float.ltp_to_body_euler); + struct FloatRMat ltp_to_imu_rmat, ltp_to_body_rmat; + FLOAT_RMAT_OF_EULERS(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_euler); + FLOAT_RMAT_COMP_INV(ltp_to_body_rmat, ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat); + + // Some stupid lines of code for neutrals + struct FloatEulers ltp_to_body_euler; + FLOAT_EULERS_OF_RMAT(ltp_to_body_euler, ltp_to_body_rmat); + ltp_to_body_euler.phi -= ins_roll_neutral; + ltp_to_body_euler.theta -= ins_pitch_neutral; + stateSetNedToBodyEulers_f(<p_to_body_euler); + + // should be replaced at the end by: + // stateSetNedToBodyRMat_f(<p_to_body_rmat); } static inline void compute_ahrs_representations(void) { #if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes) - ahrs_float.ltp_to_imu_euler.phi = atan2(accel_float.y,accel_float.z); // atan2(acc_y,acc_z) - ahrs_float.ltp_to_imu_euler.theta = -asin((accel_float.x)/GRAVITY); // asin(acc_x) - ahrs_float.ltp_to_imu_euler.psi = 0; + ahrs_impl.ltp_to_imu_euler.phi = atan2(accel_float.y,accel_float.z); // atan2(acc_y,acc_z) + ahrs_impl.ltp_to_imu_euler.theta = -asin((accel_float.x)/GRAVITY); // asin(acc_x) + ahrs_impl.ltp_to_imu_euler.psi = 0; #else - ahrs_float.ltp_to_imu_euler.phi = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]); - ahrs_float.ltp_to_imu_euler.theta = -asin(DCM_Matrix[2][0]); - ahrs_float.ltp_to_imu_euler.psi = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]); - ahrs_float.ltp_to_imu_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ + ahrs_impl.ltp_to_imu_euler.phi = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]); + ahrs_impl.ltp_to_imu_euler.theta = -asin(DCM_Matrix[2][0]); + ahrs_impl.ltp_to_imu_euler.psi = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]); + ahrs_impl.ltp_to_imu_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ #endif set_body_orientation_and_rates(); @@ -604,8 +599,3 @@ static inline void compute_ahrs_representations(void) { */ } -#ifdef AHRS_UPDATE_FW_ESTIMATOR -void ahrs_update_fw_estimator( void ) { - // export results to estimator -} -#endif //AHRS_UPDATE_FW_ESTIMATOR diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h index 31f82f60e6..80ae0b1b13 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h @@ -27,13 +27,10 @@ struct AhrsFloatDCM { struct FloatRates gyro_bias; struct FloatRates rate_correction; - /* - Holds float version of IMU alignement - in order to be able to run against the fixed point - version of the IMU - */ - struct FloatQuat body_to_imu_quat; + + struct FloatEulers ltp_to_imu_euler; struct FloatRMat body_to_imu_rmat; + struct FloatRates imu_rate; float gps_speed; float gps_acceleration; @@ -44,13 +41,10 @@ struct AhrsFloatDCM { extern struct AhrsFloatDCM ahrs_impl; -#ifdef AHRS_UPDATE_FW_ESTIMATOR +// FIXME neutrals should be a feature of state interface ? extern float ins_roll_neutral; extern float ins_pitch_neutral; -void ahrs_update_fw_estimator(void); -#endif - // DCM Parameters From 22cfea33141e3d1f90af840f1e76a80fc1225d3d Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Mon, 20 Aug 2012 17:03:27 +0200 Subject: [PATCH 33/62] [ahrs] int_cmpl_euler ahrs not using the old ahrs structure --- .../subsystems/ahrs/ahrs_int_cmpl_euler.c | 105 ++++++++---------- .../subsystems/ahrs/ahrs_int_cmpl_euler.h | 5 +- 2 files changed, 47 insertions(+), 63 deletions(-) diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c index dc0ed746b4..c5a56a3bf6 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c @@ -33,6 +33,22 @@ #define FACE_REINJ_1 1024 #endif +#ifndef AHRS_MAG_OFFSET +#define AHRS_MAG_OFFSET 0. +#endif + +#ifdef AHRS_UPDATE_FW_ESTIMATOR +// remotely settable (for FW) +#ifndef INS_ROLL_NEUTRAL_DEFAULT +#define INS_ROLL_NEUTRAL_DEFAULT 0 +#endif +#ifndef INS_PITCH_NEUTRAL_DEFAULT +#define INS_PITCH_NEUTRAL_DEFAULT 0 +#endif +float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; +float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; +#endif + struct AhrsIntCmplEuler ahrs_impl; @@ -52,26 +68,14 @@ static inline void set_body_state_from_euler(void); void ahrs_init(void) { ahrs.status = AHRS_UNINIT; - /* set ltp_to_body to zero */ - INT_EULERS_ZERO(ahrs.ltp_to_body_euler); - INT32_QUAT_ZERO(ahrs.ltp_to_body_quat); - INT32_RMAT_ZERO(ahrs.ltp_to_body_rmat); - INT_RATES_ZERO(ahrs.body_rate); - /* set ltp_to_imu so that body is zero */ - QUAT_COPY(ahrs.ltp_to_imu_quat, imu.body_to_imu_quat); - RMAT_COPY(ahrs.ltp_to_imu_rmat, imu.body_to_imu_rmat); - INT32_EULERS_OF_RMAT(ahrs.ltp_to_imu_euler, ahrs.ltp_to_imu_rmat); - INT_RATES_ZERO(ahrs.imu_rate); + INT32_EULERS_OF_RMAT(ahrs_impl.ltp_to_imu_euler, imu.body_to_imu_rmat); + INT_RATES_ZERO(ahrs_impl.imu_rate); INT_RATES_ZERO(ahrs_impl.gyro_bias); ahrs_impl.reinj_1 = FACE_REINJ_1; -#ifdef IMU_MAG_OFFSET - ahrs_mag_offset = IMU_MAG_OFFSET; -#else - ahrs_mag_offset = 0.; -#endif + ahrs_impl.mag_offset = AHRS_MAG_OFFSET; } void ahrs_align(void) { @@ -84,7 +88,7 @@ void ahrs_align(void) { EULERS_COPY(ahrs_impl.measurement, ahrs_impl.hi_res_euler); /* Compute LTP to IMU eulers */ - EULERS_SDIV(ahrs.ltp_to_imu_euler, ahrs_impl.hi_res_euler, F_UPDATE); + EULERS_SDIV(ahrs_impl.ltp_to_imu_euler, ahrs_impl.hi_res_euler, F_UPDATE); set_body_state_from_euler(); @@ -154,11 +158,11 @@ void ahrs_propagate(void) { #endif /* low pass rate */ #if USE_NOISE_FILTER - RATES_SUM_SCALED(ahrs.imu_rate, ahrs.imu_rate, uf_rate, NOISE_FILTER_GAIN); - RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, NOISE_FILTER_GAIN+1); + RATES_SUM_SCALED(ahrs_impl.imu_rate, ahrs_impl.imu_rate, uf_rate, NOISE_FILTER_GAIN); + RATES_SDIV(ahrs_impl.imu_rate, ahrs_impl.imu_rate, NOISE_FILTER_GAIN+1); #else - RATES_ADD(ahrs.imu_rate, uf_rate); - RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, 2); + RATES_ADD(ahrs_impl.imu_rate, uf_rate); + RATES_SDIV(ahrs_impl.imu_rate, ahrs_impl.imu_rate, 2); #endif #if USE_NOISE_CUT } @@ -167,7 +171,7 @@ void ahrs_propagate(void) { /* integrate eulers */ struct Int32Eulers euler_dot; - INT32_EULERS_DOT_OF_RATES(euler_dot, ahrs.ltp_to_imu_euler, ahrs.imu_rate); + INT32_EULERS_DOT_OF_RATES(euler_dot, ahrs_impl.ltp_to_imu_euler, ahrs_impl.imu_rate); EULERS_ADD(ahrs_impl.hi_res_euler, euler_dot); /* low pass measurement */ @@ -187,7 +191,7 @@ void ahrs_propagate(void) { /* Compute LTP to IMU eulers */ - EULERS_SDIV(ahrs.ltp_to_imu_euler, ahrs_impl.hi_res_euler, F_UPDATE); + EULERS_SDIV(ahrs_impl.ltp_to_imu_euler, ahrs_impl.hi_res_euler, F_UPDATE); set_body_state_from_euler(); @@ -216,7 +220,7 @@ void ahrs_update_accel(void) { void ahrs_update_mag(void) { - get_psi_measurement_from_mag(&ahrs_impl.measurement.psi, ahrs.ltp_to_imu_euler.phi, ahrs.ltp_to_imu_euler.theta, imu.mag); + get_psi_measurement_from_mag(&ahrs_impl.measurement.psi, ahrs_impl.ltp_to_imu_euler.phi, ahrs_impl.ltp_to_imu_euler.theta, imu.mag); } @@ -261,54 +265,33 @@ __attribute__ ((always_inline)) static inline void get_psi_measurement_from_mag( // sphi_ctheta * imu.mag.y + // cphi_ctheta * imu.mag.z; float m_psi = -atan2(me, mn); - *psi_meas = ((m_psi - ahrs_mag_offset)*(float)(1<<(INT32_ANGLE_FRAC))*F_UPDATE); + *psi_meas = ((m_psi - ahrs_impl.mag_offset)*(float)(1<<(INT32_ANGLE_FRAC))*F_UPDATE); } /* Rotate angles and rates from imu to body frame and set state */ __attribute__ ((always_inline)) static inline void set_body_state_from_euler(void) { + struct Int32RMat ltp_to_imu_rmat, ltp_to_body_rmat; /* Compute LTP to IMU rotation matrix */ - INT32_RMAT_OF_EULERS(ahrs.ltp_to_imu_rmat, ahrs.ltp_to_imu_euler); + INT32_RMAT_OF_EULERS(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_euler); /* Compute LTP to BODY rotation matrix */ - INT32_RMAT_COMP_INV(ahrs.ltp_to_body_rmat, ahrs.ltp_to_imu_rmat, imu.body_to_imu_rmat); + INT32_RMAT_COMP_INV(ltp_to_body_rmat, ltp_to_imu_rmat, imu.body_to_imu_rmat); /* Set state */ - stateSetNedToBodyRMat_i(&ahrs.ltp_to_body_rmat); - - /* compute body rates */ - INT32_RMAT_TRANSP_RATEMULT(ahrs.body_rate, imu.body_to_imu_rmat, ahrs.imu_rate); - /* Set state */ - stateSetBodyRates_i(&ahrs.body_rate); - -} - - #ifdef AHRS_UPDATE_FW_ESTIMATOR -// TODO use ahrs result directly -#include "estimator.h" -// remotely settable -#ifndef INS_ROLL_NEUTRAL_DEFAULT -#define INS_ROLL_NEUTRAL_DEFAULT 0 + struct Int32Eulers ltp_to_body_euler; + INT32_EULERS_OF_RMAT(ltp_to_body_euler, ltp_to_body_rmat); + ltp_to_body_euler.phi -= ANGLE_BFP_OF_REAL(ins_roll_neutral); + ltp_to_body_euler.theta -= ANGLE_BFP_OF_REAL(ins_pitch_neutral); + stateSetNedToBodyEulers_i(<p_to_body_euler); +#else + stateSetNedToBodyRMat_i(<p_to_body_rmat); #endif -#ifndef INS_PITCH_NEUTRAL_DEFAULT -#define INS_PITCH_NEUTRAL_DEFAULT 0 -#endif -float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; -float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; -void ahrs_update_fw_estimator(void) -{ - struct FloatEulers att; - // export results to estimator - EULERS_FLOAT_OF_BFP(att, ahrs.ltp_to_body_euler); - estimator_phi = att.phi - ins_roll_neutral; - estimator_theta = att.theta - ins_pitch_neutral; - estimator_psi = att.psi; - - struct FloatRates rates; - RATES_FLOAT_OF_BFP(rates, ahrs.body_rate); - estimator_p = rates.p; - estimator_q = rates.q; - estimator_r = rates.r; + struct Int32Rates body_rate; + /* compute body rates */ + INT32_RMAT_TRANSP_RATEMULT(body_rate, imu.body_to_imu_rmat, ahrs_impl.imu_rate); + /* Set state */ + stateSetBodyRates_i(&body_rate); } -#endif //AHRS_UPDATE_FW_ESTIMATOR + diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h index b6ad467aa4..9210793a28 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h @@ -28,19 +28,20 @@ struct AhrsIntCmplEuler { struct Int32Rates gyro_bias; + struct Int32Rates imu_rate; struct Int32Eulers hi_res_euler; struct Int32Eulers measure; struct Int32Eulers residual; struct Int32Eulers measurement; + struct Int32Eulers ltp_to_imu_euler; int32_t reinj_1; + float mag_offset; }; extern struct AhrsIntCmplEuler ahrs_impl; #ifdef AHRS_UPDATE_FW_ESTIMATOR -// TODO copy ahrs to state instead of estimator -void ahrs_update_fw_estimator(void); extern float ins_roll_neutral; extern float ins_pitch_neutral; #endif From d379af9647a0f9916dbaffc51f91d7e0afe6c742 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 21 Aug 2012 14:47:50 +0200 Subject: [PATCH 34/62] [ahrs] int_cmpl_quat not using old ahrs structure --- .../subsystems/ahrs/ahrs_int_cmpl_quat.c | 149 ++++++++---------- .../subsystems/ahrs/ahrs_int_cmpl_quat.h | 4 +- 2 files changed, 68 insertions(+), 85 deletions(-) diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c index 213c32291a..940cb95f65 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -54,8 +54,19 @@ static inline void ahrs_update_mag_2d(void); struct AhrsIntCmpl ahrs_impl; +#ifdef AHRS_UPDATE_FW_ESTIMATOR +// remotely settable +#ifndef INS_ROLL_NEUTRAL_DEFAULT +#define INS_ROLL_NEUTRAL_DEFAULT 0 +#endif +#ifndef INS_PITCH_NEUTRAL_DEFAULT +#define INS_PITCH_NEUTRAL_DEFAULT 0 +#endif +float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; +float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; +#endif + static inline void set_body_state_from_quat(void); -static inline void compute_imu_orientation(void); void ahrs_init(void) { @@ -63,17 +74,9 @@ void ahrs_init(void) { ahrs_impl.ltp_vel_norm_valid = FALSE; ahrs_impl.heading_aligned = FALSE; - /* set ltp_to_body to zero */ - INT_EULERS_ZERO(ahrs.ltp_to_body_euler); - INT32_QUAT_ZERO(ahrs.ltp_to_body_quat); - INT32_RMAT_ZERO(ahrs.ltp_to_body_rmat); - INT_RATES_ZERO(ahrs.body_rate); - /* set ltp_to_imu so that body is zero */ - QUAT_COPY(ahrs.ltp_to_imu_quat, imu.body_to_imu_quat); - RMAT_COPY(ahrs.ltp_to_imu_rmat, imu.body_to_imu_rmat); - INT32_EULERS_OF_RMAT(ahrs.ltp_to_imu_euler, ahrs.ltp_to_imu_rmat); - INT_RATES_ZERO(ahrs.imu_rate); + QUAT_COPY(ahrs_impl.ltp_to_imu_quat, imu.body_to_imu_quat); + INT_RATES_ZERO(ahrs_impl.imu_rate); INT_RATES_ZERO(ahrs_impl.gyro_bias); INT_RATES_ZERO(ahrs_impl.rate_correction); @@ -97,11 +100,11 @@ void ahrs_align(void) { #if USE_MAGNETOMETER /* Compute an initial orientation from accel and mag directly as quaternion */ - ahrs_int_get_quat_from_accel_mag(&ahrs.ltp_to_imu_quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); + ahrs_int_get_quat_from_accel_mag(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); ahrs_impl.heading_aligned = TRUE; #else /* Compute an initial orientation from accel and just set heading to zero */ - ahrs_int_get_quat_from_accel(&ahrs.ltp_to_imu_quat, &ahrs_aligner.lp_accel); + ahrs_int_get_quat_from_accel(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel); ahrs_impl.heading_aligned = FALSE; #endif @@ -130,11 +133,11 @@ void ahrs_propagate(void) { /* low pass rate */ #ifdef AHRS_PROPAGATE_LOW_PASS_RATES - RATES_SMUL(ahrs.imu_rate, ahrs.imu_rate,2); - RATES_ADD(ahrs.imu_rate, omega); - RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, 3); + RATES_SMUL(ahrs_impl.imu_rate, ahrs_impl.imu_rate,2); + RATES_ADD(ahrs_impl.imu_rate, omega); + RATES_SDIV(ahrs_impl.imu_rate, ahrs_impl.imu_rate, 3); #else - RATES_COPY(ahrs.imu_rate, omega); + RATES_COPY(ahrs_impl.imu_rate, omega); #endif /* add correction */ @@ -143,8 +146,8 @@ void ahrs_propagate(void) { INT_RATES_ZERO(ahrs_impl.rate_correction); /* integrate quaternion */ - INT32_QUAT_INTEGRATE_FI(ahrs.ltp_to_imu_quat, ahrs_impl.high_rez_quat, omega, AHRS_PROPAGATE_FREQUENCY); - INT32_QUAT_NORMALIZE(ahrs.ltp_to_imu_quat); + INT32_QUAT_INTEGRATE_FI(ahrs_impl.ltp_to_imu_quat, ahrs_impl.high_rez_quat, omega, AHRS_PROPAGATE_FREQUENCY); + INT32_QUAT_NORMALIZE(ahrs_impl.ltp_to_imu_quat); set_body_state_from_quat(); @@ -156,9 +159,11 @@ void ahrs_propagate(void) { void ahrs_update_accel(void) { // c2 = ltp z-axis in imu-frame - struct Int32Vect3 c2 = { RMAT_ELMT(ahrs.ltp_to_imu_rmat, 0,2), - RMAT_ELMT(ahrs.ltp_to_imu_rmat, 1,2), - RMAT_ELMT(ahrs.ltp_to_imu_rmat, 2,2)}; + struct Int32RMat ltp_to_imu_rmat; + INT32_RMAT_OF_QUAT(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat); + struct Int32Vect3 c2 = { RMAT_ELMT(ltp_to_imu_rmat, 0,2), + RMAT_ELMT(ltp_to_imu_rmat, 1,2), + RMAT_ELMT(ltp_to_imu_rmat, 2,2)}; struct Int32Vect3 residual; struct Int32Vect3 pseudo_gravity_measurement; @@ -175,7 +180,7 @@ void ahrs_update_accel(void) { // FIXME: check overflows ! const struct Int32Vect3 vel_tangential_body = {(ahrs_impl.ltp_vel_norm>>INT32_ACCEL_FRAC), 0.0, 0.0}; struct Int32Vect3 acc_c_body; - VECT3_RATES_CROSS_VECT3(acc_c_body, ahrs.body_rate, vel_tangential_body); + VECT3_RATES_CROSS_VECT3(acc_c_body, (*stateGetBodyRates_i()), vel_tangential_body); INT32_VECT3_RSHIFT(acc_c_body, acc_c_body, INT32_SPEED_FRAC+INT32_RATE_FRAC-INT32_ACCEL_FRAC-INT32_ACCEL_FRAC); /* convert centrifucal acceleration from body to imu frame */ @@ -246,11 +251,14 @@ void ahrs_update_mag(void) { static inline void ahrs_update_mag_full(void) { + + struct Int32RMat ltp_to_imu_rmat; + INT32_RMAT_OF_QUAT(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat); const struct Int32Vect3 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X), MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z)}; struct Int32Vect3 expected_imu; - INT32_RMAT_VMULT(expected_imu, ahrs.ltp_to_imu_rmat, expected_ltp); + INT32_RMAT_VMULT(expected_imu, ltp_to_imu_rmat, expected_ltp); struct Int32Vect3 residual; INT32_VECT3_CROSS_PRODUCT(residual, imu.mag, expected_imu); @@ -272,11 +280,12 @@ static inline void ahrs_update_mag_full(void) { static inline void ahrs_update_mag_2d(void) { + struct Int32RMat ltp_to_imu_rmat; + INT32_RMAT_OF_QUAT(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat); const struct Int32Vect2 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X), MAG_BFP_OF_REAL(AHRS_H_Y)}; - struct Int32Vect3 measured_ltp; - INT32_RMAT_TRANSP_VMULT(measured_ltp, ahrs.ltp_to_imu_rmat, imu.mag); + INT32_RMAT_TRANSP_VMULT(measured_ltp, ltp_to_imu_rmat, imu.mag); struct Int32Vect3 residual_ltp = { 0, @@ -284,7 +293,7 @@ static inline void ahrs_update_mag_2d(void) { (measured_ltp.x * expected_ltp.y - measured_ltp.y * expected_ltp.x)/(1<<5)}; struct Int32Vect3 residual_imu; - INT32_RMAT_VMULT(residual_imu, ahrs.ltp_to_imu_rmat, residual_ltp); + INT32_RMAT_VMULT(residual_imu, ltp_to_imu_rmat, residual_ltp); // residual_ltp FRAC = 2 * MAG_FRAC = 22 // rate_correction FRAC = RATE_FRAC = 12 @@ -371,7 +380,9 @@ void ahrs_update_heading(int32_t heading) { (expected_ltp.x * heading_y - expected_ltp.y * heading_x)/(1<phi - ins_roll_neutral; - estimator_theta = att->theta - ins_pitch_neutral; - estimator_psi = att->psi; - - struct FloatRates* rates; - rates = stateGetBodyRates_f(); - estimator_p = rates->p; - estimator_q = rates->q; - estimator_r = rates->r; - -} -#endif //AHRS_UPDATE_FW_ESTIMATOR diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h index cb83a507ea..955853704c 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h @@ -28,9 +28,11 @@ struct AhrsIntCmpl { struct Int32Rates gyro_bias; + struct Int32Rates imu_rate; struct Int32Rates rate_correction; struct Int64Quat high_rez_quat; struct Int64Rates high_rez_bias; + struct Int32Quat ltp_to_imu_quat; int32_t ltp_vel_norm; bool_t ltp_vel_norm_valid; bool_t correct_gravity; @@ -55,8 +57,6 @@ void ahrs_update_heading(int32_t heading); void ahrs_realign_heading(int32_t heading); #ifdef AHRS_UPDATE_FW_ESTIMATOR -// TODO copy ahrs to state instead of estimator -void ahrs_update_fw_estimator(void); extern float ins_roll_neutral; extern float ins_pitch_neutral; #endif From 85fc5319c518e8f7c9328efcac4af348f5fe2bae Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 21 Aug 2012 15:05:12 +0200 Subject: [PATCH 35/62] [ahrs] infrared ahrs not using old ahrs structure --- sw/airborne/subsystems/ahrs/ahrs_infrared.c | 65 +++++++-------------- sw/airborne/subsystems/ahrs/ahrs_infrared.h | 3 - 2 files changed, 21 insertions(+), 47 deletions(-) diff --git a/sw/airborne/subsystems/ahrs/ahrs_infrared.c b/sw/airborne/subsystems/ahrs/ahrs_infrared.c index c95c966a3e..f5270a4390 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_infrared.c +++ b/sw/airborne/subsystems/ahrs/ahrs_infrared.c @@ -28,22 +28,12 @@ #include "state.h" #include "estimator.h" // for wind FIXME use state interface - +float heading; void ahrs_init(void) { ahrs.status = AHRS_UNINIT; - /* set ltp_to_body to zero */ - FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat); - FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler); - FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat); - FLOAT_RATES_ZERO(ahrs_float.body_rate); - - /* set ltp_to_body to zero */ - FLOAT_QUAT_ZERO(ahrs_float.ltp_to_imu_quat); - FLOAT_EULERS_ZERO(ahrs_float.ltp_to_imu_euler); - FLOAT_RMAT_ZERO(ahrs_float.ltp_to_imu_rmat); - FLOAT_RATES_ZERO(ahrs_float.imu_rate); + heading = 0.; } void ahrs_align(void) { @@ -54,15 +44,17 @@ void ahrs_align(void) { } void ahrs_propagate(void) { + struct FloatRates body_rate = { 0., 0., 0. }; #ifdef ADC_CHANNEL_GYRO_P - ahrs_float.body_rate.p = RATE_FLOAT_OF_BFP(imu.gyro.p); + body_rate.p = RATE_FLOAT_OF_BFP(imu.gyro.p); #endif #ifdef ADC_CHANNEL_GYRO_Q - ahrs_float.body_rate.q = RATE_FLOAT_OF_BFP(imu.gyro.q); + body_rate.q = RATE_FLOAT_OF_BFP(imu.gyro.q); #endif #ifdef ADC_CHANNEL_GYRO_R - ahrs_float.body_rate.r = RATE_FLOAT_OF_BFP(imu.gyro.r); + body_rate.r = RATE_FLOAT_OF_BFP(imu.gyro.r); #endif + stateSetBodyRates_f(&body_rate); } void ahrs_update_accel(void) { @@ -80,42 +72,27 @@ void ahrs_update_gps(void) { // wind_north and wind_east initialized to 0, so still correct if not updated float w_vn = cosf(course_f) * hspeed_mod_f - wind_north; float w_ve = sinf(course_f) * hspeed_mod_f - wind_east; - ahrs_float.ltp_to_body_euler.psi = atan2f(w_ve, w_vn); - if (ahrs_float.ltp_to_body_euler.psi < 0.) - ahrs_float.ltp_to_body_euler.psi += 2 * M_PI; + heading = atan2f(w_ve, w_vn); + if (heading < 0.) + heading += 2 * M_PI; - ahrs_update_fw_estimator(); } void ahrs_update_infrared(void) { - ahrs_float.ltp_to_body_euler.phi = atan2(infrared.roll, infrared.top) - infrared.roll_neutral; + float phi = atan2(infrared.roll, infrared.top) - infrared.roll_neutral; + float theta = atan2(infrared.pitch, infrared.top) - infrared.pitch_neutral; - ahrs_float.ltp_to_body_euler.theta = atan2(infrared.pitch, infrared.top) - infrared.pitch_neutral; + if (theta < -M_PI_2) theta += M_PI; + else if (theta > M_PI_2) theta -= M_PI; - if (ahrs_float.ltp_to_body_euler.theta < -M_PI_2) - ahrs_float.ltp_to_body_euler.theta += M_PI; - else if (ahrs_float.ltp_to_body_euler.theta > M_PI_2) - ahrs_float.ltp_to_body_euler.theta -= M_PI; + if (phi >= 0) phi *= infrared.correction_right; + else phi *= infrared.correction_left; - if (ahrs_float.ltp_to_body_euler.phi >= 0) - ahrs_float.ltp_to_body_euler.phi *= infrared.correction_right; - else - ahrs_float.ltp_to_body_euler.phi *= infrared.correction_left; + if (theta >= 0) theta *= infrared.correction_up; + else theta *= infrared.correction_down; - if (ahrs_float.ltp_to_body_euler.theta >= 0) - ahrs_float.ltp_to_body_euler.theta *= infrared.correction_up; - else - ahrs_float.ltp_to_body_euler.theta *= infrared.correction_down; - - ahrs_update_fw_estimator(); -} - - -// TODO use ahrs result directly -void ahrs_update_fw_estimator(void) -{ - // export results to estimator - stateSetNedToBodyEulers_f(&ahrs_float.ltp_to_body_euler); - stateSetBodyRates_f(&ahrs_float.body_rate); + struct FloatEulers att = { phi, theta, heading }; + stateSetNedToBodyEulers_f(&att); } + diff --git a/sw/airborne/subsystems/ahrs/ahrs_infrared.h b/sw/airborne/subsystems/ahrs/ahrs_infrared.h index a4d2870756..02c677260f 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_infrared.h +++ b/sw/airborne/subsystems/ahrs/ahrs_infrared.h @@ -33,7 +33,4 @@ extern float ins_pitch_neutral; extern void ahrs_update_infrared(void); -// TODO copy ahrs to state instead of estimator -extern void ahrs_update_fw_estimator(void); - #endif /* AHRS_INFRARED_H */ From ecce79c04f64699d84ccec34992f7dc1af32342d Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 21 Aug 2012 15:16:38 +0200 Subject: [PATCH 36/62] [ahrs] simulation ahrs not using old structure --- sw/airborne/subsystems/ahrs/ahrs_sim.c | 88 ++++++-------------------- sw/airborne/subsystems/ahrs/ahrs_sim.h | 3 - 2 files changed, 20 insertions(+), 71 deletions(-) diff --git a/sw/airborne/subsystems/ahrs/ahrs_sim.c b/sw/airborne/subsystems/ahrs/ahrs_sim.c index 0a5232ef2f..9293ad768b 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_sim.c +++ b/sw/airborne/subsystems/ahrs/ahrs_sim.c @@ -36,48 +36,40 @@ extern float sim_q; extern float sim_r; extern bool_t ahrs_sim_available; - -void set_body_orientation_and_rates(void); +#ifdef AHRS_UPDATE_FW_ESTIMATOR +// remotely settable +#ifndef INS_ROLL_NEUTRAL_DEFAULT +#define INS_ROLL_NEUTRAL_DEFAULT 0 +#endif +#ifndef INS_PITCH_NEUTRAL_DEFAULT +#define INS_PITCH_NEUTRAL_DEFAULT 0 +#endif +float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; +float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; +#endif //AHRS_UPDATE_FW_ESTIMATOR void update_ahrs_from_sim(void) { - ahrs_float.ltp_to_imu_euler.phi = sim_phi; - ahrs_float.ltp_to_imu_euler.theta = sim_theta; - ahrs_float.ltp_to_imu_euler.psi = sim_psi; - - ahrs_float.imu_rate.p = sim_p; - ahrs_float.imu_rate.q = sim_q; - ahrs_float.imu_rate.r = sim_r; - - /* set quaternion and rotation matrix representations as well */ - //FLOAT_QUAT_OF_EULERS(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_imu_euler); - //FLOAT_RMAT_OF_EULERS(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_euler); - - set_body_orientation_and_rates(); + struct FloatEulers ltp_to_imu_euler = { sim_phi, sim_theta, sim_psi }; #ifdef AHRS_UPDATE_FW_ESTIMATOR - //ahrs_update_fw_estimator(); + ltp_to_imu_euler.phi - ins_roll_neutral; + ltp_to_imu_euler.theta - ins_pitch_neutral; #endif + struct FloatRates imu_rate = { sim_p, sim_q, sim_r }; + /* set ltp_to_body to same as ltp_to_imu, currently no difference simulated */ + stateSetNedToBodyEulers_f(<p_to_imu_euler); + stateSetBodyRates_f(&imu_rate); + } void ahrs_init(void) { //ahrs_float.status = AHRS_UNINIT; - // set to running for now and only use ahrs.status, not ahrs_float.status + // set to running for now ahrs.status = AHRS_RUNNING; ahrs_sim_available = FALSE; - /* set ltp_to_body to zero */ - FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat); - FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler); - FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat); - FLOAT_RATES_ZERO(ahrs_float.body_rate); - - /* set ltp_to_imu to same as ltp_to_body, currently no difference simulated */ - QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_body_quat); - EULERS_COPY(ahrs_float.ltp_to_imu_euler, ahrs_float.ltp_to_body_euler); - RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_body_rmat); - RATES_COPY(ahrs_float.imu_rate, ahrs_float.body_rate); } void ahrs_align(void) @@ -88,9 +80,6 @@ void ahrs_align(void) update_ahrs_from_sim(); - /* Compute initial body orientation */ - //set_body_orientation_and_rates(); - ahrs.status = AHRS_RUNNING; } @@ -112,40 +101,3 @@ void ahrs_update_gps(void) { } - -/* - * Set state interface - */ -void set_body_orientation_and_rates(void) { - - /* set ltp_to_body to same as ltp_to_imu, currently no difference simulated */ - stateSetNedToBodyEulers_f(&ahrs_float.ltp_to_imu_euler); - stateSetBodyRates_f(&ahrs_float.imu_rate); - -} - - -#ifdef AHRS_UPDATE_FW_ESTIMATOR -// TODO use ahrs result directly -#include "estimator.h" -// remotely settable -#ifndef INS_ROLL_NEUTRAL_DEFAULT -#define INS_ROLL_NEUTRAL_DEFAULT 0 -#endif -#ifndef INS_PITCH_NEUTRAL_DEFAULT -#define INS_PITCH_NEUTRAL_DEFAULT 0 -#endif -float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; -float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; -void ahrs_update_fw_estimator(void) -{ - // really subtract ins neutrals here? - estimator_phi = ahrs_float.ltp_to_body_euler.phi - ins_roll_neutral; - estimator_theta = ahrs_float.ltp_to_body_euler.theta - ins_pitch_neutral; - estimator_psi = ahrs_float.ltp_to_body_euler.psi; - - estimator_p = ahrs_float.body_rate.p; - estimator_q = ahrs_float.body_rate.q; - estimator_r = ahrs_float.body_rate.r; -} -#endif //AHRS_UPDATE_FW_ESTIMATOR diff --git a/sw/airborne/subsystems/ahrs/ahrs_sim.h b/sw/airborne/subsystems/ahrs/ahrs_sim.h index ee36d402be..8d4146587e 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_sim.h +++ b/sw/airborne/subsystems/ahrs/ahrs_sim.h @@ -28,9 +28,6 @@ extern bool_t ahrs_sim_available; #ifdef AHRS_UPDATE_FW_ESTIMATOR -#include "estimator.h" -// TODO copy ahrs to state instead of estimator -void ahrs_update_fw_estimator(void); extern float ins_roll_neutral; extern float ins_pitch_neutral; #endif From 5d75792b690e5b28f303ce0c7496195c0f223f72 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 21 Aug 2012 16:25:09 +0200 Subject: [PATCH 37/62] [ahrs] float_cmpl ahrs not using old structure compiling but not tested --- sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c | 190 +++++++----------- sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h | 13 +- 2 files changed, 84 insertions(+), 119 deletions(-) diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c index d4228b4616..aebfffa21f 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c @@ -54,16 +54,27 @@ #define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY #endif +#ifdef AHRS_UPDATE_FW_ESTIMATOR +// remotely settable +#ifndef INS_ROLL_NEUTRAL_DEFAULT +#define INS_ROLL_NEUTRAL_DEFAULT 0 +#endif +#ifndef INS_PITCH_NEUTRAL_DEFAULT +#define INS_PITCH_NEUTRAL_DEFAULT 0 +#endif +float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; +float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; +#endif //AHRS_UPDATE_FW_ESTIMATOR + + void ahrs_update_mag_full(void); void ahrs_update_mag_2d(void); void ahrs_update_mag_2d_dumb(void); -static inline void compute_imu_quat_and_euler_from_rmat(void); -static inline void compute_imu_rmat_and_euler_from_quat(void); static inline void compute_body_orientation_and_rates(void); -struct AhrsFloatCmplRmat ahrs_impl; +struct AhrsFloatCmpl ahrs_impl; void ahrs_init(void) { ahrs.status = AHRS_UNINIT; @@ -76,18 +87,11 @@ void ahrs_init(void) { FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler); FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler); - /* Set ltp_to_body to zero */ - FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat); - FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler); - FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat); - FLOAT_RATES_ZERO(ahrs_float.body_rate); - /* Set ltp_to_imu so that body is zero */ - QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat); - RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat); - EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler); + QUAT_COPY(ahrs_impl.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat); + RMAT_COPY(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat); - FLOAT_RATES_ZERO(ahrs_float.imu_rate); + FLOAT_RATES_ZERO(ahrs_impl.imu_rate); #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN ahrs_impl.correct_gravity = TRUE; @@ -101,24 +105,20 @@ void ahrs_align(void) { #if USE_MAGNETOMETER /* Compute an initial orientation from accel and mag directly as quaternion */ - ahrs_float_get_quat_from_accel_mag(&ahrs_float.ltp_to_imu_quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); + ahrs_float_get_quat_from_accel_mag(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); ahrs_impl.heading_aligned = TRUE; #else /* Compute an initial orientation from accel and just set heading to zero */ - ahrs_float_get_quat_from_accel(&ahrs_float.ltp_to_imu_quat, &ahrs_aligner.lp_accel); + ahrs_float_get_quat_from_accel(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel); ahrs_impl.heading_aligned = FALSE; #endif - /* Convert initial orientation from quat to euler and rotation matrix representations. */ - compute_imu_rmat_and_euler_from_quat(); + /* Convert initial orientation from quat to rotation matrix representations. */ + FLOAT_RMAT_OF_QUAT(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat); /* Compute initial body orientation */ compute_body_orientation_and_rates(); - /* compute fixed point representations */ - AHRS_INT_OF_FLOAT(); - AHRS_IMU_INT_OF_FLOAT(); - /* used averaged gyro as initial value for bias */ struct Int32Rates bias0; RATES_COPY(bias0, ahrs_aligner.lp_gyro); @@ -138,9 +138,9 @@ void ahrs_propagate(void) { #ifdef AHRS_PROPAGATE_LOW_PASS_RATES const float alpha = 0.1; - FLOAT_RATES_LIN_CMB(ahrs_float.imu_rate, ahrs_float.imu_rate, (1.-alpha), gyro_float, alpha); + FLOAT_RATES_LIN_CMB(ahrs_impl.imu_rate, ahrs_impl.imu_rate, (1.-alpha), gyro_float, alpha); #else - RATES_COPY(ahrs_float.imu_rate,gyro_float); + RATES_COPY(ahrs_impl.imu_rate,gyro_float); #endif /* add correction */ @@ -151,28 +151,25 @@ void ahrs_propagate(void) { const float dt = 1./AHRS_PROPAGATE_FREQUENCY; #if AHRS_PROPAGATE_RMAT - FLOAT_RMAT_INTEGRATE_FI(ahrs_float.ltp_to_imu_rmat, omega, dt ); - float_rmat_reorthogonalize(&ahrs_float.ltp_to_imu_rmat); - compute_imu_quat_and_euler_from_rmat(); + FLOAT_RMAT_INTEGRATE_FI(ahrs_impl.ltp_to_imu_rmat, omega, dt); + float_rmat_reorthogonalize(&ahrs_impl.ltp_to_imu_rmat); + FLOAT_QUAT_OF_RMAT(ahrs_impl.ltp_to_imu_quat, ahrs_impl.ltp_to_imu_rmat); #endif #if AHRS_PROPAGATE_QUAT - FLOAT_QUAT_INTEGRATE(ahrs_float.ltp_to_imu_quat, omega, dt); - FLOAT_QUAT_NORMALIZE(ahrs_float.ltp_to_imu_quat); - compute_imu_rmat_and_euler_from_quat(); + FLOAT_QUAT_INTEGRATE(ahrs_impl.ltp_to_imu_quat, omega, dt); + FLOAT_QUAT_NORMALIZE(ahrs_impl.ltp_to_imu_quat); + FLOAT_RMAT_OF_QUAT(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat); #endif compute_body_orientation_and_rates(); - /* compute fixed point representations */ - AHRS_INT_OF_FLOAT(); - AHRS_IMU_INT_OF_FLOAT(); } void ahrs_update_accel(void) { /* last column of roation matrix = ltp z-axis in imu-frame */ - struct FloatVect3 c2 = { RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 0,2), - RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 1,2), - RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 2,2)}; + struct FloatVect3 c2 = { RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 0,2), + RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 1,2), + RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 2,2)}; struct FloatVect3 imu_accel_float; ACCELS_FLOAT_OF_BFP(imu_accel_float, imu.accel); @@ -189,9 +186,9 @@ void ahrs_update_accel(void) { */ const struct FloatVect3 vel_tangential_body = {ahrs_impl.ltp_vel_norm, 0.0, 0.0}; struct FloatVect3 acc_c_body; - VECT3_RATES_CROSS_VECT3(acc_c_body, ahrs_float.body_rate, vel_tangential_body); + VECT3_RATES_CROSS_VECT3(acc_c_body, *stateGetBodyRates_f(), vel_tangential_body); - /* convert centrifucal acceleration from body to imu frame */ + /* convert centrifugal acceleration from body to imu frame */ struct FloatVect3 acc_c_imu; FLOAT_RMAT_VECT3_MUL(acc_c_imu, ahrs_impl.body_to_imu_rmat, acc_c_body); @@ -237,7 +234,7 @@ void ahrs_update_mag_full(void) { const struct FloatVect3 expected_ltp = {AHRS_H_X, AHRS_H_Y, AHRS_H_Z}; struct FloatVect3 expected_imu; - FLOAT_RMAT_VECT3_MUL(expected_imu, ahrs_float.ltp_to_imu_rmat, expected_ltp); + FLOAT_RMAT_VECT3_MUL(expected_imu, ahrs_impl.ltp_to_imu_rmat, expected_ltp); struct FloatVect3 measured_imu; MAGS_FLOAT_OF_BFP(measured_imu, imu.mag); @@ -263,7 +260,7 @@ void ahrs_update_mag_2d(void) { struct FloatVect3 measured_imu; MAGS_FLOAT_OF_BFP(measured_imu, imu.mag); struct FloatVect3 measured_ltp; - FLOAT_RMAT_VECT3_TRANSP_MUL(measured_ltp, ahrs_float.ltp_to_imu_rmat, measured_imu); + FLOAT_RMAT_VECT3_TRANSP_MUL(measured_ltp, ahrs_impl.ltp_to_imu_rmat, measured_imu); const struct FloatVect3 residual_ltp = { 0, @@ -273,7 +270,7 @@ void ahrs_update_mag_2d(void) { // printf("res : %f\n", residual_ltp.z); struct FloatVect3 residual_imu; - FLOAT_RMAT_VECT3_MUL(residual_imu, ahrs_float.ltp_to_imu_rmat, residual_ltp); + FLOAT_RMAT_VECT3_MUL(residual_imu, ahrs_impl.ltp_to_imu_rmat, residual_ltp); const float mag_rate_update_gain = 2.5; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain); @@ -287,19 +284,21 @@ void ahrs_update_mag_2d(void) { void ahrs_update_mag_2d_dumb(void) { /* project mag on local tangeant plane */ + struct FloatEulers ltp_to_imu_euler; + FLOAT_EULERS_OF_RMAT(ltp_to_imu_euler, ahrs_impl.ltp_to_imu_rmat); struct FloatVect3 magf; MAGS_FLOAT_OF_BFP(magf, imu.mag); - const float cphi = cosf(ahrs_float.ltp_to_imu_euler.phi); - const float sphi = sinf(ahrs_float.ltp_to_imu_euler.phi); - const float ctheta = cosf(ahrs_float.ltp_to_imu_euler.theta); - const float stheta = sinf(ahrs_float.ltp_to_imu_euler.theta); + const float cphi = cosf(ltp_to_imu_euler.phi); + const float sphi = sinf(ltp_to_imu_euler.phi); + const float ctheta = cosf(ltp_to_imu_euler.theta); + const float stheta = sinf(ltp_to_imu_euler.theta); const float mn = ctheta * magf.x + sphi*stheta*magf.y + cphi*stheta*magf.z; const float me = 0. * magf.x + cphi *magf.y - sphi *magf.z; - const float res_norm = -RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 0,0)*me + RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 1,0)*mn; - const struct FloatVect3 r2 = {RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 2,0), - RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 2,1), - RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 2,2)}; + const float res_norm = -RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 0,0)*me + RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 1,0)*mn; + const struct FloatVect3 r2 = {RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 2,0), + RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 2,1), + RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 2,2)}; const float mag_rate_update_gain = 2.5; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, r2, (mag_rate_update_gain*res_norm)); const float mag_bias_update_gain = -2.5e-4; @@ -343,11 +342,12 @@ void ahrs_update_heading(float heading) { FLOAT_ANGLE_NORMALIZE(heading); + struct FloatRMat* ltp_to_body_rmat = stateGetNedToBodyRMat_f(); // row 0 of ltp_to_body_rmat = body x-axis in ltp frame // we only consider x and y struct FloatVect2 expected_ltp = - { RMAT_ELMT(ahrs_float.ltp_to_body_rmat, 0, 0), - RMAT_ELMT(ahrs_float.ltp_to_body_rmat, 0, 1) }; + { RMAT_ELMT(*ltp_to_body_rmat, 0, 0), + RMAT_ELMT(*ltp_to_body_rmat, 0, 1) }; // expected_heading cross measured_heading struct FloatVect3 residual_ltp = @@ -356,7 +356,7 @@ void ahrs_update_heading(float heading) { expected_ltp.x * sinf(heading) - expected_ltp.y * cosf(heading)}; struct FloatVect3 residual_imu; - FLOAT_RMAT_VECT3_MUL(residual_imu, ahrs_float.ltp_to_imu_rmat, residual_ltp); + FLOAT_RMAT_VECT3_MUL(residual_imu, ahrs_impl.ltp_to_imu_rmat, residual_ltp); const float heading_rate_update_gain = 2.5; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, heading_rate_update_gain); @@ -386,9 +386,10 @@ void ahrs_realign_heading(float heading) { q_h_new.qz = sinf(heading/2.0); q_h_new.qi = cosf(heading/2.0); + struct FloatQuat* ltp_to_body_quat = stateGetNedToBodyQuat_f(); /* quaternion representing current heading only */ struct FloatQuat q_h; - QUAT_COPY(q_h, ahrs_float.ltp_to_body_quat); + QUAT_COPY(q_h, *ltp_to_body_quat); q_h.qx = 0.; q_h.qy = 0.; FLOAT_QUAT_NORMALIZE(q_h); @@ -399,79 +400,42 @@ void ahrs_realign_heading(float heading) { /* correct current heading in body frame */ struct FloatQuat q; - FLOAT_QUAT_COMP_NORM_SHORTEST(q, q_c, ahrs_float.ltp_to_body_quat); - QUAT_COPY(ahrs_float.ltp_to_body_quat, q); + FLOAT_QUAT_COMP_NORM_SHORTEST(q, q_c, *ltp_to_body_quat); - /* compute ltp to imu rotation quaternion */ - FLOAT_QUAT_COMP(ahrs_float.ltp_to_imu_quat, - ahrs_float.ltp_to_body_quat, ahrs_impl.body_to_imu_quat); + /* compute ltp to imu rotation quaternion and matrix */ + FLOAT_QUAT_COMP(ahrs_impl.ltp_to_imu_quat, q, ahrs_impl.body_to_imu_quat); + FLOAT_RMAT_OF_QUAT(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat); - /* compute other body orientation representations */ - FLOAT_RMAT_OF_QUAT(ahrs_float.ltp_to_body_rmat, ahrs_float.ltp_to_body_quat); - FLOAT_EULERS_OF_RMAT(ahrs_float.ltp_to_body_euler, ahrs_float.ltp_to_body_rmat); - - /* compute other ltp to imu rotation representations */ - compute_imu_rmat_and_euler_from_quat(); - - /* compute fixed point representations */ - AHRS_INT_OF_FLOAT(); - AHRS_IMU_INT_OF_FLOAT(); + /* set state */ + stateSetNedToBodyQuat_f(&q); ahrs_impl.heading_aligned = TRUE; } -/** - * Compute ltp to imu rotation in euler angles and quaternion representations - * from the rotation matrix representation - */ -static inline void compute_imu_quat_and_euler_from_rmat(void) { - FLOAT_QUAT_OF_RMAT(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_imu_rmat); - FLOAT_EULERS_OF_RMAT(ahrs_float.ltp_to_imu_euler, ahrs_float.ltp_to_imu_rmat); -} - - -static inline void compute_imu_rmat_and_euler_from_quat(void) { - FLOAT_RMAT_OF_QUAT(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_quat); - FLOAT_EULERS_OF_RMAT(ahrs_float.ltp_to_imu_euler, ahrs_float.ltp_to_imu_rmat); -} - - - /** * Compute body orientation and rates from imu orientation and rates */ static inline void compute_body_orientation_and_rates(void) { - FLOAT_QUAT_COMP_INV(ahrs_float.ltp_to_body_quat, - ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat); - FLOAT_RMAT_COMP_INV(ahrs_float.ltp_to_body_rmat, - ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat); - FLOAT_EULERS_OF_RMAT(ahrs_float.ltp_to_body_euler, ahrs_float.ltp_to_body_rmat); - FLOAT_RMAT_TRANSP_RATEMULT(ahrs_float.body_rate, ahrs_impl.body_to_imu_rmat, ahrs_float.imu_rate); -} - - + /* Compute LTP to BODY quaternion */ + struct FloatQuat ltp_to_body_quat; + FLOAT_QUAT_COMP_INV(ltp_to_body_quat, ahrs_impl.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat); + /* Set state */ #ifdef AHRS_UPDATE_FW_ESTIMATOR -// TODO use ahrs result directly -#include "estimator.h" -// remotely settable -#ifndef INS_ROLL_NEUTRAL_DEFAULT -#define INS_ROLL_NEUTRAL_DEFAULT 0 + struct FloatEulers neutrals_to_body_eulers = { ins_roll_neutral, ins_pitch_neutral, 0. }; + struct FloatQuat neutrals_to_body_quat, ltp_to_neutrals_quat; + FLOAT_QUAT_OF_EULERS(neutrals_to_body_quat, neutrals_to_body_eulers); + FLOAT_QUAT_NORMALIZE(neutrals_to_body_quat); + FLOAT_QUAT_COMP_INV(ltp_to_neutrals_quat, ltp_to_body_quat, neutrals_to_body_quat); + stateSetNedToBodyQuat_f(<p_to_neutrals_quat); +#else + stateSetNedToBodyQuat_f(<p_to_body_quat); #endif -#ifndef INS_PITCH_NEUTRAL_DEFAULT -#define INS_PITCH_NEUTRAL_DEFAULT 0 -#endif -float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; -float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; -void ahrs_update_fw_estimator(void) -{ - // export results to estimator - estimator_phi = ahrs_float.ltp_to_body_euler.phi - ins_roll_neutral; - estimator_theta = ahrs_float.ltp_to_body_euler.theta - ins_pitch_neutral; - estimator_psi = ahrs_float.ltp_to_body_euler.psi; - estimator_p = ahrs_float.body_rate.p; - estimator_q = ahrs_float.body_rate.q; - estimator_r = ahrs_float.body_rate.r; + /* compute body rates */ + struct FloatRates body_rate; + FLOAT_RMAT_TRANSP_RATEMULT(body_rate, ahrs_impl.body_to_imu_rmat, ahrs_impl.imu_rate); + stateSetBodyRates_f(&body_rate); } -#endif //AHRS_UPDATE_FW_ESTIMATOR + + diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h index 6b01d4969b..22715f68fd 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h @@ -1,6 +1,4 @@ /* - * $Id$ - * * Copyright (C) 2010 The Paparazzi Team * * This file is part of paparazzi. @@ -21,14 +19,17 @@ * Boston, MA 02111-1307, USA. */ -#ifndef AHRS_FLOAT_CMPL_RMAT -#define AHRS_FLOAT_CMPL_RMAT +#ifndef AHRS_FLOAT_CMPL +#define AHRS_FLOAT_CMPL #include "std.h" -struct AhrsFloatCmplRmat { +struct AhrsFloatCmpl { struct FloatRates gyro_bias; struct FloatRates rate_correction; + struct FloatRates imu_rate; + struct FloatQuat ltp_to_imu_quat; + struct FloatRMat ltp_to_imu_rmat; /* for gravity correction during coordinated turns */ float ltp_vel_norm; bool_t ltp_vel_norm_valid; @@ -45,7 +46,7 @@ struct AhrsFloatCmplRmat { struct FloatRMat body_to_imu_rmat; }; -extern struct AhrsFloatCmplRmat ahrs_impl; +extern struct AhrsFloatCmpl ahrs_impl; /** Update yaw based on a heading measurement. From 2e097ad2f4b958c60d01bcd08e09b5e11b8d81ac Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 21 Aug 2012 16:26:14 +0200 Subject: [PATCH 38/62] [ahrs] remove old ahrs structre (execpt status) --- sw/airborne/subsystems/ahrs.c | 3 -- sw/airborne/subsystems/ahrs.h | 69 ++++------------------------------- 2 files changed, 7 insertions(+), 65 deletions(-) diff --git a/sw/airborne/subsystems/ahrs.c b/sw/airborne/subsystems/ahrs.c index f68a86af67..44e7cafb30 100644 --- a/sw/airborne/subsystems/ahrs.c +++ b/sw/airborne/subsystems/ahrs.c @@ -22,7 +22,4 @@ #include "subsystems/ahrs.h" struct Ahrs ahrs; -struct AhrsFloat ahrs_float; - -float ahrs_mag_offset; diff --git a/sw/airborne/subsystems/ahrs.h b/sw/airborne/subsystems/ahrs.h index 1eafd751e6..76c9438fbc 100644 --- a/sw/airborne/subsystems/ahrs.h +++ b/sw/airborne/subsystems/ahrs.h @@ -39,73 +39,13 @@ #include AHRS_TYPE_H #endif -/** Attitude and Heading Reference System state (fixed point version) */ +/** Attitude and Heading Reference System state */ struct Ahrs { - - struct Int32Quat ltp_to_imu_quat; ///< Rotation from LocalTangentPlane to IMU frame as unit quaternion - struct Int32Eulers ltp_to_imu_euler; ///< Rotation from LocalTangentPlane to IMU frame as Euler angles - struct Int32RMat ltp_to_imu_rmat; ///< Rotation from LocalTangentPlane to IMU frame as Rotation Matrix - struct Int32Rates imu_rate; ///< Rotational velocity in IMU frame - - struct Int32Quat ltp_to_body_quat; ///< Rotation from LocalTangentPlane to body frame as unit quaternion - struct Int32Eulers ltp_to_body_euler; ///< Rotation from LocalTangentPlane to body frame as Euler angles - struct Int32RMat ltp_to_body_rmat; ///< Rotation from LocalTangentPlane to body frame as Rotation Matrix - struct Int32Rates body_rate; ///< Rotational velocity in body frame - uint8_t status; ///< status of the AHRS, AHRS_UNINIT or AHRS_RUNNING }; -/** Attitude and Heading Reference System state (floating point version) */ -struct AhrsFloat { - struct FloatQuat ltp_to_imu_quat; ///< Rotation from LocalTangentPlane to IMU frame as unit quaternion - struct FloatEulers ltp_to_imu_euler; ///< Rotation from LocalTangentPlane to IMU frame as Euler angles - struct FloatRMat ltp_to_imu_rmat; ///< Rotation from LocalTangentPlane to IMU frame as Rotation Matrix - struct FloatRates imu_rate; ///< Rotational velocity in IMU frame - struct FloatRates imu_rate_previous; - struct FloatRates imu_rate_d; - - struct FloatQuat ltp_to_body_quat; ///< Rotation from LocalTangentPlane to body frame as unit quaternion - struct FloatEulers ltp_to_body_euler; ///< Rotation from LocalTangentPlane to body frame as Euler angles - struct FloatRMat ltp_to_body_rmat; ///< Rotation from LocalTangentPlane to body frame as Rotation Matrix - struct FloatRates body_rate; ///< Rotational velocity in body frame - struct FloatRates body_rate_d; - - // always use status from fixed point ahrs struct for now - //uint8_t status; -}; - -/** global AHRS state (fixed point version) */ +/** global AHRS state */ extern struct Ahrs ahrs; -/** global AHRS state (floating point version) */ -extern struct AhrsFloat ahrs_float; - -extern float ahrs_mag_offset; - -#define AHRS_FLOAT_OF_INT32() { \ - QUAT_FLOAT_OF_BFP(ahrs_float.ltp_to_body_quat, ahrs.ltp_to_body_quat); \ - EULERS_FLOAT_OF_BFP(ahrs_float.ltp_to_body_euler, ahrs.ltp_to_body_euler); \ - RATES_FLOAT_OF_BFP(ahrs_float.body_rate, ahrs.body_rate); \ - } - -#define AHRS_INT_OF_FLOAT() { \ - QUAT_BFP_OF_REAL(ahrs.ltp_to_body_quat, ahrs_float.ltp_to_body_quat); \ - EULERS_BFP_OF_REAL(ahrs.ltp_to_body_euler, ahrs_float.ltp_to_body_euler); \ - RMAT_BFP_OF_REAL(ahrs.ltp_to_body_rmat, ahrs_float.ltp_to_body_rmat); \ - RATES_BFP_OF_REAL(ahrs.body_rate, ahrs_float.body_rate); \ - } - -#define AHRS_IMU_INT_OF_FLOAT() { \ - QUAT_BFP_OF_REAL(ahrs.ltp_to_imu_quat, ahrs_float.ltp_to_imu_quat); \ - EULERS_BFP_OF_REAL(ahrs.ltp_to_imu_euler, ahrs_float.ltp_to_imu_euler); \ - RMAT_BFP_OF_REAL(ahrs.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_rmat); \ - RATES_BFP_OF_REAL(ahrs.imu_rate, ahrs_float.imu_rate); \ -} - -/* copy attitude to state interface */ -#define AHRS_BODY_TO_STATE() { \ - stateSetNedToBodyQuat_i(&ahrs.ltp_to_body_quat); \ - stateSetBodyRates_i(&ahrs.body_rate); \ - } /** AHRS initialization. Called at startup. * Needs to be implemented by each AHRS algorithm. @@ -135,6 +75,11 @@ extern void ahrs_update_accel(void); * Needs to be implemented by each AHRS algorithm. */ extern void ahrs_update_mag(void); + +/** Update AHRS state with GPS measurements. + * Reads the global #gps data struct. + * Needs to be implemented by each AHRS algorithm. + */ extern void ahrs_update_gps(void); #endif /* AHRS_H */ From 5ab06a973f787b77ea7855f91edd5e9bf6add4c8 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 21 Aug 2012 16:26:56 +0200 Subject: [PATCH 39/62] [fw ahrs] no need to update old fixed wing estimator --- sw/airborne/firmwares/fixedwing/main_ap.c | 3 --- 1 file changed, 3 deletions(-) diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index 6f22bfeca7..53f6379eb1 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -685,7 +685,6 @@ static inline void on_gyro_event( void ) { ahrs_propagate(); ahrs_update_accel(); - ahrs_update_fw_estimator(); #ifdef AHRS_TRIGGERED_ATTITUDE_LOOP new_ins_attitude = 1; @@ -725,8 +724,6 @@ static inline void on_gyro_event( void ) { ahrs_update_accel(); } - ahrs_update_fw_estimator(); - #ifdef AHRS_TRIGGERED_ATTITUDE_LOOP new_ins_attitude = 1; #endif From acb6eaadadbb7bb6e8432d4a27557c329337f120 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 21 Aug 2012 16:29:12 +0200 Subject: [PATCH 40/62] [telemetry] update rotorcraft telemetry to state interface --- sw/airborne/firmwares/rotorcraft/telemetry.h | 212 +++++++++---------- 1 file changed, 104 insertions(+), 108 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index e72b4d6de0..4c4695a7a2 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -213,30 +213,28 @@ } #ifdef STABILISATION_ATTITUDE_TYPE_INT -#define PERIODIC_SEND_STAB_ATTITUDE(_trans, _dev) { \ - DOWNLINK_SEND_STAB_ATTITUDE_INT(_trans, _dev, \ - &ahrs.body_rate.p, \ - &ahrs.body_rate.q, \ - &ahrs.body_rate.r, \ - &ahrs.ltp_to_body_euler.phi, \ - &ahrs.ltp_to_body_euler.theta, \ - &ahrs.ltp_to_body_euler.psi, \ - &stab_att_sp_euler.phi, \ - &stab_att_sp_euler.theta, \ - &stab_att_sp_euler.psi, \ - &stabilization_att_sum_err.phi, \ - &stabilization_att_sum_err.theta, \ - &stabilization_att_sum_err.psi, \ - &stabilization_att_fb_cmd[COMMAND_ROLL], \ - &stabilization_att_fb_cmd[COMMAND_PITCH], \ - &stabilization_att_fb_cmd[COMMAND_YAW], \ - &stabilization_att_ff_cmd[COMMAND_ROLL], \ - &stabilization_att_ff_cmd[COMMAND_PITCH], \ - &stabilization_att_ff_cmd[COMMAND_YAW], \ - &stabilization_cmd[COMMAND_ROLL], \ - &stabilization_cmd[COMMAND_PITCH], \ - &stabilization_cmd[COMMAND_YAW]); \ - } +#define PERIODIC_SEND_STAB_ATTITUDE(_trans, _dev) { \ + struct Int32Rates* body_rate = stateGetBodyRates_i(); \ + struct Int32Eulers* att = stateGetNedToBodyEulers_i(); \ + DOWNLINK_SEND_STAB_ATTITUDE_INT(_trans, _dev, \ + &(body_rate->p), &(body_rate->q), &(body_rate->r), \ + &(att->phi), &(att->theta), &(att->psi), \ + &stab_att_sp_euler.phi, \ + &stab_att_sp_euler.theta, \ + &stab_att_sp_euler.psi, \ + &stabilization_att_sum_err.phi, \ + &stabilization_att_sum_err.theta, \ + &stabilization_att_sum_err.psi, \ + &stabilization_att_fb_cmd[COMMAND_ROLL], \ + &stabilization_att_fb_cmd[COMMAND_PITCH], \ + &stabilization_att_fb_cmd[COMMAND_YAW], \ + &stabilization_att_ff_cmd[COMMAND_ROLL], \ + &stabilization_att_ff_cmd[COMMAND_PITCH], \ + &stabilization_att_ff_cmd[COMMAND_YAW], \ + &stabilization_cmd[COMMAND_ROLL], \ + &stabilization_cmd[COMMAND_PITCH], \ + &stabilization_cmd[COMMAND_YAW]); \ +} #define PERIODIC_SEND_STAB_ATTITUDE_REF(_trans, _dev) { \ @@ -257,32 +255,29 @@ #endif /* STABILISATION_ATTITUDE_TYPE_INT */ #ifdef STABILISATION_ATTITUDE_TYPE_FLOAT -#define PERIODIC_SEND_STAB_ATTITUDE(_trans, _dev) { \ - DOWNLINK_SEND_STAB_ATTITUDE_FLOAT(_trans, _dev, \ - &ahrs_float.body_rate.p, \ - &ahrs_float.body_rate.q, \ - &ahrs_float.body_rate.r, \ - &ahrs_float.ltp_to_body_euler.phi, \ - &ahrs_float.ltp_to_body_euler.theta, \ - &ahrs_float.ltp_to_body_euler.psi, \ - &stab_att_ref_euler.phi, \ - &stab_att_ref_euler.theta, \ - &stab_att_ref_euler.psi, \ - &stabilization_att_sum_err_eulers.phi, \ - &stabilization_att_sum_err_eulers.theta, \ - &stabilization_att_sum_err_eulers.psi, \ - &stabilization_att_fb_cmd[COMMAND_ROLL], \ - &stabilization_att_fb_cmd[COMMAND_PITCH], \ - &stabilization_att_fb_cmd[COMMAND_YAW], \ - &stabilization_att_ff_cmd[COMMAND_ROLL], \ - &stabilization_att_ff_cmd[COMMAND_PITCH], \ - &stabilization_att_ff_cmd[COMMAND_YAW], \ - &stabilization_cmd[COMMAND_ROLL], \ - &stabilization_cmd[COMMAND_PITCH], \ - &stabilization_cmd[COMMAND_YAW], \ - &ahrs_float.body_rate_d.p, \ - &ahrs_float.body_rate_d.q, \ - &ahrs_float.body_rate_d.r); \ +#define PERIODIC_SEND_STAB_ATTITUDE(_trans, _dev) { \ + struct FloatRates* body_rate = stateGetBodyRates_f(); \ + struct FloatEulers* att = stateGetNedToBodyEulers_f(); \ + float foo; \ + DOWNLINK_SEND_STAB_ATTITUDE_FLOAT(_trans, _dev, \ + &(body_rate->p), &(body_rate->q), &(body_rate->r), \ + &(att->phi), &(att->theta), &(att->psi), \ + &stab_att_ref_euler.phi, \ + &stab_att_ref_euler.theta, \ + &stab_att_ref_euler.psi, \ + &stabilization_att_sum_err_eulers.phi, \ + &stabilization_att_sum_err_eulers.theta, \ + &stabilization_att_sum_err_eulers.psi, \ + &stabilization_att_fb_cmd[COMMAND_ROLL], \ + &stabilization_att_fb_cmd[COMMAND_PITCH], \ + &stabilization_att_fb_cmd[COMMAND_YAW], \ + &stabilization_att_ff_cmd[COMMAND_ROLL], \ + &stabilization_att_ff_cmd[COMMAND_PITCH], \ + &stabilization_att_ff_cmd[COMMAND_YAW], \ + &stabilization_cmd[COMMAND_ROLL], \ + &stabilization_cmd[COMMAND_PITCH], \ + &stabilization_cmd[COMMAND_YAW], \ + &foo, &foo, &foo); \ } #define PERIODIC_SEND_STAB_ATTITUDE_REF(_trans, _dev) { \ @@ -332,9 +327,9 @@ #include "subsystems/ahrs/ahrs_int_cmpl_euler.h" #define PERIODIC_SEND_FILTER(_trans, _dev) { \ DOWNLINK_SEND_FILTER(_trans, _dev, \ - &ahrs.ltp_to_imu_euler.phi, \ - &ahrs.ltp_to_imu_euler.theta, \ - &ahrs.ltp_to_imu_euler.psi, \ + &ahrs_impl.ltp_to_imu_euler.phi, \ + &ahrs_impl.ltp_to_imu_euler.theta, \ + &ahrs_impl.ltp_to_imu_euler.psi, \ &ahrs_impl.measure.phi, \ &ahrs_impl.measure.theta, \ &ahrs_impl.measure.psi, \ @@ -420,64 +415,65 @@ #endif #if defined STABILISATION_ATTITUDE_TYPE_QUAT && defined STABILISATION_ATTITUDE_TYPE_INT -#define PERIODIC_SEND_AHRS_REF_QUAT(_trans, _dev) { \ - DOWNLINK_SEND_AHRS_REF_QUAT(_trans, _dev, \ - &stab_att_ref_quat.qi, \ - &stab_att_ref_quat.qx, \ - &stab_att_ref_quat.qy, \ - &stab_att_ref_quat.qz, \ - &ahrs.ltp_to_body_quat.qi, \ - &ahrs.ltp_to_body_quat.qx, \ - &ahrs.ltp_to_body_quat.qy, \ - &ahrs.ltp_to_body_quat.qz); \ +#define PERIODIC_SEND_AHRS_REF_QUAT(_trans, _dev) { \ + DOWNLINK_SEND_AHRS_REF_QUAT(_trans, _dev, \ + &stab_att_ref_quat.qi, \ + &stab_att_ref_quat.qx, \ + &stab_att_ref_quat.qy, \ + &stab_att_ref_quat.qz, \ + &(stateGetNedToBodyQuat_i()->qi), \ + &(stateGetNedToBodyQuat_i()->qx), \ + &(stateGetNedToBodyQuat_i()->qy), \ + &(stateGetNedToBodyQuat_i()->qz)); \ } #else #define PERIODIC_SEND_AHRS_REF_QUAT(_trans, _dev) {} #endif /* STABILISATION_ATTITUDE_TYPE_QUAT */ -#define PERIODIC_SEND_AHRS_QUAT_INT(_trans, _dev) { \ - DOWNLINK_SEND_AHRS_QUAT_INT(_trans, _dev, \ - &ahrs.ltp_to_imu_quat.qi, \ - &ahrs.ltp_to_imu_quat.qx, \ - &ahrs.ltp_to_imu_quat.qy, \ - &ahrs.ltp_to_imu_quat.qz, \ - &(stateGetNedToBodyQuat_i()->qi), \ - &(stateGetNedToBodyQuat_i()->qx), \ - &(stateGetNedToBodyQuat_i()->qy), \ - &(stateGetNedToBodyQuat_i()->qz)); \ +#define PERIODIC_SEND_AHRS_QUAT_INT(_trans, _dev) { \ + DOWNLINK_SEND_AHRS_QUAT_INT(_trans, _dev, \ + &ahrs_impl.ltp_to_imu_quat.qi, \ + &ahrs_impl.ltp_to_imu_quat.qx, \ + &ahrs_impl.ltp_to_imu_quat.qy, \ + &ahrs_impl.ltp_to_imu_quat.qz, \ + &(stateGetNedToBodyQuat_i()->qi), \ + &(stateGetNedToBodyQuat_i()->qx), \ + &(stateGetNedToBodyQuat_i()->qy), \ + &(stateGetNedToBodyQuat_i()->qz)); \ } -#define PERIODIC_SEND_AHRS_EULER_INT(_trans, _dev) { \ - DOWNLINK_SEND_AHRS_EULER_INT(_trans, _dev, \ - &ahrs.ltp_to_imu_euler.phi, \ - &ahrs.ltp_to_imu_euler.theta, \ - &ahrs.ltp_to_imu_euler.psi, \ - &ahrs.ltp_to_body_euler.phi, \ - &ahrs.ltp_to_body_euler.theta, \ - &ahrs.ltp_to_body_euler.psi); \ +#define PERIODIC_SEND_AHRS_EULER_INT(_trans, _dev) { \ + DOWNLINK_SEND_AHRS_EULER_INT(_trans, _dev, \ + &ahrs_impl.ltp_to_imu_euler.phi, \ + &ahrs_impl.ltp_to_imu_euler.theta, \ + &ahrs_impl.ltp_to_imu_euler.psi, \ + &(stateGetNedToBodyEulers_i()->phi), \ + &(stateGetNedToBodyEulers_i()->theta), \ + &(stateGetNedToBodyEulers_i()->psi)); \ } -#define PERIODIC_SEND_AHRS_RMAT_INT(_trans, _dev) { \ - DOWNLINK_SEND_AHRS_RMAT(_trans, _dev, \ - &ahrs.ltp_to_imu_rmat.m[0], \ - &ahrs.ltp_to_imu_rmat.m[1], \ - &ahrs.ltp_to_imu_rmat.m[2], \ - &ahrs.ltp_to_imu_rmat.m[3], \ - &ahrs.ltp_to_imu_rmat.m[4], \ - &ahrs.ltp_to_imu_rmat.m[5], \ - &ahrs.ltp_to_imu_rmat.m[6], \ - &ahrs.ltp_to_imu_rmat.m[7], \ - &ahrs.ltp_to_imu_rmat.m[8], \ - &ahrs.ltp_to_body_rmat.m[0], \ - &ahrs.ltp_to_body_rmat.m[1], \ - &ahrs.ltp_to_body_rmat.m[2], \ - &ahrs.ltp_to_body_rmat.m[3], \ - &ahrs.ltp_to_body_rmat.m[4], \ - &ahrs.ltp_to_body_rmat.m[5], \ - &ahrs.ltp_to_body_rmat.m[6], \ - &ahrs.ltp_to_body_rmat.m[7], \ - &ahrs.ltp_to_body_rmat.m[8]); \ - } +#define PERIODIC_SEND_AHRS_RMAT_INT(_trans, _dev) { \ + struct Int32RMat* att_rmat = stateGetNedToBodyRMat_i(); \ + DOWNLINK_SEND_AHRS_RMAT(_trans, _dev, \ + &ahrs_impl.ltp_to_imu_rmat.m[0], \ + &ahrs_impl.ltp_to_imu_rmat.m[1], \ + &ahrs_impl.ltp_to_imu_rmat.m[2], \ + &ahrs_impl.ltp_to_imu_rmat.m[3], \ + &ahrs_impl.ltp_to_imu_rmat.m[4], \ + &ahrs_impl.ltp_to_imu_rmat.m[5], \ + &ahrs_impl.ltp_to_imu_rmat.m[6], \ + &ahrs_impl.ltp_to_imu_rmat.m[7], \ + &ahrs_impl.ltp_to_imu_rmat.m[8], \ + &(att_rmat->m[0]), \ + &(att_rmat->m[1]), \ + &(att_rmat->m[2]), \ + &(att_rmat->m[3]), \ + &(att_rmat->m[4]), \ + &(att_rmat->m[5]), \ + &(att_rmat->m[6]), \ + &(att_rmat->m[7]), \ + &(att_rmat->m[8])); \ +} @@ -749,12 +745,12 @@ &stabilization_cmd[COMMAND_PITCH], \ &stabilization_cmd[COMMAND_YAW], \ &stabilization_cmd[COMMAND_THRUST], \ - &ahrs.ltp_to_imu_euler.phi, \ - &ahrs.ltp_to_imu_euler.theta, \ - &ahrs.ltp_to_imu_euler.psi, \ - &ahrs.ltp_to_body_euler.phi, \ - &ahrs.ltp_to_body_euler.theta, \ - &ahrs.ltp_to_body_euler.psi); \ + &ahrs_impl.ltp_to_imu_euler.phi, \ + &ahrs_impl.ltp_to_imu_euler.theta, \ + &ahrs_impl.ltp_to_imu_euler.psi, \ + &(stateGetNedToBodyEulers_i()->phi), \ + &(stateGetNedToBodyEulers_i()->theta), \ + &(stateGetNedToBodyEulers_i()->psi)); \ } #ifdef USE_I2C0 From d266fa6698f03d28489738a4f42f6b15ff4b845c Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 21 Aug 2012 16:29:47 +0200 Subject: [PATCH 41/62] [fix] fix multiple definition warning --- sw/airborne/subsystems/ins/hf_float.h | 1 + 1 file changed, 1 insertion(+) diff --git a/sw/airborne/subsystems/ins/hf_float.h b/sw/airborne/subsystems/ins/hf_float.h index 8c7669cc3e..eb8c6aa1f3 100644 --- a/sw/airborne/subsystems/ins/hf_float.h +++ b/sw/airborne/subsystems/ins/hf_float.h @@ -26,6 +26,7 @@ #include "std.h" #include "math/pprz_algebra_float.h" +#include "generated/airframe.h" #define HFF_STATE_SIZE 2 From 040b52597038a6d457ef5a2636305057a00a1e52 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 21 Aug 2012 16:31:21 +0200 Subject: [PATCH 42/62] [ahrs] forgot to remove unused function declaration --- sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h index 22715f68fd..491ca8cdc1 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h @@ -63,8 +63,6 @@ void ahrs_update_heading(float heading); void ahrs_realign_heading(float heading); #ifdef AHRS_UPDATE_FW_ESTIMATOR -// TODO copy ahrs to state instead of estimator -void ahrs_update_fw_estimator(void); extern float ins_roll_neutral; extern float ins_pitch_neutral; #endif From 4aa4297223cb1f5f77ce245080e9c25baa466efe Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 21 Aug 2012 18:32:33 +0200 Subject: [PATCH 43/62] [ins] improving and cleaning ins interface - INS interface build for FW and Rotorcraft on the same basis as AHRS - For FW, most of the old estimator (vertical kalman filter) as been moved to the new ins_float. Some triggers are missing for main_ap, especially the baro_event since the Baro interface is not done here. - Some more integration is needed to remove old estimator.c - INS modules should follow the general interface - FW main_ap is not very clear concerning the INS (nor the AHRS) because of the freq scaling part from CDW --- .../subsystems/fixedwing/autopilot.makefile | 3 + .../subsystems/rotorcraft/ins.makefile | 2 + .../rotorcraft/ins_extended.makefile | 4 +- conf/flight_plans/basic.xml | 6 +- sw/airborne/estimator.c | 166 ----------- sw/airborne/estimator.h | 92 +----- sw/airborne/firmwares/fixedwing/main_ap.c | 14 +- sw/airborne/subsystems/ins.c | 247 +--------------- sw/airborne/subsystems/ins.h | 99 ++++--- sw/airborne/subsystems/ins/ins_float.c | 201 +++++++++++++ sw/airborne/subsystems/ins/ins_float.h | 78 ++++++ sw/airborne/subsystems/ins/ins_int.c | 265 ++++++++++++++++++ sw/airborne/subsystems/ins/ins_int.h | 72 +++++ .../ins_int_extended.c} | 2 +- 14 files changed, 695 insertions(+), 556 deletions(-) create mode 100644 sw/airborne/subsystems/ins/ins_float.c create mode 100644 sw/airborne/subsystems/ins/ins_float.h create mode 100644 sw/airborne/subsystems/ins/ins_int.c create mode 100644 sw/airborne/subsystems/ins/ins_int.h rename sw/airborne/subsystems/{ins_extended.c => ins/ins_int_extended.c} (99%) diff --git a/conf/firmwares/subsystems/fixedwing/autopilot.makefile b/conf/firmwares/subsystems/fixedwing/autopilot.makefile index 4423d5d81d..b2f1000dce 100644 --- a/conf/firmwares/subsystems/fixedwing/autopilot.makefile +++ b/conf/firmwares/subsystems/fixedwing/autopilot.makefile @@ -163,6 +163,9 @@ fbw_srcs += $(SRC_FIRMWARE)/fbw_downlink.c ap_CFLAGS += -DAP ap_srcs += $(SRC_FIRMWARE)/main_ap.c ap_srcs += $(SRC_FIXEDWING)/estimator.c +ap_CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_float.h\" +ap_srcs += $(SRC_FIXEDWING)/subsystems/ins.c +ap_srcs += $(SRC_FIXEDWING)/subsystems/ins/ins_float.c ap_srcs += $(SRC_FIRMWARE)/ap_downlink.c ap_srcs += state.c diff --git a/conf/firmwares/subsystems/rotorcraft/ins.makefile b/conf/firmwares/subsystems/rotorcraft/ins.makefile index 7340c078a2..c23c0333ca 100644 --- a/conf/firmwares/subsystems/rotorcraft/ins.makefile +++ b/conf/firmwares/subsystems/rotorcraft/ins.makefile @@ -2,7 +2,9 @@ # simple INS with float vertical filter # +$(TARGET).CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_int.h\" $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins.c +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_int.c # vertical filter float version $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/vf_float.c diff --git a/conf/firmwares/subsystems/rotorcraft/ins_extended.makefile b/conf/firmwares/subsystems/rotorcraft/ins_extended.makefile index e872ac1f77..4928c4c495 100644 --- a/conf/firmwares/subsystems/rotorcraft/ins_extended.makefile +++ b/conf/firmwares/subsystems/rotorcraft/ins_extended.makefile @@ -2,7 +2,9 @@ # extended INS with vertical filter using sonar in a better way (flap ground) # -$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins_extended.c +$(TARGET).CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_int.h\" +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins.c +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_int_extended.c # vertical filter float version $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/vf_extended_float.c diff --git a/conf/flight_plans/basic.xml b/conf/flight_plans/basic.xml index 151f18a087..f1c0fb690e 100644 --- a/conf/flight_plans/basic.xml +++ b/conf/flight_plans/basic.xml @@ -36,7 +36,7 @@ - + @@ -78,10 +78,10 @@ - + - + diff --git a/sw/airborne/estimator.c b/sw/airborne/estimator.c index 0539844040..3426a5ef53 100644 --- a/sw/airborne/estimator.c +++ b/sw/airborne/estimator.c @@ -36,183 +36,17 @@ #include "subsystems/gps.h" #include "subsystems/nav.h" -/* position in meters */ -float estimator_x; -float estimator_y; -float estimator_z; - -float estimator_z_dot; - -/* attitude in radian */ -float estimator_phi; -float estimator_psi; -float estimator_theta; - -/* rates in radians per second */ -float estimator_p; -float estimator_q; -float estimator_r; - /* flight time in seconds */ uint16_t estimator_flight_time; -/* flight time in seconds */ -float estimator_t; - -/* horizontal speed in module and dir */ -float estimator_hspeed_mod; -float estimator_hspeed_dir; /* wind */ float wind_east, wind_north; float estimator_airspeed; float estimator_AOA; -#define NORM_RAD_ANGLE2(x) { \ - while (x > 2 * M_PI) x -= 2 * M_PI; \ - while (x < 0 ) x += 2 * M_PI; \ - } - - -#define EstimatorSetSpeedCart(vx, vy, vz) { \ - estimator_vx = vx; \ - estimator_vy = vy; \ - estimator_vz = vz; \ -} - - void estimator_init( void ) { - struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 }; - stateSetLocalUtmOrigin_f(&utm0); - - stateSetPositionUtm_f(&utm0); - - EstimatorSetAlt(0.); - estimator_flight_time = 0; } - -bool_t alt_kalman_enabled; - -#ifdef ALT_KALMAN - -#ifndef ALT_KALMAN_ENABLED -#define ALT_KALMAN_ENABLED FALSE -#endif - -#define GPS_SIGMA2 1. -#define GPS_DT 0.25 -#define GPS_R 2. - -#define BARO_DT 0.1 - -static float p[2][2]; - -void alt_kalman_reset( void ) { - p[0][0] = 1.; - p[0][1] = 0.; - p[1][0] = 0.; - p[1][1] = 1.; -} - -void alt_kalman_init( void ) { - alt_kalman_enabled = ALT_KALMAN_ENABLED; - alt_kalman_reset(); -} - -void alt_kalman(float gps_z) { - float DT; - float R; - float SIGMA2; - -#if USE_BARO_MS5534A - if (alt_baro_enabled) { - DT = BARO_DT; - R = baro_MS5534A_r; - SIGMA2 = baro_MS5534A_sigma2; - } else -#elif USE_BARO_ETS - if (baro_ets_enabled) { - DT = BARO_ETS_DT; - R = baro_ets_r; - SIGMA2 = baro_ets_sigma2; - } else -#elif USE_BARO_BMP - if (baro_bmp_enabled) { - DT = BARO_BMP_DT; - R = baro_bmp_r; - SIGMA2 = baro_bmp_sigma2; - } else -#endif - { - DT = GPS_DT; - R = GPS_R; - SIGMA2 = GPS_SIGMA2; - } - - float q[2][2]; - q[0][0] = DT*DT*DT*DT/4.; - q[0][1] = DT*DT*DT/2.; - q[1][0] = DT*DT*DT/2.; - q[1][1] = DT*DT; - - - /* predict */ - estimator_z += estimator_z_dot * DT; - p[0][0] = p[0][0]+p[1][0]*DT+DT*(p[0][1]+p[1][1]*DT) + SIGMA2*q[0][0]; - p[0][1] = p[0][1]+p[1][1]*DT + SIGMA2*q[0][1]; - p[1][0] = p[1][0]+p[1][1]*DT + SIGMA2*q[1][0]; - p[1][1] = p[1][1] + SIGMA2*q[1][1]; - - /* error estimate */ - float e = p[0][0] + R; - - if (fabs(e) > 1e-5) { - float k_0 = p[0][0] / e; - float k_1 = p[1][0] / e; - e = gps_z - estimator_z; - - /* correction */ - estimator_z += k_0 * e; - estimator_z_dot += k_1 * e; - - p[1][0] = -p[0][0]*k_1+p[1][0]; - p[1][1] = -p[0][1]*k_1+p[1][1]; - p[0][0] = p[0][0] * (1-k_0); - p[0][1] = p[0][1] * (1-k_0); - } - -#ifdef DEBUG_ALT_KALMAN - DOWNLINK_SEND_ALT_KALMAN(DefaultChannel,DefaultDevice,&(p[0][0]),&(p[0][1]),&(p[1][0]), &(p[1][1])); -#endif -} - -#endif // ALT_KALMAN - -void estimator_update_state_gps( void ) { - struct UtmCoor_f utm; - utm.east = gps.utm_pos.east / 100.; - utm.north = gps.utm_pos.north / 100.; - utm.zone = nav_utm_zone0; - -#if !USE_BARO_BMP && !USE_BARO_ETS && !USE_BARO_MS5534A - float falt = gps.hmsl / 1000.; - EstimatorSetAlt(falt); -#endif - utm.alt = estimator_z; - // set position - stateSetPositionUtm_f(&utm); - - struct NedCoor_f ned_vel = { - gps.ned_vel.x / 100., - gps.ned_vel.y / 100., - gps.ned_vel.z / 100. - }; - // set velocity - stateSetSpeedNed_f(&ned_vel); - - // Heading estimation now in ahrs_infrared - -} diff --git a/sw/airborne/estimator.h b/sw/airborne/estimator.h index e24a468469..13dd0bed62 100644 --- a/sw/airborne/estimator.h +++ b/sw/airborne/estimator.h @@ -24,6 +24,7 @@ /** \file estimator.h * \brief State estimation, fusioning sensors + * TODO will be removed when the remaining variables are integrated to the state interface */ #ifndef ESTIMATOR_H @@ -35,43 +36,8 @@ #include "state.h" -#ifdef BARO_MS5534A -#include "baro_MS5534A.h" -#endif - -#if USE_BARO_ETS -#include "modules/sensors/baro_ets.h" -#endif - -#if USE_BARO_BMP -#include "modules/sensors/baro_bmp.h" -#endif - -/* position in meters, ENU frame, relative to reference */ -extern float estimator_x; ///< east position in meters -extern float estimator_y; ///< north position in meters -extern float estimator_z; ///< altitude above MSL in meters - -/* attitude in radians */ -extern float estimator_phi; ///< roll angle in rad, + = right -extern float estimator_psi; ///< heading in rad, CW, 0 = N -extern float estimator_theta; ///< pitch angle in rad, + = up - -/* speed in meters per second */ -extern float estimator_z_dot; - -/* rates in radians per second */ -extern float estimator_p; -extern float estimator_q; -extern float estimator_r; - /** flight time in seconds. */ extern uint16_t estimator_flight_time; -extern float estimator_t; - -/* horizontal ground speed in module and dir (m/s, rad (CW/North)) */ -extern float estimator_hspeed_mod; ///< module of horizontal ground speed in m/s -extern float estimator_hspeed_dir; ///< direction of horizontal ground speed in rad (CW/North) /* Wind and airspeed estimation sent by the GCS */ extern float wind_east, wind_north; /* m/s */ @@ -81,60 +47,4 @@ extern float estimator_AOA; ///< angle of attack in rad void estimator_init( void ); -void estimator_update_state_gps( void ); - -extern bool_t alt_kalman_enabled; -#ifdef ALT_KALMAN -extern void alt_kalman_reset( void ); -extern void alt_kalman_init( void ); -extern void alt_kalman( float ); -#endif - - -#define GetPosX() (stateGetPositionEnu_f()->x) -#define GetPosY() (stateGetPositionEnu_f()->y) -#define GetPosAlt() (stateGetPositionEnu_f()->z) - - -#ifdef ALT_KALMAN -#define EstimatorSetPosXY(x, y) { estimator_x = x; estimator_y = y; } - -#if USE_BARO_MS5534A || USE_BARO_ETS || USE_BARO_BMP -/* Kalman filter cannot be disabled in this mode (no z_dot) */ -#define EstimatorSetAlt(z) alt_kalman(z) -#else /* USE_BARO_x */ -#define EstimatorSetAlt(z) { \ - if (!alt_kalman_enabled) { \ - estimator_z = z; \ - } else { \ - alt_kalman(z); \ - } \ -} -#endif /* ! USE_BARO_x */ - -#define EstimatorSetSpeedPol(vhmod, vhdir, vz) { \ - estimator_hspeed_mod = vhmod; \ - estimator_hspeed_dir = vhdir; \ - if (!alt_kalman_enabled) estimator_z_dot = vz; \ -} -#else /* ALT_KALMAN */ -#define EstimatorSetPosXY(x, y) { estimator_x = x; estimator_y = y; } -#define EstimatorSetAlt(z) { estimator_z = z; } -#define EstimatorSetSpeedPol(vhmod, vhdir, vz) { \ - estimator_hspeed_mod = vhmod; \ - estimator_hspeed_dir = vhdir; \ - estimator_z_dot = vz; \ -} - -#endif - -#define EstimatorSetAirspeed(airspeed) { estimator_airspeed = airspeed; } -#define EstimatorSetAOA(AOA) { estimator_AOA = AOA; } - -#define EstimatorSetAtt(phi, psi, theta) { estimator_phi = phi; estimator_psi = psi; estimator_theta = theta; } -#define EstimatorSetPhiPsi(phi, psi) { estimator_phi = phi; estimator_psi = psi; } - -#define EstimatorSetRate(p, q, r) { estimator_p = p; estimator_q = q; estimator_r = r; } - - #endif /* ESTIMATOR_H */ diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index 53f6379eb1..bc37a5441e 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -57,6 +57,7 @@ // autopilot & control #include "firmwares/fixedwing/autopilot.h" #include "estimator.h" +#include "subsystems/ins.h" #include "firmwares/fixedwing/stabilization/stabilization_attitude.h" #include CTRL_TYPE_H #include "subsystems/nav.h" @@ -167,9 +168,7 @@ void init_ap( void ) { ahrs_init(); #endif -#if USE_INS ins_init(); -#endif /************* Links initialization ***************/ #if defined MCU_SPI_LINK @@ -183,9 +182,6 @@ void init_ap( void ) { h_ctl_init(); v_ctl_init(); estimator_init(); -#ifdef ALT_KALMAN - alt_kalman_init(); -#endif nav_init(); modules_init(); @@ -547,9 +543,7 @@ void sensors_task( void ) { ahrs_propagate(); #endif -#if USE_INS - ins_periodic_task(); -#endif + ins_periodic(); } @@ -583,7 +577,7 @@ void monitor_task( void ) { kill_throttle |= launch && (dist2_to_home > Square(KILL_MODE_DISTANCE)); if (!estimator_flight_time && - estimator_hspeed_mod > MIN_SPEED_FOR_TAKEOFF) { + *stateGetHorizontalSpeedNorm_f() > MIN_SPEED_FOR_TAKEOFF) { estimator_flight_time = 1; launch = TRUE; /* Not set in non auto launch */ uint16_t time_sec = sys_time.nb_sec; @@ -647,7 +641,7 @@ void event_task_ap( void ) { #if USE_GPS static inline void on_gps_solution( void ) { - estimator_update_state_gps(); + ins_update_gps(); #if USE_AHRS ahrs_update_gps(); #endif diff --git a/sw/airborne/subsystems/ins.c b/sw/airborne/subsystems/ins.c index d69d018fd2..0eb0a55e02 100644 --- a/sw/airborne/subsystems/ins.c +++ b/sw/airborne/subsystems/ins.c @@ -1,5 +1,5 @@ /* - * Copyright (C) 2008-2010 The Paparazzi Team + * Copyright (C) 2008-2012 The Paparazzi Team * * This file is part of paparazzi. * @@ -21,248 +21,5 @@ #include "subsystems/ins.h" -#include "subsystems/imu.h" -#include "subsystems/sensors/baro.h" -#include "subsystems/gps.h" +struct Ins ins; -#include "generated/airframe.h" -#include "math/pprz_algebra_int.h" -#include "math/pprz_algebra_float.h" - -#include "subsystems/ahrs.h" - -#if USE_VFF -#include "subsystems/ins/vf_float.h" -#endif - -#if USE_HFF -#include "subsystems/ins/hf_float.h" -#endif - -#ifdef SITL -#include "nps_fdm.h" -#include -#endif - - -#include "math/pprz_geodetic_int.h" - -#include "generated/flight_plan.h" - -/* gps transformed to LTP-NED */ -struct LtpDef_i ins_ltp_def; - bool_t ins_ltp_initialised; -struct NedCoor_i ins_gps_pos_cm_ned; -struct NedCoor_i ins_gps_speed_cm_s_ned; -#if USE_HFF -/* horizontal gps transformed to NED in meters as float */ -struct FloatVect2 ins_gps_pos_m_ned; -struct FloatVect2 ins_gps_speed_m_s_ned; -#endif -bool_t ins_hf_realign; - -/* barometer */ -#if USE_VFF -int32_t ins_qfe; -bool_t ins_baro_initialised; -int32_t ins_baro_alt; -#if USE_SONAR -bool_t ins_update_on_agl; -int32_t ins_sonar_offset; -#endif -#endif -bool_t ins_vf_realign; - -/* output */ -struct NedCoor_i ins_ltp_pos; -struct NedCoor_i ins_ltp_speed; -struct NedCoor_i ins_ltp_accel; - - -void ins_init() { -#if USE_INS_NAV_INIT - ins_ltp_initialised = TRUE; - - /** FIXME: should use the same code than MOVE_WP in firmwares/rotorcraft/datalink.c */ - struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */ - llh_nav0.lat = INT32_RAD_OF_DEG(NAV_LAT0); - llh_nav0.lon = INT32_RAD_OF_DEG(NAV_LON0); - /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */ - llh_nav0.alt = NAV_ALT0 + NAV_MSL0; - - struct EcefCoor_i ecef_nav0; - ecef_of_lla_i(&ecef_nav0, &llh_nav0); - - ltp_def_from_ecef_i(&ins_ltp_def, &ecef_nav0); - ins_ltp_def.hmsl = NAV_ALT0; - stateSetLocalOrigin_i(&ins_ltp_def); -#else - ins_ltp_initialised = FALSE; -#endif -#if USE_VFF - ins_baro_initialised = FALSE; -#if USE_SONAR - ins_update_on_agl = FALSE; -#endif - vff_init(0., 0., 0.); -#endif - ins_vf_realign = FALSE; - ins_hf_realign = FALSE; -#if USE_HFF - b2_hff_init(0., 0., 0., 0.); -#endif - INT32_VECT3_ZERO(ins_ltp_pos); - INT32_VECT3_ZERO(ins_ltp_speed); - INT32_VECT3_ZERO(ins_ltp_accel); -} - -void ins_periodic( void ) { -} - -#if USE_HFF -void ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed) { - b2_hff_realign(pos, speed); -} -#else -void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) {} -#endif /* USE_HFF */ - - -void ins_realign_v(float z) { -#if USE_VFF - vff_realign(z); -#endif -} - -void ins_propagate() { - /* untilt accels */ - struct Int32Vect3 accel_meas_body; - INT32_RMAT_TRANSP_VMULT(accel_meas_body, imu.body_to_imu_rmat, imu.accel); - struct Int32Vect3 accel_meas_ltp; - INT32_RMAT_TRANSP_VMULT(accel_meas_ltp, (*stateGetNedToBodyRMat_i()), accel_meas_body); - -#if USE_VFF - float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z); - if (baro.status == BS_RUNNING && ins_baro_initialised) { - vff_propagate(z_accel_meas_float); - ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot); - ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot); - ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z); - } - else { // feed accel from the sensors - // subtract -9.81m/s2 (acceleration measured due to gravity, but vehivle not accelerating in ltp) - ins_ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81); - } -#else - ins_ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81); -#endif /* USE_VFF */ - -#if USE_HFF - /* propagate horizontal filter */ - b2_hff_propagate(); -#else - ins_ltp_accel.x = accel_meas_ltp.x; - ins_ltp_accel.y = accel_meas_ltp.y; -#endif /* USE_HFF */ - - INS_NED_TO_STATE(); -} - -void ins_update_baro() { -#if USE_VFF - if (baro.status == BS_RUNNING) { - if (!ins_baro_initialised) { - ins_qfe = baro.absolute; - ins_baro_initialised = TRUE; - } - if (ins_vf_realign) { - ins_vf_realign = FALSE; - ins_qfe = baro.absolute; -#if USE_SONAR - ins_sonar_offset = sonar_meas; -#endif - vff_realign(0.); - ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot); - ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot); - ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z); - } - else { /* not realigning, so normal update with baro measurement */ - ins_baro_alt = ((baro.absolute - ins_qfe) * INS_BARO_SENS_NUM)/INS_BARO_SENS_DEN; - float alt_float = POS_FLOAT_OF_BFP(ins_baro_alt); - vff_update(alt_float); - } - } - INS_NED_TO_STATE(); -#endif -} - - -void ins_update_gps(void) { -#if USE_GPS - if (gps.fix == GPS_FIX_3D) { - if (!ins_ltp_initialised) { - ltp_def_from_ecef_i(&ins_ltp_def, &gps.ecef_pos); - ins_ltp_def.lla.alt = gps.lla_pos.alt; - ins_ltp_def.hmsl = gps.hmsl; - ins_ltp_initialised = TRUE; - stateSetLocalOrigin_i(&ins_ltp_def); - } - ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &gps.ecef_pos); - ned_of_ecef_vect_i(&ins_gps_speed_cm_s_ned, &ins_ltp_def, &gps.ecef_vel); -#if USE_HFF - VECT2_ASSIGN(ins_gps_pos_m_ned, ins_gps_pos_cm_ned.x, ins_gps_pos_cm_ned.y); - VECT2_SDIV(ins_gps_pos_m_ned, ins_gps_pos_m_ned, 100.); - VECT2_ASSIGN(ins_gps_speed_m_s_ned, ins_gps_speed_cm_s_ned.x, ins_gps_speed_cm_s_ned.y); - VECT2_SDIV(ins_gps_speed_m_s_ned, ins_gps_speed_m_s_ned, 100.); - if (ins_hf_realign) { - ins_hf_realign = FALSE; -#ifdef SITL - struct FloatVect2 true_pos, true_speed; - VECT2_COPY(true_pos, fdm.ltpprz_pos); - VECT2_COPY(true_speed, fdm.ltpprz_ecef_vel); - b2_hff_realign(true_pos, true_speed); -#else - const struct FloatVect2 zero = {0.0, 0.0}; - b2_hff_realign(ins_gps_pos_m_ned, zero); -#endif - } - b2_hff_update_gps(); -#if !USE_VFF /* vff not used */ - ins_ltp_pos.z = (ins_gps_pos_cm_ned.z * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN; - ins_ltp_speed.z = (ins_gps_speed_cm_s_ned.z * INT32_SPEED_OF_CM_S_NUM) INT32_SPEED_OF_CM_S_DEN; -#endif /* vff not used */ -#endif /* hff used */ - - -#if !USE_HFF /* hff not used */ -#if !USE_VFF /* neither hf nor vf used */ - INT32_VECT3_SCALE_3(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); - INT32_VECT3_SCALE_3(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN); -#else /* only vff used */ - INT32_VECT2_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); - INT32_VECT2_SCALE_2(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN); -#endif - -#if USE_GPS_LAG_HACK - VECT2_COPY(d_pos, ins_ltp_speed); - INT32_VECT2_RSHIFT(d_pos, d_pos, 11); - VECT2_ADD(ins_ltp_pos, d_pos); -#endif -#endif /* hff not used */ - - INS_NED_TO_STATE(); - } -#endif /* USE_GPS */ -} - -void ins_update_sonar() { -#if USE_SONAR && USE_VFF - static int32_t sonar_filtered = 0; - sonar_filtered = (sonar_meas + 2*sonar_filtered) / 3; - /* update baro_qfe assuming a flat ground */ - if (ins_update_on_agl && baro.status == BS_RUNNING) { - int32_t d_sonar = (((int32_t)sonar_filtered - ins_sonar_offset) * INS_SONAR_SENS_NUM) / INS_SONAR_SENS_DEN; - ins_qfe = baro.absolute + (d_sonar * (INS_BARO_SENS_DEN))/INS_BARO_SENS_NUM; - } -#endif -} diff --git a/sw/airborne/subsystems/ins.h b/sw/airborne/subsystems/ins.h index db13d60f5b..4ce715988e 100644 --- a/sw/airborne/subsystems/ins.h +++ b/sw/airborne/subsystems/ins.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2008-2010 The Paparazzi Team + * Copyright (C) 2008-2012 The Paparazzi Team * * This file is part of paparazzi. * @@ -25,54 +25,75 @@ #include "std.h" #include "math/pprz_geodetic_int.h" #include "math/pprz_algebra_float.h" - #include "state.h" -/* gps transformed to LTP-NED */ -extern struct LtpDef_i ins_ltp_def; -extern bool_t ins_ltp_initialised; -extern struct NedCoor_i ins_gps_pos_cm_ned; -extern struct NedCoor_i ins_gps_speed_cm_s_ned; +#define INS_UNINIT 0 +#define INS_RUNNING 1 -/* barometer */ -#if USE_VFF || USE_VFF_EXTENDED -extern int32_t ins_baro_alt; -extern int32_t ins_qfe; -extern bool_t ins_baro_initialised; -#if USE_SONAR -extern bool_t ins_update_on_agl; /* use sonar to update agl if available */ -extern int32_t ins_sonar_offset; -#endif +/* underlying includes (needed for parameters) */ +#ifdef INS_TYPE_H +#include INS_TYPE_H #endif -/* output LTP NED */ -extern struct NedCoor_i ins_ltp_pos; -extern struct NedCoor_i ins_ltp_speed; -extern struct NedCoor_i ins_ltp_accel; -#if USE_HFF -/* horizontal gps transformed to NED in meters as float */ -extern struct FloatVect2 ins_gps_pos_m_ned; -extern struct FloatVect2 ins_gps_speed_m_s_ned; -#endif +/** Inertial Navigation System state */ +struct Ins { + uint8_t status; ///< status of the INS +}; +/** global INS state */ +extern struct Ins ins; + +// TODO add to struct extern bool_t ins_hf_realign; extern bool_t ins_vf_realign; -extern void ins_init( void ); -extern void ins_periodic( void ); -extern void ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed); -extern void ins_realign_v(float z); -extern void ins_propagate( void ); -extern void ins_update_baro( void ); -extern void ins_update_gps( void ); -extern void ins_update_sonar( void ); -/* copy position and speed to state interface */ -#define INS_NED_TO_STATE() { \ - stateSetPositionNed_i(&ins_ltp_pos); \ - stateSetSpeedNed_i(&ins_ltp_speed); \ - stateSetAccelNed_i(&ins_ltp_accel); \ - } +/** INS initialization. Called at startup. + * Needs to be implemented by each INS algorithm. + */ +extern void ins_init( void ); + +/** INS periodic call. + * Needs to be implemented by each INS algorithm. + */ +extern void ins_periodic( void ); + +/** INS horizontal realign. + * @param pos new horizontal position to set + * @param speed new horizontal speed to set + * Needs to be implemented by each INS algorithm. + */ +extern void ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed); + +/** INS vertical realign. + * @param z new altitude to set + * Needs to be implemented by each INS algorithm. + */ +extern void ins_realign_v(float z); + +/** Propagation. Usually integrates the gyro rates to angles. + * Reads the global #imu data struct. + * Needs to be implemented by each INS algorithm. + */ +extern void ins_propagate( void ); + +/** Update INS state with barometer measurements. + * Reads the global #baro data struct. + * Needs to be implemented by each INS algorithm. + */ +extern void ins_update_baro( void ); + +/** Update INS state with GPS measurements. + * Reads the global #gps data struct. + * Needs to be implemented by each INS algorithm. + */ +extern void ins_update_gps( void ); + +/** Update INS state with sonar measurements. + * Reads the global #sonar data struct. + * Needs to be implemented by each INS algorithm. + */ +extern void ins_update_sonar( void ); #endif /* INS_H */ diff --git a/sw/airborne/subsystems/ins/ins_float.c b/sw/airborne/subsystems/ins/ins_float.c new file mode 100644 index 0000000000..a3216975e2 --- /dev/null +++ b/sw/airborne/subsystems/ins/ins_float.c @@ -0,0 +1,201 @@ +/* + * Paparazzi autopilot $Id$ + * + * Copyright (C) 2004-2010 The Paparazzi Team + * + * This file is part of paparazzi. + * + * paparazzi 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 2, or (at your option) + * any later version. + * + * paparazzi 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 paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + +/** \file estimator.c + * \brief State estimate, fusioning sensors + */ + +#include "subsystems/ins/ins_float.h" + +#include +#include + +#include "state.h" +#include "mcu_periph/uart.h" +#include "ap_downlink.h" +#include "subsystems/gps.h" +#include "subsystems/nav.h" + +/* vertical position and speed in meters */ +float estimator_z; +float estimator_z_dot; + +void ins_init() { + + struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 }; + stateSetLocalUtmOrigin_f(&utm0); + + stateSetPositionUtm_f(&utm0); + +#ifdef ALT_KALMAN + alt_kalman_init(); +#endif + + EstimatorSetAlt(0.); + +} + +void ins_periodic( void ) { +} + +void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) { +} + +void ins_realign_v(float z __attribute__ ((unused))) { +} + +void ins_propagate() { +} + +void ins_update_baro() { + // TODO update kalman filter with baro struct +} + + +void ins_update_gps(void) { +#if USE_GPS + struct UtmCoor_f utm; + utm.east = gps.utm_pos.east / 100.; + utm.north = gps.utm_pos.north / 100.; + utm.zone = nav_utm_zone0; + +#if !USE_BARO_BMP && !USE_BARO_ETS && !USE_BARO_MS5534A + float falt = gps.hmsl / 1000.; + EstimatorSetAlt(falt); +#endif + utm.alt = estimator_z; + // set position + stateSetPositionUtm_f(&utm); + + struct NedCoor_f ned_vel = { + gps.ned_vel.x / 100., + gps.ned_vel.y / 100., + gps.ned_vel.z / 100. + }; + // set velocity + stateSetSpeedNed_f(&ned_vel); + +#endif +} + +void ins_update_sonar() { +} + +bool_t alt_kalman_enabled; + +#ifdef ALT_KALMAN + +#ifndef ALT_KALMAN_ENABLED +#define ALT_KALMAN_ENABLED FALSE +#endif + +#define GPS_SIGMA2 1. +#define GPS_DT 0.25 +#define GPS_R 2. + +#define BARO_DT 0.1 + +static float p[2][2]; + +void alt_kalman_reset( void ) { + p[0][0] = 1.; + p[0][1] = 0.; + p[1][0] = 0.; + p[1][1] = 1.; +} + +void alt_kalman_init( void ) { + alt_kalman_enabled = ALT_KALMAN_ENABLED; + alt_kalman_reset(); +} + +void alt_kalman(float z_meas) { + float DT; + float R; + float SIGMA2; + +#if USE_BARO_MS5534A + if (alt_baro_enabled) { + DT = BARO_DT; + R = baro_MS5534A_r; + SIGMA2 = baro_MS5534A_sigma2; + } else +#elif USE_BARO_ETS + if (baro_ets_enabled) { + DT = BARO_ETS_DT; + R = baro_ets_r; + SIGMA2 = baro_ets_sigma2; + } else +#elif USE_BARO_BMP + if (baro_bmp_enabled) { + DT = BARO_BMP_DT; + R = baro_bmp_r; + SIGMA2 = baro_bmp_sigma2; + } else +#endif + { + DT = GPS_DT; + R = GPS_R; + SIGMA2 = GPS_SIGMA2; + } + + float q[2][2]; + q[0][0] = DT*DT*DT*DT/4.; + q[0][1] = DT*DT*DT/2.; + q[1][0] = DT*DT*DT/2.; + q[1][1] = DT*DT; + + + /* predict */ + estimator_z += estimator_z_dot * DT; + p[0][0] = p[0][0]+p[1][0]*DT+DT*(p[0][1]+p[1][1]*DT) + SIGMA2*q[0][0]; + p[0][1] = p[0][1]+p[1][1]*DT + SIGMA2*q[0][1]; + p[1][0] = p[1][0]+p[1][1]*DT + SIGMA2*q[1][0]; + p[1][1] = p[1][1] + SIGMA2*q[1][1]; + + /* error estimate */ + float e = p[0][0] + R; + + if (fabs(e) > 1e-5) { + float k_0 = p[0][0] / e; + float k_1 = p[1][0] / e; + e = z_meas - estimator_z; + + /* correction */ + estimator_z += k_0 * e; + estimator_z_dot += k_1 * e; + + p[1][0] = -p[0][0]*k_1+p[1][0]; + p[1][1] = -p[0][1]*k_1+p[1][1]; + p[0][0] = p[0][0] * (1-k_0); + p[0][1] = p[0][1] * (1-k_0); + } + +#ifdef DEBUG_ALT_KALMAN + DOWNLINK_SEND_ALT_KALMAN(DefaultChannel,DefaultDevice,&(p[0][0]),&(p[0][1]),&(p[1][0]), &(p[1][1])); +#endif +} + +#endif // ALT_KALMAN + diff --git a/sw/airborne/subsystems/ins/ins_float.h b/sw/airborne/subsystems/ins/ins_float.h new file mode 100644 index 0000000000..d2cab0b407 --- /dev/null +++ b/sw/airborne/subsystems/ins/ins_float.h @@ -0,0 +1,78 @@ +/* + * Copyright (C) 2004-2006 Pascal Brisset, Antoine Drouin + * Copyright (C) 2012 Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi 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 2, or (at your option) + * any later version. + * + * paparazzi 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 paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + +#ifndef INS_FLOAT_H +#define INS_FLOAT_H + +#include "subsystems/ins.h" + +#include +#include "std.h" +#include "state.h" + +#ifdef BARO_MS5534A +#include "baro_MS5534A.h" +#endif + +#if USE_BARO_ETS +#include "modules/sensors/baro_ets.h" +#endif + +#if USE_BARO_BMP +#include "modules/sensors/baro_bmp.h" +#endif + + +/* position in meters, ENU frame, relative to reference */ +extern float estimator_z; ///< altitude above MSL in meters + +/* speed in meters per second */ +extern float estimator_z_dot; + +extern bool_t alt_kalman_enabled; +#ifdef ALT_KALMAN +extern void alt_kalman_reset( void ); +extern void alt_kalman_init( void ); +extern void alt_kalman( float ); +#endif + +#ifdef ALT_KALMAN + +#if USE_BARO_MS5534A || USE_BARO_ETS || USE_BARO_BMP +/* Kalman filter cannot be disabled in this mode (no z_dot) */ +#define EstimatorSetAlt(z) alt_kalman(z) +#else /* USE_BARO_x */ +#define EstimatorSetAlt(z) { \ + if (!alt_kalman_enabled) { \ + estimator_z = z; \ + } else { \ + alt_kalman(z); \ + } \ +} +#endif /* ! USE_BARO_x */ + +#else /* ALT_KALMAN */ +#define EstimatorSetAlt(z) { estimator_z = z; } +#endif + +#endif /* INS_FLOAT_H */ diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c new file mode 100644 index 0000000000..ef8efb4bf6 --- /dev/null +++ b/sw/airborne/subsystems/ins/ins_int.c @@ -0,0 +1,265 @@ +/* + * Copyright (C) 2008-2010 The Paparazzi Team + * + * This file is part of paparazzi. + * + * paparazzi 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 2, or (at your option) + * any later version. + * + * paparazzi 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 paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +#include "subsystems/ins/ins_int.h" + +#include "subsystems/imu.h" +#include "subsystems/sensors/baro.h" +#include "subsystems/gps.h" + +#include "generated/airframe.h" + +#if USE_VFF +#include "subsystems/ins/vf_float.h" +#endif + +#if USE_HFF +#include "subsystems/ins/hf_float.h" +#endif + +#ifdef SITL +#include "nps_fdm.h" +#include +#endif + + +#include "math/pprz_geodetic_int.h" + +#include "generated/flight_plan.h" + +/* gps transformed to LTP-NED */ +struct LtpDef_i ins_ltp_def; + bool_t ins_ltp_initialised; +struct NedCoor_i ins_gps_pos_cm_ned; +struct NedCoor_i ins_gps_speed_cm_s_ned; +#if USE_HFF +/* horizontal gps transformed to NED in meters as float */ +struct FloatVect2 ins_gps_pos_m_ned; +struct FloatVect2 ins_gps_speed_m_s_ned; +#endif +bool_t ins_hf_realign; + +/* barometer */ +#if USE_VFF +int32_t ins_qfe; +bool_t ins_baro_initialised; +int32_t ins_baro_alt; +#if USE_SONAR +bool_t ins_update_on_agl; +int32_t ins_sonar_offset; +#endif +#endif +bool_t ins_vf_realign; + +/* output */ +struct NedCoor_i ins_ltp_pos; +struct NedCoor_i ins_ltp_speed; +struct NedCoor_i ins_ltp_accel; + + +void ins_init() { +#if USE_INS_NAV_INIT + ins_ltp_initialised = TRUE; + + /** FIXME: should use the same code than MOVE_WP in firmwares/rotorcraft/datalink.c */ + struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */ + llh_nav0.lat = INT32_RAD_OF_DEG(NAV_LAT0); + llh_nav0.lon = INT32_RAD_OF_DEG(NAV_LON0); + /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */ + llh_nav0.alt = NAV_ALT0 + NAV_MSL0; + + struct EcefCoor_i ecef_nav0; + ecef_of_lla_i(&ecef_nav0, &llh_nav0); + + ltp_def_from_ecef_i(&ins_ltp_def, &ecef_nav0); + ins_ltp_def.hmsl = NAV_ALT0; + stateSetLocalOrigin_i(&ins_ltp_def); +#else + ins_ltp_initialised = FALSE; +#endif +#if USE_VFF + ins_baro_initialised = FALSE; +#if USE_SONAR + ins_update_on_agl = FALSE; +#endif + vff_init(0., 0., 0.); +#endif + ins_vf_realign = FALSE; + ins_hf_realign = FALSE; +#if USE_HFF + b2_hff_init(0., 0., 0., 0.); +#endif + INT32_VECT3_ZERO(ins_ltp_pos); + INT32_VECT3_ZERO(ins_ltp_speed); + INT32_VECT3_ZERO(ins_ltp_accel); + + // TODO correct init + ins.status = INS_RUNNING; + +} + +void ins_periodic( void ) { +} + +void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) { +#if USE_HFF + b2_hff_realign(pos, speed); +#endif /* USE_HFF */ +} + +void ins_realign_v(float z __attribute__ ((unused))) { +#if USE_VFF + vff_realign(z); +#endif +} + +void ins_propagate() { + /* untilt accels */ + struct Int32Vect3 accel_meas_body; + INT32_RMAT_TRANSP_VMULT(accel_meas_body, imu.body_to_imu_rmat, imu.accel); + struct Int32Vect3 accel_meas_ltp; + INT32_RMAT_TRANSP_VMULT(accel_meas_ltp, (*stateGetNedToBodyRMat_i()), accel_meas_body); + +#if USE_VFF + float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z); + if (baro.status == BS_RUNNING && ins_baro_initialised) { + vff_propagate(z_accel_meas_float); + ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot); + ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot); + ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z); + } + else { // feed accel from the sensors + // subtract -9.81m/s2 (acceleration measured due to gravity, but vehivle not accelerating in ltp) + ins_ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81); + } +#else + ins_ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81); +#endif /* USE_VFF */ + +#if USE_HFF + /* propagate horizontal filter */ + b2_hff_propagate(); +#else + ins_ltp_accel.x = accel_meas_ltp.x; + ins_ltp_accel.y = accel_meas_ltp.y; +#endif /* USE_HFF */ + + INS_NED_TO_STATE(); +} + +void ins_update_baro() { +#if USE_VFF + if (baro.status == BS_RUNNING) { + if (!ins_baro_initialised) { + ins_qfe = baro.absolute; + ins_baro_initialised = TRUE; + } + if (ins_vf_realign) { + ins_vf_realign = FALSE; + ins_qfe = baro.absolute; +#if USE_SONAR + ins_sonar_offset = sonar_meas; +#endif + vff_realign(0.); + ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot); + ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot); + ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z); + } + else { /* not realigning, so normal update with baro measurement */ + ins_baro_alt = ((baro.absolute - ins_qfe) * INS_BARO_SENS_NUM)/INS_BARO_SENS_DEN; + float alt_float = POS_FLOAT_OF_BFP(ins_baro_alt); + vff_update(alt_float); + } + } + INS_NED_TO_STATE(); +#endif +} + + +void ins_update_gps(void) { +#if USE_GPS + if (gps.fix == GPS_FIX_3D) { + if (!ins_ltp_initialised) { + ltp_def_from_ecef_i(&ins_ltp_def, &gps.ecef_pos); + ins_ltp_def.lla.alt = gps.lla_pos.alt; + ins_ltp_def.hmsl = gps.hmsl; + ins_ltp_initialised = TRUE; + stateSetLocalOrigin_i(&ins_ltp_def); + } + ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &gps.ecef_pos); + ned_of_ecef_vect_i(&ins_gps_speed_cm_s_ned, &ins_ltp_def, &gps.ecef_vel); +#if USE_HFF + VECT2_ASSIGN(ins_gps_pos_m_ned, ins_gps_pos_cm_ned.x, ins_gps_pos_cm_ned.y); + VECT2_SDIV(ins_gps_pos_m_ned, ins_gps_pos_m_ned, 100.); + VECT2_ASSIGN(ins_gps_speed_m_s_ned, ins_gps_speed_cm_s_ned.x, ins_gps_speed_cm_s_ned.y); + VECT2_SDIV(ins_gps_speed_m_s_ned, ins_gps_speed_m_s_ned, 100.); + if (ins_hf_realign) { + ins_hf_realign = FALSE; +#ifdef SITL + struct FloatVect2 true_pos, true_speed; + VECT2_COPY(true_pos, fdm.ltpprz_pos); + VECT2_COPY(true_speed, fdm.ltpprz_ecef_vel); + b2_hff_realign(true_pos, true_speed); +#else + const struct FloatVect2 zero = {0.0, 0.0}; + b2_hff_realign(ins_gps_pos_m_ned, zero); +#endif + } + b2_hff_update_gps(); +#if !USE_VFF /* vff not used */ + ins_ltp_pos.z = (ins_gps_pos_cm_ned.z * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN; + ins_ltp_speed.z = (ins_gps_speed_cm_s_ned.z * INT32_SPEED_OF_CM_S_NUM) INT32_SPEED_OF_CM_S_DEN; +#endif /* vff not used */ +#endif /* hff used */ + + +#if !USE_HFF /* hff not used */ +#if !USE_VFF /* neither hf nor vf used */ + INT32_VECT3_SCALE_3(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); + INT32_VECT3_SCALE_3(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN); +#else /* only vff used */ + INT32_VECT2_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); + INT32_VECT2_SCALE_2(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN); +#endif + +#if USE_GPS_LAG_HACK + VECT2_COPY(d_pos, ins_ltp_speed); + INT32_VECT2_RSHIFT(d_pos, d_pos, 11); + VECT2_ADD(ins_ltp_pos, d_pos); +#endif +#endif /* hff not used */ + + INS_NED_TO_STATE(); + } +#endif /* USE_GPS */ +} + +void ins_update_sonar() { +#if USE_SONAR && USE_VFF + static int32_t sonar_filtered = 0; + sonar_filtered = (sonar_meas + 2*sonar_filtered) / 3; + /* update baro_qfe assuming a flat ground */ + if (ins_update_on_agl && baro.status == BS_RUNNING) { + int32_t d_sonar = (((int32_t)sonar_filtered - ins_sonar_offset) * INS_SONAR_SENS_NUM) / INS_SONAR_SENS_DEN; + ins_qfe = baro.absolute + (d_sonar * (INS_BARO_SENS_DEN))/INS_BARO_SENS_NUM; + } +#endif +} diff --git a/sw/airborne/subsystems/ins/ins_int.h b/sw/airborne/subsystems/ins/ins_int.h new file mode 100644 index 0000000000..6699d45f16 --- /dev/null +++ b/sw/airborne/subsystems/ins/ins_int.h @@ -0,0 +1,72 @@ +/* + * Copyright (C) 2008-2012 The Paparazzi Team + * + * This file is part of paparazzi. + * + * paparazzi 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 2, or (at your option) + * any later version. + * + * paparazzi 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 paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +#ifndef INS_INT_H +#define INS_INT_H + +#include "subsystems/ins.h" +#include "std.h" +#include "math/pprz_geodetic_int.h" +#include "math/pprz_algebra_float.h" + +// TODO integrate all internal state to the structure +///** Ins implementation state (fixed point) */ +//struct InsInt { +//}; +// +///** global INS state */ +//extern struct InsInt ins_impl; + +/* gps transformed to LTP-NED */ +extern struct LtpDef_i ins_ltp_def; +extern bool_t ins_ltp_initialised; +extern struct NedCoor_i ins_gps_pos_cm_ned; +extern struct NedCoor_i ins_gps_speed_cm_s_ned; + +/* barometer */ +#if USE_VFF || USE_VFF_EXTENDED +extern int32_t ins_baro_alt; +extern int32_t ins_qfe; +extern bool_t ins_baro_initialised; +#if USE_SONAR +extern bool_t ins_update_on_agl; /* use sonar to update agl if available */ +extern int32_t ins_sonar_offset; +#endif +#endif + +/* output LTP NED */ +extern struct NedCoor_i ins_ltp_pos; +extern struct NedCoor_i ins_ltp_speed; +extern struct NedCoor_i ins_ltp_accel; +#if USE_HFF +/* horizontal gps transformed to NED in meters as float */ +extern struct FloatVect2 ins_gps_pos_m_ned; +extern struct FloatVect2 ins_gps_speed_m_s_ned; +#endif + +/* copy position and speed to state interface */ +#define INS_NED_TO_STATE() { \ + stateSetPositionNed_i(&ins_ltp_pos); \ + stateSetSpeedNed_i(&ins_ltp_speed); \ + stateSetAccelNed_i(&ins_ltp_accel); \ +} + +#endif /* INS_INT_H */ diff --git a/sw/airborne/subsystems/ins_extended.c b/sw/airborne/subsystems/ins/ins_int_extended.c similarity index 99% rename from sw/airborne/subsystems/ins_extended.c rename to sw/airborne/subsystems/ins/ins_int_extended.c index f1d6e3976f..fcc97e4990 100644 --- a/sw/airborne/subsystems/ins_extended.c +++ b/sw/airborne/subsystems/ins/ins_int_extended.c @@ -20,7 +20,7 @@ * Boston, MA 02111-1307, USA. */ -#include "subsystems/ins.h" +#include "subsystems/ins/ins_int.h" #include "subsystems/imu.h" #include "subsystems/sensors/baro.h" From 0ffc2808755e380d8f6d213b0250fd720505bbdf Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 22 Aug 2012 13:54:46 +0200 Subject: [PATCH 44/62] [baro] add baro interface to FW So we can trigger ins_baro_update. Start to solve issue #106 --- sw/airborne/firmwares/fixedwing/main_ap.c | 29 +++++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index bc37a5441e..d9c3f75224 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -53,6 +53,9 @@ #if USE_AHRS_ALIGNER #include "subsystems/ahrs/ahrs_aligner.h" #endif +#if USE_BAROMETER +#include "subsystems/sensors/baro.h" +#endif // autopilot & control #include "firmwares/fixedwing/autopilot.h" @@ -96,6 +99,10 @@ volatile uint8_t ahrs_timeout_counter = 0; static inline void on_gps_solution( void ); #endif +#if USE_BAROMETER +static inline void on_baro_abs_event( void ); +static inline void on_baro_dif_event( void ); +#endif bool_t power_switch; @@ -168,6 +175,10 @@ void init_ap( void ) { ahrs_init(); #endif +#if USE_BAROMETER + baro_init(); +#endif + ins_init(); /************* Links initialization ***************/ @@ -543,6 +554,10 @@ void sensors_task( void ) { ahrs_propagate(); #endif +#if USE_BAROMETER + baro_periodic(); +#endif + ins_periodic(); } @@ -611,6 +626,9 @@ void event_task_ap( void ) { GpsEvent(on_gps_solution); #endif /* USE_GPS */ +#if USE_BAROMETER + BaroEvent(on_baro_abs_event, on_baro_dif_event); +#endif DatalinkEvent(); @@ -740,3 +758,14 @@ static inline void on_mag_event(void) #endif // USE_AHRS +#if USE_BAROMETER + +static inline void on_baro_abs_event( void ) { + ins_update_baro(); +} + +static inline void on_baro_dif_event( void ) { + +} + +#endif // USE_BAROMETER From 70adb45cd7a960d2987b5a975ccf6de8561c790f Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 22 Aug 2012 14:27:46 +0200 Subject: [PATCH 45/62] [baro] baro_board module is not needed in FW for boards with baro need to handle baro measures coming from modules --- .../subsystems/fixedwing/autopilot.makefile | 12 +++++++ sw/airborne/boards/navgo/baro_board.c | 4 +-- sw/airborne/boards/navgo/baro_board.h | 8 ++--- sw/airborne/boards/umarim/baro_board.c | 33 ++++++------------- sw/airborne/boards/umarim/baro_board.h | 29 ++++++---------- sw/airborne/firmwares/fixedwing/ap_downlink.h | 3 +- 6 files changed, 39 insertions(+), 50 deletions(-) diff --git a/conf/firmwares/subsystems/fixedwing/autopilot.makefile b/conf/firmwares/subsystems/fixedwing/autopilot.makefile index b2f1000dce..c7ecfc010a 100644 --- a/conf/firmwares/subsystems/fixedwing/autopilot.makefile +++ b/conf/firmwares/subsystems/fixedwing/autopilot.makefile @@ -169,6 +169,18 @@ ap_srcs += $(SRC_FIXEDWING)/subsystems/ins/ins_float.c ap_srcs += $(SRC_FIRMWARE)/ap_downlink.c ap_srcs += state.c +# BARO +ifeq ($(BOARD), umarim) +ifeq ($(BOARD_VERSION), 1.0) +ap_srcs += boards/umarim/baro_board.c +ap_CFLAGS += -DUSE_I2C1 -DUSE_ADS1114_1 +ap_CFLAGS += -DADS1114_I2C_DEVICE=i2c1 +ap_srcs += peripherals/ads1114.c +endif +else ifeq ($(BOARD), lisa_l) +ap_CFLAGS += -DUSE_I2C2 +endif + ###################################################################### ## diff --git a/sw/airborne/boards/navgo/baro_board.c b/sw/airborne/boards/navgo/baro_board.c index b42b69ee3a..58d72edadd 100644 --- a/sw/airborne/boards/navgo/baro_board.c +++ b/sw/airborne/boards/navgo/baro_board.c @@ -33,7 +33,7 @@ struct Baro baro; /* Counter to init mcp355x at startup */ -#define STARTUP_COUNTER 200 +#define BARO_STARTUP_COUNTER 200 uint16_t startup_cnt; void baro_init( void ) { @@ -45,7 +45,7 @@ void baro_init( void ) { #ifdef ROTORCRAFT_BARO_LED LED_OFF(ROTORCRAFT_BARO_LED); #endif - startup_cnt = STARTUP_COUNTER; + startup_cnt = BARO_STARTUP_COUNTER; } void baro_periodic( void ) { diff --git a/sw/airborne/boards/navgo/baro_board.h b/sw/airborne/boards/navgo/baro_board.h index 72065a498a..56137a6806 100644 --- a/sw/airborne/boards/navgo/baro_board.h +++ b/sw/airborne/boards/navgo/baro_board.h @@ -26,8 +26,8 @@ -#ifndef BOARDS_UMARIM_BARO_H -#define BOARDS_UMARIM_BARO_H +#ifndef BOARDS_NAVGO_BARO_H +#define BOARDS_NAVGO_BARO_H #include "std.h" @@ -36,10 +36,10 @@ #define BaroEvent(_b_abs_handler, _b_diff_handler) { \ mcp355x_event(); \ if (mcp355x_data_available) { \ - baro.absolute = mcp355x_data; \ + baro.absolute = mcp355x_data; \ _b_abs_handler(); \ mcp355x_data_available = FALSE; \ } \ } -#endif // BOARDS_UMARIM_BARO_H +#endif // BOARDS_NAVGO_BARO_H diff --git a/sw/airborne/boards/umarim/baro_board.c b/sw/airborne/boards/umarim/baro_board.c index 4bc9d9767f..c83675d17b 100644 --- a/sw/airborne/boards/umarim/baro_board.c +++ b/sw/airborne/boards/umarim/baro_board.c @@ -31,41 +31,28 @@ /* Common Baro struct */ struct Baro baro; -#if USE_BARO_AS_ALTIMETER -/* Number of values to compute an offset at startup */ -#define OFFSET_NBSAMPLES_AVRG 100 - -/* Weight for offset IIR filter */ -#define OFFSET_FILTER 7 - -float baro_alt; -float baro_alt_offset; -uint16_t offset_cnt; -#endif +/* Counter to init ads1114 at startup */ +#define BARO_STARTUP_COUNTER 200 +uint16_t startup_cnt; void baro_init( void ) { ads1114_init(); baro.status = BS_UNINITIALIZED; baro.absolute = 0; baro.differential = 0; /* not handled on this board, use extra module (ex: airspeed_ads1114) */ -#if USE_BARO_AS_ALTIMETER - baro_alt = 0.; - baro_alt_offset = 0.; - offset_cnt = OFFSET_NBSAMPLES_AVRG; -#endif + startup_cnt = BARO_STARTUP_COUNTER; } void baro_periodic( void ) { -#if USE_BARO_AS_ALTIMETER if (baro.status == BS_UNINITIALIZED && BARO_ABS_ADS.data_available) { - // IIR filter to compute an initial offset - baro_alt_offset = (OFFSET_FILTER * baro_alt_offset + (float)baro.absolute) / (OFFSET_FILTER + 1); - // decrease init counter - --offset_cnt; - if (offset_cnt == 0) baro.status = BS_RUNNING; + // Run some loops to get correct readings from the adc + --startup_cnt; + BARO_ABS_ADS.data_available = FALSE; + if (startup_cnt == 0) { + baro.status = BS_RUNNING; + } } -#endif // Read the ADC ads1114_read(&BARO_ABS_ADS); } diff --git a/sw/airborne/boards/umarim/baro_board.h b/sw/airborne/boards/umarim/baro_board.h index 62f09ca49c..a296e6a28c 100644 --- a/sw/airborne/boards/umarim/baro_board.h +++ b/sw/airborne/boards/umarim/baro_board.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2010 ENAC + * Copyright (C) 2010-2012 Gautier Hattenberger * * This file is part of paparazzi. * @@ -33,24 +33,15 @@ #include "std.h" #include "peripherals/ads1114.h" -#define BARO_FILTER_GAIN 5 - /* There is no differential pressure on the board but * it can be available from an external sensor * */ -#define DIFF_FILTER_GAIN 5 - -#if USE_BARO_AS_ALTIMETER -extern float baro_alt; -extern float baro_alt_offset; -#define BaroAltHandler() { baro_alt = BARO_SENS*(baro_alt_offset - (float)baro.absolute); } -#endif #define BARO_ABS_ADS ads1114_1 #define BaroAbs(_ads, _handler) { \ if (_ads.data_available) { \ - baro.absolute = (baro.absolute + BARO_FILTER_GAIN*Ads1114GetValue(_ads)) / (BARO_FILTER_GAIN+1); \ + baro.absolute = Ads1114GetValue(_ads); \ if (baro.status == BS_RUNNING) { \ _handler(); \ _ads.data_available = FALSE; \ @@ -65,14 +56,14 @@ extern float baro_alt_offset; #ifndef BARO_DIFF_ADS #define BARO_DIFF_ADS ads1114_2 #endif -#define BaroDiff(_ads, _handler) { \ - if (_ads.data_available) { \ - baro.differential = (baro.differential + DIFF_FILTER_GAIN*Ads1114GetValue(_ads)) / (DIFF_FILTER_GAIN+1); \ - if (baro.status == BS_RUNNING) { \ - _handler(); \ - _ads.data_available = FALSE; \ - } \ - } \ +#define BaroDiff(_ads, _handler) { \ + if (_ads.data_available) { \ + baro.differential = Ads1114GetValue(_ads); \ + if (baro.status == BS_RUNNING) { \ + _handler(); \ + _ads.data_available = FALSE; \ + } \ + } \ } #else // Not using differential with ADS1114 diff --git a/sw/airborne/firmwares/fixedwing/ap_downlink.h b/sw/airborne/firmwares/fixedwing/ap_downlink.h index b6bec10c9c..c4a6520e61 100644 --- a/sw/airborne/firmwares/fixedwing/ap_downlink.h +++ b/sw/airborne/firmwares/fixedwing/ap_downlink.h @@ -290,8 +290,7 @@ #define PERIODIC_SEND_SCP_STATUS(_trans, _dev) {} #endif -//FIXME: we need a better switch here... -#if BOARD_HAS_BARO && USE_BARO_AS_ALTIMETER +#if USE_BAROMETER #include "subsystems/sensors/baro.h" #define PERIODIC_SEND_BARO_RAW(_trans, _dev) { \ DOWNLINK_SEND_BARO_RAW(_trans, _dev, \ From a41ecf51166abb01c75ade9e0b8ef2b36e95c9aa Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 22 Aug 2012 15:46:38 +0200 Subject: [PATCH 46/62] [baro] adapt baro_board module to the new interface Some modules can be used directly, some needs some more changes or adaptation --- conf/modules/baro_board.xml | 22 ++----------- conf/telemetry/default_fixedwing.xml | 7 ++++ sw/airborne/modules/sensors/baro_MS5534A.h | 2 ++ sw/airborne/modules/sensors/baro_amsys.h | 6 ++-- sw/airborne/modules/sensors/baro_bmp.c | 3 ++ sw/airborne/modules/sensors/baro_bmp.h | 3 ++ .../modules/sensors/baro_board_module.h | 32 +++++++++++++++++-- sw/airborne/modules/sensors/baro_ets.h | 2 ++ sw/airborne/modules/sensors/baro_scp.h | 2 ++ .../modules/sensors/pressure_board_navarro.h | 2 ++ 10 files changed, 58 insertions(+), 23 deletions(-) diff --git a/conf/modules/baro_board.xml b/conf/modules/baro_board.xml index e378dea4ce..b87687e982 100644 --- a/conf/modules/baro_board.xml +++ b/conf/modules/baro_board.xml @@ -2,30 +2,14 @@ - Temporary hack to use baro interface on fixedwing + Allow to use baro interface on fixedwing with external barometers
- - - - - - -ifeq ($(BOARD), navgo) - ap.CFLAGS += -DUSE_I2C1 -DUSE_ADS1114_1 - ap.CFLAGS += -DADS1114_I2C_DEVICE=i2c1 - ap.srcs += peripherals/ads1114.c -else ifeq ($(BOARD), umarim) - ap.CFLAGS += -DUSE_I2C1 -DUSE_ADS1114_1 - ap.CFLAGS += -DADS1114_I2C_DEVICE=i2c1 - ap.srcs += peripherals/ads1114.c -else ifeq ($(BOARD), lisa_l) - ap.CFLAGS += -DUSE_I2C2 -endif - + + diff --git a/conf/telemetry/default_fixedwing.xml b/conf/telemetry/default_fixedwing.xml index 89fee5251a..fcfa6320f3 100644 --- a/conf/telemetry/default_fixedwing.xml +++ b/conf/telemetry/default_fixedwing.xml @@ -62,6 +62,13 @@ + + + + + + + diff --git a/sw/airborne/modules/sensors/baro_MS5534A.h b/sw/airborne/modules/sensors/baro_MS5534A.h index d8e39137ba..f9c9ebef4e 100644 --- a/sw/airborne/modules/sensors/baro_MS5534A.h +++ b/sw/airborne/modules/sensors/baro_MS5534A.h @@ -55,6 +55,8 @@ void baro_MS5534A_event_task( void ); void baro_MS5534A_event( void ); +#define BaroMS5534AUpdate(_b) { if (baro_MS5534A_available) { _b = baro_MS5534A_pressure; baro_MS5534A_available = FALSE; } } + #endif // USE_BARO_MS5534A #endif // BARO_MS5534A_H diff --git a/sw/airborne/modules/sensors/baro_amsys.h b/sw/airborne/modules/sensors/baro_amsys.h index 9e0884944d..7844695db6 100644 --- a/sw/airborne/modules/sensors/baro_amsys.h +++ b/sw/airborne/modules/sensors/baro_amsys.h @@ -30,9 +30,9 @@ #define BARO_AMSYS_DT 0.05 -// extern uint16_t baro_amsys_adc; +extern uint16_t baro_amsys_adc; // extern float baro_amsys_offset; -// extern bool_t baro_amsys_valid; +extern bool_t baro_amsys_valid; // extern bool_t baro_amsys_updated; // extern bool_t baro_amsys_enabled; extern float baro_amsys_altitude; @@ -48,4 +48,6 @@ extern void baro_amsys_read_event( void ); #define BaroAmsysEvent() { if (baro_amsys_i2c_trans.status == I2CTransSuccess) baro_amsys_read_event(); } +#define BaroAmsysUpdate(_b) { if (baro_amsys_valid) { _b = baro_amsys_adc; baro_amsys_valid = FALSE; } } + #endif // BARO_AMSYS_H diff --git a/sw/airborne/modules/sensors/baro_bmp.c b/sw/airborne/modules/sensors/baro_bmp.c index 2e1af7ad70..b5ba065da1 100644 --- a/sw/airborne/modules/sensors/baro_bmp.c +++ b/sw/airborne/modules/sensors/baro_bmp.c @@ -74,6 +74,7 @@ float baro_bmp_sigma2; // Global variables uint8_t baro_bmp_status; +bool_t baro_bmp_valid; uint32_t baro_bmp_pressure; uint16_t baro_bmp_temperature; int32_t baro_bmp_altitude, baro_bmp,baro_bmp_temp,baro_bmp_offset; @@ -92,6 +93,7 @@ uint16_t baro_bmp_cnt; void baro_bmp_init( void ) { baro_bmp_status = BARO_BMP_UNINIT; + baro_bmp_valid = FALSE; baro_bmp_r = BARO_BMP_R; baro_bmp_sigma2 = BARO_BMP_SIGMA2; baro_bmp_enabled = TRUE; @@ -234,6 +236,7 @@ void baro_bmp_event( void ) { if (baro_bmp_offset_init) { baro_bmp_altitude = ground_alt + baro_bmp_temp; // New value available + baro_bmp_valid = TRUE; #if USE_BARO_BMP #pragma message "USING BARO BMP" EstimatorSetAlt(baro_bmp_altitude); diff --git a/sw/airborne/modules/sensors/baro_bmp.h b/sw/airborne/modules/sensors/baro_bmp.h index 0d82a37b3c..53adeb184d 100644 --- a/sw/airborne/modules/sensors/baro_bmp.h +++ b/sw/airborne/modules/sensors/baro_bmp.h @@ -40,6 +40,7 @@ extern float baro_bmp_r; extern float baro_bmp_sigma2; extern uint8_t baro_bmp_status; +extern bool_t baro_bmp_valid; extern uint32_t baro_bmp_pressure; extern uint16_t baro_bmp_temperature; extern int32_t baro_bmp_altitude; @@ -50,4 +51,6 @@ void baro_bmp_init(void); void baro_bmp_periodic(void); void baro_bmp_event(void); +#define BaroBmpUpdate(_b) { if (baro_bmp_valid) { _b = baro_bmp_pressure; baro_bmp_valid = FALSE; } } + #endif diff --git a/sw/airborne/modules/sensors/baro_board_module.h b/sw/airborne/modules/sensors/baro_board_module.h index 5ba3d796b6..f8d88e5595 100644 --- a/sw/airborne/modules/sensors/baro_board_module.h +++ b/sw/airborne/modules/sensors/baro_board_module.h @@ -29,7 +29,35 @@ #include "subsystems/sensors/baro.h" -static inline void baro_abs(void) {} -static inline void baro_diff(void) {} +/** Absolute baro macro mapping. + * Select the baro module you want to use to feed the common baro interface + * in your airframe file when configuring baro_board module + * ex: + * for module baro_ets + * + */ +#ifndef BARO_ABS_EVENT +#define BARO_ABS_EVENT NoBaro +#endif + +/** Differential baro macro mapping. + * TODO + */ +#ifndef BARO_DIFF_EVENT +#define BARO_DIFF_EVENT NoBaro +#endif + +#define NoBaro(_b) {} + +/** BaroEvent macro. + * Need to be maped to one the external baro running has a module + */ +#define BaroEvent(_b_abs_handler, _b_diff_handler) { \ + BARO_ABS_EVENT(baro.absolute); \ + BARO_DIFF_EVENT(baro.differential); \ + _b_abs_handler(); \ + _b_diff_handler(); \ +} + #endif diff --git a/sw/airborne/modules/sensors/baro_ets.h b/sw/airborne/modules/sensors/baro_ets.h index bfdc568b6d..86e45a83ef 100644 --- a/sw/airborne/modules/sensors/baro_ets.h +++ b/sw/airborne/modules/sensors/baro_ets.h @@ -60,4 +60,6 @@ extern void baro_ets_read_event( void ); #define BaroEtsEvent() { if (baro_ets_i2c_trans.status == I2CTransSuccess) baro_ets_read_event(); } +#define BaroEtsUpdate(_b) { if (baro_ets_valid) { _b = baro_ets_adc; baro_ets_valid = FALSE; } } + #endif // BARO_ETS_H diff --git a/sw/airborne/modules/sensors/baro_scp.h b/sw/airborne/modules/sensors/baro_scp.h index c69534ec0f..2619adb96d 100644 --- a/sw/airborne/modules/sensors/baro_scp.h +++ b/sw/airborne/modules/sensors/baro_scp.h @@ -20,4 +20,6 @@ void baro_scp_init(void); void baro_scp_periodic(void); void baro_scp_event(void); +#define BaroScpUpdate(_b) { if (baro_scp_available) { _b = baro_scp_pressure; baro_scp_available = FALSE; } } + #endif diff --git a/sw/airborne/modules/sensors/pressure_board_navarro.h b/sw/airborne/modules/sensors/pressure_board_navarro.h index f5e78f2a2f..3ba8b2d208 100644 --- a/sw/airborne/modules/sensors/pressure_board_navarro.h +++ b/sw/airborne/modules/sensors/pressure_board_navarro.h @@ -54,6 +54,8 @@ extern void pbn_read_event( void ); #define PbnEvent() { if (pbn_trans.status == I2CTransSuccess) pbn_read_event(); } +#define BaroPbnUpdate(_b) { if (data_valid) { _b = altitude_adc; data_valid = FALSE; } } + #define PERIODIC_SEND_PBN(_chan) DOWNLINK_SEND_PBN(DefaultChannel, DefaultDevice,&airspeed_adc,&altitude_adc,&pbn_airspeed,&pbn_altitude,&airspeed_offset,&altitude_offset); #endif // PRESSURE_BOARD_NAVARRO_H From 8943a251e3f4791ac931e14440b6c2f83d48c913 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 22 Aug 2012 16:04:09 +0200 Subject: [PATCH 47/62] [ins] feed ins estimator on baro event --- sw/airborne/subsystems/ins/ins_float.c | 33 ++++++++++++++++++++++++++ 1 file changed, 33 insertions(+) diff --git a/sw/airborne/subsystems/ins/ins_float.c b/sw/airborne/subsystems/ins/ins_float.c index a3216975e2..bc9298fe38 100644 --- a/sw/airborne/subsystems/ins/ins_float.c +++ b/sw/airborne/subsystems/ins/ins_float.c @@ -41,6 +41,16 @@ float estimator_z; float estimator_z_dot; +#if USE_BAROMETER +#include "subsystems/sensors/baro.h" +int32_t ins_qfe; +bool_t ins_baro_initialised; +float ins_baro_alt; +#endif + +bool_t ins_hf_realign; +bool_t ins_vf_realign; + void ins_init() { struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 }; @@ -52,6 +62,13 @@ void ins_init() { alt_kalman_init(); #endif +#if USE_BAROMETER + ins_qfe = 0;; + ins_baro_initialised = FALSE; + ins_baro_alt = 0.; +#endif + ins_vf_realign = FALSE; + EstimatorSetAlt(0.); } @@ -69,7 +86,23 @@ void ins_propagate() { } void ins_update_baro() { +#if USE_BAROMETER // TODO update kalman filter with baro struct + if (baro.status == BS_RUNNING) { + if (!ins_baro_initialised) { + ins_qfe = baro.absolute; + ins_baro_initialised = TRUE; + } + if (ins_vf_realign) { + ins_vf_realign = FALSE; + ins_qfe = baro.absolute; + } + else { /* not realigning, so normal update with baro measurement */ + ins_baro_alt = (baro.absolute - ins_qfe) * INS_BARO_SENS; + EstimatorSetAlt(ins_baro_alt); + } + } +#endif } From a6499df753604435f0415fe9a4313d09a77c3e31 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 22 Aug 2012 18:14:17 +0200 Subject: [PATCH 48/62] [fix] dirty hack to compile telemetry --- sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h | 1 + 1 file changed, 1 insertion(+) diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h index 955853704c..0f7842ce9b 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h @@ -33,6 +33,7 @@ struct AhrsIntCmpl { struct Int64Quat high_rez_quat; struct Int64Rates high_rez_bias; struct Int32Quat ltp_to_imu_quat; + struct Int32Eulers ltp_to_imu_eulers; // FIXME to compile telemetry int32_t ltp_vel_norm; bool_t ltp_vel_norm_valid; bool_t correct_gravity; From 9cef69187b49f6e501e980d046a32121eb2197b6 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 23 Aug 2012 11:25:05 +0200 Subject: [PATCH 49/62] [fix] fix typo in ugly fix... --- sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h index 0f7842ce9b..c9d3581794 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h @@ -33,7 +33,7 @@ struct AhrsIntCmpl { struct Int64Quat high_rez_quat; struct Int64Rates high_rez_bias; struct Int32Quat ltp_to_imu_quat; - struct Int32Eulers ltp_to_imu_eulers; // FIXME to compile telemetry + struct Int32Eulers ltp_to_imu_euler; // FIXME to compile telemetry int32_t ltp_vel_norm; bool_t ltp_vel_norm_valid; bool_t correct_gravity; From 3e7906e5d996e43f2186fb09b16b77137e01c860 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 23 Aug 2012 11:29:25 +0200 Subject: [PATCH 50/62] [wind] use wind from state interface --- sw/airborne/subsystems/navigation/discsurvey.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/sw/airborne/subsystems/navigation/discsurvey.c b/sw/airborne/subsystems/navigation/discsurvey.c index 2feadf41f0..7bcdbd0d98 100644 --- a/sw/airborne/subsystems/navigation/discsurvey.c +++ b/sw/airborne/subsystems/navigation/discsurvey.c @@ -24,7 +24,8 @@ bool_t disc_survey_init( float grid ) { } bool_t disc_survey( uint8_t center, float radius) { - float wind_dir = atan2(wind_north, wind_east) + M_PI; + struct FloatVect2 wind = stateGetHorizontalSpeedDir_f(); + float wind_dir = atan2(wind->x, wind->y) + M_PI; /** Not null even if wind_east=wind_north=0 */ float upwind_x = cos(wind_dir); From 08cdc771ebcaa605130e783239e33d52765bf3dc Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 23 Aug 2012 11:29:50 +0200 Subject: [PATCH 51/62] [flight plan] update flight plans to use state interface --- conf/flight_plans/EMAV2008.xml | 10 +++--- conf/flight_plans/EMAV2009.xml | 2 +- conf/flight_plans/IS.xml | 14 ++++---- conf/flight_plans/basic.xml | 6 ++-- conf/flight_plans/ccc_gl.xml | 18 +++++------ conf/flight_plans/corsica.xml | 6 ++-- conf/flight_plans/creidlitz.xml | 6 ++-- conf/flight_plans/form_follow.xml | 4 +-- conf/flight_plans/form_leader.xml | 4 +-- conf/flight_plans/fp_tp_auto.xml | 14 ++++---- conf/flight_plans/generic.xml | 6 ++-- conf/flight_plans/grosslobke_demo.xml | 8 ++--- conf/flight_plans/grosslobke_kreise.xml | 2 +- conf/flight_plans/hsif.xml | 6 ++-- conf/flight_plans/huit.xml | 2 +- conf/flight_plans/ingolfsskali.xml | 8 ++--- conf/flight_plans/joystick.xml | 6 ++-- conf/flight_plans/kalscott.xml | 32 +++++++++---------- conf/flight_plans/kv_svalbard.xml | 6 ++-- conf/flight_plans/landing.xml | 4 +-- conf/flight_plans/mav06.xml | 8 ++--- conf/flight_plans/mav07.xml | 12 +++---- conf/flight_plans/mav08.xml | 6 ++-- conf/flight_plans/muret_for.xml | 2 +- conf/flight_plans/nordlys.xml | 10 +++--- conf/flight_plans/poles.xml | 6 ++-- conf/flight_plans/sinsat.xml | 2 +- conf/flight_plans/slayer_training.xml | 6 ++-- conf/flight_plans/snav.xml | 6 ++-- conf/flight_plans/tcas.xml | 6 ++-- conf/flight_plans/versatile.xml | 20 ++++++------ conf/flight_plans/versatile_airspeed.xml | 18 +++++------ .../versatile_carto_fixe_muret.xml | 6 ++-- conf/flight_plans/xsens_cachejunction.xml | 6 ++-- sw/airborne/subsystems/nav.h | 5 +++ 35 files changed, 144 insertions(+), 139 deletions(-) diff --git a/conf/flight_plans/EMAV2008.xml b/conf/flight_plans/EMAV2008.xml index 4e10939332..560eabb11d 100644 --- a/conf/flight_plans/EMAV2008.xml +++ b/conf/flight_plans/EMAV2008.xml @@ -48,7 +48,7 @@ - + @@ -84,14 +84,14 @@ - + - + - + @@ -114,7 +114,7 @@ - + diff --git a/conf/flight_plans/EMAV2009.xml b/conf/flight_plans/EMAV2009.xml index fbb603488b..00960a73aa 100644 --- a/conf/flight_plans/EMAV2009.xml +++ b/conf/flight_plans/EMAV2009.xml @@ -26,7 +26,7 @@ - + diff --git a/conf/flight_plans/IS.xml b/conf/flight_plans/IS.xml index dddd2b9cb5..c04ad1b09f 100644 --- a/conf/flight_plans/IS.xml +++ b/conf/flight_plans/IS.xml @@ -79,8 +79,8 @@ - - + + @@ -107,15 +107,15 @@ - + - + - + @@ -148,12 +148,12 @@ - + diff --git a/conf/flight_plans/basic.xml b/conf/flight_plans/basic.xml index f1c0fb690e..4385051247 100644 --- a/conf/flight_plans/basic.xml +++ b/conf/flight_plans/basic.xml @@ -36,7 +36,7 @@ - + @@ -78,10 +78,10 @@ - + - + diff --git a/conf/flight_plans/ccc_gl.xml b/conf/flight_plans/ccc_gl.xml index d913ac6c92..ea161cde0a 100644 --- a/conf/flight_plans/ccc_gl.xml +++ b/conf/flight_plans/ccc_gl.xml @@ -45,7 +45,7 @@ - + @@ -66,19 +66,19 @@ - + - + - + - + @@ -112,10 +112,10 @@ - + - + @@ -155,11 +155,11 @@ - + - + diff --git a/conf/flight_plans/corsica.xml b/conf/flight_plans/corsica.xml index b429892412..36064950a2 100644 --- a/conf/flight_plans/corsica.xml +++ b/conf/flight_plans/corsica.xml @@ -33,7 +33,7 @@ - + @@ -63,10 +63,10 @@ - + - + diff --git a/conf/flight_plans/creidlitz.xml b/conf/flight_plans/creidlitz.xml index c53a001f24..67f4f2487d 100644 --- a/conf/flight_plans/creidlitz.xml +++ b/conf/flight_plans/creidlitz.xml @@ -17,7 +17,7 @@ - + @@ -34,7 +34,7 @@ - + @@ -46,7 +46,7 @@ - + diff --git a/conf/flight_plans/form_follow.xml b/conf/flight_plans/form_follow.xml index 010cbc178d..41355289ce 100644 --- a/conf/flight_plans/form_follow.xml +++ b/conf/flight_plans/form_follow.xml @@ -35,8 +35,8 @@ - - + + diff --git a/conf/flight_plans/form_leader.xml b/conf/flight_plans/form_leader.xml index c22cbf6813..a98d81c56e 100644 --- a/conf/flight_plans/form_leader.xml +++ b/conf/flight_plans/form_leader.xml @@ -45,8 +45,8 @@ - - + + diff --git a/conf/flight_plans/fp_tp_auto.xml b/conf/flight_plans/fp_tp_auto.xml index e676f007c6..c72620f3d9 100644 --- a/conf/flight_plans/fp_tp_auto.xml +++ b/conf/flight_plans/fp_tp_auto.xml @@ -44,7 +44,7 @@ - + @@ -65,19 +65,19 @@ - + - + - + - + @@ -111,10 +111,10 @@ - + - + diff --git a/conf/flight_plans/generic.xml b/conf/flight_plans/generic.xml index dafbcfced3..b1fc8ab8ea 100644 --- a/conf/flight_plans/generic.xml +++ b/conf/flight_plans/generic.xml @@ -24,13 +24,13 @@ - + - + - + diff --git a/conf/flight_plans/grosslobke_demo.xml b/conf/flight_plans/grosslobke_demo.xml index 31be09e05c..6961d146fa 100644 --- a/conf/flight_plans/grosslobke_demo.xml +++ b/conf/flight_plans/grosslobke_demo.xml @@ -55,7 +55,7 @@ - + @@ -77,10 +77,10 @@ - + - + @@ -114,7 +114,7 @@ diff --git a/conf/flight_plans/grosslobke_kreise.xml b/conf/flight_plans/grosslobke_kreise.xml index 8a6d83dfb1..170f85b524 100644 --- a/conf/flight_plans/grosslobke_kreise.xml +++ b/conf/flight_plans/grosslobke_kreise.xml @@ -43,7 +43,7 @@ - + diff --git a/conf/flight_plans/hsif.xml b/conf/flight_plans/hsif.xml index 26825eb6e7..71ba46343a 100644 --- a/conf/flight_plans/hsif.xml +++ b/conf/flight_plans/hsif.xml @@ -19,15 +19,15 @@ - + - + - + diff --git a/conf/flight_plans/huit.xml b/conf/flight_plans/huit.xml index 520e714a92..623c0b0af9 100644 --- a/conf/flight_plans/huit.xml +++ b/conf/flight_plans/huit.xml @@ -14,7 +14,7 @@ - + diff --git a/conf/flight_plans/ingolfsskali.xml b/conf/flight_plans/ingolfsskali.xml index 2666e2b307..bbc10eb850 100644 --- a/conf/flight_plans/ingolfsskali.xml +++ b/conf/flight_plans/ingolfsskali.xml @@ -15,7 +15,7 @@ - + @@ -66,11 +66,11 @@ - + - + @@ -79,7 +79,7 @@ - + diff --git a/conf/flight_plans/joystick.xml b/conf/flight_plans/joystick.xml index c1efecaf3f..971f2dd544 100644 --- a/conf/flight_plans/joystick.xml +++ b/conf/flight_plans/joystick.xml @@ -34,7 +34,7 @@ - + @@ -80,10 +80,10 @@ - + - + diff --git a/conf/flight_plans/kalscott.xml b/conf/flight_plans/kalscott.xml index b7401c953a..4fce807cde 100644 --- a/conf/flight_plans/kalscott.xml +++ b/conf/flight_plans/kalscott.xml @@ -26,47 +26,47 @@ - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + diff --git a/conf/flight_plans/kv_svalbard.xml b/conf/flight_plans/kv_svalbard.xml index 153104cc99..e8b7ca8325 100644 --- a/conf/flight_plans/kv_svalbard.xml +++ b/conf/flight_plans/kv_svalbard.xml @@ -18,7 +18,7 @@ - + @@ -36,7 +36,7 @@ - + @@ -48,7 +48,7 @@ - + diff --git a/conf/flight_plans/landing.xml b/conf/flight_plans/landing.xml index 953b26464b..f1f2aed593 100644 --- a/conf/flight_plans/landing.xml +++ b/conf/flight_plans/landing.xml @@ -18,10 +18,10 @@ - + - + diff --git a/conf/flight_plans/mav06.xml b/conf/flight_plans/mav06.xml index 864c6ccd0e..464950ca4e 100644 --- a/conf/flight_plans/mav06.xml +++ b/conf/flight_plans/mav06.xml @@ -30,11 +30,11 @@ - + - + @@ -50,7 +50,7 @@ - + --> @@ -89,7 +89,7 @@ - + --> diff --git a/conf/flight_plans/mav07.xml b/conf/flight_plans/mav07.xml index d4e3429ae0..176f4e2bb9 100644 --- a/conf/flight_plans/mav07.xml +++ b/conf/flight_plans/mav07.xml @@ -57,7 +57,7 @@ - + @@ -73,7 +73,7 @@ - + @@ -130,10 +130,10 @@ - + - + @@ -144,11 +144,11 @@ - + - + diff --git a/conf/flight_plans/mav08.xml b/conf/flight_plans/mav08.xml index 084723612c..35b5c415e1 100644 --- a/conf/flight_plans/mav08.xml +++ b/conf/flight_plans/mav08.xml @@ -42,7 +42,7 @@ - + @@ -98,10 +98,10 @@ - + - + diff --git a/conf/flight_plans/muret_for.xml b/conf/flight_plans/muret_for.xml index 233cf0308a..af46942179 100644 --- a/conf/flight_plans/muret_for.xml +++ b/conf/flight_plans/muret_for.xml @@ -10,7 +10,7 @@ - + diff --git a/conf/flight_plans/nordlys.xml b/conf/flight_plans/nordlys.xml index ae8584808c..d82892b5f7 100644 --- a/conf/flight_plans/nordlys.xml +++ b/conf/flight_plans/nordlys.xml @@ -20,7 +20,7 @@ - + @@ -38,7 +38,7 @@ - + @@ -71,7 +71,7 @@ - + @@ -85,10 +85,10 @@ - + - + diff --git a/conf/flight_plans/poles.xml b/conf/flight_plans/poles.xml index 539367ea89..480a51b46e 100644 --- a/conf/flight_plans/poles.xml +++ b/conf/flight_plans/poles.xml @@ -32,7 +32,7 @@ - + @@ -70,10 +70,10 @@ - + - + diff --git a/conf/flight_plans/sinsat.xml b/conf/flight_plans/sinsat.xml index beb5132f5a..8186ca27e6 100644 --- a/conf/flight_plans/sinsat.xml +++ b/conf/flight_plans/sinsat.xml @@ -7,7 +7,7 @@ - + diff --git a/conf/flight_plans/slayer_training.xml b/conf/flight_plans/slayer_training.xml index 92d84fe58c..86a3f046e7 100644 --- a/conf/flight_plans/slayer_training.xml +++ b/conf/flight_plans/slayer_training.xml @@ -36,7 +36,7 @@ - + @@ -75,10 +75,10 @@ - + - + diff --git a/conf/flight_plans/snav.xml b/conf/flight_plans/snav.xml index 0dd2e92c73..71c3faa947 100644 --- a/conf/flight_plans/snav.xml +++ b/conf/flight_plans/snav.xml @@ -34,7 +34,7 @@ - + @@ -55,10 +55,10 @@ - + - + diff --git a/conf/flight_plans/tcas.xml b/conf/flight_plans/tcas.xml index e2b3045e20..c30f6295f7 100644 --- a/conf/flight_plans/tcas.xml +++ b/conf/flight_plans/tcas.xml @@ -34,7 +34,7 @@ - + @@ -72,10 +72,10 @@ - + - + diff --git a/conf/flight_plans/versatile.xml b/conf/flight_plans/versatile.xml index 571ab05e04..9767e9cfde 100644 --- a/conf/flight_plans/versatile.xml +++ b/conf/flight_plans/versatile.xml @@ -44,7 +44,7 @@ - + @@ -65,25 +65,25 @@ - + - + - + - + - + @@ -119,10 +119,10 @@ - + - + @@ -162,11 +162,11 @@ - + - + diff --git a/conf/flight_plans/versatile_airspeed.xml b/conf/flight_plans/versatile_airspeed.xml index 1a0aeafbc5..1427adf72c 100644 --- a/conf/flight_plans/versatile_airspeed.xml +++ b/conf/flight_plans/versatile_airspeed.xml @@ -45,7 +45,7 @@ - + @@ -69,25 +69,25 @@ - + - + - + - + - + @@ -134,7 +134,7 @@ - + @@ -177,11 +177,11 @@ - + - + diff --git a/conf/flight_plans/versatile_carto_fixe_muret.xml b/conf/flight_plans/versatile_carto_fixe_muret.xml index 0c52f2d526..d89466c368 100644 --- a/conf/flight_plans/versatile_carto_fixe_muret.xml +++ b/conf/flight_plans/versatile_carto_fixe_muret.xml @@ -61,7 +61,7 @@ float varsweepsize=110; - + @@ -112,7 +112,7 @@ float varsweepsize=110; - + @@ -140,7 +140,7 @@ float varsweepsize=110; - + diff --git a/conf/flight_plans/xsens_cachejunction.xml b/conf/flight_plans/xsens_cachejunction.xml index 2179cf4094..4a1859f9d1 100644 --- a/conf/flight_plans/xsens_cachejunction.xml +++ b/conf/flight_plans/xsens_cachejunction.xml @@ -24,7 +24,7 @@ - + @@ -89,10 +89,10 @@ - + - + diff --git a/sw/airborne/subsystems/nav.h b/sw/airborne/subsystems/nav.h index 31ea51d063..e719594fa7 100644 --- a/sw/airborne/subsystems/nav.h +++ b/sw/airborne/subsystems/nav.h @@ -194,4 +194,9 @@ bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float ap #define nav_SetNavRadius(x) { if (x==1) nav_radius = DEFAULT_CIRCLE_RADIUS; else if (x==-1) nav_radius = -DEFAULT_CIRCLE_RADIUS; else nav_radius = x; } #define NavKillThrottle() { kill_throttle = 1; } + +#define GetPosX() (stateGetPositionUtm_f()->north) +#define GetPosY() (stateGetPositionUtm_f()->east) +#define GetPosAlt() (stateGetPositionUtm_f()->alt) + #endif /* NAV_H */ From 8571102444d5f91fabc60719ce4d5ab45d885a56 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 23 Aug 2012 13:28:47 +0200 Subject: [PATCH 52/62] [wind] wind handled by state interface everywhere --- sw/airborne/estimator.c | 2 -- sw/airborne/estimator.h | 1 - sw/airborne/firmwares/fixedwing/datalink.c | 8 +++++--- sw/airborne/modules/enose/anemotaxis.c | 9 ++++++--- sw/airborne/subsystems/ahrs/ahrs_infrared.c | 5 ++--- sw/airborne/subsystems/nav.c | 3 ++- sw/airborne/subsystems/navigation/bomb.c | 11 ++++++----- sw/airborne/subsystems/navigation/gls.c | 3 ++- sw/airborne/subsystems/navigation/snav.c | 2 +- 9 files changed, 24 insertions(+), 20 deletions(-) diff --git a/sw/airborne/estimator.c b/sw/airborne/estimator.c index 3426a5ef53..63056d6e1d 100644 --- a/sw/airborne/estimator.c +++ b/sw/airborne/estimator.c @@ -39,8 +39,6 @@ /* flight time in seconds */ uint16_t estimator_flight_time; -/* wind */ -float wind_east, wind_north; float estimator_airspeed; float estimator_AOA; diff --git a/sw/airborne/estimator.h b/sw/airborne/estimator.h index 13dd0bed62..ffc5dba655 100644 --- a/sw/airborne/estimator.h +++ b/sw/airborne/estimator.h @@ -40,7 +40,6 @@ extern uint16_t estimator_flight_time; /* Wind and airspeed estimation sent by the GCS */ -extern float wind_east, wind_north; /* m/s */ extern float estimator_airspeed; ///< m/s extern float estimator_AOA; ///< angle of attack in rad diff --git a/sw/airborne/firmwares/fixedwing/datalink.c b/sw/airborne/firmwares/fixedwing/datalink.c index 2e2f305c98..ae0195f0cd 100644 --- a/sw/airborne/firmwares/fixedwing/datalink.c +++ b/sw/airborne/firmwares/fixedwing/datalink.c @@ -134,14 +134,16 @@ void dl_parse_msg(void) { #endif /** NAV */ #ifdef WIND_INFO if (msg_id == DL_WIND_INFO && DL_WIND_INFO_ac_id(dl_buffer) == AC_ID) { - wind_east = DL_WIND_INFO_east(dl_buffer); - wind_north = DL_WIND_INFO_north(dl_buffer); + struct FloatVect2 wind; + wind.x = DL_WIND_INFO_north(dl_buffer); + wind.y = DL_WIND_INFO_east(dl_buffer); + stateSetHorizontalWindspeed_f(&wind); #if !USE_AIRSPEED float airspeed = DL_WIND_INFO_airspeed(dl_buffer); stateSetAirspeed_f(&airspeed); #endif #ifdef WIND_INFO_RET - DOWNLINK_SEND_WIND_INFO_RET(DefaultChannel, DefaultDevice, &wind_east, &wind_north, stateGetAirspeed_f()); + DOWNLINK_SEND_WIND_INFO_RET(DefaultChannel, DefaultDevice, &wind.y, &wind.x, stateGetAirspeed_f()); #endif } else #endif /** WIND_INFO */ diff --git a/sw/airborne/modules/enose/anemotaxis.c b/sw/airborne/modules/enose/anemotaxis.c index 2a25214ec5..d9ec30630c 100644 --- a/sw/airborne/modules/enose/anemotaxis.c +++ b/sw/airborne/modules/enose/anemotaxis.c @@ -18,7 +18,8 @@ static void last_plume_was_here( void ) { } bool_t nav_anemotaxis_downwind( uint8_t c, float radius ) { - float wind_dir = atan2(wind_north, wind_east); + struct FloatVect2* wind = stateGetHorizontalWindspeed_f(); + float wind_dir = atan2(wind->x, wind->y); waypoints[c].x = waypoints[WP_HOME].x + radius*cos(wind_dir); waypoints[c].y = waypoints[WP_HOME].y + radius*sin(wind_dir); return FALSE; @@ -27,7 +28,8 @@ bool_t nav_anemotaxis_downwind( uint8_t c, float radius ) { bool_t nav_anemotaxis_init( uint8_t c ) { status = UTURN; sign = 1; - float wind_dir = atan2(wind_north, wind_east); + struct FloatVect2* wind = stateGetHorizontalWindspeed_f(); + float wind_dir = atan2(wind->x, wind->y); waypoints[c].x = stateGetPositionEnu_f()->x + DEFAULT_CIRCLE_RADIUS*cos(wind_dir+M_PI); waypoints[c].y = stateGetPositionEnu_f()->y + DEFAULT_CIRCLE_RADIUS*sin(wind_dir+M_PI); last_plume_was_here(); @@ -42,7 +44,8 @@ bool_t nav_anemotaxis( uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume ) { // DownlinkSendWp(plume); } - float wind_dir = atan2(wind_north, wind_east) + M_PI; + struct FloatVect2* wind = stateGetHorizontalWindspeed_f(); + float wind_dir = atan2(wind->x, wind->y) + M_PI; /** Not null even if wind_east=wind_north=0 */ float upwind_x = cos(wind_dir); diff --git a/sw/airborne/subsystems/ahrs/ahrs_infrared.c b/sw/airborne/subsystems/ahrs/ahrs_infrared.c index f5270a4390..e3c2a5f0b3 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_infrared.c +++ b/sw/airborne/subsystems/ahrs/ahrs_infrared.c @@ -26,7 +26,6 @@ #include "subsystems/gps.h" #include "state.h" -#include "estimator.h" // for wind FIXME use state interface float heading; @@ -70,8 +69,8 @@ void ahrs_update_gps(void) { // Heading estimator from wind-information, usually computed with -DWIND_INFO // wind_north and wind_east initialized to 0, so still correct if not updated - float w_vn = cosf(course_f) * hspeed_mod_f - wind_north; - float w_ve = sinf(course_f) * hspeed_mod_f - wind_east; + float w_vn = cosf(course_f) * hspeed_mod_f - stateGetHorizontalWindspeed_f()->x; + float w_ve = sinf(course_f) * hspeed_mod_f - stateGetHorizontalWindspeed_f()->y; heading = atan2f(w_ve, w_vn); if (heading < 0.) heading += 2 * M_PI; diff --git a/sw/airborne/subsystems/nav.c b/sw/airborne/subsystems/nav.c index a00afe617a..83115f5b87 100644 --- a/sw/airborne/subsystems/nav.c +++ b/sw/airborne/subsystems/nav.c @@ -258,10 +258,11 @@ static inline bool_t nav_compute_final_from_glide(uint8_t wp_af, uint8_t wp_td, Computes Top Of Descent waypoint from Touch Down and Approach Fix waypoints, using glide airspeed, glide vertical speed and wind */ static inline bool_t compute_TOD(uint8_t _af, uint8_t _td, uint8_t _tod, float glide_airspeed, float glide_vspeed) { + struct FloatVect2* wind = stateGetHorizontalWindspeed_f(); float td_af_x = WaypointX(_af) - WaypointX(_td); float td_af_y = WaypointY(_af) - WaypointY(_td); float td_af = sqrt( td_af_x*td_af_x + td_af_y*td_af_y); - float td_tod = (WaypointAlt(_af) - WaypointAlt(_td)) / glide_vspeed * (glide_airspeed - sqrt(wind_east*wind_east + wind_north*wind_north)); + float td_tod = (WaypointAlt(_af) - WaypointAlt(_td)) / glide_vspeed * (glide_airspeed - sqrt(wind->x*wind->x + wind->y*wind->y)); WaypointX(_tod) = WaypointX(_td) + td_af_x / td_af * td_tod; WaypointY(_tod) = WaypointY(_td) + td_af_y / td_af * td_tod; WaypointAlt(_tod) = WaypointAlt(_af); diff --git a/sw/airborne/subsystems/navigation/bomb.c b/sw/airborne/subsystems/navigation/bomb.c index a3c8d5ead0..4622f3767e 100644 --- a/sw/airborne/subsystems/navigation/bomb.c +++ b/sw/airborne/subsystems/navigation/bomb.c @@ -57,9 +57,9 @@ static void integrate( uint8_t wp_target ) { /* Inspired from Arnold Schroeter's code */ int i = 0; while (bomb_z > 0. && i < MAX_STEPS) { - /* relative wind experienced by the ball */ - float airx = -bomb_vx + wind_east; - float airy = -bomb_vy + wind_north; + /* relative wind experienced by the ball (wind in NED frame) */ + float airx = -bomb_vx + stateGetHorizontalWindspeed_f()->y; + float airy = -bomb_vy + stateGetHorizontalWindspeed_f()->x; float airz = -bomb_vz; /* alpha / m * air */ @@ -132,8 +132,9 @@ unit_t bomb_compute_approach( uint8_t wp_target, uint8_t wp_start, float bomb_ra if (bomb_radius < 0) bomb_start_qdr += M_PI; - bomb_vx = x1 * airspeed + wind_east; - bomb_vy = y_1 * airspeed + wind_north; + // wind in NED frame + bomb_vx = x1 * airspeed + stateGetHorizontalWindspeed_f()->y; + bomb_vy = y_1 * airspeed + stateGetHorizontalWindspeed_f()->x; bomb_vz = 0.; float vx0 = bomb_vx; diff --git a/sw/airborne/subsystems/navigation/gls.c b/sw/airborne/subsystems/navigation/gls.c index dc8d271496..b7e7806d58 100644 --- a/sw/airborne/subsystems/navigation/gls.c +++ b/sw/airborne/subsystems/navigation/gls.c @@ -101,7 +101,8 @@ bool_t gls_init(uint8_t _af, uint8_t _tod, uint8_t _td) { init = TRUE; #if USE_AIRSPEED - //float wind_additional = sqrt(wind_east*wind_east + wind_north*wind_north); // should be gusts only! + //struct FloatVect2* wind = stateGetHorizontalWindspeed_f(); + //float wind_additional = sqrt(wind->x*wind->x + wind->y*wind->y); // should be gusts only! //Bound(wind_additional, 0, 0.5); //target_speed = FL_ENVE_V_S * 1.3 + wind_additional; FIXME target_speed = APP_TARGET_SPEED; // ok for now! diff --git a/sw/airborne/subsystems/navigation/snav.c b/sw/airborne/subsystems/navigation/snav.c index ffa9706238..10e4903188 100644 --- a/sw/airborne/subsystems/navigation/snav.c +++ b/sw/airborne/subsystems/navigation/snav.c @@ -161,7 +161,7 @@ bool_t snav_on_time(float nominal_radius) { /* Recompute ground speeds every 10 s */ if (ground_speed_timer == 0) { ground_speed_timer = 40; /* every 10s, called at 40Hz */ - compute_ground_speed(airspeed, wind_east, wind_north); + compute_ground_speed(airspeed, stateGetHorizontalWindspeed_f()->y, stateGetHorizontalWindspeed_f()->x); // Wind in NED frame } ground_speed_timer--; From 7d2f0fe7e498c75416c864e9d907aa753ea3e183 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 23 Aug 2012 13:35:37 +0200 Subject: [PATCH 53/62] [airspeed] remove airspeed from old estimator --- sw/airborne/estimator.c | 1 - sw/airborne/estimator.h | 3 --- sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c | 2 +- sw/airborne/modules/com/generic_com.c | 3 ++- 4 files changed, 3 insertions(+), 6 deletions(-) diff --git a/sw/airborne/estimator.c b/sw/airborne/estimator.c index 63056d6e1d..3e72d7fbb4 100644 --- a/sw/airborne/estimator.c +++ b/sw/airborne/estimator.c @@ -39,7 +39,6 @@ /* flight time in seconds */ uint16_t estimator_flight_time; -float estimator_airspeed; float estimator_AOA; void estimator_init( void ) { diff --git a/sw/airborne/estimator.h b/sw/airborne/estimator.h index ffc5dba655..7a0a2be2c3 100644 --- a/sw/airborne/estimator.h +++ b/sw/airborne/estimator.h @@ -39,9 +39,6 @@ /** flight time in seconds. */ extern uint16_t estimator_flight_time; -/* Wind and airspeed estimation sent by the GCS */ -extern float estimator_airspeed; ///< m/s - extern float estimator_AOA; ///< angle of attack in rad void estimator_init( void ); diff --git a/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c index f721a9ef1f..fe930e0d2e 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c +++ b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c @@ -228,7 +228,7 @@ static float low_pass_vdot(float v) void v_ctl_climb_loop( void ) { // Airspeed outerloop: positive means we need to accelerate - float speed_error = v_ctl_auto_airspeed_setpoint - estimator_airspeed; + float speed_error = v_ctl_auto_airspeed_setpoint - (*stateGetAirspeed_f()); // Speed Controller to PseudoControl: gain 1 -> 5m/s error = 0.5g acceleration v_ctl_desired_acceleration = speed_error * v_ctl_airspeed_pgain / 9.81f; diff --git a/sw/airborne/modules/com/generic_com.c b/sw/airborne/modules/com/generic_com.c index 255b082940..9f9dcc33f9 100644 --- a/sw/airborne/modules/com/generic_com.c +++ b/sw/airborne/modules/com/generic_com.c @@ -27,6 +27,7 @@ #include "modules/com/generic_com.h" #include "mcu_periph/i2c.h" +#include "state.h" #include "estimator.h" #include "subsystems/gps.h" #include "subsystems/electrical.h" @@ -76,7 +77,7 @@ void generic_com_periodic( void ) { FillBufWith16bit(com_trans.buf, 9, (int16_t)(gps.lla_pos.alt/1000)); // altitude (meters) FillBufWith16bit(com_trans.buf, 11, gps.gspeed); // ground speed (cm/s) FillBufWith16bit(com_trans.buf, 13, (int16_t)(gps.course/1e4)); // course (1e3rad) - FillBufWith16bit(com_trans.buf, 15, (uint16_t)(estimator_airspeed*100)); // TAS (cm/s) + FillBufWith16bit(com_trans.buf, 15, (uint16_t)((*stateGetAirspeed_f())*100)); // TAS (cm/s) com_trans.buf[17] = electrical.vsupply; // decivolts com_trans.buf[18] = (uint8_t)(energy/100); // deciAh com_trans.buf[19] = (uint8_t)(ap_state->commands[COMMAND_THROTTLE]*100/MAX_PPRZ); From 21ebb391481431b0cb12838a2cc0e1dfe21d4e7e Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 23 Aug 2012 13:47:05 +0200 Subject: [PATCH 54/62] [fix] was a pointer here --- sw/airborne/subsystems/navigation/discsurvey.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/airborne/subsystems/navigation/discsurvey.c b/sw/airborne/subsystems/navigation/discsurvey.c index 7bcdbd0d98..52e6706ee6 100644 --- a/sw/airborne/subsystems/navigation/discsurvey.c +++ b/sw/airborne/subsystems/navigation/discsurvey.c @@ -24,7 +24,7 @@ bool_t disc_survey_init( float grid ) { } bool_t disc_survey( uint8_t center, float radius) { - struct FloatVect2 wind = stateGetHorizontalSpeedDir_f(); + struct FloatVect2* wind = stateGetHorizontalSpeedDir_f(); float wind_dir = atan2(wind->x, wind->y) + M_PI; /** Not null even if wind_east=wind_north=0 */ From 2048817289eadcc1e5975ab91aa9017bb1d29312 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 23 Aug 2012 15:27:06 +0200 Subject: [PATCH 55/62] [baro] fix some direct calling to altitude update --- sw/airborne/modules/sensors/baro_MS5534A.c | 9 ++++----- sw/airborne/modules/sensors/baro_amsys.c | 8 +++----- sw/airborne/modules/sensors/baro_bmp.c | 8 ++------ sw/airborne/modules/sensors/baro_ets.c | 5 +---- 4 files changed, 10 insertions(+), 20 deletions(-) diff --git a/sw/airborne/modules/sensors/baro_MS5534A.c b/sw/airborne/modules/sensors/baro_MS5534A.c index eaa752b7d2..d9e2a24cbb 100644 --- a/sw/airborne/modules/sensors/baro_MS5534A.c +++ b/sw/airborne/modules/sensors/baro_MS5534A.c @@ -35,7 +35,6 @@ #include "ap_downlink.h" #endif #include "subsystems/nav.h" -#include "estimator.h" #include "state.h" bool_t baro_MS5534A_do_reset; @@ -259,11 +258,11 @@ void baro_MS5534A_event( void ) { spi_message_received = FALSE; baro_MS5534A_event_task(); if (baro_MS5534A_available) { - baro_MS5534A_available = FALSE; + // baro_MS5534A_available = FALSE; // Checked by INS baro_MS5534A_z = ground_alt +((float)baro_MS5534A_ground_pressure - baro_MS5534A_pressure)*0.084; - if (alt_baro_enabled) { - EstimatorSetAlt(baro_MS5534A_z); - } + // if (alt_baro_enabled) { + // EstimatorSetAlt(baro_MS5534A_z); // Updated by INS + // } } } } diff --git a/sw/airborne/modules/sensors/baro_amsys.c b/sw/airborne/modules/sensors/baro_amsys.c index d69f663219..23f07b9ced 100644 --- a/sw/airborne/modules/sensors/baro_amsys.c +++ b/sw/airborne/modules/sensors/baro_amsys.c @@ -25,7 +25,6 @@ #include "sensors/baro_amsys.h" #include "mcu_periph/i2c.h" -#include "estimator.h" #include "state.h" #include #include "generated/flight_plan.h" // for ground alt @@ -73,6 +72,7 @@ // Global variables uint16_t pBaroRaw; uint16_t tBaroRaw; +uint16_t baro_amsys_adc; float baro_amsys_offset; bool_t baro_amsys_valid; float baro_amsys_altitude; @@ -127,8 +127,8 @@ void baro_amsys_read_periodic( void ) { #else // SITL pBaroRaw = 0; baro_amsys_altitude = gps.hmsl / 1000.0; + baro_amsys_adc = baro_amsys_altitude / BARO_AMSYS_SCALE; baro_amsys_valid = TRUE; - EstimatorSetAlt(baro_amsys_altitude); #endif } @@ -146,6 +146,7 @@ void baro_amsys_read_event( void ) { else baro_amsys_valid = TRUE; + baro_amsys_adc = pBaroRaw; // Continue only if a new altimeter value was received //if (baro_amsys_valid && GpsFixValid()) { @@ -181,10 +182,7 @@ void baro_amsys_read_event( void ) { // Lowpassfiltering and convert pressure to altitude baro_amsys_altitude = baro_filter * baro_old + (1 - baro_filter) * (baro_amsys_offset-baro_amsys_p)/(1.2041*9.81); baro_old = baro_amsys_altitude; - - //New value available - //EstimatorSetAlt(baro_amsys_abs_altitude); } baro_amsys_abs_altitude=baro_amsys_altitude+ref_alt_init; } /*else { diff --git a/sw/airborne/modules/sensors/baro_bmp.c b/sw/airborne/modules/sensors/baro_bmp.c index b5ba065da1..8935bc23bd 100644 --- a/sw/airborne/modules/sensors/baro_bmp.c +++ b/sw/airborne/modules/sensors/baro_bmp.c @@ -37,7 +37,6 @@ #include "mcu_periph/uart.h" #include "messages.h" #include "subsystems/datalink/downlink.h" -#include "estimator.h" #include "state.h" #include "subsystems/nav.h" @@ -127,7 +126,8 @@ void baro_bmp_periodic( void ) { } #else // SITL baro_bmp_altitude = gps.hmsl / 1000.0; - EstimatorSetAlt(baro_bmp_altitude); + baro_bmp_pressure = baro_bmp_altitude; //FIXME do a proper scaling here + baro_bmp_valid = TRUE; #endif } @@ -237,10 +237,6 @@ void baro_bmp_event( void ) { baro_bmp_altitude = ground_alt + baro_bmp_temp; // New value available baro_bmp_valid = TRUE; -#if USE_BARO_BMP -#pragma message "USING BARO BMP" - EstimatorSetAlt(baro_bmp_altitude); -#endif #ifdef SENSOR_SYNC_SEND DOWNLINK_SEND_BMP_STATUS(DefaultChannel, DefaultDevice, &bmp_up, &bmp_ut, &bmp_p, &bmp_t); diff --git a/sw/airborne/modules/sensors/baro_ets.c b/sw/airborne/modules/sensors/baro_ets.c index 4380d76756..f1bf697227 100644 --- a/sw/airborne/modules/sensors/baro_ets.c +++ b/sw/airborne/modules/sensors/baro_ets.c @@ -37,7 +37,6 @@ #include "sensors/baro_ets.h" #include "mcu_periph/i2c.h" -#include "estimator.h" #include "state.h" #include @@ -98,10 +97,9 @@ void baro_ets_read_periodic( void ) { if (baro_ets_i2c_trans.status == I2CTransDone) I2CReceive(BARO_ETS_I2C_DEV, baro_ets_i2c_trans, BARO_ETS_ADDR, 2); #else // SITL - baro_ets_adc = 0; baro_ets_altitude = gps.hmsl / 1000.0; + baro_ets_adc = baro_ets_altitude / BARO_ETS_SCALE; baro_ets_valid = TRUE; - EstimatorSetAlt(baro_ets_altitude); #endif } @@ -150,7 +148,6 @@ void baro_ets_read_event( void ) { if (baro_ets_offset_init) { baro_ets_altitude = ground_alt + BARO_ETS_SCALE * (float)(baro_ets_offset-baro_ets_adc); // New value available - EstimatorSetAlt(baro_ets_altitude); #ifdef BARO_ETS_TELEMETRY DOWNLINK_SEND_BARO_ETS(DefaultChannel, DefaultDevice, &baro_ets_adc, &baro_ets_offset, &baro_ets_altitude); #endif From 50a74d912546a062fa17b3ea5deb18a28ef24b9e Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 23 Aug 2012 15:30:44 +0200 Subject: [PATCH 56/62] [fix] call the correction function... --- sw/airborne/subsystems/navigation/discsurvey.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/airborne/subsystems/navigation/discsurvey.c b/sw/airborne/subsystems/navigation/discsurvey.c index 52e6706ee6..309451fa5c 100644 --- a/sw/airborne/subsystems/navigation/discsurvey.c +++ b/sw/airborne/subsystems/navigation/discsurvey.c @@ -24,7 +24,7 @@ bool_t disc_survey_init( float grid ) { } bool_t disc_survey( uint8_t center, float radius) { - struct FloatVect2* wind = stateGetHorizontalSpeedDir_f(); + struct FloatVect2* wind = stateGetHorizontalWindspeed_f(); float wind_dir = atan2(wind->x, wind->y) + M_PI; /** Not null even if wind_east=wind_north=0 */ From 1ba1d242953c0fbb4f72172df3050548f972ef22 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 23 Aug 2012 15:40:58 +0200 Subject: [PATCH 57/62] [nps] ahrs bypass feeds state interface --- sw/simulator/nps/nps_autopilot_rotorcraft.c | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/sw/simulator/nps/nps_autopilot_rotorcraft.c b/sw/simulator/nps/nps_autopilot_rotorcraft.c index 3796fddaf7..9c17dcfbe7 100644 --- a/sw/simulator/nps/nps_autopilot_rotorcraft.c +++ b/sw/simulator/nps/nps_autopilot_rotorcraft.c @@ -30,6 +30,7 @@ #include "baro_board.h" #include "subsystems/electrical.h" #include "mcu_periph/sys_time.h" +#include "state.h" #include "actuators/supervision.h" @@ -109,12 +110,12 @@ void nps_autopilot_run_step(double time __attribute__ ((unused))) { #include "math/pprz_algebra.h" void sim_overwrite_ahrs(void) { - EULERS_BFP_OF_REAL(ahrs.ltp_to_body_euler, fdm.ltp_to_body_eulers); + struct Int32Quat quat; + QUAT_BFP_OF_REAL(quat, fdm.ltp_to_body_quat); + stateSetNedToBodyQuat_f(&quat); - QUAT_BFP_OF_REAL(ahrs.ltp_to_body_quat, fdm.ltp_to_body_quat); - - RATES_BFP_OF_REAL(ahrs.body_rate, fdm.body_ecef_rotvel); - - INT32_RMAT_OF_QUAT(ahrs.ltp_to_body_rmat, ahrs.ltp_to_body_quat); + struct Int32Rates rates; + RATES_BFP_OF_REAL(rates, fdm.body_ecef_rotvel); + stateSetBodyRates_f(&rates); } From 04750bd9340f99f53b005e22932f9e8710e64c5a Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 23 Aug 2012 16:30:29 +0200 Subject: [PATCH 58/62] [fix] update integer not float... --- sw/simulator/nps/nps_autopilot_rotorcraft.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sw/simulator/nps/nps_autopilot_rotorcraft.c b/sw/simulator/nps/nps_autopilot_rotorcraft.c index 9c17dcfbe7..08d5d19dd2 100644 --- a/sw/simulator/nps/nps_autopilot_rotorcraft.c +++ b/sw/simulator/nps/nps_autopilot_rotorcraft.c @@ -112,10 +112,10 @@ void sim_overwrite_ahrs(void) { struct Int32Quat quat; QUAT_BFP_OF_REAL(quat, fdm.ltp_to_body_quat); - stateSetNedToBodyQuat_f(&quat); + stateSetNedToBodyQuat_i(&quat); struct Int32Rates rates; RATES_BFP_OF_REAL(rates, fdm.body_ecef_rotvel); - stateSetBodyRates_f(&rates); + stateSetBodyRates_i(&rates); } From bbdaa965d53d28582b776fc47f1b6f532f69cbf2 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 23 Aug 2012 17:09:59 +0200 Subject: [PATCH 59/62] [flight_time] estimator_flight_time is now autopilot_flight_time --- conf/flight_plans/EMAV2009.xml | 2 +- conf/flight_plans/basic.xml | 2 +- conf/flight_plans/corsica.xml | 2 +- conf/flight_plans/creidlitz.xml | 2 +- conf/flight_plans/grosslobke_demo.xml | 2 +- conf/flight_plans/grosslobke_kreise.xml | 2 +- conf/flight_plans/hsif.xml | 2 +- conf/flight_plans/huit.xml | 2 +- conf/flight_plans/ingolfsskali.xml | 2 +- conf/flight_plans/joystick.xml | 2 +- conf/flight_plans/kalscott.xml | 2 +- conf/flight_plans/kv_svalbard.xml | 2 +- conf/flight_plans/mav08.xml | 2 +- conf/flight_plans/muret_for.xml | 2 +- conf/flight_plans/nordlys.xml | 2 +- conf/flight_plans/poles.xml | 2 +- conf/flight_plans/sinsat.xml | 2 +- conf/flight_plans/snav.xml | 2 +- conf/flight_plans/tcas.xml | 2 +- conf/flight_plans/xsens_cachejunction.xml | 2 +- conf/settings/fixedwing_basic.xml | 2 +- sw/airborne/estimator.c | 10 ---------- sw/airborne/estimator.h | 3 --- sw/airborne/firmwares/fixedwing/ap_downlink.h | 2 +- sw/airborne/firmwares/fixedwing/autopilot.h | 5 ++++- sw/airborne/firmwares/fixedwing/main_ap.c | 15 +++++++++------ sw/airborne/modules/com/generic_com.c | 2 +- sw/airborne/modules/gsm/gsm.c | 6 +++--- sw/ground_segment/cockpit/live.ml | 2 +- .../tmtc/GSM/SMS_Ground_UDtest_final.c | 6 +++--- sw/in_progress/satcom/tcp2ivy.c | 12 ++++++------ sw/in_progress/satcom/tcp2ivy_generic.c | 12 ++++++------ sw/lib/ocaml/expr_syntax.ml | 2 +- 33 files changed, 56 insertions(+), 63 deletions(-) diff --git a/conf/flight_plans/EMAV2009.xml b/conf/flight_plans/EMAV2009.xml index 00960a73aa..5cbeaab4c3 100644 --- a/conf/flight_plans/EMAV2009.xml +++ b/conf/flight_plans/EMAV2009.xml @@ -28,7 +28,7 @@ - + diff --git a/conf/flight_plans/basic.xml b/conf/flight_plans/basic.xml index 4385051247..382e64ab26 100644 --- a/conf/flight_plans/basic.xml +++ b/conf/flight_plans/basic.xml @@ -38,7 +38,7 @@ - + diff --git a/conf/flight_plans/corsica.xml b/conf/flight_plans/corsica.xml index 36064950a2..02db9b1e70 100644 --- a/conf/flight_plans/corsica.xml +++ b/conf/flight_plans/corsica.xml @@ -35,7 +35,7 @@ - + diff --git a/conf/flight_plans/creidlitz.xml b/conf/flight_plans/creidlitz.xml index 67f4f2487d..8b79b6ca84 100644 --- a/conf/flight_plans/creidlitz.xml +++ b/conf/flight_plans/creidlitz.xml @@ -36,7 +36,7 @@ - + diff --git a/conf/flight_plans/grosslobke_demo.xml b/conf/flight_plans/grosslobke_demo.xml index 6961d146fa..7741ca2013 100644 --- a/conf/flight_plans/grosslobke_demo.xml +++ b/conf/flight_plans/grosslobke_demo.xml @@ -24,7 +24,7 @@ - + diff --git a/conf/flight_plans/grosslobke_kreise.xml b/conf/flight_plans/grosslobke_kreise.xml index 170f85b524..a6732defbf 100644 --- a/conf/flight_plans/grosslobke_kreise.xml +++ b/conf/flight_plans/grosslobke_kreise.xml @@ -12,7 +12,7 @@ - + diff --git a/conf/flight_plans/hsif.xml b/conf/flight_plans/hsif.xml index 71ba46343a..8cff597509 100644 --- a/conf/flight_plans/hsif.xml +++ b/conf/flight_plans/hsif.xml @@ -21,7 +21,7 @@ - + diff --git a/conf/flight_plans/huit.xml b/conf/flight_plans/huit.xml index 623c0b0af9..0819bb0fe8 100644 --- a/conf/flight_plans/huit.xml +++ b/conf/flight_plans/huit.xml @@ -13,7 +13,7 @@ - + diff --git a/conf/flight_plans/ingolfsskali.xml b/conf/flight_plans/ingolfsskali.xml index bbc10eb850..5b811161c1 100644 --- a/conf/flight_plans/ingolfsskali.xml +++ b/conf/flight_plans/ingolfsskali.xml @@ -23,7 +23,7 @@ - + diff --git a/conf/flight_plans/joystick.xml b/conf/flight_plans/joystick.xml index 971f2dd544..b39d6694d5 100644 --- a/conf/flight_plans/joystick.xml +++ b/conf/flight_plans/joystick.xml @@ -36,7 +36,7 @@ - + diff --git a/conf/flight_plans/kalscott.xml b/conf/flight_plans/kalscott.xml index 4fce807cde..49d59220b2 100644 --- a/conf/flight_plans/kalscott.xml +++ b/conf/flight_plans/kalscott.xml @@ -28,7 +28,7 @@ - + diff --git a/conf/flight_plans/kv_svalbard.xml b/conf/flight_plans/kv_svalbard.xml index e8b7ca8325..499614ccdf 100644 --- a/conf/flight_plans/kv_svalbard.xml +++ b/conf/flight_plans/kv_svalbard.xml @@ -38,7 +38,7 @@ - + diff --git a/conf/flight_plans/mav08.xml b/conf/flight_plans/mav08.xml index 35b5c415e1..3424afc1a9 100644 --- a/conf/flight_plans/mav08.xml +++ b/conf/flight_plans/mav08.xml @@ -44,7 +44,7 @@ - + diff --git a/conf/flight_plans/muret_for.xml b/conf/flight_plans/muret_for.xml index af46942179..961a2816c4 100644 --- a/conf/flight_plans/muret_for.xml +++ b/conf/flight_plans/muret_for.xml @@ -9,7 +9,7 @@ - + diff --git a/conf/flight_plans/nordlys.xml b/conf/flight_plans/nordlys.xml index d82892b5f7..7d06ace28b 100644 --- a/conf/flight_plans/nordlys.xml +++ b/conf/flight_plans/nordlys.xml @@ -26,7 +26,7 @@ - + diff --git a/conf/flight_plans/poles.xml b/conf/flight_plans/poles.xml index 480a51b46e..0d0e539340 100644 --- a/conf/flight_plans/poles.xml +++ b/conf/flight_plans/poles.xml @@ -34,7 +34,7 @@ - + diff --git a/conf/flight_plans/sinsat.xml b/conf/flight_plans/sinsat.xml index 8186ca27e6..306e5d616a 100644 --- a/conf/flight_plans/sinsat.xml +++ b/conf/flight_plans/sinsat.xml @@ -6,7 +6,7 @@ - + diff --git a/conf/flight_plans/snav.xml b/conf/flight_plans/snav.xml index 71c3faa947..7dc6d806a0 100644 --- a/conf/flight_plans/snav.xml +++ b/conf/flight_plans/snav.xml @@ -36,7 +36,7 @@ - + diff --git a/conf/flight_plans/tcas.xml b/conf/flight_plans/tcas.xml index c30f6295f7..ab79ecda5d 100644 --- a/conf/flight_plans/tcas.xml +++ b/conf/flight_plans/tcas.xml @@ -36,7 +36,7 @@ - + diff --git a/conf/flight_plans/xsens_cachejunction.xml b/conf/flight_plans/xsens_cachejunction.xml index 4a1859f9d1..0214c4f467 100644 --- a/conf/flight_plans/xsens_cachejunction.xml +++ b/conf/flight_plans/xsens_cachejunction.xml @@ -26,7 +26,7 @@ - + diff --git a/conf/settings/fixedwing_basic.xml b/conf/settings/fixedwing_basic.xml index abdfe89a29..84eb2165f5 100644 --- a/conf/settings/fixedwing_basic.xml +++ b/conf/settings/fixedwing_basic.xml @@ -8,7 +8,7 @@ - + diff --git a/sw/airborne/estimator.c b/sw/airborne/estimator.c index 3e72d7fbb4..d0d96df807 100644 --- a/sw/airborne/estimator.c +++ b/sw/airborne/estimator.c @@ -30,20 +30,10 @@ #include #include "estimator.h" -#include "state.h" -#include "mcu_periph/uart.h" -#include "ap_downlink.h" -#include "subsystems/gps.h" -#include "subsystems/nav.h" - -/* flight time in seconds */ -uint16_t estimator_flight_time; float estimator_AOA; void estimator_init( void ) { - estimator_flight_time = 0; - } diff --git a/sw/airborne/estimator.h b/sw/airborne/estimator.h index 7a0a2be2c3..486e90f599 100644 --- a/sw/airborne/estimator.h +++ b/sw/airborne/estimator.h @@ -36,9 +36,6 @@ #include "state.h" -/** flight time in seconds. */ -extern uint16_t estimator_flight_time; - extern float estimator_AOA; ///< angle of attack in rad void estimator_init( void ); diff --git a/sw/airborne/firmwares/fixedwing/ap_downlink.h b/sw/airborne/firmwares/fixedwing/ap_downlink.h index c4a6520e61..a8cdeb4cd3 100644 --- a/sw/airborne/firmwares/fixedwing/ap_downlink.h +++ b/sw/airborne/firmwares/fixedwing/ap_downlink.h @@ -63,7 +63,7 @@ &v_ctl_throttle_slewed, \ &vsupply, \ &s, \ - &estimator_flight_time, \ + &autopilot_flight_time, \ &kill_throttle, \ &block_time, \ &stage_time, \ diff --git a/sw/airborne/firmwares/fixedwing/autopilot.h b/sw/airborne/firmwares/fixedwing/autopilot.h index a7d7e16a1e..9df70ea617 100644 --- a/sw/airborne/firmwares/fixedwing/autopilot.h +++ b/sw/airborne/firmwares/fixedwing/autopilot.h @@ -55,6 +55,9 @@ extern uint8_t pprz_mode; extern bool_t kill_throttle; +/** flight time in seconds. */ +extern uint16_t autopilot_flight_time; + // FIXME, move to control #define LATERAL_MODE_MANUAL 0 @@ -95,7 +98,7 @@ extern bool_t power_switch; #endif // POWER_SWITCH_LED #define autopilot_ResetFlightTimeAndLaunch(_) { \ - estimator_flight_time = 0; launch = FALSE; \ + autopilot_flight_time = 0; launch = FALSE; \ } /* CONTROL_RATE will be removed in the next release diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index d9c3f75224..3de67679c2 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -122,6 +122,9 @@ bool_t kill_throttle = FALSE; bool_t launch = FALSE; +/* flight time in seconds */ +uint16_t autopilot_flight_time = 0; + /** Supply voltage in deciVolt. * This the ap copy of the measurement from fbw @@ -382,7 +385,7 @@ static inline void telecommand_task( void ) { current = fbw_state->current; #ifdef RADIO_CONTROL - if (!estimator_flight_time) { + if (!autopilot_flight_time) { if (pprz_mode == PPRZ_MODE_AUTO2 && fbw_state->channels[RADIO_THROTTLE] > THROTTLE_THRESHOLD_TAKEOFF) { launch = TRUE; } @@ -517,7 +520,7 @@ void attitude_loop( void ) { h_ctl_pitch_setpoint = nav_pitch; Bound(h_ctl_pitch_setpoint, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT); - if (kill_throttle || (!estimator_flight_time && !launch)) + if (kill_throttle || (!autopilot_flight_time && !launch)) v_ctl_throttle_setpoint = 0; } @@ -577,8 +580,8 @@ void sensors_task( void ) { /** monitor stuff run at 1Hz */ void monitor_task( void ) { - if (estimator_flight_time) - estimator_flight_time++; + if (autopilot_flight_time) + autopilot_flight_time++; #if defined DATALINK || defined SITL datalink_time++; #endif @@ -591,9 +594,9 @@ void monitor_task( void ) { kill_throttle |= (t >= LOW_BATTERY_DELAY); kill_throttle |= launch && (dist2_to_home > Square(KILL_MODE_DISTANCE)); - if (!estimator_flight_time && + if (!autopilot_flight_time && *stateGetHorizontalSpeedNorm_f() > MIN_SPEED_FOR_TAKEOFF) { - estimator_flight_time = 1; + autopilot_flight_time = 1; launch = TRUE; /* Not set in non auto launch */ uint16_t time_sec = sys_time.nb_sec; DOWNLINK_SEND_TAKEOFF(DefaultChannel, DefaultDevice, &time_sec); diff --git a/sw/airborne/modules/com/generic_com.c b/sw/airborne/modules/com/generic_com.c index 9f9dcc33f9..8d31627e32 100644 --- a/sw/airborne/modules/com/generic_com.c +++ b/sw/airborne/modules/com/generic_com.c @@ -83,7 +83,7 @@ void generic_com_periodic( void ) { com_trans.buf[19] = (uint8_t)(ap_state->commands[COMMAND_THROTTLE]*100/MAX_PPRZ); com_trans.buf[20] = pprz_mode; com_trans.buf[21] = nav_block; - FillBufWith16bit(com_trans.buf, 22, estimator_flight_time); + FillBufWith16bit(com_trans.buf, 22, autopilot_flight_time); I2CTransmit(GENERIC_COM_I2C_DEV, com_trans, GENERIC_COM_SLAVE_ADDR, NB_DATA); } diff --git a/sw/airborne/modules/gsm/gsm.c b/sw/airborne/modules/gsm/gsm.c index 5f2c10212f..c011c16d23 100644 --- a/sw/airborne/modules/gsm/gsm.c +++ b/sw/airborne/modules/gsm/gsm.c @@ -41,7 +41,7 @@ Reporting: In: OK Out: AT+CMGS=\"GCS_NUMBER\" In: > - Out: gps.utm_pos.east, gps.utm_pos.north, gps.course, gps.hmsl, gps.gspeed, -gps.ned_vel.z, vsupply, estimator_flight_time, rssi CTRLZ + Out: gps.utm_pos.east, gps.utm_pos.north, gps.course, gps.hmsl, gps.gspeed, -gps.ned_vel.z, vsupply, autopilot_flight_time, rssi CTRLZ Receiving: In: +CMTI: ..., @@ -409,9 +409,9 @@ void gsm_send_report_continue(void) uint8_t rssi = atoi(gsm_buf + strlen("+CSQ: ")); // Donnee GPS :ne sont pas envoyes gps_mode, gps.tow, gps.utm_pos.zone, gps_nb_ovrn - // Donnees batterie (seuls vsupply et estimator_flight_time sont envoyes) + // Donnees batterie (seuls vsupply et autopilot_flight_time sont envoyes) // concatenation de toutes les infos en un seul message à transmettre - sprintf(data_to_send, "%ld %ld %d %ld %d %d %d %d %d", gps.utm_pos.east, gps.utm_pos.north, gps_course, gps.hmsl, gps.gspeed, -gps.ned_vel.z, vsupply, estimator_flight_time, rssi); + sprintf(data_to_send, "%ld %ld %d %ld %d %d %d %d %d", gps.utm_pos.east, gps.utm_pos.north, gps_course, gps.hmsl, gps.gspeed, -gps.ned_vel.z, vsupply, autopilot_flight_time, rssi); // send the number and wait for the prompt char buf[32]; diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml index 42dddbcfd4..f089089b23 100644 --- a/sw/ground_segment/cockpit/live.ml +++ b/sw/ground_segment/cockpit/live.ml @@ -697,7 +697,7 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id connect "kill_throttle" ac.strip#connect_kill; connect "nav_shift" ac.strip#connect_shift_lateral; connect "pprz_mode" ac.strip#connect_mode; - connect "estimator_flight_time" ac.strip#connect_flight_time; + connect "autopilot_flight_time" ac.strip#connect_flight_time; let get_ac_unix_time = fun () -> ac.last_unix_time in connect ~warning:false "snav_desired_tow" (ac.strip#connect_apt get_ac_unix_time); begin (* Periodically update the appointment *) diff --git a/sw/ground_segment/tmtc/GSM/SMS_Ground_UDtest_final.c b/sw/ground_segment/tmtc/GSM/SMS_Ground_UDtest_final.c index 7bb3f9dc55..af8faf2717 100644 --- a/sw/ground_segment/tmtc/GSM/SMS_Ground_UDtest_final.c +++ b/sw/ground_segment/tmtc/GSM/SMS_Ground_UDtest_final.c @@ -99,7 +99,7 @@ pile *MaPile = NULL; typedef enum {INIT, AT, CMGF, SMSMODE, CNMI, CPMS, ERREUR} Etat_Liste; // Informations extraites du SMS recu de l'avion -char extr_gps_utm_east[15], extr_gps_utm_north[15], extr_gps_course[15], extr_gps_alt[15], extr_gps_gspeed[15], extr_gps_climb[15], extr_vsupply[15], extr_estimator_flight_time[15], extr_qualite_signal_GSM[10]; +char extr_gps_utm_east[15], extr_gps_utm_north[15], extr_gps_course[15], extr_gps_alt[15], extr_gps_gspeed[15], extr_gps_climb[15], extr_vsupply[15], extr_autopilot_flight_time[15], extr_qualite_signal_GSM[10]; char reponse_attendue[20]; int prompt_recu = 0; @@ -472,10 +472,10 @@ void decoupage( char message_complet[]) Extraction(data_to_cut, ' ', 1, 1, ' ', 1, 0, extr_gps_gspeed); Extraction(data_to_cut, ' ', 1, 1, ' ', 1, 0, extr_gps_climb); Extraction(data_to_cut, ' ', 1, 1, ' ', 1, 0, extr_vsupply); - Extraction(data_to_cut, ' ', 1, 1, ' ', 1, 0, extr_estimator_flight_time); + Extraction(data_to_cut, ' ', 1, 1, ' ', 1, 0, extr_autopilot_flight_time); Extraction(data_to_cut, ' ', 1, 1, '\r', 1, 0, extr_qualite_signal_GSM); - printf("Message :\n utm_east %s\n utm_north %s\n course %s\n alt %s\n speed %s\n climb %s\n bat %s\n flight_time %s\n signal %s\n", extr_gps_utm_east, extr_gps_utm_north, extr_gps_course, extr_gps_alt, extr_gps_gspeed, extr_gps_climb, extr_vsupply, extr_estimator_flight_time, extr_qualite_signal_GSM); + printf("Message :\n utm_east %s\n utm_north %s\n course %s\n alt %s\n speed %s\n climb %s\n bat %s\n flight_time %s\n signal %s\n", extr_gps_utm_east, extr_gps_utm_north, extr_gps_course, extr_gps_alt, extr_gps_gspeed, extr_gps_climb, extr_vsupply, extr_autopilot_flight_time, extr_qualite_signal_GSM); fflush(stdout); } diff --git a/sw/in_progress/satcom/tcp2ivy.c b/sw/in_progress/satcom/tcp2ivy.c index e6e65712c1..a569130387 100644 --- a/sw/in_progress/satcom/tcp2ivy.c +++ b/sw/in_progress/satcom/tcp2ivy.c @@ -75,7 +75,7 @@ unsigned char electrical_vsupply; unsigned char nav_block; unsigned char energy; unsigned char throttle; -unsigned short estimator_flight_time; +unsigned short autopilot_flight_time; unsigned char nav_utm_zone0; float latlong_utm_x, latlong_utm_y; unsigned char pprz_mode; @@ -196,8 +196,8 @@ static gboolean read_data(GIOChannel *chan, GIOCondition cond, gpointer data) { pprz_mode = buf[19]; // com_trans.buf[20] = nav_block; nav_block = buf[20]; -// FillBufWith16bit(com_trans.buf, 21, estimator_flight_time); - estimator_flight_time = buf2ushort(&buf[21]); +// FillBufWith16bit(com_trans.buf, 21, autopilot_flight_time); + autopilot_flight_time = buf2ushort(&buf[21]); //gps_lat = 52.2648312 * 1e7; //gps_lon = 9.9939456 * 1e7; @@ -211,7 +211,7 @@ static gboolean read_data(GIOChannel *chan, GIOCondition cond, gpointer data) { //throttle = 51; //pprz_mode = 2; //nav_block = 1; -//estimator_flight_time = 123; +//autopilot_flight_time = 123; nav_utm_zone0 = (gps_lon/10000000+180) / 6 + 1; latlong_utm_of(RadOfDeg(gps_lat/1e7), RadOfDeg(gps_lon/1e7), nav_utm_zone0); @@ -230,7 +230,7 @@ printf("energy %d\n", energy); printf("throttle %d\n", throttle); printf("pprz_mode %d\n", pprz_mode); printf("nav_block %d\n", nav_block); -printf("estimator_flight_time %d\n", estimator_flight_time); +printf("autopilot_flight_time %d\n", autopilot_flight_time); printf("gps_utm_east %d\n", gps_utm_east); printf("gps_utm_north %d\n", gps_utm_north); @@ -316,7 +316,7 @@ printf("gps_utm_zone %d\n", gps_utm_zone); throttle * MAX_PPRZ / 100, electrical_vsupply, 0, // amps - estimator_flight_time, + autopilot_flight_time, 0, // kill_auto_throttle 0, // block_time 0, // stage_time diff --git a/sw/in_progress/satcom/tcp2ivy_generic.c b/sw/in_progress/satcom/tcp2ivy_generic.c index e40ce3665f..caa9f88138 100644 --- a/sw/in_progress/satcom/tcp2ivy_generic.c +++ b/sw/in_progress/satcom/tcp2ivy_generic.c @@ -78,7 +78,7 @@ unsigned char electrical_vsupply; unsigned char nav_block; unsigned short energy; unsigned char throttle; -unsigned short estimator_flight_time; +unsigned short autopilot_flight_time; unsigned char nav_utm_zone0; float latlong_utm_x, latlong_utm_y; unsigned char pprz_mode; @@ -133,8 +133,8 @@ static gboolean read_data(GIOChannel *chan, GIOCondition cond, gpointer data) { pprz_mode = buf[19]; // com_trans.buf[21] = nav_block; nav_block = buf[20]; - // FillBufWith16bit(com_trans.buf, 22, estimator_flight_time); - estimator_flight_time = buf2ushort(&buf[21]); + // FillBufWith16bit(com_trans.buf, 22, autopilot_flight_time); + autopilot_flight_time = buf2ushort(&buf[21]); #if 0 gps_lat = 52.2648312 * 1e7; @@ -148,7 +148,7 @@ static gboolean read_data(GIOChannel *chan, GIOCondition cond, gpointer data) { throttle = 51; pprz_mode = 2; nav_block = 1; - estimator_flight_time = 123; + autopilot_flight_time = 123; #endif printf("**** message received from iridium module ****\n"); @@ -163,7 +163,7 @@ static gboolean read_data(GIOChannel *chan, GIOCondition cond, gpointer data) { printf("throttle %d\n", throttle); printf("pprz_mode %d\n", pprz_mode); printf("nav_block %d\n", nav_block); - printf("estimator_flight_time %d\n", estimator_flight_time); + printf("autopilot_flight_time %d\n", autopilot_flight_time); fflush(stdout); IvySendMsg("%d GENERIC_COM %d %d %d %d %d %d %d %d %d %d %d %d", @@ -179,7 +179,7 @@ static gboolean read_data(GIOChannel *chan, GIOCondition cond, gpointer data) { throttle, pprz_mode, nav_block, - estimator_flight_time); + autopilot_flight_time); } else { diff --git a/sw/lib/ocaml/expr_syntax.ml b/sw/lib/ocaml/expr_syntax.ml index e553008ba1..fd9d5ad2c0 100644 --- a/sw/lib/ocaml/expr_syntax.ml +++ b/sw/lib/ocaml/expr_syntax.ml @@ -72,7 +72,7 @@ let functions = [ let variables = [ "launch"; "estimator_z"; - "estimator_flight_time"; + "autopilot_flight_time"; "estimator_hspeed_mod"; "estimator_theta"; "circle_count"; From e9794e8986a0e8a78fc2d6afe14c6d27da0d7650 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 23 Aug 2012 17:34:44 +0200 Subject: [PATCH 60/62] [AoA] add angle of attack and sideslip to state interface --- sw/airborne/estimator.c | 2 - sw/airborne/estimator.h | 2 - .../stabilization/stabilization_attitude.c | 2 +- sw/airborne/modules/sensors/AOA_adc.c | 3 +- sw/airborne/state.h | 63 ++++++++++++++++++- 5 files changed, 62 insertions(+), 10 deletions(-) diff --git a/sw/airborne/estimator.c b/sw/airborne/estimator.c index d0d96df807..e4852a9fd1 100644 --- a/sw/airborne/estimator.c +++ b/sw/airborne/estimator.c @@ -31,8 +31,6 @@ #include "estimator.h" -float estimator_AOA; - void estimator_init( void ) { } diff --git a/sw/airborne/estimator.h b/sw/airborne/estimator.h index 486e90f599..55b8e31780 100644 --- a/sw/airborne/estimator.h +++ b/sw/airborne/estimator.h @@ -36,8 +36,6 @@ #include "state.h" -extern float estimator_AOA; ///< angle of attack in rad - void estimator_init( void ); #endif /* ESTIMATOR_H */ diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c index 93e08c31a5..b868d790b6 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c @@ -433,7 +433,7 @@ inline static void h_ctl_pitch_loop( void ) { err = att->theta - h_ctl_pitch_loop_setpoint; break; case H_CTL_PITCH_MODE_AOA: - err = estimator_AOA - h_ctl_pitch_loop_setpoint; //TODO AOA into state interface + err = (*stateGetAngleOfAttack_f()) - h_ctl_pitch_loop_setpoint; break; default: err = att->theta - h_ctl_pitch_loop_setpoint; diff --git a/sw/airborne/modules/sensors/AOA_adc.c b/sw/airborne/modules/sensors/AOA_adc.c index ee9b58f832..be84620009 100644 --- a/sw/airborne/modules/sensors/AOA_adc.c +++ b/sw/airborne/modules/sensors/AOA_adc.c @@ -28,7 +28,6 @@ #include "mcu_periph/adc.h" #include BOARD_CONFIG #include "generated/airframe.h" -#include "estimator.h" #include "state.h" #include "std.h" //Messages @@ -79,6 +78,6 @@ void AOA_adc_update( void ) { RunOnceEvery(30, DOWNLINK_SEND_AOA_adc(DefaultChannel, DefaultDevice, &adc_AOA_val, &AOA)); #ifdef USE_AOA - EstimatorSetAOA(AOA); + stateSetAngleOfAttack_f(AOA); #endif } diff --git a/sw/airborne/state.h b/sw/airborne/state.h index a7bc524ff4..a06296529f 100644 --- a/sw/airborne/state.h +++ b/sw/airborne/state.h @@ -125,6 +125,8 @@ #define AIRSPEED_I 1 #define WINDSPEED_F 2 #define AIRSPEED_F 3 +#define AOA_F 4 +#define SIDESLIP_F 5 /**@}*/ @@ -470,10 +472,23 @@ struct State { struct FloatVect2 h_windspeed_f; /** - * Norm of horizontal ground speed. + * Norm of relative air speed. * Unit: m/s */ float airspeed_f; + + /** + * Angle of attack + * Unit: rad + */ + float angle_of_attack_f; + + /** + * Sideslip angle + * Unit: rad + */ + float sideslip_f; + /** @}*/ }; @@ -1205,6 +1220,16 @@ static inline bool_t stateIsAirspeedValid(void) { return (state.wind_air_status &= ~((1< Date: Thu, 23 Aug 2012 17:56:44 +0200 Subject: [PATCH 61/62] [state] finally getting rid of old estimator --- conf/airframes/examples/demo_module.xml | 211 ------------------ .../subsystems/fixedwing/autopilot.makefile | 1 - conf/settings/estimation/fw_baro_kalman.xml | 2 +- sw/airborne/arch/sim/jsbsim_gps.c | 1 - sw/airborne/arch/sim/jsbsim_hw.h | 1 - sw/airborne/arch/sim/jsbsim_ir.c | 1 - sw/airborne/arch/sim/sim_ap.c | 1 - sw/airborne/arch/sim/sim_baro.c | 4 +- sw/airborne/arch/sim/sim_gps.c | 1 - sw/airborne/estimator.c | 37 --- sw/airborne/estimator.h | 41 ---- sw/airborne/firmwares/fixedwing/autopilot.h | 1 - .../fixedwing/guidance/energy_ctrl.c | 10 +- sw/airborne/firmwares/fixedwing/main_ap.c | 5 +- sw/airborne/modules/com/generic_com.c | 1 - sw/airborne/modules/gsm/gsm.c | 1 - sw/airborne/rc_settings.c | 1 - 17 files changed, 11 insertions(+), 309 deletions(-) delete mode 100644 conf/airframes/examples/demo_module.xml delete mode 100644 sw/airborne/estimator.c delete mode 100644 sw/airborne/estimator.h diff --git a/conf/airframes/examples/demo_module.xml b/conf/airframes/examples/demo_module.xml deleted file mode 100644 index 94ef391cea..0000000000 --- a/conf/airframes/examples/demo_module.xml +++ /dev/null @@ -1,211 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- - -
- - - - - - - - - - - -
- - -
- -
- - - - -
- -
- - - - - - - - - - - - - - - - - - - - -
- -
- -
- -
- - - - - - -
- -
- - - - - - - - - - - - - - - - - - - - - - -
- -
- - - - - - - - - - - - - -
- -
- - - -
- -
- - - - - - - - -
- -
- - - - - -
- - -CONFIG = \"tiny_2_1_1.h\" - -include $(PAPARAZZI_SRC)/conf/firmwares/tiny.makefile - -FLASH_MODE=IAP - -ap.CFLAGS += -DFBW -DAP -DBOARD_CONFIG=$(CONFIG) -DUSE_LED -DSYS_TIME_LED=1 -ap.srcs = mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c $(SRC_ARCH)/armVIC.c $(SRC_FIRMWARE)/main_fbw.c $(SRC_FIRMWARE)/main_ap.c $(SRC_FIRMWARE)/main.c - -ap.CFLAGS += -DINTER_MCU -ap.srcs += inter_mcu.c - -ap.srcs += commands.c - -########## RC actuators + radio -ap.CFLAGS += -DACTUATORS=\"servos_4017_hw.h\" -DSERVOS_4017 -ap.srcs += $(SRC_ARCH)/servos_4017_hw.c actuators.c - -ap.CFLAGS += -DRADIO_CONTROL -ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c - -########## Modem -ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 -DPPRZ_UART=Uart1 -DDATALINK=PPRZ -DUART1_BAUD=B9600 -ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c - -########## ADC -ap.CFLAGS += -DADC -DUSE_ADC_0 -DUSE_ADC_1 -DUSE_ADC_2 -DUSE_ADC_3 -ap.srcs += $(SRC_ARCH)/adc_hw.c - -########## GPS -ap.CFLAGS += -DUSE_GPS -DUBX -DUSE_UART0 -DGPS_LINK=Uart0 -DUART0_BAUD=B38400 -DGPS_USE_LATLONG -ap.srcs += gps_ubx.c gps.c latlong.c - -########## IR sensors -ap.CFLAGS += -DUSE_INFRARED -DALT_KALMAN -ap.srcs += infrared.c estimator.c - -########## Nav -ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM -ap.srcs += subsystems/nav.c fw_h_ctl.c fw_v_ctl.c -ap.srcs += subsystems/navigation/nav_survey_rectangle.c -ap.srcs += subsystems/navigation/nav_line.c - - -# Config for SITL simulation -include $(PAPARAZZI_SRC)/conf/firmwares/sitl.makefile -sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN - -sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c - - - diff --git a/conf/firmwares/subsystems/fixedwing/autopilot.makefile b/conf/firmwares/subsystems/fixedwing/autopilot.makefile index c7ecfc010a..0e91400c10 100644 --- a/conf/firmwares/subsystems/fixedwing/autopilot.makefile +++ b/conf/firmwares/subsystems/fixedwing/autopilot.makefile @@ -162,7 +162,6 @@ fbw_srcs += $(SRC_FIRMWARE)/fbw_downlink.c ap_CFLAGS += -DAP ap_srcs += $(SRC_FIRMWARE)/main_ap.c -ap_srcs += $(SRC_FIXEDWING)/estimator.c ap_CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_float.h\" ap_srcs += $(SRC_FIXEDWING)/subsystems/ins.c ap_srcs += $(SRC_FIXEDWING)/subsystems/ins/ins_float.c diff --git a/conf/settings/estimation/fw_baro_kalman.xml b/conf/settings/estimation/fw_baro_kalman.xml index 34f11f32de..66012cb48d 100644 --- a/conf/settings/estimation/fw_baro_kalman.xml +++ b/conf/settings/estimation/fw_baro_kalman.xml @@ -6,7 +6,7 @@ - + diff --git a/sw/airborne/arch/sim/jsbsim_gps.c b/sw/airborne/arch/sim/jsbsim_gps.c index f489ac35a4..94ca3e4715 100644 --- a/sw/airborne/arch/sim/jsbsim_gps.c +++ b/sw/airborne/arch/sim/jsbsim_gps.c @@ -9,7 +9,6 @@ #include "generated/flight_plan.h" #include "autopilot.h" #include "subsystems/gps.h" -#include "estimator.h" #include "math/pprz_geodetic_float.h" #include "math/pprz_geodetic_int.h" diff --git a/sw/airborne/arch/sim/jsbsim_hw.h b/sw/airborne/arch/sim/jsbsim_hw.h index 1202dce646..fa32784663 100644 --- a/sw/airborne/arch/sim/jsbsim_hw.h +++ b/sw/airborne/arch/sim/jsbsim_hw.h @@ -31,7 +31,6 @@ #include "std.h" #include "inter_mcu.h" #include "firmwares/fixedwing/autopilot.h" -#include "estimator.h" #include "subsystems/gps.h" #include "subsystems/navigation/traffic_info.h" #include "generated/flight_plan.h" diff --git a/sw/airborne/arch/sim/jsbsim_ir.c b/sw/airborne/arch/sim/jsbsim_ir.c index f850dd9574..1259852536 100644 --- a/sw/airborne/arch/sim/jsbsim_ir.c +++ b/sw/airborne/arch/sim/jsbsim_ir.c @@ -7,7 +7,6 @@ #include "jsbsim_hw.h" #include -#include "estimator.h" #ifndef JSBSIM_IR_ROLL_NEUTRAL #define JSBSIM_IR_ROLL_NEUTRAL 0. diff --git a/sw/airborne/arch/sim/sim_ap.c b/sw/airborne/arch/sim/sim_ap.c index c0ad122a24..7eaffac836 100644 --- a/sw/airborne/arch/sim/sim_ap.c +++ b/sw/airborne/arch/sim/sim_ap.c @@ -12,7 +12,6 @@ #include "std.h" #include "inter_mcu.h" #include "autopilot.h" -#include "estimator.h" #include "subsystems/gps.h" #include "subsystems/navigation/traffic_info.h" #include "generated/settings.h" diff --git a/sw/airborne/arch/sim/sim_baro.c b/sw/airborne/arch/sim/sim_baro.c index 923961727b..cf6221bb29 100644 --- a/sw/airborne/arch/sim/sim_baro.c +++ b/sw/airborne/arch/sim/sim_baro.c @@ -1,5 +1,5 @@ #include -#include "estimator.h" +#include "state.h" #include "subsystems/nav.h" #include "subsystems/gps.h" #include "baro_MS5534A.h" @@ -32,6 +32,6 @@ void baro_MS5534A_send(void) { void baro_MS5534A_event_task( void ) { baro_MS5534A_pressure = baro_MS5534A_ground_pressure - (gps.hmsl/1000.-ground_alt) / 0.08 + ((10.*random()) / RAND_MAX); - baro_MS5534A_temp = 10 + estimator_z; + baro_MS5534A_temp = 10 + stateGetPositionUtm_f()->alt; baro_MS5534A_available = TRUE; } diff --git a/sw/airborne/arch/sim/sim_gps.c b/sw/airborne/arch/sim/sim_gps.c index 9e51ba9330..70a5a8c18e 100644 --- a/sw/airborne/arch/sim/sim_gps.c +++ b/sw/airborne/arch/sim/sim_gps.c @@ -9,7 +9,6 @@ #include "generated/flight_plan.h" #include "autopilot.h" #include "subsystems/gps.h" -#include "estimator.h" #include "math/pprz_geodetic_float.h" #include "math/pprz_geodetic_int.h" diff --git a/sw/airborne/estimator.c b/sw/airborne/estimator.c deleted file mode 100644 index e4852a9fd1..0000000000 --- a/sw/airborne/estimator.c +++ /dev/null @@ -1,37 +0,0 @@ -/* - * Paparazzi autopilot $Id$ - * - * Copyright (C) 2004-2010 The Paparazzi Team - * - * This file is part of paparazzi. - * - * paparazzi 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 2, or (at your option) - * any later version. - * - * paparazzi 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 paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -/** \file estimator.c - * \brief State estimate, fusioning sensors - */ - -#include -#include - -#include "estimator.h" - -void estimator_init( void ) { - -} - diff --git a/sw/airborne/estimator.h b/sw/airborne/estimator.h deleted file mode 100644 index 55b8e31780..0000000000 --- a/sw/airborne/estimator.h +++ /dev/null @@ -1,41 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2004-2006 Pascal Brisset, Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi 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 2, or (at your option) - * any later version. - * - * paparazzi 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 paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -/** \file estimator.h - * \brief State estimation, fusioning sensors - * TODO will be removed when the remaining variables are integrated to the state interface - */ - -#ifndef ESTIMATOR_H -#define ESTIMATOR_H - -#include - -#include "std.h" - -#include "state.h" - -void estimator_init( void ); - -#endif /* ESTIMATOR_H */ diff --git a/sw/airborne/firmwares/fixedwing/autopilot.h b/sw/airborne/firmwares/fixedwing/autopilot.h index 9df70ea617..bd8f20c1dd 100644 --- a/sw/airborne/firmwares/fixedwing/autopilot.h +++ b/sw/airborne/firmwares/fixedwing/autopilot.h @@ -33,7 +33,6 @@ #include #include "std.h" #include "mcu_periph/sys_time.h" -#include "estimator.h" #define THRESHOLD_MANUAL_PPRZ (MIN_PPRZ / 2) diff --git a/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c index fe930e0d2e..81403c64ae 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c +++ b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c @@ -53,7 +53,7 @@ #pragma message "CAUTION! Using TOTAL ENERGY CONTROLLER: Experimental!" #include "firmwares/fixedwing/guidance/energy_ctrl.h" -#include "estimator.h" +#include "state.h" #include "subsystems/nav.h" #include "generated/airframe.h" #include "firmwares/fixedwing/autopilot.h" @@ -145,13 +145,13 @@ static void ac_char_update(float throttle, float pitch, float climb, float accel { ac_char_climb_count++; ac_char_average(&ac_char_climb_pitch, pitch * 57.6f, ac_char_climb_count ); - ac_char_average(&ac_char_climb_max , estimator_z_dot, ac_char_climb_count ); + ac_char_average(&ac_char_climb_max , stateGetSpeedEnu_f()->z, ac_char_climb_count ); } else if (throttle <= 0.0f) { ac_char_descend_count++; ac_char_average(&ac_char_descend_pitch, pitch * 57.6f , ac_char_descend_count ); - ac_char_average(&ac_char_descend_max , estimator_z_dot , ac_char_descend_count ); + ac_char_average(&ac_char_descend_max , stateGetSpeedEnu_f()->z , ac_char_descend_count ); } else if ((climb > -0.125) && (climb < 0.125)) { @@ -194,7 +194,7 @@ void v_ctl_altitude_loop( void ) if (v_ctl_auto_airspeed_setpoint <= 0.0f) v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED; // Altitude Controller - v_ctl_altitude_error = v_ctl_altitude_setpoint - estimator_z; + v_ctl_altitude_error = v_ctl_altitude_setpoint - stateCalcPositionUtm_f()->alt; float sp = v_ctl_altitude_pgain * v_ctl_altitude_error + v_ctl_altitude_pre_climb ; BoundAbs(sp, v_ctl_max_climb); @@ -247,7 +247,7 @@ void v_ctl_climb_loop( void ) float vdot_err = low_pass_vdot( v_ctl_desired_acceleration - vdot ); // Flight Path Outerloop: positive means needs to climb more: needs extra energy - float gamma_err = (v_ctl_climb_setpoint - estimator_z_dot) / v_ctl_auto_airspeed_setpoint; + float gamma_err = (v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z) / v_ctl_auto_airspeed_setpoint; // Total Energy Error: positive means energy should be added float en_tot_err = gamma_err + vdot_err; diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index 3de67679c2..ab24f631f4 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -58,8 +58,8 @@ #endif // autopilot & control +#include "state.h" #include "firmwares/fixedwing/autopilot.h" -#include "estimator.h" #include "subsystems/ins.h" #include "firmwares/fixedwing/stabilization/stabilization_attitude.h" #include CTRL_TYPE_H @@ -184,6 +184,8 @@ void init_ap( void ) { ins_init(); + stateInit(); + /************* Links initialization ***************/ #if defined MCU_SPI_LINK link_mcu_init(); @@ -195,7 +197,6 @@ void init_ap( void ) { /************ Internal status ***************/ h_ctl_init(); v_ctl_init(); - estimator_init(); nav_init(); modules_init(); diff --git a/sw/airborne/modules/com/generic_com.c b/sw/airborne/modules/com/generic_com.c index 8d31627e32..b99dc3d174 100644 --- a/sw/airborne/modules/com/generic_com.c +++ b/sw/airborne/modules/com/generic_com.c @@ -28,7 +28,6 @@ #include "mcu_periph/i2c.h" #include "state.h" -#include "estimator.h" #include "subsystems/gps.h" #include "subsystems/electrical.h" #include "generated/airframe.h" diff --git a/sw/airborne/modules/gsm/gsm.c b/sw/airborne/modules/gsm/gsm.c index c011c16d23..4389f87100 100644 --- a/sw/airborne/modules/gsm/gsm.c +++ b/sw/airborne/modules/gsm/gsm.c @@ -71,7 +71,6 @@ Receiving: #include "ap_subsystems/datalink/downlink.h" #include "subsystems/gps.h" #include "autopilot.h" -#include "estimator.h" //#include "subsystems/navigation/common_nav.h" //why is should this be needed? #include "generated/settings.h" diff --git a/sw/airborne/rc_settings.c b/sw/airborne/rc_settings.c index 15573d398a..494bf6c606 100644 --- a/sw/airborne/rc_settings.c +++ b/sw/airborne/rc_settings.c @@ -30,7 +30,6 @@ #include "autopilot.h" #include "subsystems/nav.h" #include "subsystems/sensors/infrared.h" -#include "estimator.h" #include "inter_mcu.h" #include "firmwares/fixedwing/stabilization/stabilization_attitude.h" From cc754c011eda0b278c9d75999a1e840a40e73e84 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 23 Aug 2012 18:07:56 +0200 Subject: [PATCH 62/62] [fix] fix function name --- sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c index 81403c64ae..73f2681e6a 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c +++ b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c @@ -194,7 +194,7 @@ void v_ctl_altitude_loop( void ) if (v_ctl_auto_airspeed_setpoint <= 0.0f) v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED; // Altitude Controller - v_ctl_altitude_error = v_ctl_altitude_setpoint - stateCalcPositionUtm_f()->alt; + v_ctl_altitude_error = v_ctl_altitude_setpoint - stateGetPositionUtm_f()->alt; float sp = v_ctl_altitude_pgain * v_ctl_altitude_error + v_ctl_altitude_pre_climb ; BoundAbs(sp, v_ctl_max_climb);