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

633 lines
24 KiB
C
Raw Blame History

/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup UAVOMavlinkBridge UAVO to Mavlink Bridge Module
* @{
*
* @file UAVOMavlinkBridge.c
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
* dRonin, http://dRonin.org/, Copyright (C) 2015-2016
* Tau Labs, http://taulabs.org, Copyright (C) 2013-2014
* @brief Bridges selected UAVObjects to Mavlink
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
* Additional note on redistribution: The copyright and license notices above
* must be maintained in each individual source file that is a derivative work
* of this source file; otherwise redistribution is prohibited.
*/
// ****************
#include "openpilot.h"
#include "stabilizationdesired.h"
#include "flightbatterysettings.h"
#include "flightbatterystate.h"
#include "gpspositionsensor.h"
#include "manualcontrolcommand.h"
#include "attitudestate.h"
#include "airspeedstate.h"
#include "actuatordesired.h"
#include "flightstatus.h"
#include "systemstats.h"
#include "homelocation.h"
#include "positionstate.h"
#include "velocitystate.h"
#include "taskinfo.h"
#include "mavlink.h"
#include "hwsettings.h"
#include "oplinkstatus.h"
#include "receiverstatus.h"
#include "manualcontrolsettings.h"
#include "custom_types.h"
#include <pios_board_io.h>
#define OPLINK_LOW_RSSI -110
#define OPLINK_HIGH_RSSI -10
// ****************
// Private functions
static void uavoMavlinkBridgeTask(void *parameters);
// ****************
// Private constants
#if defined(PIOS_MAVLINK_STACK_SIZE)
#define STACK_SIZE_BYTES PIOS_MAVLINK_STACK_SIZE
#else
#define STACK_SIZE_BYTES 696
#endif
#define TASK_PRIORITY (tskIDLE_PRIORITY)
#define TASK_RATE_HZ 10
static void mavlink_send_extended_status();
static void mavlink_send_rc_channels();
static void mavlink_send_position();
static void mavlink_send_extra1();
static void mavlink_send_extra2();
static const struct {
uint8_t rate;
void (*handler)();
} mav_rates[] = {
[MAV_DATA_STREAM_EXTENDED_STATUS] = {
.rate = 2, // Hz
.handler = mavlink_send_extended_status,
},
[MAV_DATA_STREAM_RC_CHANNELS] = {
.rate = 5, // Hz
.handler = mavlink_send_rc_channels,
},
[MAV_DATA_STREAM_POSITION] = {
.rate = 2, // Hz
.handler = mavlink_send_position,
},
[MAV_DATA_STREAM_EXTRA1] = {
.rate = 10, // Hz
.handler = mavlink_send_extra1,
},
[MAV_DATA_STREAM_EXTRA2] = {
.rate = 2, // Hz
.handler = mavlink_send_extra2,
},
};
#define MAXSTREAMS NELEMENTS(mav_rates)
// ****************
// Private variables
static bool module_enabled = false;
static uint8_t *stream_ticks;
static mavlink_message_t *mav_msg;
static void updateSettings();
/**
* Initialise the module
* \return -1 if initialisation failed
* \return 0 on success
*/
static int32_t uavoMavlinkBridgeStart(void)
{
if (module_enabled) {
// Start tasks
xTaskHandle taskHandle;
xTaskCreate(uavoMavlinkBridgeTask, "uavoMavlinkBridge", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_UAVOMAVLINKBRIDGE, taskHandle);
return 0;
}
return -1;
}
/**
* Initialise the module
* \return -1 if initialisation failed
* \return 0 on success
*/
static int32_t uavoMavlinkBridgeInitialize(void)
{
if (PIOS_COM_MAVLINK) {
updateSettings();
mav_msg = pios_malloc(sizeof(*mav_msg));
stream_ticks = pios_malloc(MAXSTREAMS);
if (mav_msg && stream_ticks) {
for (unsigned x = 0; x < MAXSTREAMS; ++x) {
stream_ticks[x] = (TASK_RATE_HZ / mav_rates[x].rate);
}
module_enabled = true;
}
}
return 0;
}
MODULE_INITCALL(uavoMavlinkBridgeInitialize, uavoMavlinkBridgeStart);
static void send_message()
{
uint16_t msg_length = MAVLINK_NUM_NON_PAYLOAD_BYTES +
mav_msg->len;
PIOS_COM_SendBuffer(PIOS_COM_MAVLINK, &mav_msg->magic, msg_length);
}
static void mavlink_send_extended_status()
{
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
FlightBatteryStateData batState;
SystemStatsData systemStats;
if (FlightBatteryStateHandle() != NULL) {
FlightBatteryStateGet(&batState);
}
SystemStatsGet(&systemStats);
uint32_t battery_capacity = 0;
if (FlightBatterySettingsHandle() != NULL) {
FlightBatterySettingsCapacityGet(&battery_capacity);
}
int8_t battery_remaining = 0;
if (battery_capacity != 0) {
if (batState.ConsumedEnergy < battery_capacity) {
battery_remaining = 100 - lroundf(batState.ConsumedEnergy / battery_capacity * 100);
}
}
mavlink_msg_sys_status_pack(0, 200, mav_msg,
// onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
0,
// onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
0,
// onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
0,
// load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
(uint16_t)systemStats.CPULoad * 10,
// voltage_battery Battery voltage, in millivolts (1 = 1 millivolt)
lroundf(batState.Voltage * 1000), // No need to check for validity, Voltage reads 0.0 when measurement is not configured,
// current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
lroundf(batState.Current * 100), // Same as for Voltage. 0 means no measurement
// battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery
battery_remaining,
// drop_rate_comm Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
0,
// errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
0,
// errors_count1 Autopilot-specific errors
0,
// errors_count2 Autopilot-specific errors
0,
// errors_count3 Autopilot-specific errors
0,
// errors_count4 Autopilot-specific errors
0);
send_message();
#endif /* PIOS_EXCLUDE_ADVANCED_FEATURES */
}
static void mavlink_send_rc_channels()
{
ManualControlCommandData manualState;
FlightStatusData flightStatus;
SystemStatsData systemStats;
ManualControlCommandGet(&manualState);
FlightStatusGet(&flightStatus);
SystemStatsGet(&systemStats);
uint8_t mavlinkRssi;
ManualControlSettingsChannelGroupsData channelGroups;
ManualControlSettingsChannelGroupsGet(&channelGroups);
#ifdef PIOS_INCLUDE_OPLINKRCVR
if ((channelGroups.Throttle == MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK) ||
(channelGroups.Throttle == MANUALCONTROLSETTINGS_CHANNELGROUPS_OPENLRS)) {
int8_t rssi;
OPLinkStatusRSSIGet(&rssi);
if (rssi < OPLINK_LOW_RSSI) {
rssi = OPLINK_LOW_RSSI;
} else if (rssi > OPLINK_HIGH_RSSI) {
rssi = OPLINK_HIGH_RSSI;
}
mavlinkRssi = ((rssi - OPLINK_LOW_RSSI) * 255) / (OPLINK_HIGH_RSSI - OPLINK_LOW_RSSI);
} else {
#endif /* PIOS_INCLUDE_OPLINKRCVR */
uint8_t quality;
ReceiverStatusQualityGet(&quality);
// MAVLink RSSI's range is 0-255
mavlinkRssi = (quality * 255) / 100;
#ifdef PIOS_INCLUDE_OPLINKRCVR
}
#endif
mavlink_msg_rc_channels_raw_pack(0, 200, mav_msg,
// time_boot_ms Timestamp (milliseconds since system boot)
systemStats.FlightTime,
// port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos.
0,
// chan1_raw RC channel 1 value, in microseconds
manualState.Channel[0],
// chan2_raw RC channel 2 value, in microseconds
manualState.Channel[1],
// chan3_raw RC channel 3 value, in microseconds
manualState.Channel[2],
// chan4_raw RC channel 4 value, in microseconds
manualState.Channel[3],
// chan5_raw RC channel 5 value, in microseconds
manualState.Channel[4],
// chan6_raw RC channel 6 value, in microseconds
manualState.Channel[5],
// chan7_raw RC channel 7 value, in microseconds
manualState.Channel[6],
// chan8_raw RC channel 8 value, in microseconds
manualState.Channel[7],
// rssi Receive signal strength indicator, 0: 0%, 255: 100%
mavlinkRssi);
send_message();
}
static void mavlink_send_position()
{
SystemStatsData systemStats;
SystemStatsGet(&systemStats);
if (GPSPositionSensorHandle() != NULL) {
GPSPositionSensorData gpsPosData;
GPSPositionSensorGet(&gpsPosData);
uint8_t gps_fix_type = 0;
switch (gpsPosData.Status) {
case GPSPOSITIONSENSOR_STATUS_NOGPS:
gps_fix_type = 0;
break;
case GPSPOSITIONSENSOR_STATUS_NOFIX:
gps_fix_type = 1;
break;
case GPSPOSITIONSENSOR_STATUS_FIX2D:
gps_fix_type = 2;
break;
case GPSPOSITIONSENSOR_STATUS_FIX3D:
case GPSPOSITIONSENSOR_STATUS_FIX3DDGNSS:
gps_fix_type = 3;
break;
}
mavlink_msg_gps_raw_int_pack(0, 200, mav_msg,
// time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
(uint64_t)systemStats.FlightTime * 1000,
// fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.
gps_fix_type,
// lat Latitude in 1E7 degrees
gpsPosData.Latitude,
// lon Longitude in 1E7 degrees
gpsPosData.Longitude,
// alt Altitude in 1E3 meters (millimeters) above MSL
gpsPosData.Altitude * 1000,
// eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
gpsPosData.HDOP * 100,
// epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
gpsPosData.VDOP * 100,
// vel GPS ground speed (m/s * 100). If unknown, set to: 65535
gpsPosData.Groundspeed * 100,
// cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535
gpsPosData.Heading * 100,
// satellites_visible Number of satellites visible. If unknown, set to 255
gpsPosData.Satellites);
send_message();
}
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
if (HomeLocationHandle() != NULL) {
HomeLocationData homeLocation;
HomeLocationGet(&homeLocation);
mavlink_msg_gps_global_origin_pack(0, 200, mav_msg,
// latitude Latitude (WGS84), expressed as * 1E7
homeLocation.Latitude,
// longitude Longitude (WGS84), expressed as * 1E7
homeLocation.Longitude,
// altitude Altitude(WGS84), expressed as * 1000
homeLocation.Altitude * 1000);
send_message();
}
#endif /* PIOS_EXCLUDE_ADVANCED_FEATURES */
// TODO add waypoint nav stuff
// wp_target_bearing
// wp_dist = mavlink_msg_nav_controller_output_get_wp_dist(&msg);
// alt_error = mavlink_msg_nav_controller_output_get_alt_error(&msg);
// aspd_error = mavlink_msg_nav_controller_output_get_aspd_error(&msg);
// xtrack_error = mavlink_msg_nav_controller_output_get_xtrack_error(&msg);
// mavlink_msg_nav_controller_output_pack
// wp_number
// mavlink_msg_mission_current_pack
}
static void mavlink_send_extra1()
{
AttitudeStateData attState;
SystemStatsData systemStats;
AttitudeStateGet(&attState);
SystemStatsGet(&systemStats);
mavlink_msg_attitude_pack(0, 200, mav_msg,
// time_boot_ms Timestamp (milliseconds since system boot)
systemStats.FlightTime,
// roll Roll angle (rad)
DEG2RAD(attState.Roll),
// pitch Pitch angle (rad)
DEG2RAD(attState.Pitch),
// yaw Yaw angle (rad)
DEG2RAD(attState.Yaw),
// rollspeed Roll angular speed (rad/s)
0,
// pitchspeed Pitch angular speed (rad/s)
0,
// yawspeed Yaw angular speed (rad/s)
0);
send_message();
}
static inline float Sq(float x)
{
return x * x;
}
#define IS_STAB_MODE(d, m) (((d).Roll == (m)) && ((d).Pitch == (m)))
static void mavlink_send_extra2()
{
float airspeed = 0;
float altitude = 0;
float groundspeed = 0;
float climbrate = 0;
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
if (AirspeedStateHandle() != NULL) {
AirspeedStateTrueAirspeedGet(&airspeed);
}
if (PositionStateHandle() != NULL) {
PositionStateDownGet(&altitude);
altitude *= -1;
}
if (VelocityStateHandle() != NULL) {
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
groundspeed = sqrtf(Sq(velocityState.North) + Sq(velocityState.East));
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;
ActuatorDesiredThrustGet(&thrust);
mavlink_msg_vfr_hud_pack(0, 200, mav_msg,
// airspeed Current airspeed in m/s
airspeed,
// groundspeed Current ground speed in m/s
groundspeed,
// heading Current heading in degrees, in compass units (0..360, 0=north)
heading,
// throttle Current throttle setting in integer percent, 0 to 100
thrust * 100,
// alt Current altitude (MSL), in meters
altitude,
// climb Current climb rate in meters/second
climbrate);
send_message();
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
StabilizationDesiredStabilizationModeData stabModeData;
StabilizationDesiredStabilizationModeGet(&stabModeData);
uint8_t armed_mode = 0;
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) {
armed_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
}
uint8_t custom_mode = CUSTOM_MODE_STAB;
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
custom_mode = CUSTOM_MODE_PHLD;
break;
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
custom_mode = CUSTOM_MODE_RTL;
break;
case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE:
case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
custom_mode = CUSTOM_MODE_AUTO;
break;
case FLIGHTSTATUS_FLIGHTMODE_LAND:
custom_mode = CUSTOM_MODE_LAND;
break;
case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
custom_mode = CUSTOM_MODE_ACRO; // or
break;
case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE:
custom_mode = CUSTOM_MODE_ATUN;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
if (IS_STAB_MODE(stabModeData, STABILIZATIONDESIRED_STABILIZATIONMODE_RATE)
|| IS_STAB_MODE(stabModeData, STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER)) {
custom_mode = CUSTOM_MODE_ACRO;
} else if (IS_STAB_MODE(stabModeData, STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO)) { // this is Acro+ mode
custom_mode = CUSTOM_MODE_ACRO;
} else if (IS_STAB_MODE(stabModeData, STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE)) {
custom_mode = CUSTOM_MODE_SPORT;
} else if ((stabModeData.Thrust == STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD)
|| (stabModeData.Thrust == STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO)) {
custom_mode = CUSTOM_MODE_ALTH;
} else { // looks like stabilized mode, whichever it is..
custom_mode = CUSTOM_MODE_STAB;
}
break;
case FLIGHTSTATUS_FLIGHTMODE_COURSELOCK:
case FLIGHTSTATUS_FLIGHTMODE_VELOCITYROAM:
case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH:
case FLIGHTSTATUS_FLIGHTMODE_ABSOLUTEPOSITION:
case FLIGHTSTATUS_FLIGHTMODE_POI:
custom_mode = CUSTOM_MODE_STAB; // Don't know any better
break;
}
mavlink_msg_heartbeat_pack(0, 200, mav_msg,
// type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
MAV_TYPE_GENERIC,
// autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM
MAV_AUTOPILOT_GENERIC, // or MAV_AUTOPILOT_OPENPILOT
// base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h
armed_mode,
// custom_mode A bitfield for use for autopilot-specific flags.
custom_mode,
// system_status System status flag, see MAV_STATE ENUM
0);
send_message();
}
/**
* Main task. It does not return.
*/
static void uavoMavlinkBridgeTask(__attribute__((unused)) void *parameters)
{
uint32_t lastSysTime;
// Main task loop
lastSysTime = xTaskGetTickCount(); // portTICK_RATE_MS;
while (1) {
vTaskDelayUntil(&lastSysTime, (1000 / TASK_RATE_HZ) / portTICK_RATE_MS);
for (unsigned i = 0; i < MAXSTREAMS; ++i) {
if (!mav_rates[i].rate || !mav_rates[i].handler) {
continue;
}
if (stream_ticks[i] == 0) {
// trigger now
uint8_t rate = mav_rates[i].rate;
if (rate > TASK_RATE_HZ) {
rate = TASK_RATE_HZ;
}
stream_ticks[i] = TASK_RATE_HZ / rate;
mav_rates[i].handler();
}
--stream_ticks[i];
}
}
}
static uint32_t hwsettings_mavlinkspeed_enum_to_baud(uint8_t baud)
{
switch (baud) {
case HWSETTINGS_MAVLINKSPEED_2400:
return 2400;
case HWSETTINGS_MAVLINKSPEED_4800:
return 4800;
case HWSETTINGS_MAVLINKSPEED_9600:
return 9600;
case HWSETTINGS_MAVLINKSPEED_19200:
return 19200;
case HWSETTINGS_MAVLINKSPEED_38400:
return 38400;
case HWSETTINGS_MAVLINKSPEED_57600:
return 57600;
default:
case HWSETTINGS_MAVLINKSPEED_115200:
return 115200;
}
}
static void updateSettings()
{
if (PIOS_COM_MAVLINK) {
// Retrieve settings
HwSettingsMAVLinkSpeedOptions mavlinkSpeed;
HwSettingsMAVLinkSpeedGet(&mavlinkSpeed);
PIOS_COM_ChangeBaud(PIOS_COM_MAVLINK, hwsettings_mavlinkspeed_enum_to_baud(mavlinkSpeed));
}
}
/**
* @}
* @}
*/
Reference in New Issue View Git Blame Copy Permalink