rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-31 16:52:10 +01:00
Code Issues Releases Activity

LP-364 CC3D related fixes. Added missing AttitudeStateYawGet().

Browse Source
This commit is contained in:
Vladimir Zidar 2016-07-20 13:00:29 +02:00 committed by Vladimir Zidar
parent b8b9d31c07
commit ba6ebba29c
1 changed files with 21 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
flight/modules/UAVOMavlinkBridge/UAVOMavlinkBridge.c
View File

@ -180,6 +180,7 @@ static void send_message()
static void mavlink_send_extended_status() static void mavlink_send_extended_status()
{ {
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
FlightBatteryStateData batState; FlightBatteryStateData batState;
SystemStatsData systemStats; SystemStatsData systemStats;
@ -230,6 +231,7 @@ static void mavlink_send_extended_status()
0); 0);
send_message(); send_message();
#endif /* PIOS_EXCLUDE_ADVANCED_FEATURES */
} }
static void mavlink_send_rc_channels() static void mavlink_send_rc_channels()
@ -349,6 +351,7 @@ static void mavlink_send_position()
send_message(); send_message();
} }
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
if (HomeLocationHandle() != NULL) { if (HomeLocationHandle() != NULL) {
HomeLocationData homeLocation; HomeLocationData homeLocation;
HomeLocationGet(&homeLocation); HomeLocationGet(&homeLocation);
@ -363,6 +366,7 @@ static void mavlink_send_position()
send_message(); send_message();
} }
#endif /* PIOS_EXCLUDE_ADVANCED_FEATURES */
// TODO add waypoint nav stuff // TODO add waypoint nav stuff
// wp_target_bearing // wp_target_bearing
@ -412,27 +416,21 @@ static inline float Sq(float x)
static void mavlink_send_extra2() static void mavlink_send_extra2()
{ {
float airspeed = 0; float airspeed = 0;
float altitude = 0;
float groundspeed = 0;
float climbrate = 0;
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
if (AirspeedStateHandle() != NULL) { if (AirspeedStateHandle() != NULL) {
AirspeedStateTrueAirspeedGet(&airspeed); AirspeedStateTrueAirspeedGet(&airspeed);
} }
float altitude = 0;
if (PositionStateHandle() != NULL) { if (PositionStateHandle() != NULL) {
PositionStateDownGet(&altitude); PositionStateDownGet(&altitude);
altitude *= -1; altitude *= -1;
} }
float attitudeYaw = 0;
// round attState.Yaw to nearest int and transfer from (-180 ... 180) to (0 ... 360)
int16_t heading = lroundf(attitudeYaw);
if (heading < 0) {
heading += 360;
}
float groundspeed = 0, climbrate = 0;
if (VelocityStateHandle() != NULL) { if (VelocityStateHandle() != NULL) {
VelocityStateData velocityState; VelocityStateData velocityState;
VelocityStateGet(&velocityState); VelocityStateGet(&velocityState);
@ -440,6 +438,17 @@ static void mavlink_send_extra2()
groundspeed = sqrtf(Sq(velocityState.North) + Sq(velocityState.East)); groundspeed = sqrtf(Sq(velocityState.North) + Sq(velocityState.East));
climbrate = velocityState.Down * -1; climbrate = velocityState.Down * -1;
} }
#endif
float attitudeYaw = 0;
AttitudeStateYawGet(&attitudeYaw);
// round attState.Yaw to nearest int and transfer from (-180 ... 180) to (0 ... 360)
int16_t heading = lroundf(attitudeYaw);
if (heading < 0) {
heading += 360;
}
float thrust; float thrust;
ActuatorDesiredThrustGet(&thrust); ActuatorDesiredThrustGet(&thrust);
@ -495,11 +504,9 @@ static void mavlink_send_extra2()
case FLIGHTSTATUS_FLIGHTMODE_MANUAL: case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
custom_mode = CUSTOM_MODE_ACRO; // or custom_mode = CUSTOM_MODE_ACRO; // or
break; break;
#if 0 /* TODO: enable this when AUTOTUNE gets merged */
case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE: case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE:
custom_mode = CUSTOM_MODE_ATUN; custom_mode = CUSTOM_MODE_ATUN;
break; break;
#endif
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: