dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'navigator_cleanup' of github.com:PX4/Firmware into navigator_cleanup

This commit is contained in:
Julian Oes 2014-04-26 11:44:09 +02:00
parent c0d4ecf2b6 fd56c15a20
commit 3411d58dfc
17 changed files with 244 additions and 132 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ROMFS/px4fmu_common/init.d/3031_phantom
View File

@ -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

2
ROMFS/px4fmu_common/init.d/3032_skywalker_x5
View File

@ -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

50
ROMFS/px4fmu_common/init.d/rcS
View File

@ -20,7 +20,7 @@ echo "[init] Looking for microSD..."
if mount -t vfat /dev/mmcsd0 /fs/microsd
then
set LOG_FILE /fs/microsd/bootlog.txt
echo "[init] microSD card mounted at /fs/microsd"
echo "[init] microSD mounted: /fs/microsd"
# Start playing the startup tune
tone_alarm start
else
@ -83,9 +83,9 @@ then
param select $PARAM_FILE
if param load
then
echo "[init] Parameters loaded: $PARAM_FILE"
echo "[init] Params loaded: $PARAM_FILE"
else
echo "[init] ERROR: Parameters loading failed: $PARAM_FILE"
echo "[init] ERROR: Params loading failed: $PARAM_FILE"
fi
#
@ -146,7 +146,7 @@ then
#
if param compare SYS_AUTOSTART 0
then
echo "[init] Don't try to find autostart script"
echo "[init] No autostart"
else
sh /etc/init.d/rc.autostart
fi
@ -156,10 +156,10 @@ then
#
if [ -f $CONFIG_FILE ]
then
echo "[init] Reading config: $CONFIG_FILE"
echo "[init] Config: $CONFIG_FILE"
sh $CONFIG_FILE
else
echo "[init] Config file not found: $CONFIG_FILE"
echo "[init] Config not found: $CONFIG_FILE"
fi
#
@ -264,10 +264,10 @@ then
then
set FMU_MODE serial
fi
else
# Try to get an USB console if not in HIL mode
nshterm /dev/ttyACM0 &
fi
# Try to get an USB console
nshterm /dev/ttyACM0 &
#
# Start the Commander (needs to be this early for in-air-restarts)
@ -401,23 +401,17 @@ then
#
if [ $MAVLINK_FLAGS == default ]
then
if [ $HIL == yes ]
# Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s
if [ $TTYS1_BUSY == yes ]
then
sleep 1
set MAVLINK_FLAGS "-r 10000 -d /dev/ttyACM0"
# Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else
set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0"
# Exit from nsh to free port for mavlink
set EXIT_ON_END yes
else
# Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s
if [ $TTYS1_BUSY == yes ]
then
# Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else
set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0"
# Exit from nsh to free port for mavlink
set EXIT_ON_END yes
else
# Start MAVLink on default port: ttyS1
set MAVLINK_FLAGS "-r 1000"
fi
# Start MAVLink on default port: ttyS1
set MAVLINK_FLAGS "-r 1000"
fi
fi
@ -436,17 +430,15 @@ then
#
# Sensors, Logging, GPS
#
echo "[init] Start sensors"
sh /etc/init.d/rc.sensors
if [ $HIL == no ]
then
echo "[init] Start logging"
sh /etc/init.d/rc.logging
echo "[init] Start GPS"
gps start
fi
gps start
#
# Start up ARDrone Motor interface
@ -561,7 +553,7 @@ then
echo "[init] Starting addons script: $EXTRAS_FILE"
sh $EXTRAS_FILE
else
echo "[init] Addons script not found: $EXTRAS_FILE"
echo "[init] No addons script: $EXTRAS_FILE"
fi
if [ $EXIT_ON_END == yes ]

2
nuttx-configs/px4fmu-v1/nsh/defconfig
View File

@ -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

2
nuttx-configs/px4fmu-v2/nsh/defconfig
View File

@ -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

141
src/drivers/blinkm/blinkm.cpp
View File

@ -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<EFBFBD>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;
}
}

11
src/drivers/drv_range_finder.h
View File

