mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-28 09:30:34 +08:00
Merge remote-tracking branch 'upstream/master' into mtecs
This commit is contained in:
@@ -10,8 +10,8 @@ sh /etc/init.d/rc.fw_defaults
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
then
|
||||
param set FW_AIRSPD_MIN 13
|
||||
param set FW_AIRSPD_TRIM 18
|
||||
param set FW_AIRSPD_MAX 40
|
||||
param set FW_AIRSPD_TRIM 15
|
||||
param set FW_AIRSPD_MAX 25
|
||||
param set FW_ATT_TC 0.3
|
||||
param set FW_L1_DAMPING 0.75
|
||||
param set FW_L1_PERIOD 15
|
||||
@@ -23,12 +23,12 @@ then
|
||||
param set FW_P_LIM_MIN -50
|
||||
param set FW_P_RMAX_NEG 0
|
||||
param set FW_P_RMAX_POS 0
|
||||
param set FW_P_ROLLFF 0
|
||||
param set FW_P_ROLLFF 1
|
||||
param set FW_RR_FF 0.5
|
||||
param set FW_RR_I 0.02
|
||||
param set FW_RR_IMAX 0.2
|
||||
param set FW_RR_P 0.08
|
||||
param set FW_R_LIM 70
|
||||
param set FW_R_LIM 50
|
||||
param set FW_R_RMAX 0
|
||||
param set FW_T_HRATE_P 0.01
|
||||
param set FW_T_RLL2THR 15
|
||||
|
||||
@@ -23,7 +23,7 @@ then
|
||||
param set FW_P_LIM_MIN -45
|
||||
param set FW_P_RMAX_NEG 0
|
||||
param set FW_P_RMAX_POS 0
|
||||
param set FW_P_ROLLFF 0
|
||||
param set FW_P_ROLLFF 1
|
||||
param set FW_RR_FF 0.3
|
||||
param set FW_RR_I 0
|
||||
param set FW_RR_IMAX 0.2
|
||||
|
||||
@@ -405,7 +405,7 @@ CONFIG_SIG_SIGWORK=4
|
||||
CONFIG_MAX_TASKS=32
|
||||
CONFIG_MAX_TASK_ARGS=10
|
||||
CONFIG_NPTHREAD_KEYS=4
|
||||
CONFIG_NFILE_DESCRIPTORS=36
|
||||
CONFIG_NFILE_DESCRIPTORS=38
|
||||
CONFIG_NFILE_STREAMS=8
|
||||
CONFIG_NAME_MAX=32
|
||||
CONFIG_PREALLOC_MQ_MSGS=4
|
||||
|
||||
@@ -439,7 +439,7 @@ CONFIG_SIG_SIGWORK=4
|
||||
CONFIG_MAX_TASKS=32
|
||||
CONFIG_MAX_TASK_ARGS=10
|
||||
CONFIG_NPTHREAD_KEYS=4
|
||||
CONFIG_NFILE_DESCRIPTORS=36
|
||||
CONFIG_NFILE_DESCRIPTORS=38
|
||||
CONFIG_NFILE_STREAMS=8
|
||||
CONFIG_NAME_MAX=32
|
||||
CONFIG_PREALLOC_MQ_MSGS=4
|
||||
|
||||
@@ -48,11 +48,14 @@
|
||||
* The recognized number off cells, will be blinked 5 times in purple color.
|
||||
* 2 Cells = 2 blinks
|
||||
* ...
|
||||
* 5 Cells = 5 blinks
|
||||
* 6 Cells = 6 blinks
|
||||
* Now the Application will show the actual selected Flightmode, GPS-Fix and Battery Warnings and Alerts.
|
||||
*
|
||||
* System disarmed:
|
||||
* The BlinkM should lit solid red.
|
||||
* System disarmed and safe:
|
||||
* The BlinkM should light solid cyan.
|
||||
*
|
||||
* System safety off but not armed:
|
||||
* The BlinkM should light flashing orange
|
||||
*
|
||||
* System armed:
|
||||
* One message is made of 4 Blinks and a pause in the same length as the 4 blinks.
|
||||
@@ -67,10 +70,10 @@
|
||||
* (X = on, _=off)
|
||||
*
|
||||
* The first 3 blinks indicates the status of the GPS-Signal (red):
|
||||
* 0-4 satellites = X-X-X-X-_-_-_-_-_-_-
|
||||
* 5 satellites = X-X-_-X-_-_-_-_-_-_-
|
||||
* 6 satellites = X-_-_-X-_-_-_-_-_-_-
|
||||
* >=7 satellites = _-_-_-X-_-_-_-_-_-_-
|
||||
* 0-4 satellites = X-X-X-X-X-_-_-_-_-_-
|
||||
* 5 satellites = X-X-_-X-X-_-_-_-_-_-
|
||||
* 6 satellites = X-_-_-X-X-_-_-_-_-_-
|
||||
* >=7 satellites = _-_-_-X-X-_-_-_-_-_-
|
||||
* If no GPS is found the first 3 blinks are white
|
||||
*
|
||||
* The fourth Blink indicates the Flightmode:
|
||||
@@ -119,6 +122,7 @@
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/safety.h>
|
||||
|
||||
static const float MAX_CELL_VOLTAGE = 4.3f;
|
||||
static const int LED_ONTIME = 120;
|
||||
@@ -166,10 +170,12 @@ private:
|
||||
enum ledColors {
|
||||
LED_OFF,
|
||||
LED_RED,
|
||||
LED_ORANGE,
|
||||
LED_YELLOW,
|
||||
LED_PURPLE,
|
||||
LED_GREEN,
|
||||
LED_BLUE,
|
||||
LED_CYAN,
|
||||
LED_WHITE,
|
||||
LED_AMBER
|
||||
};
|
||||
@@ -380,6 +386,7 @@ BlinkM::led()
|
||||
static int vehicle_control_mode_sub_fd;
|
||||
static int vehicle_gps_position_sub_fd;
|
||||
static int actuator_armed_sub_fd;
|
||||
static int safety_sub_fd;
|
||||
|
||||
static int num_of_cells = 0;
|
||||
static int detected_cells_runcount = 0;
|
||||
@@ -402,16 +409,20 @@ BlinkM::led()
|
||||
|
||||
if(!topic_initialized) {
|
||||
vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
|
||||
orb_set_interval(vehicle_status_sub_fd, 1000);
|
||||
orb_set_interval(vehicle_status_sub_fd, 250);
|
||||
|
||||
vehicle_control_mode_sub_fd = orb_subscribe(ORB_ID(vehicle_control_mode));
|
||||
orb_set_interval(vehicle_control_mode_sub_fd, 1000);
|
||||
orb_set_interval(vehicle_control_mode_sub_fd, 250);
|
||||
|
||||
actuator_armed_sub_fd = orb_subscribe(ORB_ID(actuator_armed));
|
||||
orb_set_interval(actuator_armed_sub_fd, 1000);
|
||||
orb_set_interval(actuator_armed_sub_fd, 250);
|
||||
|
||||
vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position));
|
||||
orb_set_interval(vehicle_gps_position_sub_fd, 1000);
|
||||
orb_set_interval(vehicle_gps_position_sub_fd, 250);
|
||||
|
||||
/* Subscribe to safety topic */
|
||||
safety_sub_fd = orb_subscribe(ORB_ID(safety));
|
||||
orb_set_interval(safety_sub_fd, 250);
|
||||
|
||||
topic_initialized = true;
|
||||
}
|
||||
@@ -433,7 +444,9 @@ BlinkM::led()
|
||||
if(num_of_cells > 4) {
|
||||
t_led_color[4] = LED_PURPLE;
|
||||
}
|
||||
t_led_color[5] = LED_OFF;
|
||||
if(num_of_cells > 5) {
|
||||
t_led_color[5] = LED_PURPLE;
|
||||
}
|
||||
t_led_color[6] = LED_OFF;
|
||||
t_led_color[7] = LED_OFF;
|
||||
t_led_blink = LED_BLINK;
|
||||
@@ -467,14 +480,17 @@ BlinkM::led()
|
||||
struct vehicle_control_mode_s vehicle_control_mode;
|
||||
struct actuator_armed_s actuator_armed;
|
||||
struct vehicle_gps_position_s vehicle_gps_position_raw;
|
||||
struct safety_s safety;
|
||||
|
||||
memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw));
|
||||
memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw));
|
||||
memset(&safety, 0, sizeof(safety));
|
||||
|
||||
bool new_data_vehicle_status;
|
||||
bool new_data_vehicle_control_mode;
|
||||
bool new_data_actuator_armed;
|
||||
bool new_data_vehicle_gps_position;
|
||||
bool new_data_safety;
|
||||
|
||||
orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);
|
||||
|
||||
@@ -520,7 +536,12 @@ BlinkM::led()
|
||||
no_data_vehicle_gps_position = 3;
|
||||
}
|
||||
|
||||
/* update safety topic */
|
||||
orb_check(safety_sub_fd, &new_data_safety);
|
||||
|
||||
if (new_data_safety) {
|
||||
orb_copy(ORB_ID(safety), safety_sub_fd, &safety);
|
||||
}
|
||||
|
||||
/* get number of used satellites in navigation */
|
||||
num_of_used_sats = 0;
|
||||
@@ -541,19 +562,7 @@ BlinkM::led()
|
||||
printf("<blinkm> cells found:%d\n", num_of_cells);
|
||||
|
||||
} else {
|
||||
if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
|
||||
/* LED Pattern for battery low warning */
|
||||
led_color_1 = LED_YELLOW;
|
||||
led_color_2 = LED_YELLOW;
|
||||
led_color_3 = LED_YELLOW;
|
||||
led_color_4 = LED_YELLOW;
|
||||
led_color_5 = LED_YELLOW;
|
||||
led_color_6 = LED_YELLOW;
|
||||
led_color_7 = LED_YELLOW;
|
||||
led_color_8 = LED_YELLOW;
|
||||
led_blink = LED_BLINK;
|
||||
|
||||
} else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
|
||||
if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
|
||||
/* LED Pattern for battery critical alerting */
|
||||
led_color_1 = LED_RED;
|
||||
led_color_2 = LED_RED;
|
||||
@@ -565,21 +574,56 @@ BlinkM::led()
|
||||
led_color_8 = LED_RED;
|
||||
led_blink = LED_BLINK;
|
||||
|
||||
} else if(vehicle_status_raw.rc_signal_lost) {
|
||||
/* LED Pattern for FAILSAFE */
|
||||
led_color_1 = LED_BLUE;
|
||||
led_color_2 = LED_BLUE;
|
||||
led_color_3 = LED_BLUE;
|
||||
led_color_4 = LED_BLUE;
|
||||
led_color_5 = LED_BLUE;
|
||||
led_color_6 = LED_BLUE;
|
||||
led_color_7 = LED_BLUE;
|
||||
led_color_8 = LED_BLUE;
|
||||
led_blink = LED_BLINK;
|
||||
|
||||
} else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
|
||||
/* LED Pattern for battery low warning */
|
||||
led_color_1 = LED_YELLOW;
|
||||
led_color_2 = LED_YELLOW;
|
||||
led_color_3 = LED_YELLOW;
|
||||
led_color_4 = LED_YELLOW;
|
||||
led_color_5 = LED_YELLOW;
|
||||
led_color_6 = LED_YELLOW;
|
||||
led_color_7 = LED_YELLOW;
|
||||
led_color_8 = LED_YELLOW;
|
||||
led_blink = LED_BLINK;
|
||||
|
||||
} else {
|
||||
/* no battery warnings here */
|
||||
|
||||
if(actuator_armed.armed == false) {
|
||||
/* system not armed */
|
||||
led_color_1 = LED_RED;
|
||||
led_color_2 = LED_RED;
|
||||
led_color_3 = LED_RED;
|
||||
led_color_4 = LED_RED;
|
||||
led_color_5 = LED_RED;
|
||||
led_color_6 = LED_RED;
|
||||
led_color_7 = LED_RED;
|
||||
led_color_8 = LED_RED;
|
||||
led_blink = LED_NOBLINK;
|
||||
|
||||
if(safety.safety_off){
|
||||
led_color_1 = LED_ORANGE;
|
||||
led_color_2 = LED_ORANGE;
|
||||
led_color_3 = LED_ORANGE;
|
||||
led_color_4 = LED_ORANGE;
|
||||
led_color_5 = LED_ORANGE;
|
||||
led_color_6 = LED_ORANGE;
|
||||
led_color_7 = LED_ORANGE;
|
||||
led_color_8 = LED_ORANGE;
|
||||
led_blink = LED_BLINK;
|
||||
}else{
|
||||
led_color_1 = LED_CYAN;
|
||||
led_color_2 = LED_CYAN;
|
||||
led_color_3 = LED_CYAN;
|
||||
led_color_4 = LED_CYAN;
|
||||
led_color_5 = LED_CYAN;
|
||||
led_color_6 = LED_CYAN;
|
||||
led_color_7 = LED_CYAN;
|
||||
led_color_8 = LED_CYAN;
|
||||
led_blink = LED_NOBLINK;
|
||||
}
|
||||
} else {
|
||||
/* armed system - initial led pattern */
|
||||
led_color_1 = LED_RED;
|
||||
@@ -593,23 +637,22 @@ BlinkM::led()
|
||||
led_blink = LED_BLINK;
|
||||
|
||||
if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) {
|
||||
|
||||
//XXX please check
|
||||
if (vehicle_control_mode.flag_control_position_enabled)
|
||||
/* indicate main control state */
|
||||
if (vehicle_status_raw.main_state == MAIN_STATE_EASY)
|
||||
led_color_4 = LED_GREEN;
|
||||
else if (vehicle_control_mode.flag_control_velocity_enabled)
|
||||
else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO)
|
||||
led_color_4 = LED_BLUE;
|
||||
else if (vehicle_control_mode.flag_control_attitude_enabled)
|
||||
else if (vehicle_status_raw.main_state == MAIN_STATE_SEATBELT)
|
||||
led_color_4 = LED_YELLOW;
|
||||
else if (vehicle_control_mode.flag_control_manual_enabled)
|
||||
led_color_4 = LED_AMBER;
|
||||
else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL)
|
||||
led_color_4 = LED_WHITE;
|
||||
else
|
||||
led_color_4 = LED_OFF;
|
||||
|
||||
led_color_5 = led_color_4;
|
||||
}
|
||||
|
||||
if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) {
|
||||
/* handling used sat�s */
|
||||
/* handling used satus */
|
||||
if(num_of_used_sats >= 7) {
|
||||
led_color_1 = LED_OFF;
|
||||
led_color_2 = LED_OFF;
|
||||
@@ -690,8 +733,11 @@ void BlinkM::setLEDColor(int ledcolor) {
|
||||
case LED_RED: // red
|
||||
set_rgb(255,0,0);
|
||||
break;
|
||||
case LED_ORANGE: // orange
|
||||
set_rgb(255,150,0);
|
||||
break;
|
||||
case LED_YELLOW: // yellow
|
||||
set_rgb(255,70,0);
|
||||
set_rgb(200,200,0);
|
||||
break;
|
||||
case LED_PURPLE: // purple
|
||||
set_rgb(255,0,255);
|
||||
@@ -702,11 +748,14 @@ void BlinkM::setLEDColor(int ledcolor) {
|
||||
case LED_BLUE: // blue
|
||||
set_rgb(0,0,255);
|
||||
break;
|
||||
case LED_CYAN: // cyan
|
||||
set_rgb(0,128,128);
|
||||
break;
|
||||
case LED_WHITE: // white
|
||||
set_rgb(255,255,255);
|
||||
break;
|
||||
case LED_AMBER: // amber
|
||||
set_rgb(255,20,0);
|
||||
set_rgb(255,65,0);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -46,6 +46,10 @@
|
||||
|
||||
#define RANGE_FINDER_DEVICE_PATH "/dev/range_finder"
|
||||
|
||||
enum RANGE_FINDER_TYPE {
|
||||
RANGE_FINDER_TYPE_LASER = 0,
|
||||
};
|
||||
|
||||
/**
|
||||
* range finder report structure. Reads from the device must be in multiples of this
|
||||
* structure.
|
||||
@@ -53,8 +57,11 @@
|
||||
struct range_finder_report {
|
||||
uint64_t timestamp;
|
||||
uint64_t error_count;
|
||||
float distance; /** in meters */
|
||||
uint8_t valid; /** 1 == within sensor range, 0 = outside sensor range */
|
||||
unsigned type; /**< type, following RANGE_FINDER_TYPE enum */
|
||||
float distance; /**< in meters */
|
||||
float minimum_distance; /**< minimum distance the sensor can measure */
|
||||
float maximum_distance; /**< maximum distance the sensor can measure */
|
||||
uint8_t valid; /**< 1 == within sensor range, 0 = outside sensor range */
|
||||
};
|
||||
|
||||
/*
|
||||
|
||||
@@ -201,9 +201,14 @@ PX4IO_Uploader::upload(const char *filenames[])
|
||||
continue;
|
||||
}
|
||||
|
||||
if (bl_rev <= 2)
|
||||
if (bl_rev <= 2) {
|
||||
ret = verify_rev2(fw_size);
|
||||
else if(bl_rev == 3) {
|
||||
} else if(bl_rev == 3) {
|
||||
ret = verify_rev3(fw_size);
|
||||
} else {
|
||||
/* verify rev 4 and higher still uses the same approach and
|
||||
* every version *needs* to be verified.
|
||||
*/
|
||||
ret = verify_rev3(fw_size);
|
||||
}
|
||||
|
||||
|
||||
@@ -959,7 +959,7 @@ FixedwingEstimator::task_main()
|
||||
}
|
||||
|
||||
// Publish results
|
||||
if (_initialized) {
|
||||
if (_initialized && (check == OK)) {
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -832,10 +832,10 @@ void Mavlink::publish_mission()
|
||||
{
|
||||
/* Initialize mission publication if necessary */
|
||||
if (_mission_pub < 0) {
|
||||
_mission_pub = orb_advertise(ORB_ID(mission), &mission);
|
||||
_mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(mission), _mission_pub, &mission);
|
||||
orb_publish(ORB_ID(offboard_mission), _mission_pub, &mission);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1953,6 +1953,7 @@ Mavlink::task_main(int argc, char *argv[])
|
||||
configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult);
|
||||
configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult);
|
||||
configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult);
|
||||
configure_stream("DISTANCE_SENSOR", 0.5f);
|
||||
break;
|
||||
|
||||
case MAVLINK_MODE_CAMERA:
|
||||
|
||||
@@ -72,6 +72,7 @@
|
||||
#include <uORB/topics/navigation_capabilities.h>
|
||||
#include <drivers/drv_rc_input.h>
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
#include <drivers/drv_range_finder.h>
|
||||
|
||||
#include "mavlink_messages.h"
|
||||
|
||||
@@ -1312,6 +1313,51 @@ protected:
|
||||
}
|
||||
};
|
||||
|
||||
class MavlinkStreamDistanceSensor : public MavlinkStream
|
||||
{
|
||||
public:
|
||||
const char *get_name()
|
||||
{
|
||||
return "DISTANCE_SENSOR";
|
||||
}
|
||||
|
||||
MavlinkStream *new_instance()
|
||||
{
|
||||
return new MavlinkStreamDistanceSensor();
|
||||
}
|
||||
|
||||
private:
|
||||
MavlinkOrbSubscription *range_sub;
|
||||
struct range_finder_report *range;
|
||||
|
||||
protected:
|
||||
void subscribe(Mavlink *mavlink)
|
||||
{
|
||||
range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder));
|
||||
range = (struct range_finder_report *)range_sub->get_data();
|
||||
}
|
||||
|
||||
void send(const hrt_abstime t)
|
||||
{
|
||||
(void)range_sub->update(t);
|
||||
|
||||
uint8_t type;
|
||||
|
||||
switch (range->type) {
|
||||
case RANGE_FINDER_TYPE_LASER:
|
||||
type = MAV_DISTANCE_SENSOR_LASER;
|
||||
break;
|
||||
}
|
||||
|
||||
uint8_t id = 0;
|
||||
uint8_t orientation = 0;
|
||||
uint8_t covariance = 20;
|
||||
|
||||
mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation,
|
||||
range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance);
|
||||
}
|
||||
};
|
||||
|
||||
MavlinkStream *streams_list[] = {
|
||||
new MavlinkStreamHeartbeat(),
|
||||
new MavlinkStreamSysStatus(),
|
||||
@@ -1338,6 +1384,7 @@ MavlinkStream *streams_list[] = {
|
||||
new MavlinkStreamAttitudeControls(),
|
||||
new MavlinkStreamNamedValueFloat(),
|
||||
new MavlinkStreamCameraCapture(),
|
||||
new MavlinkStreamDistanceSensor(),
|
||||
new MavlinkStreamViconPositionEstimate(),
|
||||
nullptr
|
||||
};
|
||||
|
||||
@@ -47,4 +47,4 @@ SRCS += mavlink_main.cpp \
|
||||
|
||||
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
|
||||
|
||||
MAXOPTIMIZATION = -Os
|
||||
MAXOPTIMIZATION = -Os
|
||||
|
||||
@@ -510,7 +510,7 @@ Navigator::offboard_mission_update(bool isrotaryWing)
|
||||
{
|
||||
struct mission_s offboard_mission;
|
||||
|
||||
if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) {
|
||||
if (orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &offboard_mission) == OK) {
|
||||
|
||||
/* Check mission feasibility, for now do not handle the return value,
|
||||
* however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
|
||||
@@ -543,7 +543,7 @@ Navigator::onboard_mission_update()
|
||||
{
|
||||
struct mission_s onboard_mission;
|
||||
|
||||
if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) {
|
||||
if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) {
|
||||
|
||||
_mission.set_onboard_mission_count(onboard_mission.count);
|
||||
_mission.set_current_onboard_mission_index(onboard_mission.current_index);
|
||||
@@ -611,7 +611,7 @@ Navigator::task_main()
|
||||
* do subscriptions
|
||||
*/
|
||||
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||
_offboard_mission_sub = orb_subscribe(ORB_ID(mission));
|
||||
_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
|
||||
_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
|
||||
_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
|
||||
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
|
||||
@@ -908,9 +908,6 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
hrt_abstime barometer_timestamp = 0;
|
||||
hrt_abstime differential_pressure_timestamp = 0;
|
||||
|
||||
/* track changes in distance status */
|
||||
bool dist_bottom_present = false;
|
||||
|
||||
/* enable logging on start if needed */
|
||||
if (log_on_start) {
|
||||
/* check GPS topic to get GPS time */
|
||||
@@ -1099,6 +1096,8 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
log_msg.body.log_LPOS.x = buf.local_pos.x;
|
||||
log_msg.body.log_LPOS.y = buf.local_pos.y;
|
||||
log_msg.body.log_LPOS.z = buf.local_pos.z;
|
||||
log_msg.body.log_LPOS.ground_dist = buf.local_pos.dist_bottom;
|
||||
log_msg.body.log_LPOS.ground_dist_rate = buf.local_pos.dist_bottom_rate;
|
||||
log_msg.body.log_LPOS.vx = buf.local_pos.vx;
|
||||
log_msg.body.log_LPOS.vy = buf.local_pos.vy;
|
||||
log_msg.body.log_LPOS.vz = buf.local_pos.vz;
|
||||
@@ -1108,19 +1107,8 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0);
|
||||
log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0);
|
||||
log_msg.body.log_LPOS.landed = buf.local_pos.landed;
|
||||
log_msg.body.log_LPOS.ground_dist_flags = (buf.local_pos.dist_bottom_valid ? 1 : 0);
|
||||
LOGBUFFER_WRITE_AND_COUNT(LPOS);
|
||||
|
||||
if (buf.local_pos.dist_bottom_valid) {
|
||||
dist_bottom_present = true;
|
||||
}
|
||||
|
||||
if (dist_bottom_present) {
|
||||
log_msg.msg_type = LOG_DIST_MSG;
|
||||
log_msg.body.log_DIST.bottom = buf.local_pos.dist_bottom;
|
||||
log_msg.body.log_DIST.bottom_rate = buf.local_pos.dist_bottom_rate;
|
||||
log_msg.body.log_DIST.flags = (buf.local_pos.dist_bottom_valid ? 1 : 0);
|
||||
LOGBUFFER_WRITE_AND_COUNT(DIST);
|
||||
}
|
||||
}
|
||||
|
||||
/* --- LOCAL POSITION SETPOINT --- */
|
||||
|
||||
@@ -101,6 +101,8 @@ struct log_LPOS_s {
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
float ground_dist;
|
||||
float ground_dist_rate;
|
||||
float vx;
|
||||
float vy;
|
||||
float vz;
|
||||
@@ -110,6 +112,7 @@ struct log_LPOS_s {
|
||||
uint8_t xy_flags;
|
||||
uint8_t z_flags;
|
||||
uint8_t landed;
|
||||
uint8_t ground_dist_flags;
|
||||
};
|
||||
|
||||
/* --- LPSP - LOCAL POSITION SETPOINT --- */
|
||||
@@ -343,7 +346,7 @@ static const struct log_format_s log_formats[] = {
|
||||
LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
|
||||
LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
|
||||
LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"),
|
||||
LOG_FORMAT(LPOS, "ffffffLLfBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"),
|
||||
LOG_FORMAT(LPOS, "ffffffffLLfBBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"),
|
||||
LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
|
||||
LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
|
||||
LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
|
||||
|
||||
@@ -127,7 +127,7 @@ ORB_DEFINE(position_setpoint_triplet, struct position_setpoint_triplet_s);
|
||||
ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s);
|
||||
|
||||
#include "topics/mission.h"
|
||||
ORB_DEFINE(mission, struct mission_s);
|
||||
ORB_DEFINE(offboard_mission, struct mission_s);
|
||||
ORB_DEFINE(onboard_mission, struct mission_s);
|
||||
|
||||
#include "topics/mission_result.h"
|
||||
|
||||
@@ -105,7 +105,7 @@ struct mission_s
|
||||
*/
|
||||
|
||||
/* register this as object request broker structure */
|
||||
ORB_DECLARE(mission);
|
||||
ORB_DECLARE(offboard_mission);
|
||||
ORB_DECLARE(onboard_mission);
|
||||
|
||||
#endif
|
||||
|
||||
Reference in New Issue
Block a user