Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-20 20:03:54 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

Fixed SPort heading scale and sourced from vehicle_local_pos

Browse Source
This commit is contained in:
Fredmcc
2017-02-12 17:44:46 +01:00
committed by Lorenz Meier
parent 5290e6cfe1
commit 90a05a0f55
1 changed files with 19 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/frsky_telemetry/sPort_data.c
+19 -2
View File
@@ -52,21 +52,25 @@
#include <uORB/topics/battery_status.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <drivers/drv_hrt.h>
#define frac(f) (f - (int)f)
static int sensor_sub = -1;
static int global_position_sub = -1;
static int local_position_sub = -1;
static int battery_status_sub = -1;
static int vehicle_status_sub = -1;
static int gps_position_sub = -1;
static struct sensor_combined_s *sensor_combined;
static struct vehicle_global_position_s *global_pos;
static struct vehicle_local_position_s *local_pos;
static struct battery_status_s *battery_status;
static struct vehicle_status_s *vehicle_status;
static struct vehicle_gps_position_s *gps_position;
@@ -80,12 +84,13 @@ bool sPort_init()
sensor_combined = malloc(sizeof(struct sensor_combined_s));
global_pos = malloc(sizeof(struct vehicle_global_position_s));
local_pos = malloc(sizeof(struct vehicle_local_position_s));
battery_status = malloc(sizeof(struct battery_status_s));
vehicle_status = malloc(sizeof(struct vehicle_status_s));
gps_position = malloc(sizeof(struct vehicle_gps_position_s));
if (sensor_combined == NULL || global_pos == NULL || battery_status == NULL || vehicle_status == NULL
if (sensor_combined == NULL || global_pos == NULL || local_pos == NULL || battery_status == NULL || vehicle_status == NULL
|| gps_position == NULL) {
return false;
}
@@ -93,6 +98,7 @@ bool sPort_init()
sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
battery_status_sub = orb_subscribe(ORB_ID(battery_status));
vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
@@ -104,6 +110,7 @@ void sPort_deinit()
{
free(sensor_combined);
free(global_pos);
free(local_pos);
free(battery_status);
free(vehicle_status);
free(gps_position);
@@ -133,6 +140,13 @@ void sPort_update_topics()
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, global_pos);
}
/* get a local copy of the local position data */
orb_check(local_position_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_local_position), local_position_sub, local_pos);
}
/* get a local copy of the vehicle status data */
orb_check(vehicle_status_sub, &updated);
@@ -146,6 +160,7 @@ void sPort_update_topics()
if (updated) {
orb_copy(ORB_ID(vehicle_gps_position), gps_position_sub, gps_position);
}
}
@@ -302,7 +317,9 @@ void sPort_send_GPS_CRS(int uart)
/* send course */
/* convert to 30 bit signed magnitude degrees*6E5 with MSb = 1 and bit 30=sign */
uint32_t iYaw = 100 * global_pos->yaw;
int32_t iYaw = local_pos->yaw * 18000.0f / M_PI_F;
if (iYaw < 0) iYaw += 36000;
sPort_send_data(uart, SMARTPORT_ID_GPS_CRS, iYaw);
}