fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 17:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

vehicle_global_position: remove velocity fields (duplicates of local vx, vy, vz)

Browse Source 
* attitude_estimator_q: get velocity from local position (if available)
This commit is contained in:
Daniel Agar
2020-03-11 23:57:43 -04:00
committed by GitHub
parent 5d33b9e999
commit a89b69b0ea
19 changed files with 98 additions and 79 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/telemetry/frsky_telemetry/frsky_data.cpp
+5 -2
View File
@@ -55,6 +55,7 @@
#include <uORB/topics/battery_status.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_status.h>
@@ -68,6 +69,7 @@ struct frsky_subscription_data_s {
uORB::SubscriptionData<battery_status_s> battery_status_sub{ORB_ID(battery_status)};
uORB::SubscriptionData<sensor_combined_s> sensor_combined_sub{ORB_ID(sensor_combined)};
uORB::SubscriptionData<vehicle_air_data_s> vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
uORB::SubscriptionData<vehicle_local_position_s> vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::SubscriptionData<vehicle_global_position_s> vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
uORB::SubscriptionData<vehicle_gps_position_s> vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
uORB::SubscriptionData<vehicle_status_s> vehicle_status_sub{ORB_ID(vehicle_status)};
@@ -149,6 +151,7 @@ void frsky_update_topics()
subscription_data->battery_status_sub.update();
subscription_data->sensor_combined_sub.update();
subscription_data->vehicle_air_data_sub.update();
subscription_data->vehicle_local_position_sub.update();
subscription_data->vehicle_global_position_sub.update();
subscription_data->vehicle_gps_position_sub.update();
subscription_data->vehicle_status_sub.update();
@@ -199,6 +202,7 @@ static float frsky_format_gps(float dec)
void frsky_send_frame2(int uart)
{
const vehicle_global_position_s &gpos = subscription_data->vehicle_global_position_sub.get();
const vehicle_local_position_s &lpos = subscription_data->vehicle_local_position_sub.get();
const battery_status_s &battery_status = subscription_data->battery_status_sub.get();
const vehicle_gps_position_s &gps = subscription_data->vehicle_gps_position_sub.get();
@@ -218,8 +222,7 @@ void frsky_send_frame2(int uart)
lat_ns = (gpos.lat < 0) ? 'S' : 'N';
lon = frsky_format_gps(fabsf(gpos.lon));
lon_ew = (gpos.lon < 0) ? 'W' : 'E';
speed = sqrtf(gpos.vel_n * gpos.vel_n + gpos.vel_e * gpos.vel_e)
* 25.0f / 46.0f;
speed = sqrtf(lpos.vx * lpos.vx + lpos.vy * lpos.vy) * 25.0f / 46.0f;
alt = gpos.alt;
}
src/drivers/telemetry/frsky_telemetry/sPort_data.cpp
+4 -4
View File
@@ -204,10 +204,10 @@ void sPort_send_ALT(int uart)
// verified scaling for "calculated" option
void sPort_send_SPD(int uart)
{
const vehicle_global_position_s &global_pos = s_port_subscription_data->vehicle_global_position_sub.get();
const vehicle_local_position_s &local_pos = s_port_subscription_data->vehicle_local_position_sub.get();
/* send data for A2 */
float speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e);
float speed = sqrtf(local_pos.vx * local_pos.vx + local_pos.vy * local_pos.vy);
uint32_t ispeed = (int)(10 * speed);
sPort_send_data(uart, SMARTPORT_ID_GPS_SPD, ispeed);
}
@@ -297,10 +297,10 @@ void sPort_send_GPS_TIME(int uart)
void sPort_send_GPS_SPD(int uart)
{
const vehicle_global_position_s &global_pos = s_port_subscription_data->vehicle_global_position_sub.get();
const vehicle_local_position_s &local_pos = s_port_subscription_data->vehicle_local_position_sub.get();
/* send 100 * knots */
float speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e);
float speed = sqrtf(local_pos.vx * local_pos.vx + local_pos.vy * local_pos.vy);
uint32_t ispeed = (int)(1944 * speed);
sPort_send_data(uart, SMARTPORT_ID_GPS_SPD, ispeed);
}