mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-03 13:58:53 +08:00
Merge branch 'master' into autostart
This commit is contained in:
Lorenz Meier
@@ -34,9 +34,10 @@ fi
|
||||
# ALWAYS start this task before the
|
||||
# preflight_check.
|
||||
#
|
||||
sensors start
|
||||
|
||||
#
|
||||
# Check sensors - run AFTER 'sensors start'
|
||||
#
|
||||
preflight_check
|
||||
if sensors start
|
||||
then
|
||||
#
|
||||
# Check sensors - run AFTER 'sensors start'
|
||||
#
|
||||
preflight_check
|
||||
fi
|
||||
@@ -5,8 +5,33 @@
|
||||
|
||||
echo "Starting MAVLink on this USB console"
|
||||
|
||||
# Stop tone alarm
|
||||
tone_alarm stop
|
||||
|
||||
# Tell MAVLink that this link is "fast"
|
||||
mavlink start -b 230400 -d /dev/console
|
||||
if mavlink stop
|
||||
then
|
||||
echo "stopped other MAVLink instance"
|
||||
fi
|
||||
mavlink start -b 230400 -d /dev/ttyACM0
|
||||
|
||||
# Start the commander
|
||||
commander start
|
||||
|
||||
# Start sensors
|
||||
sh /etc/init.d/rc.sensors
|
||||
|
||||
# Start one of the estimators
|
||||
if attitude_estimator_ekf start
|
||||
then
|
||||
echo "estimating attitude"
|
||||
fi
|
||||
|
||||
# Start GPS
|
||||
if gps start
|
||||
then
|
||||
echo "GPS started"
|
||||
fi
|
||||
|
||||
echo "MAVLink started, exiting shell.."
|
||||
|
||||
|
||||
@@ -7,7 +7,6 @@
|
||||
# can change this to prevent automatic startup of the flight script.
|
||||
#
|
||||
set MODE autostart
|
||||
set USB autoconnect
|
||||
|
||||
#
|
||||
# Try to mount the microSD card.
|
||||
@@ -66,31 +65,44 @@ then
|
||||
sh /fs/microsd/etc/rc.txt
|
||||
fi
|
||||
|
||||
#
|
||||
# Check for USB host
|
||||
#
|
||||
if [ $USB != autoconnect ]
|
||||
then
|
||||
echo "[init] not connecting USB"
|
||||
else
|
||||
if sercon
|
||||
then
|
||||
echo "[init] USB interface connected"
|
||||
else
|
||||
if [ -f /dev/ttyACM0 ]
|
||||
echo "[init] NSH via USB"
|
||||
then
|
||||
else
|
||||
echo "[init] No USB connected"
|
||||
fi
|
||||
fi
|
||||
fi
|
||||
|
||||
# if this is an APM build then there will be a rc.APM script
|
||||
# from an EXTERNAL_SCRIPTS build option
|
||||
if [ -f /etc/init.d/rc.APM ]
|
||||
then
|
||||
echo Running rc.APM
|
||||
if sercon
|
||||
then
|
||||
echo "[init] USB interface connected"
|
||||
fi
|
||||
|
||||
echo "Running rc.APM"
|
||||
# if APM startup is successful then nsh will exit
|
||||
sh /etc/init.d/rc.APM
|
||||
fi
|
||||
|
||||
if [ $MODE == autostart ]
|
||||
then
|
||||
|
||||
#
|
||||
# Start the ORB (first app to start)
|
||||
#
|
||||
uorb start
|
||||
|
||||
#
|
||||
# Load microSD params
|
||||
#
|
||||
if ramtron start
|
||||
then
|
||||
param select /ramtron/params
|
||||
if [ -f /ramtron/params ]
|
||||
then
|
||||
param load /ramtron/params
|
||||
fi
|
||||
else
|
||||
param select /fs/microsd/params
|
||||
if [ -f /fs/microsd/params ]
|
||||
then
|
||||
param load /fs/microsd/params
|
||||
fi
|
||||
fi
|
||||
|
||||
fi
|
||||
|
||||
@@ -194,7 +194,7 @@ __EXPORT int nsh_archinitialize(void)
|
||||
/* initial LED state */
|
||||
drv_led_start();
|
||||
led_off(LED_AMBER);
|
||||
led_on(LED_BLUE);
|
||||
led_off(LED_BLUE);
|
||||
|
||||
|
||||
/* Configure SPI-based devices */
|
||||
|
||||
@@ -1221,7 +1221,8 @@ start()
|
||||
int fd;
|
||||
|
||||
if (g_dev != nullptr)
|
||||
errx(1, "already started");
|
||||
/* if already started, the still command succeeded */
|
||||
errx(0, "already started");
|
||||
|
||||
/* create the driver, attempt expansion bus first */
|
||||
g_dev = new HMC5883(PX4_I2C_BUS_EXPANSION);
|
||||
|
||||
+83
-83
@@ -72,8 +72,8 @@ struct gam_module_poll_msg {
|
||||
|
||||
/* The Electric Air Module message. */
|
||||
struct eam_module_msg {
|
||||
uint8_t start; /**< Start byte */
|
||||
uint8_t eam_sensor_id; /**< EAM sensor */
|
||||
uint8_t start; /**< Start byte */
|
||||
uint8_t eam_sensor_id; /**< EAM sensor */
|
||||
uint8_t warning;
|
||||
uint8_t sensor_text_id;
|
||||
uint8_t alarm_inverse1;
|
||||
@@ -125,52 +125,52 @@ struct eam_module_msg {
|
||||
#define GAM_SENSOR_TEXT_ID 0xd0
|
||||
|
||||
struct gam_module_msg {
|
||||
uint8_t start; // start byte constant value 0x7c
|
||||
uint8_t gam_sensor_id; // EAM sensort id. constat value 0x8d
|
||||
uint8_t warning_beeps; // 1=A 2=B ... 0x1a=Z 0 = no alarm
|
||||
uint8_t sensor_text_id; // constant value 0xd0
|
||||
uint8_t alarm_invers1; // alarm bitmask. Value is displayed inverted
|
||||
uint8_t alarm_invers2; // alarm bitmask. Value is displayed inverted
|
||||
uint8_t cell1; // cell 1 voltage lower value. 0.02V steps, 124=2.48V
|
||||
uint8_t start; /**< Start byte */
|
||||
uint8_t gam_sensor_id; /**< GAM sensor id */
|
||||
uint8_t warning_beeps;
|
||||
uint8_t sensor_text_id;
|
||||
uint8_t alarm_invers1;
|
||||
uint8_t alarm_invers2;
|
||||
uint8_t cell1; /**< Lipo cell voltages. Not supported. */
|
||||
uint8_t cell2;
|
||||
uint8_t cell3;
|
||||
uint8_t cell4;
|
||||
uint8_t cell5;
|
||||
uint8_t cell6;
|
||||
uint8_t batt1_L; // battery 1 voltage LSB value. 0.1V steps. 50 = 5.5V
|
||||
uint8_t batt1_L; /**< Battery 1 voltage LSB value. 0.1V steps. 50 = 5.5V */
|
||||
uint8_t batt1_H;
|
||||
uint8_t batt2_L; // battery 2 voltage LSB value. 0.1V steps. 50 = 5.5V
|
||||
uint8_t batt2_L; /**< Battery 2 voltage LSB value. 0.1V steps. 50 = 5.5V */
|
||||
uint8_t batt2_H;
|
||||
uint8_t temperature1; // temperature 1. offset of 20. a value of 20 = 0°C
|
||||
uint8_t temperature2; // temperature 2. offset of 20. a value of 20 = 0°C
|
||||
uint8_t fuel_procent; // Fuel capacity in %. Values 0--100
|
||||
// graphical display ranges: 0-25% 50% 75% 100%
|
||||
uint8_t fuel_ml_L; // Fuel in ml scale. Full = 65535!
|
||||
uint8_t fuel_ml_H; //
|
||||
uint8_t rpm_L; // RPM in 10 RPM steps. 300 = 3000rpm
|
||||
uint8_t rpm_H; //
|
||||
uint8_t altitude_L; // altitude in meters. offset of 500, 500 = 0m
|
||||
uint8_t altitude_H; //
|
||||
uint8_t climbrate_L; // climb rate in 0.01m/s. Value of 30000 = 0.00 m/s
|
||||
uint8_t climbrate_H; //
|
||||
uint8_t climbrate3s; // climb rate in m/3sec. Value of 120 = 0m/3sec
|
||||
uint8_t current_L; // current in 0.1A steps
|
||||
uint8_t current_H; //
|
||||
uint8_t main_voltage_L; // Main power voltage using 0.1V steps
|
||||
uint8_t main_voltage_H; //
|
||||
uint8_t batt_cap_L; // used battery capacity in 10mAh steps
|
||||
uint8_t batt_cap_H; //
|
||||
uint8_t speed_L; // (air?) speed in km/h(?) we are using ground speed here per default
|
||||
uint8_t speed_H; //
|
||||
uint8_t min_cell_volt; // minimum cell voltage in 2mV steps. 124 = 2,48V
|
||||
uint8_t min_cell_volt_num; // number of the cell with the lowest voltage
|
||||
uint8_t rpm2_L; // RPM in 10 RPM steps. 300 = 3000rpm
|
||||
uint8_t rpm2_H; //
|
||||
uint8_t general_error_number; // Voice error == 12. TODO: more docu
|
||||
uint8_t pressure; // Pressure up to 16bar. 0,1bar scale. 20 = 2bar
|
||||
uint8_t version; // version number TODO: more info?
|
||||
uint8_t stop; // stop byte
|
||||
uint8_t checksum; // checksum
|
||||
uint8_t temperature1; /**< Temperature 1. offset of 20. a value of 20 = 0°C */
|
||||
uint8_t temperature2; /**< Temperature 2. offset of 20. a value of 20 = 0°C */
|
||||
uint8_t fuel_procent; /**< Fuel capacity in %. Values 0 - 100 */
|
||||
/**< Graphical display ranges: 0 25% 50% 75% 100% */
|
||||
uint8_t fuel_ml_L; /**< Fuel in ml scale. Full = 65535 */
|
||||
uint8_t fuel_ml_H;
|
||||
uint8_t rpm_L; /**< RPM in 10 RPM steps. 300 = 3000rpm */
|
||||
uint8_t rpm_H;
|
||||
uint8_t altitude_L; /**< Altitude in meters. offset of 500, 500 = 0m */
|
||||
uint8_t altitude_H;
|
||||
uint8_t climbrate_L; /**< Climb rate in 0.01m/s. Value of 30000 = 0.00 m/s */
|
||||
uint8_t climbrate_H;
|
||||
uint8_t climbrate3s; /**< Climb rate in m/3sec. Value of 120 = 0m/3sec */
|
||||
uint8_t current_L; /**< Current in 0.1A steps */
|
||||
uint8_t current_H;
|
||||
uint8_t main_voltage_L; /**< Main power voltage using 0.1V steps */
|
||||
uint8_t main_voltage_H;
|
||||
uint8_t batt_cap_L; /**< Used battery capacity in 10mAh steps */
|
||||
uint8_t batt_cap_H;
|
||||
uint8_t speed_L; /**< Speed in km/h */
|
||||
uint8_t speed_H;
|
||||
uint8_t min_cell_volt; /**< Minimum cell voltage in 2mV steps. 124 = 2,48V */
|
||||
uint8_t min_cell_volt_num; /**< Number of the cell with the lowest voltage */
|
||||
uint8_t rpm2_L; /**< RPM in 10 RPM steps. 300 = 3000rpm */
|
||||
uint8_t rpm2_H;
|
||||
uint8_t general_error_number; /**< Voice error == 12. TODO: more docu */
|
||||
uint8_t pressure; /**< Pressure up to 16bar. 0,1bar scale. 20 = 2bar */
|
||||
uint8_t version;
|
||||
uint8_t stop; /**< Stop byte */
|
||||
uint8_t checksum; /**< Lower 8-bits of all bytes summed */
|
||||
};
|
||||
|
||||
/* GPS sensor constants. */
|
||||
@@ -184,52 +184,52 @@ struct gam_module_msg {
|
||||
struct gps_module_msg {
|
||||
uint8_t start; /**< Start byte */
|
||||
uint8_t sensor_id; /**< GPS sensor ID*/
|
||||
uint8_t warning; /**< Byte 3: 0…= warning beeps */
|
||||
uint8_t warning; /**< 0…= warning beeps */
|
||||
uint8_t sensor_text_id; /**< GPS Sensor text mode ID */
|
||||
uint8_t alarm_inverse1; /**< Byte 5: 01 inverse status */
|
||||
uint8_t alarm_inverse2; /**< Byte 6: 00 inverse status status 1 = no GPS Signal */
|
||||
uint8_t flight_direction; /**< Byte 7: 119 = Flightdir./dir. 1 = 2°; 0° (North), 9 0° (East), 180° (South), 270° (West) */
|
||||
uint8_t gps_speed_L; /**< Byte 8: 8 = /GPS speed low byte 8km/h */
|
||||
uint8_t gps_speed_H; /**< Byte 9: 0 = /GPS speed high byte */
|
||||
uint8_t alarm_inverse1; /**< 01 inverse status */
|
||||
uint8_t alarm_inverse2; /**< 00 inverse status status 1 = no GPS Signal */
|
||||
uint8_t flight_direction; /**< 119 = Flightdir./dir. 1 = 2°; 0° (North), 9 0° (East), 180° (South), 270° (West) */
|
||||
uint8_t gps_speed_L; /**< 8 = /GPS speed low byte 8km/h */
|
||||
uint8_t gps_speed_H; /**< 0 = /GPS speed high byte */
|
||||
|
||||
uint8_t latitude_ns; /**< Byte 10: 000 = N = 48°39’988 */
|
||||
uint8_t latitude_min_L; /**< Byte 11: 231 0xE7 = 0x12E7 = 4839 */
|
||||
uint8_t latitude_min_H; /**< Byte 12: 018 18 = 0x12 */
|
||||
uint8_t latitude_sec_L; /**< Byte 13: 171 220 = 0xDC = 0x03DC =0988 */
|
||||
uint8_t latitude_sec_H; /**< Byte 14: 016 3 = 0x03 */
|
||||
uint8_t latitude_ns; /**< 000 = N = 48°39’988 */
|
||||
uint8_t latitude_min_L; /**< 231 0xE7 = 0x12E7 = 4839 */
|
||||
uint8_t latitude_min_H; /**< 018 18 = 0x12 */
|
||||
uint8_t latitude_sec_L; /**< 171 220 = 0xDC = 0x03DC =0988 */
|
||||
uint8_t latitude_sec_H; /**< 016 3 = 0x03 */
|
||||
|
||||
uint8_t longitude_ew; /**< Byte 15: 000 = E= 9° 25’9360 */
|
||||
uint8_t longitude_min_L; /**< Byte 16: 150 157 = 0x9D = 0x039D = 0925 */
|
||||
uint8_t longitude_min_H; /**< Byte 17: 003 3 = 0x03 */
|
||||
uint8_t longitude_sec_L; /**< Byte 18: 056 144 = 0x90 0x2490 = 9360*/
|
||||
uint8_t longitude_sec_H; /**< Byte 19: 004 36 = 0x24 */
|
||||
uint8_t longitude_ew; /**< 000 = E= 9° 25’9360 */
|
||||
uint8_t longitude_min_L; /**< 150 157 = 0x9D = 0x039D = 0925 */
|
||||
uint8_t longitude_min_H; /**< 003 3 = 0x03 */
|
||||
uint8_t longitude_sec_L; /**< 056 144 = 0x90 0x2490 = 9360 */
|
||||
uint8_t longitude_sec_H; /**< 004 36 = 0x24 */
|
||||
|
||||
uint8_t distance_L; /**< Byte 20: 027 123 = /distance low byte 6 = 6 m */
|
||||
uint8_t distance_H; /**< Byte 21: 036 35 = /distance high byte */
|
||||
uint8_t altitude_L; /**< Byte 22: 243 244 = /Altitude low byte 500 = 0m */
|
||||
uint8_t altitude_H; /**< Byte 23: 001 1 = /Altitude high byte */
|
||||
uint8_t resolution_L; /**< Byte 24: 48 = Low Byte m/s resolution 0.01m 48 = 30000 = 0.00m/s (1=0.01m/s) */
|
||||
uint8_t resolution_H; /**< Byte 25: 117 = High Byte m/s resolution 0.01m */
|
||||
uint8_t unknown1; /**< Byte 26: 120 = 0m/3s */
|
||||
uint8_t gps_num_sat; /**< Byte 27: GPS.Satellites (number of satelites) (1 byte) */
|
||||
uint8_t gps_fix_char; /**< Byte 28: GPS.FixChar. (GPS fix character. display, if DGPS, 2D oder 3D) (1 byte) */
|
||||
uint8_t home_direction; /**< Byte 29: HomeDirection (direction from starting point to Model position) (1 byte) */
|
||||
uint8_t angle_x_direction; /**< Byte 30: angle x-direction (1 byte) */
|
||||
uint8_t angle_y_direction; /**< Byte 31: angle y-direction (1 byte) */
|
||||
uint8_t angle_z_direction; /**< Byte 32: angle z-direction (1 byte) */
|
||||
uint8_t gyro_x_L; /**< Byte 33: gyro x low byte (2 bytes) */
|
||||
uint8_t gyro_x_H; /**< Byte 34: gyro x high byte */
|
||||
uint8_t gyro_y_L; /**< Byte 35: gyro y low byte (2 bytes) */
|
||||
uint8_t gyro_y_H; /**< Byte 36: gyro y high byte */
|
||||
uint8_t gyro_z_L; /**< Byte 37: gyro z low byte (2 bytes) */
|
||||
uint8_t gyro_z_H; /**< Byte 38: gyro z high byte */
|
||||
uint8_t vibration; /**< Byte 39: vibration (1 bytes) */
|
||||
uint8_t ascii4; /**< Byte 40: 00 ASCII Free Character [4] */
|
||||
uint8_t ascii5; /**< Byte 41: 00 ASCII Free Character [5] */
|
||||
uint8_t gps_fix; /**< Byte 42: 00 ASCII Free Character [6], we use it for GPS FIX */
|
||||
uint8_t version; /**< Byte 43: 00 version number */
|
||||
uint8_t stop; /**< Byte 44: 0x7D Ende byte */
|
||||
uint8_t checksum; /**< Byte 45: Parity Byte */
|
||||
uint8_t distance_L; /**< 027 123 = /distance low byte 6 = 6 m */
|
||||
uint8_t distance_H; /**< 036 35 = /distance high byte */
|
||||
uint8_t altitude_L; /**< 243 244 = /Altitude low byte 500 = 0m */
|
||||
uint8_t altitude_H; /**< 001 1 = /Altitude high byte */
|
||||
uint8_t resolution_L; /**< 48 = Low Byte m/s resolution 0.01m 48 = 30000 = 0.00m/s (1=0.01m/s) */
|
||||
uint8_t resolution_H; /**< 117 = High Byte m/s resolution 0.01m */
|
||||
uint8_t unknown1; /**< 120 = 0m/3s */
|
||||
uint8_t gps_num_sat; /**< GPS.Satellites (number of satelites) (1 byte) */
|
||||
uint8_t gps_fix_char; /**< GPS.FixChar. (GPS fix character. display, if DGPS, 2D oder 3D) (1 byte) */
|
||||
uint8_t home_direction; /**< HomeDirection (direction from starting point to Model position) (1 byte) */
|
||||
uint8_t angle_x_direction; /**< angle x-direction (1 byte) */
|
||||
uint8_t angle_y_direction; /**< angle y-direction (1 byte) */
|
||||
uint8_t angle_z_direction; /**< angle z-direction (1 byte) */
|
||||
uint8_t gyro_x_L; /**< gyro x low byte (2 bytes) */
|
||||
uint8_t gyro_x_H; /**< gyro x high byte */
|
||||
uint8_t gyro_y_L; /**< gyro y low byte (2 bytes) */
|
||||
uint8_t gyro_y_H; /**< gyro y high byte */
|
||||
uint8_t gyro_z_L; /**< gyro z low byte (2 bytes) */
|
||||
uint8_t gyro_z_H; /**< gyro z high byte */
|
||||
uint8_t vibration; /**< vibration (1 bytes) */
|
||||
uint8_t ascii4; /**< 00 ASCII Free Character [4] */
|
||||
uint8_t ascii5; /**< 00 ASCII Free Character [5] */
|
||||
uint8_t gps_fix; /**< 00 ASCII Free Character [6], we use it for GPS FIX */
|
||||
uint8_t version;
|
||||
uint8_t stop; /**< Stop byte */
|
||||
uint8_t checksum; /**< Lower 8-bits of all bytes summed */
|
||||
};
|
||||
|
||||
// The maximum size of a message.
|
||||
|
||||
@@ -1063,7 +1063,8 @@ start()
|
||||
int fd;
|
||||
|
||||
if (g_dev != nullptr)
|
||||
errx(1, "already started");
|
||||
/* if already started, the still command succeeded */
|
||||
errx(0, "already started");
|
||||
|
||||
/* create the driver */
|
||||
g_dev = new MPU6000(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_MPU);
|
||||
|
||||
@@ -969,7 +969,8 @@ start()
|
||||
int fd;
|
||||
|
||||
if (g_dev != nullptr)
|
||||
errx(1, "already started");
|
||||
/* if already started, the still command succeeded */
|
||||
errx(0, "already started");
|
||||
|
||||
/* create the driver */
|
||||
g_dev = new MS5611(MS5611_BUS);
|
||||
|
||||
@@ -282,7 +282,7 @@ void tune_error(void)
|
||||
|
||||
void do_rc_calibration(int status_pub, struct vehicle_status_s *status)
|
||||
{
|
||||
if (current_status.offboard_control_signal_lost) {
|
||||
if (current_status.rc_signal_lost) {
|
||||
mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal.");
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -420,12 +420,12 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf
|
||||
case 921600: speed = B921600; break;
|
||||
|
||||
default:
|
||||
fprintf(stderr, "[mavlink] ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud);
|
||||
warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud);
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
/* open uart */
|
||||
printf("[mavlink] UART is %s, baudrate is %d\n", uart_name, baud);
|
||||
warnx("UART is %s, baudrate is %d\n", uart_name, baud);
|
||||
uart = open(uart_name, O_RDWR | O_NOCTTY);
|
||||
|
||||
/* Try to set baud rate */
|
||||
@@ -433,37 +433,35 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf
|
||||
int termios_state;
|
||||
*is_usb = false;
|
||||
|
||||
/* make some wild guesses including that USB serial is indicated by either /dev/ttyACM0 or /dev/console */
|
||||
if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/console") != OK) {
|
||||
/* Back up the original uart configuration to restore it after exit */
|
||||
if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
|
||||
fprintf(stderr, "[mavlink] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state);
|
||||
close(uart);
|
||||
return -1;
|
||||
}
|
||||
/* Back up the original uart configuration to restore it after exit */
|
||||
if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
|
||||
warnx("ERROR get termios config %s: %d\n", uart_name, termios_state);
|
||||
close(uart);
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* Fill the struct for the new configuration */
|
||||
tcgetattr(uart, &uart_config);
|
||||
/* Fill the struct for the new configuration */
|
||||
tcgetattr(uart, &uart_config);
|
||||
|
||||
/* Clear ONLCR flag (which appends a CR for every LF) */
|
||||
uart_config.c_oflag &= ~ONLCR;
|
||||
/* Clear ONLCR flag (which appends a CR for every LF) */
|
||||
uart_config.c_oflag &= ~ONLCR;
|
||||
|
||||
/* USB serial is indicated by /dev/ttyACM0*/
|
||||
if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/ttyACM1") != OK) {
|
||||
|
||||
/* Set baud rate */
|
||||
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
|
||||
fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
|
||||
warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
|
||||
close(uart);
|
||||
return -1;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
|
||||
fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
|
||||
close(uart);
|
||||
return -1;
|
||||
}
|
||||
|
||||
} else {
|
||||
*is_usb = true;
|
||||
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
|
||||
warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
|
||||
close(uart);
|
||||
return -1;
|
||||
}
|
||||
|
||||
return uart;
|
||||
@@ -751,8 +749,7 @@ int mavlink_thread_main(int argc, char *argv[])
|
||||
pthread_join(uorb_receive_thread, NULL);
|
||||
|
||||
/* Reset the UART flags to original state */
|
||||
if (!usb_uart)
|
||||
tcsetattr(uart, TCSANOW, &uart_config_original);
|
||||
tcsetattr(uart, TCSANOW, &uart_config_original);
|
||||
|
||||
thread_running = false;
|
||||
|
||||
|
||||
@@ -62,7 +62,7 @@ float calc_indicated_airspeed(float differential_pressure)
|
||||
if (differential_pressure > 0) {
|
||||
return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
|
||||
} else {
|
||||
return -sqrtf((2.0f*fabs(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
|
||||
return -sqrtf((2.0f*fabsf(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
|
||||
}
|
||||
|
||||
}
|
||||
@@ -106,6 +106,6 @@ float calc_true_airspeed(float total_pressure, float static_pressure, float temp
|
||||
return sqrtf((2.0f*(pressure_difference)) / density);
|
||||
} else
|
||||
{
|
||||
return -sqrtf((2.0f*fabs(pressure_difference)) / density);
|
||||
return -sqrtf((2.0f*fabsf(pressure_difference)) / density);
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user