aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--ROMFS/px4fmu_common/init.d/rc.usb2
-rw-r--r--makefiles/config_px4fmu-v1_default.mk5
-rw-r--r--src/drivers/airspeed/airspeed.cpp7
-rw-r--r--src/drivers/hmc5883/hmc5883.cpp3
-rw-r--r--src/drivers/ms5611/ms5611.cpp4
-rw-r--r--src/drivers/px4io/px4io.cpp2
-rw-r--r--src/drivers/sf0x/sf0x.cpp3
-rw-r--r--src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp6
-rw-r--r--src/lib/ecl/attitude_fw/ecl_roll_controller.cpp6
-rw-r--r--src/modules/commander/commander.cpp2
-rw-r--r--src/modules/mavlink/mavlink_main.cpp84
-rw-r--r--src/modules/mavlink/mavlink_messages.cpp29
-rw-r--r--src/modules/mavlink/mavlink_receiver.cpp90
-rw-r--r--src/modules/mavlink/mavlink_receiver.h2
-rw-r--r--src/modules/sensors/sensor_params.c10
-rw-r--r--src/modules/sensors/sensors.cpp54
-rw-r--r--src/systemcmds/tests/test_sensors.c2
17 files changed, 204 insertions, 107 deletions
diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb
index f1c21f295..ee040a706 100644
--- a/ROMFS/px4fmu_common/init.d/rc.usb
+++ b/ROMFS/px4fmu_common/init.d/rc.usb
@@ -23,7 +23,7 @@ mavlink stream -d /dev/ttyACM0 -s RC_CHANNELS_RAW -r 5
usleep 100000
mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20
usleep 100000
-mavlink stream -d /dev/ttyACM0 -s GLOBAL_POSITION_SETPOINT_INT -r 20
+mavlink stream -d /dev/ttyACM0 -s POSITION_TARGET_GLOBAL_INT -r 10
usleep 100000
# Exit shell to make it available to MAVLink
diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index a7c10f52f..97eddfdd2 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -25,7 +25,6 @@ MODULES += drivers/mpu6000
MODULES += drivers/hmc5883
MODULES += drivers/ms5611
MODULES += drivers/mb12xx
-MODULES += drivers/ll40ls
MODULES += drivers/gps
MODULES += drivers/hil
MODULES += drivers/hott/hott_telemetry
@@ -44,7 +43,6 @@ MODULES += modules/sensors
#
MODULES += systemcmds/mtd
MODULES += systemcmds/bl_update
-MODULES += systemcmds/i2c
MODULES += systemcmds/mixer
MODULES += systemcmds/param
MODULES += systemcmds/perf
@@ -152,5 +150,4 @@ endef
# command priority stack entrypoint
BUILTIN_COMMANDS := \
$(call _B, sercon, , 2048, sercon_main ) \
- $(call _B, serdis, , 2048, serdis_main ) \
- $(call _B, sysinfo, , 2048, sysinfo_main )
+ $(call _B, serdis, , 2048, serdis_main )
diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp
index 41942aacd..293690d27 100644
--- a/src/drivers/airspeed/airspeed.cpp
+++ b/src/drivers/airspeed/airspeed.cpp
@@ -165,7 +165,7 @@ Airspeed::probe()
*/
_retries = 4;
int ret = measure();
- _retries = 2;
+ _retries = 0;
return ret;
}
@@ -381,7 +381,10 @@ Airspeed::cycle_trampoline(void *arg)
Airspeed *dev = (Airspeed *)arg;
dev->cycle();
- dev->update_status();
+ // XXX we do not know if this is
+ // really helping - do not update the
+ // subsys state right now
+ //dev->update_status();
}
void
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index 0e9a961ac..e4ecfa6b5 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -1283,14 +1283,13 @@ int HMC5883::set_excitement(unsigned enable)
if (OK != ret)
perf_count(_comms_errors);
+ _conf_reg &= ~0x03; // reset previous excitement mode
if (((int)enable) < 0) {
_conf_reg |= 0x01;
} else if (enable > 0) {
_conf_reg |= 0x02;
- } else {
- _conf_reg &= ~0x03;
}
// ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)_conf_reg);
diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp
index 873fa62c4..889643d0d 100644
--- a/src/drivers/ms5611/ms5611.cpp
+++ b/src/drivers/ms5611/ms5611.cpp
@@ -130,7 +130,7 @@ protected:
float _T;
/* altitude conversion calibration */
- unsigned _msl_pressure; /* in kPa */
+ unsigned _msl_pressure; /* in Pa */
orb_advert_t _baro_topic;
@@ -466,7 +466,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
irqrestore(flags);
return -ENOMEM;
}
- irqrestore(flags);
+ irqrestore(flags);
return OK;
}
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 32069cf09..97919538f 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1968,7 +1968,7 @@ PX4IO::print_status(bool extended_status)
printf("actuators");
for (unsigned i = 0; i < _max_actuators; i++)
- printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i));
+ printf(" %hi", int16_t(io_reg_get(PX4IO_PAGE_ACTUATORS, i)));
printf("\n");
printf("servos");
diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp
index bca1715fa..80ecab2ee 100644
--- a/src/drivers/sf0x/sf0x.cpp
+++ b/src/drivers/sf0x/sf0x.cpp
@@ -598,7 +598,8 @@ SF0X::collect()
memcpy(_linebuf, buf, (lend + 1) - (i + 1));
}
- if (_linebuf[i] == '.') {
+ /* we need a digit before the dot and a dot for a valid number */
+ if (i > 0 && _linebuf[i] == '.') {
valid = true;
}
}
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
index 46db788a6..926a8db2a 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
@@ -169,7 +169,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
- float id = _rate_error * dt;
+ float id = _rate_error * dt * scaler;
/*
* anti-windup: do not allow integrator to increase if actuator is at limit
@@ -190,7 +190,9 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
/* Apply PI rate controller and store non-limited output */
- _last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed
+ _last_output = _bodyrate_setpoint * _k_ff * scaler +
+ _rate_error * _k_p * scaler * scaler
+ + integrator_constrained; //scaler is proportional to 1/airspeed
// warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
// warnx("roll: _last_output %.4f", (double)_last_output);
return math::constrain(_last_output, -1.0f, 1.0f);
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
index 9894a34d7..94bd26f03 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
@@ -135,7 +135,7 @@ float ECL_RollController::control_bodyrate(float pitch,
if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
- float id = _rate_error * dt;
+ float id = _rate_error * dt * scaler;
/*
* anti-windup: do not allow integrator to increase if actuator is at limit
@@ -157,7 +157,9 @@ float ECL_RollController::control_bodyrate(float pitch,
//warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max);
/* Apply PI rate controller and store non-limited output */
- _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed
+ _last_output = _bodyrate_setpoint * _k_ff * scaler +
+ _rate_error * _k_p * scaler * scaler
+ + integrator_constrained; //scaler is proportional to 1/airspeed
return math::constrain(_last_output, -1.0f, 1.0f);
}
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 6c2c03070..74deda8cc 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -129,7 +129,7 @@ extern struct system_load_s system_load;
#define POSITION_TIMEOUT (2 * 1000 * 1000) /**< consider the local or global position estimate invalid after 600ms */
#define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */
#define RC_TIMEOUT 500000
-#define DL_TIMEOUT 5 * 1000* 1000
+#define DL_TIMEOUT (10 * 1000 * 1000)
#define OFFBOARD_TIMEOUT 500000
#define DIFFPRESS_TIMEOUT 2000000
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 9a39d3bed..976b6d4d5 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -134,44 +134,44 @@ Mavlink::Mavlink() :
_mode(MAVLINK_MODE_NORMAL),
_channel(MAVLINK_COMM_0),
_logbuffer {},
- _total_counter(0),
- _receive_thread {},
- _verbose(false),
- _forwarding_on(false),
- _passing_on(false),
- _ftp_on(false),
- _uart_fd(-1),
- _baudrate(57600),
- _datarate(1000),
- _datarate_events(500),
- _rate_mult(1.0f),
- _mavlink_param_queue_index(0),
- mavlink_link_termination_allowed(false),
- _subscribe_to_stream(nullptr),
- _subscribe_to_stream_rate(0.0f),
- _flow_control_enabled(true),
- _last_write_success_time(0),
- _last_write_try_time(0),
- _bytes_tx(0),
- _bytes_txerr(0),
- _bytes_rx(0),
- _bytes_timestamp(0),
- _rate_tx(0.0f),
- _rate_txerr(0.0f),
- _rate_rx(0.0f),
- _rstatus {},
- _message_buffer {},
- _message_buffer_mutex {},
- _send_mutex {},
- _param_initialized(false),
- _param_system_id(0),
- _param_component_id(0),
- _param_system_type(0),
- _param_use_hil_gps(0),
-
- /* performance counters */
- _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
- _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
+ _total_counter(0),
+ _receive_thread {},
+ _verbose(false),
+ _forwarding_on(false),
+ _passing_on(false),
+ _ftp_on(false),
+ _uart_fd(-1),
+ _baudrate(57600),
+ _datarate(1000),
+ _datarate_events(500),
+ _rate_mult(1.0f),
+ _mavlink_param_queue_index(0),
+ mavlink_link_termination_allowed(false),
+ _subscribe_to_stream(nullptr),
+ _subscribe_to_stream_rate(0.0f),
+ _flow_control_enabled(true),
+ _last_write_success_time(0),
+ _last_write_try_time(0),
+ _bytes_tx(0),
+ _bytes_txerr(0),
+ _bytes_rx(0),
+ _bytes_timestamp(0),
+ _rate_tx(0.0f),
+ _rate_txerr(0.0f),
+ _rate_rx(0.0f),
+ _rstatus {},
+ _message_buffer {},
+ _message_buffer_mutex {},
+ _send_mutex {},
+ _param_initialized(false),
+ _param_system_id(0),
+ _param_component_id(0),
+ _param_system_type(0),
+ _param_use_hil_gps(0),
+
+ /* performance counters */
+ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
+ _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
{
fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl;
@@ -217,6 +217,8 @@ Mavlink::Mavlink() :
errx(1, "instance ID is out of range");
break;
}
+
+ _rstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
}
Mavlink::~Mavlink()
@@ -1388,8 +1390,8 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("GLOBAL_POSITION_INT", 3.0f);
configure_stream("LOCAL_POSITION_NED", 3.0f);
configure_stream("RC_CHANNELS_RAW", 1.0f);
- configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f);
- configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f);
+ configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
+ configure_stream("ATTITUDE_TARGET", 3.0f);
configure_stream("DISTANCE_SENSOR", 0.5f);
break;
@@ -1653,6 +1655,8 @@ Mavlink::display_status()
printf("\tGCS heartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time));
}
+ printf("\tmavlink chan: #%u\n", _channel);
+
if (_rstatus.timestamp > 0) {
printf("\ttype:\t\t");
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index c10be77b5..af4d46a36 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1333,7 +1333,7 @@ protected:
}
for (unsigned i = 0; i < 8; i++) {
- if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
+ if (act.output[i] > PWM_LOWEST_MIN / 2) {
if (i < n) {
/* scale PWM out 900..2100 us to 0..1 for rotors */
out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
@@ -1344,7 +1344,7 @@ protected:
}
} else {
- /* send 0 when disarmed */
+ /* send 0 when disarmed and for disabled channels */
out[i] = 0.0f;
}
}
@@ -1353,15 +1353,20 @@ protected:
/* fixed wing: scale throttle to 0..1 and other channels to -1..1 */
for (unsigned i = 0; i < 8; i++) {
- if (i != 3) {
- /* scale PWM out 900..2100 us to -1..1 for normal channels */
- out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
+ if (act.output[i] > PWM_LOWEST_MIN / 2) {
+ if (i != 3) {
+ /* scale PWM out 900..2100 us to -1..1 for normal channels */
+ out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
+
+ } else {
+ /* scale PWM out 900..2100 us to 0..1 for throttle */
+ out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
+ }
} else {
- /* scale PWM out 900..2100 us to 0..1 for throttle */
- out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
+ /* set 0 for disabled channels */
+ out[i] = 0.0f;
}
-
}
}
@@ -1637,10 +1642,10 @@ protected:
msg.chan2_raw = (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX;
msg.chan3_raw = (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX;
msg.chan4_raw = (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX;
- msg.chan4_raw = (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX;
- msg.chan4_raw = (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX;
- msg.chan4_raw = (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX;
- msg.chan4_raw = (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX;
+ msg.chan5_raw = (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX;
+ msg.chan6_raw = (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX;
+ msg.chan7_raw = (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX;
+ msg.chan8_raw = (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX;
msg.rssi = rc.rssi;
_mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS_RAW, &msg);
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index c0fae0a2f..a602344fd 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -112,7 +112,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_telemetry_status_pub(-1),
_rc_pub(-1),
_manual_pub(-1),
- _radio_status_available(false),
_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
_hil_frames(0),
_old_timestamp(0),
@@ -137,6 +136,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_command_long(msg);
break;
+ case MAVLINK_MSG_ID_COMMAND_INT:
+ handle_message_command_int(msg);
+ break;
+
case MAVLINK_MSG_ID_OPTICAL_FLOW:
handle_message_optical_flow(msg);
break;
@@ -277,6 +280,62 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
}
void
+MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
+{
+ /* command */
+ mavlink_command_int_t cmd_mavlink;
+ mavlink_msg_command_int_decode(msg, &cmd_mavlink);
+
+ if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid)
+ || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
+ //check for MAVLINK terminate command
+ if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
+ /* This is the link shutdown command, terminate mavlink */
+ warnx("terminated by remote command");
+ fflush(stdout);
+ usleep(50000);
+
+ /* terminate other threads and this thread */
+ _mavlink->_task_should_exit = true;
+
+ } else {
+
+ if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
+ warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID",
+ mavlink_system.sysid, mavlink_system.compid);
+ return;
+ }
+
+ struct vehicle_command_s vcmd;
+ memset(&vcmd, 0, sizeof(vcmd));
+
+ /* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */
+ vcmd.param1 = cmd_mavlink.param1;
+ vcmd.param2 = cmd_mavlink.param2;
+ vcmd.param3 = cmd_mavlink.param3;
+ vcmd.param4 = cmd_mavlink.param4;
+ /* these are coordinates as 1e7 scaled integers to work around the 32 bit floating point limits */
+ vcmd.param5 = ((double)cmd_mavlink.x) / 1e7;
+ vcmd.param6 = ((double)cmd_mavlink.y) / 1e7;
+ vcmd.param7 = cmd_mavlink.z;
+ // XXX do proper translation
+ vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command;
+ vcmd.target_system = cmd_mavlink.target_system;
+ vcmd.target_component = cmd_mavlink.target_component;
+ vcmd.source_system = msg->sysid;
+ vcmd.source_component = msg->compid;
+
+ if (_cmd_pub < 0) {
+ _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
+
+ } else {
+ orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd);
+ }
+ }
+ }
+}
+
+void
MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
{
/* optical flow */
@@ -430,9 +489,6 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
} else {
orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
}
-
- /* this means that heartbeats alone won't be published to the radio status no more */
- _radio_status_available = true;
}
}
@@ -474,25 +530,17 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
- hrt_abstime tnow = hrt_absolute_time();
-
- /* always set heartbeat, publish only if telemetry link not up */
- tstatus.heartbeat_time = tnow;
-
- /* if no radio status messages arrive, lets at least publish that heartbeats were received */
- if (!_radio_status_available) {
+ /* set heartbeat time and topic time and publish -
+ * the telem status also gets updated on telemetry events
+ */
+ tstatus.timestamp = hrt_absolute_time();
+ tstatus.heartbeat_time = tstatus.timestamp;
- tstatus.timestamp = tnow;
- /* telem_time indicates the timestamp of a telemetry status packet and we got none */
- tstatus.telem_time = 0;
- tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
+ if (_telemetry_status_pub < 0) {
+ _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
- if (_telemetry_status_pub < 0) {
- _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
-
- } else {
- orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
- }
+ } else {
+ orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
}
}
}
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 014193100..91125736c 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -110,6 +110,7 @@ private:
void handle_message(mavlink_message_t *msg);
void handle_message_command_long(mavlink_message_t *msg);
+ void handle_message_command_int(mavlink_message_t *msg);
void handle_message_optical_flow(mavlink_message_t *msg);
void handle_message_set_mode(mavlink_message_t *msg);
void handle_message_vicon_position_estimate(mavlink_message_t *msg);
@@ -151,7 +152,6 @@ private:
orb_advert_t _telemetry_status_pub;
orb_advert_t _rc_pub;
orb_advert_t _manual_pub;
- bool _radio_status_available;
int _control_mode_sub;
int _hil_frames;
uint64_t _old_timestamp;
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 7ce6ef5ef..229bfe3ce 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -214,6 +214,16 @@ PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f);
*/
PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0);
+/**
+ * QNH for barometer
+ *
+ * @min 500
+ * @max 1500
+ * @group Sensor Calibration
+ * @unit hPa
+ */
+PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f);
+
/**
* Board rotation
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 4e8a8c01d..aac297ef8 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -143,6 +143,12 @@
#define STICK_ON_OFF_LIMIT 0.75f
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
/**
* Sensor app start / stop handling function
*
@@ -235,7 +241,7 @@ private:
math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
math::Matrix<3, 3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */
bool _mag_is_external; /**< true if the active mag is on an external board */
-
+
uint64_t _battery_discharged; /**< battery discharged current in mA*ms */
hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */
@@ -258,7 +264,7 @@ private:
int board_rotation;
int external_mag_rotation;
-
+
float board_offset[3];
int rc_map_roll;
@@ -301,6 +307,8 @@ private:
float battery_voltage_scaling;
float battery_current_scaling;
+ float baro_qnh;
+
} _parameters; /**< local copies of interesting parameters */
struct {
@@ -354,9 +362,11 @@ private:
param_t board_rotation;
param_t external_mag_rotation;
-
+
param_t board_offset[3];
+ param_t baro_qnh;
+
} _parameter_handles; /**< handles for interesting parameters */
@@ -462,12 +472,6 @@ private:
namespace sensors
{
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-static const int ERROR = -1;
-
Sensors *g_sensors = nullptr;
}
@@ -611,12 +615,15 @@ Sensors::Sensors() :
/* rotations */
_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
_parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT");
-
+
/* rotation offsets */
_parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF");
_parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF");
_parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF");
+ /* Barometer QNH */
+ _parameter_handles.baro_qnh = param_find("SENS_BARO_QNH");
+
/* fetch initial parameter values */
parameters_update();
}
@@ -828,19 +835,38 @@ Sensors::parameters_update()
get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation);
-
+
param_get(_parameter_handles.board_offset[0], &(_parameters.board_offset[0]));
param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1]));
param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2]));
-
+
/** fine tune board offset on parameter update **/
- math::Matrix<3, 3> board_rotation_offset;
+ math::Matrix<3, 3> board_rotation_offset;
board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0],
M_DEG_TO_RAD_F * _parameters.board_offset[1],
M_DEG_TO_RAD_F * _parameters.board_offset[2]);
-
+
_board_rotation = _board_rotation * board_rotation_offset;
+ /* update barometer qnh setting */
+ param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh));
+ int fd;
+ fd = open(BARO_DEVICE_PATH, 0);
+ if (fd < 0) {
+ warn("%s", BARO_DEVICE_PATH);
+ errx(1, "FATAL: no barometer found");
+
+ } else {
+ warnx("qnh ioctl, %lu", (unsigned long)(_parameters.baro_qnh * 100));
+ int ret = ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100));
+ if (ret) {
+ warnx("qnh could not be set");
+ close(fd);
+ return ERROR;
+ }
+ close(fd);
+ }
+
return OK;
}
diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c
index a4f17eebd..e005bf9c1 100644
--- a/src/systemcmds/tests/test_sensors.c
+++ b/src/systemcmds/tests/test_sensors.c
@@ -331,7 +331,7 @@ mag(int argc, char *argv[])
float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z);
- if (len < 1.0f || len > 3.0f) {
+ if (len < 0.25f || len > 3.0f) {
warnx("MAG scale error!");
return ERROR;
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-12 20:49:42 +0000