yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues 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

21
src/drivers/frsky_telemetry/sPort_data.c
View File

@@ -52,21 +52,25 @@
#include <uORB/topics/battery_status.h> #include <uORB/topics/battery_status.h>
#include <uORB/topics/sensor_combined.h> #include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_global_position.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_status.h>
#include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_gps_position.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#define frac(f) (f - (int)f) #define frac(f) (f - (int)f)
static int sensor_sub = -1; static int sensor_sub = -1;
static int global_position_sub = -1; static int global_position_sub = -1;
static int local_position_sub = -1;
static int battery_status_sub = -1; static int battery_status_sub = -1;
static int vehicle_status_sub = -1; static int vehicle_status_sub = -1;
static int gps_position_sub = -1; static int gps_position_sub = -1;
static struct sensor_combined_s *sensor_combined; static struct sensor_combined_s *sensor_combined;
static struct vehicle_global_position_s *global_pos; 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 battery_status_s *battery_status;
static struct vehicle_status_s *vehicle_status; static struct vehicle_status_s *vehicle_status;
static struct vehicle_gps_position_s *gps_position; static struct vehicle_gps_position_s *gps_position;
@@ -80,12 +84,13 @@ bool sPort_init()
sensor_combined = malloc(sizeof(struct sensor_combined_s)); sensor_combined = malloc(sizeof(struct sensor_combined_s));
global_pos = malloc(sizeof(struct vehicle_global_position_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)); battery_status = malloc(sizeof(struct battery_status_s));
vehicle_status = malloc(sizeof(struct vehicle_status_s)); vehicle_status = malloc(sizeof(struct vehicle_status_s));
gps_position = malloc(sizeof(struct vehicle_gps_position_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) { || gps_position == NULL) {
return false; return false;
} }
@@ -93,6 +98,7 @@ bool sPort_init()
sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); 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)); battery_status_sub = orb_subscribe(ORB_ID(battery_status));
vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
@@ -104,6 +110,7 @@ void sPort_deinit()
{ {
free(sensor_combined); free(sensor_combined);
free(global_pos); free(global_pos);
free(local_pos);
free(battery_status); free(battery_status);
free(vehicle_status); free(vehicle_status);
free(gps_position); free(gps_position);
@@ -133,6 +140,13 @@ void sPort_update_topics()
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, global_pos); 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 */ /* get a local copy of the vehicle status data */
orb_check(vehicle_status_sub, &updated); orb_check(vehicle_status_sub, &updated);
@@ -146,6 +160,7 @@ void sPort_update_topics()
if (updated) { if (updated) {
orb_copy(ORB_ID(vehicle_gps_position), gps_position_sub, gps_position); 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 */ /* send course */
/* convert to 30 bit signed magnitude degrees*6E5 with MSb = 1 and bit 30=sign */ /* 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); sPort_send_data(uart, SMARTPORT_ID_GPS_CRS, iYaw);
} }