@ -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 */
};
/*

9
src/drivers/px4io/px4io_uploader.cpp
View File

@ -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);
}

2
src/modules/commander/commander.cpp
View File

@ -969,7 +969,7 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
/* only consider battery voltage if system has been running 2s and battery voltage is valid */
if (status.hil_state == HIL_STATE_OFF && hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
status.battery_voltage = battery.voltage_filtered_v;
status.battery_current = battery.current_a;
status.condition_battery_voltage_valid = true;

19
src/modules/dataman/dataman.c
View File

@ -44,7 +44,9 @@
#include <stdlib.h>
#include <fcntl.h>
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
#include <queue.h>
#include <string.h>
#include "dataman.h"
@ -594,6 +596,20 @@ task_main(int argc, char *argv[])
sem_init(&g_work_queued_sema, 1, 0);
/* See if the data manage file exists and is a multiple of the sector size */
g_task_fd = open(k_data_manager_device_path, O_RDONLY | O_BINARY);
if (g_task_fd >= 0) {
/* File exists, check its size */
int file_size = lseek(g_task_fd, 0, SEEK_END);
if ((file_size % k_sector_size) != 0) {
warnx("Incompatible data manager file %s, resetting it", k_data_manager_device_path);
close(g_task_fd);
unlink(k_data_manager_device_path);
}
else
close(g_task_fd);
}
/* Open or create the data manager file */
g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY);
@ -603,7 +619,7 @@ task_main(int argc, char *argv[])
return -1;
}
if (lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) {
if ((unsigned)lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) {
close(g_task_fd);
warnx("Could not seek data manager file %s", k_data_manager_device_path);
sem_post(&g_init_sema); /* Don't want to hang startup */
@ -776,4 +792,3 @@ dataman_main(int argc, char *argv[])
exit(1);
}

2
src/modules/dataman/dataman.h
View File

@ -79,7 +79,7 @@ extern "C" {
} dm_reset_reason;
/* Maximum size in bytes of a single item instance */
#define DM_MAX_DATA_SIZE 126
#define DM_MAX_DATA_SIZE 124
/* Retrieve from the data manager store */
__EXPORT ssize_t

2
src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
View File

@ -964,7 +964,7 @@ FixedwingEstimator::task_main()
}
// Publish results
if (_initialized) {
if (_initialized && (check == OK)) {

6
src/modules/mavlink/mavlink_main.cpp
View File

@ -574,6 +574,11 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
/* open uart */
_uart_fd = open(uart_name, O_RDWR | O_NOCTTY);
if (_uart_fd < 0) {
return _uart_fd;
}
/* Try to set baud rate */
struct termios uart_config;
int termios_state;
@ -1964,6 +1969,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:

49
src/modules/mavlink/mavlink_messages.cpp
View File

@ -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"
@ -271,7 +272,7 @@ protected:
status->load * 1000.0f,
status->battery_voltage * 1000.0f,
status->battery_current * 1000.0f,
status->battery_remaining,
status->battery_remaining * 100.0f,
status->drop_rate_comm,
status->errors_comm,
status->errors_count1,
@ -1313,6 +1314,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(),
@ -1339,6 +1385,7 @@ MavlinkStream *streams_list[] = {
new MavlinkStreamAttitudeControls(),
new MavlinkStreamNamedValueFloat(),
new MavlinkStreamCameraCapture(),
new MavlinkStreamDistanceSensor(),
new MavlinkStreamViconPositionEstimate(),
nullptr
};

2
src/modules/mavlink/module.mk
View File

@ -47,4 +47,4 @@ SRCS += mavlink_main.cpp \
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
MAXOPTIMIZATION = -Os
MAXOPTIMIZATION = -Os

18
src/modules/sdlog2/sdlog2.c
View File

@ -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 --- */

51
src/modules/sdlog2/sdlog2_messages.h
View File

@ -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 --- */
@ -340,30 +343,30 @@ struct log_PARM_s {
/* construct list of all message formats */
static const struct log_format_s log_formats[] = {
/* business-level messages, ID < 0x80 */
LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
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(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"),
LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"),
LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"),
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"),
LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
LOG_FORMAT(GPOS, "LLfffffB", "Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"),
LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"),
LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"),
LOG_FORMAT(PWR, "fffBBBBB", "Periph_5V,Servo_5V,RSSI,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"),
LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"),
LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
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, "ffffffffLLfBBBB", "X,Y,Z,dist,distR,VX,VY,VZ,RLat,RLon,RAlt,XYFlg,ZFlg,LFlg,GFlg"),
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"),
LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"),
LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"),
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"),
LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
LOG_FORMAT(GPOS, "LLfffffB", "Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"),
LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
LOG_FORMAT(ESC, "HBBBHHHHHHfH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"),
LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,nStat,statNaN,covNaN,kGainNaN"),
LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),
LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"),
/* system-level messages, ID >= 0x80 */
/* FMT: don't write format of format message, it's useless */