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

LP-291 msp_send_altitude(): use PositionState instead of BaroSensor and add VelocityState to provide climb rate

Browse Source
This commit is contained in:
Vladimir Zidar 2016-04-24 01:46:13 +02:00
parent bb0f15024b
commit fec3924ee0
1 changed files with 22 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
flight/modules/UAVOMSPBridge/UAVOMSPBridge.c
View File

@ -1,6 +1,6 @@
/**
******************************************************************************
* @addtogroup TauLabsModules TauLabs Modules
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup UAVOMSPBridge UAVO to MSP Bridge Module
* @{
@ -51,7 +51,8 @@
#include "systemstats.h"
#include "systemalarms.h"
#include "homelocation.h"
#include "barosensor.h"
#include "positionstate.h"
#include "velocitystate.h"
#include "stabilizationdesired.h"
#include "taskinfo.h"
#include "stabilizationsettings.h"
@ -574,21 +575,32 @@ static void msp_send_altitude(struct msp_bridge *m)
union {
uint8_t buf[0];
struct {
int32_t alt; // cm
uint16_t vario; // cm/s
} __attribute__((packed)) baro;
int32_t estimatedAltitude; // cm
int16_t vario; // cm/s
} __attribute__((packed)) altitude;
} data;
if (BaroSensorHandle() != NULL) {
BaroSensorData baro;
BaroSensorGet(&baro);
if (PositionStateHandle() != NULL) {
PositionStateData positionState;
PositionStateGet(&positionState);
data.baro.alt = (int32_t)roundf(baro.Altitude * 100.0f);
data.altitude.estimatedAltitude = (int32_t)roundf(positionState.Down * -100.0f);
} else {
data.altitude.estimatedAltitude = 0;
}
if( VelocityStateHandle() != NULL) {
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
data.altitude.vario = (int16_t)roundf(velocityState.Down * -100.0f);
} else {
data.altitude.vario = 0;
}
msp_send(m, MSP_ALTITUDE, data.buf, sizeof(data));
}
}
#endif /* PIOS_EXCLUDE_ADVANCED_FEATURES */