aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--ROMFS/px4fmu_common/init.d/3035_viper2
-rw-r--r--ROMFS/px4fmu_common/init.d/rc.interface5
-rw-r--r--ROMFS/px4fmu_common/init.d/rcS9
-rw-r--r--ROMFS/px4fmu_common/mixers/FMU_AERT.mix11
-rwxr-xr-xROMFS/px4fmu_common/mixers/Viper.mix13
-rw-r--r--src/drivers/drv_pwm_output.h5
-rw-r--r--src/drivers/px4fmu/fmu.cpp4
-rw-r--r--src/lib/launchdetection/CatapultLaunchMethod.cpp4
-rw-r--r--src/modules/commander/commander.cpp5
-rw-r--r--src/modules/mavlink/mavlink_messages.cpp26
-rw-r--r--src/modules/px4iofirmware/registers.c4
-rw-r--r--src/modules/sdlog2/sdlog2.c21
-rw-r--r--src/modules/sdlog2/sdlog2_messages.h15
-rw-r--r--src/modules/uORB/topics/vehicle_command.h19
-rw-r--r--src/modules/uavcan/sensors/gnss.cpp10
-rw-r--r--src/modules/uavcan/sensors/gnss.hpp2
-rw-r--r--src/modules/uavcan/sensors/sensor_bridge.cpp36
-rw-r--r--src/modules/uavcan/sensors/sensor_bridge.hpp15
-rw-r--r--src/modules/uavcan/uavcan_main.cpp13
19 files changed, 159 insertions, 60 deletions
diff --git a/ROMFS/px4fmu_common/init.d/3035_viper b/ROMFS/px4fmu_common/init.d/3035_viper
index a4c1e832d..3714b612f 100644
--- a/ROMFS/px4fmu_common/init.d/3035_viper
+++ b/ROMFS/px4fmu_common/init.d/3035_viper
@@ -8,3 +8,5 @@
sh /etc/init.d/rc.fw_defaults
set MIXER Viper
+
+set FAILSAFE "-c567 -p 1000"
diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface
index 1de0abb58..e44cd0953 100644
--- a/ROMFS/px4fmu_common/init.d/rc.interface
+++ b/ROMFS/px4fmu_common/init.d/rc.interface
@@ -77,4 +77,9 @@ then
pwm max -c $PWM_OUTPUTS -p $PWM_MAX
fi
fi
+
+ if [ $FAILSAFE != none ]
+ then
+ pwm failsafe -d $OUTPUT_DEV $FAILSAFE
+ fi
fi
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 195771905..ea04ece34 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -66,6 +66,9 @@ then
#
sercon
+ # Try to get an USB console
+ nshterm /dev/ttyACM0 &
+
#
# Start the ORB (first app to start)
#
@@ -96,11 +99,9 @@ then
#
if rgbled start
then
- echo "[init] RGB Led"
else
if blinkm start
then
- echo "[init] BlinkM"
blinkm systemstate
fi
fi
@@ -129,6 +130,7 @@ then
set LOAD_DEFAULT_APPS yes
set GPS yes
set GPS_FAKE no
+ set FAILSAFE none
#
# Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts
@@ -279,9 +281,6 @@ then
fi
fi
- # Try to get an USB console
- nshterm /dev/ttyACM0 &
-
#
# Start the datamanager (and do not abort boot if it fails)
#
diff --git a/ROMFS/px4fmu_common/mixers/FMU_AERT.mix b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix
index 0ec663e35..7fed488af 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_AERT.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix
@@ -64,21 +64,22 @@ O: 10000 10000 0 -10000 10000
S: 0 3 0 20000 -10000 -10000 10000
-Gimbal / flaps / payload mixer for last four channels
+Gimbal / flaps / payload mixer for last four channels,
+using the payload control group
-----------------------------------------------------
M: 1
O: 10000 10000 0 -10000 10000
-S: 0 4 10000 10000 0 -10000 10000
+S: 2 0 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
-S: 0 5 10000 10000 0 -10000 10000
+S: 2 1 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
-S: 0 6 10000 10000 0 -10000 10000
+S: 2 2 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
-S: 0 7 10000 10000 0 -10000 10000
+S: 2 3 10000 10000 0 -10000 10000
diff --git a/ROMFS/px4fmu_common/mixers/Viper.mix b/ROMFS/px4fmu_common/mixers/Viper.mix
index 79c4447be..5a0381bd8 100755
--- a/ROMFS/px4fmu_common/mixers/Viper.mix
+++ b/ROMFS/px4fmu_common/mixers/Viper.mix
@@ -52,21 +52,20 @@ M: 1
O: 10000 10000 0 -10000 10000
S: 0 3 0 20000 -10000 -10000 10000
-Gimbal / flaps / payload mixer for last four channels
+Inputs to the mixer come from channel group 2 (payload), channels 0
+(bay servo 1), 1 (bay servo 2) and 3 (drop release).
-----------------------------------------------------
M: 1
O: 10000 10000 0 -10000 10000
-S: 0 4 10000 10000 0 -10000 10000
+S: 2 0 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
-S: 0 5 10000 10000 0 -10000 10000
+S: 2 1 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
-S: 0 6 10000 10000 0 -10000 10000
+S: 2 2 -10000 -10000 0 -10000 10000
+
-M: 1
-O: 10000 10000 0 -10000 10000
-S: 0 7 10000 10000 0 -10000 10000
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index 84815fdfb..5aff6825b 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -95,6 +95,11 @@ __BEGIN_DECLS
#define PWM_LOWEST_MAX 1700
/**
+ * Do not output a channel with this value
+ */
+#define PWM_IGNORE_THIS_CHANNEL UINT16_MAX
+
+/**
* Servo output signal type, value is actual servo output pulse
* width in microseconds.
*/
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 82977a032..122a3cd17 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -1272,7 +1272,9 @@ PX4FMU::write(file *filp, const char *buffer, size_t len)
memcpy(values, buffer, count * 2);
for (uint8_t i = 0; i < count; i++) {
- up_pwm_servo_set(i, values[i]);
+ if (values[i] != PWM_IGNORE_THIS_CHANNEL) {
+ up_pwm_servo_set(i, values[i]);
+ }
}
return count * 2;
diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp
index c555a0a69..9d479832f 100644
--- a/src/lib/launchdetection/CatapultLaunchMethod.cpp
+++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp
@@ -66,10 +66,10 @@ void CatapultLaunchMethod::update(float accel_x)
last_timestamp = hrt_absolute_time();
if (accel_x > threshold_accel.get()) {
- integrator += accel_x * dt;
+ integrator += dt;
// warnx("*** integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f",
// (double)integrator, (double)threshold_accel.get(), (double)threshold_time.get(), (double)accel_x, (double)dt);
- if (integrator > threshold_accel.get() * threshold_time.get()) {
+ if (integrator > threshold_time.get()) {
launchDetected = true;
}
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 5673037b8..9885176b7 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -639,6 +639,11 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
case VEHICLE_CMD_PREFLIGHT_CALIBRATION:
case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
case VEHICLE_CMD_PREFLIGHT_STORAGE:
+ case VEHICLE_CMD_CUSTOM_0:
+ case VEHICLE_CMD_CUSTOM_1:
+ case VEHICLE_CMD_CUSTOM_2:
+ case VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY:
+ case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY:
/* ignore commands that handled in low prio loop */
break;
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 5a92004a6..a2f3828ff 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -886,8 +886,8 @@ protected:
msg.eph = cm_uint16_from_m_float(gps.eph);
msg.epv = cm_uint16_from_m_float(gps.epv);
msg.vel = cm_uint16_from_m_float(gps.vel_m_s),
- msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
- msg.satellites_visible = gps.satellites_used;
+ msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
+ msg.satellites_visible = gps.satellites_used;
_mavlink->send_message(MAVLINK_MSG_ID_GPS_RAW_INT, &msg);
}
@@ -957,11 +957,11 @@ protected:
msg.lat = pos.lat * 1e7;
msg.lon = pos.lon * 1e7;
msg.alt = pos.alt * 1000.0f;
- msg.relative_alt = (pos.alt - home.alt) * 1000.0f;
- msg.vx = pos.vel_n * 100.0f;
- msg.vy = pos.vel_e * 100.0f;
- msg.vz = pos.vel_d * 100.0f;
- msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f;
+ msg.relative_alt = (pos.alt - home.alt) * 1000.0f;
+ msg.vx = pos.vel_n * 100.0f;
+ msg.vy = pos.vel_e * 100.0f;
+ msg.vz = pos.vel_d * 100.0f;
+ msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f;
_mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, &msg);
}
@@ -1022,9 +1022,9 @@ protected:
msg.x = pos.x;
msg.y = pos.y;
msg.z = pos.z;
- msg.vx = pos.vx;
- msg.vy = pos.vy;
- msg.vz = pos.vz;
+ msg.vx = pos.vx;
+ msg.vy = pos.vy;
+ msg.vz = pos.vz;
_mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED, &msg);
}
@@ -1085,9 +1085,9 @@ protected:
msg.x = pos.x;
msg.y = pos.y;
msg.z = pos.z;
- msg.roll = pos.roll;
- msg.pitch = pos.pitch;
- msg.yaw = pos.yaw;
+ msg.roll = pos.roll;
+ msg.pitch = pos.pitch;
+ msg.yaw = pos.yaw;
_mavlink->send_message(MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, &msg);
}
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 43161aa70..0da778b6f 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -285,7 +285,9 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
/* XXX range-check value? */
- r_page_servos[offset] = *values;
+ if (*values != PWM_IGNORE_THIS_CHANNEL) {
+ r_page_servos[offset] = *values;
+ }
offset++;
num_values--;
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 6c4b49452..dbda8ea6f 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -76,6 +76,7 @@
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/satellite_info.h>
#include <uORB/topics/vehicle_vicon_position.h>
+#include <uORB/topics/vision_position_estimate.h>
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/battery_status.h>
@@ -934,6 +935,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct vehicle_global_position_s global_pos;
struct position_setpoint_triplet_s triplet;
struct vehicle_vicon_position_s vicon_pos;
+ struct vision_position_estimate vision_pos;
struct optical_flow_s flow;
struct rc_channels_s rc;
struct differential_pressure_s diff_pres;
@@ -984,6 +986,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_EST1_s log_EST1;
struct log_PWR_s log_PWR;
struct log_VICN_s log_VICN;
+ struct log_VISN_s log_VISN;
struct log_GS0A_s log_GS0A;
struct log_GS0B_s log_GS0B;
struct log_GS1A_s log_GS1A;
@@ -1013,6 +1016,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int gps_pos_sub;
int sat_info_sub;
int vicon_pos_sub;
+ int vision_pos_sub;
int flow_sub;
int rc_sub;
int airspeed_sub;
@@ -1043,6 +1047,7 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
+ subs.vision_pos_sub = orb_subscribe(ORB_ID(vision_position_estimate));
subs.flow_sub = orb_subscribe(ORB_ID(optical_flow));
subs.rc_sub = orb_subscribe(ORB_ID(rc_channels));
subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
@@ -1459,6 +1464,22 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_VICN.yaw = buf.vicon_pos.yaw;
LOGBUFFER_WRITE_AND_COUNT(VICN);
}
+
+ /* --- VISION POSITION --- */
+ if (copy_if_updated(ORB_ID(vision_position_estimate), subs.vision_pos_sub, &buf.vision_pos)) {
+ log_msg.msg_type = LOG_VISN_MSG;
+ log_msg.body.log_VISN.x = buf.vision_pos.x;
+ log_msg.body.log_VISN.y = buf.vision_pos.y;
+ log_msg.body.log_VISN.z = buf.vision_pos.z;
+ log_msg.body.log_VISN.vx = buf.vision_pos.vx;
+ log_msg.body.log_VISN.vy = buf.vision_pos.vy;
+ log_msg.body.log_VISN.vz = buf.vision_pos.vz;
+ log_msg.body.log_VISN.qx = buf.vision_pos.q[0];
+ log_msg.body.log_VISN.qy = buf.vision_pos.q[1];
+ log_msg.body.log_VISN.qz = buf.vision_pos.q[2];
+ log_msg.body.log_VISN.qw = buf.vision_pos.q[3];
+ LOGBUFFER_WRITE_AND_COUNT(VISN);
+ }
/* --- FLOW --- */
if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) {
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index fb7609435..6741c7e25 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -391,6 +391,20 @@ struct log_TEL_s {
uint64_t heartbeat_time;
};
+/* --- VISN - VISION POSITION --- */
+#define LOG_VISN_MSG 38
+struct log_VISN_s {
+ float x;
+ float y;
+ float z;
+ float vx;
+ float vy;
+ float vz;
+ float qx;
+ float qy;
+ float qz;
+ float qw;
+};
/********** SYSTEM MESSAGES, ID > 0x80 **********/
@@ -449,6 +463,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"),
LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),
LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"),
+ LOG_FORMAT(VISN, "ffffffffff", "X,Y,Z,VX,VY,VZ,QuatX,QuatY,QuatZ,QuatW"),
LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h
index 7db33d98b..44aa50572 100644
--- a/src/modules/uORB/topics/vehicle_command.h
+++ b/src/modules/uORB/topics/vehicle_command.h
@@ -1,9 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
- * @author Lorenz Meier <lm@inf.ethz.ch>
+ * Copyright (C) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -37,6 +34,10 @@
/**
* @file vehicle_command.h
* Definition of the vehicle command uORB topic.
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
*/
#ifndef TOPIC_VEHICLE_COMMAND_H_
@@ -52,6 +53,9 @@
* but can contain additional ones.
*/
enum VEHICLE_CMD {
+ VEHICLE_CMD_CUSTOM_0 = 0, /* test command */
+ VEHICLE_CMD_CUSTOM_1 = 1, /* test command */
+ VEHICLE_CMD_CUSTOM_2 = 2, /* test command */
VEHICLE_CMD_NAV_WAYPOINT = 16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */
VEHICLE_CMD_NAV_LOITER_UNLIM = 17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */
VEHICLE_CMD_NAV_LOITER_TURNS = 18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */
@@ -87,7 +91,8 @@ enum VEHICLE_CMD {
VEHICLE_CMD_MISSION_START = 300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */
VEHICLE_CMD_COMPONENT_ARM_DISARM = 400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */
VEHICLE_CMD_START_RX_PAIR = 500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */
- VEHICLE_CMD_ENUM_END = 501, /* | */
+ VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001, /**< Prepare a payload deployment in the flight plan */
+ VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 /**< Control a pre-programmed payload deployment */
};
/**
@@ -115,8 +120,8 @@ struct vehicle_command_s {
float param2; /**< Parameter 2, as defined by MAVLink VEHICLE_CMD enum. */
float param3; /**< Parameter 3, as defined by MAVLink VEHICLE_CMD enum. */
float param4; /**< Parameter 4, as defined by MAVLink VEHICLE_CMD enum. */
- float param5; /**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum. */
- float param6; /**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum. */
+ double param5; /**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum. */
+ double param6; /**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum. */
float param7; /**< Parameter 7, as defined by MAVLink VEHICLE_CMD enum. */
enum VEHICLE_CMD command; /**< Command ID, as defined MAVLink by VEHICLE_CMD enum. */
uint8_t target_system; /**< System which should execute the command */
diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp
index 8548660fe..0d67aad47 100644
--- a/src/modules/uavcan/sensors/gnss.cpp
+++ b/src/modules/uavcan/sensors/gnss.cpp
@@ -70,6 +70,16 @@ unsigned UavcanGnssBridge::get_num_redundant_channels() const
return (_receiver_node_id < 0) ? 0 : 1;
}
+void UavcanGnssBridge::print_status() const
+{
+ printf("RX errors: %d, receiver node id: ", _sub_fix.getFailureCount());
+ if (_receiver_node_id < 0) {
+ printf("N/A\n");
+ } else {
+ printf("%d\n", _receiver_node_id);
+ }
+}
+
void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg)
{
// This bridge does not support redundant GNSS receivers yet.
diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp
index c2b6e4195..e8466b401 100644
--- a/src/modules/uavcan/sensors/gnss.hpp
+++ b/src/modules/uavcan/sensors/gnss.hpp
@@ -66,6 +66,8 @@ public:
unsigned get_num_redundant_channels() const override;
+ void print_status() const override;
+
private:
/**
* GNSS fix message will be reported via this callback.
diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp
index a98596f9c..9608ce680 100644
--- a/src/modules/uavcan/sensors/sensor_bridge.cpp
+++ b/src/modules/uavcan/sensors/sensor_bridge.cpp
@@ -58,7 +58,7 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase()
{
for (unsigned i = 0; i < _max_channels; i++) {
- if (_channels[i].redunancy_channel_id >= 0) {
+ if (_channels[i].node_id >= 0) {
(void)unregister_class_devname(_class_devname, _channels[i].class_instance);
}
}
@@ -66,13 +66,15 @@ UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase()
delete [] _channels;
}
-void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const void *report)
+void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
{
+ assert(report != nullptr);
+
Channel *channel = nullptr;
// Checking if such channel already exists
for (unsigned i = 0; i < _max_channels; i++) {
- if (_channels[i].redunancy_channel_id == redundancy_channel_id) {
+ if (_channels[i].node_id == node_id) {
channel = _channels + i;
break;
}
@@ -84,11 +86,11 @@ void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const
return; // Give up immediately - saves some CPU time
}
- log("adding channel %d...", redundancy_channel_id);
+ log("adding channel %d...", node_id);
// Search for the first free channel
for (unsigned i = 0; i < _max_channels; i++) {
- if (_channels[i].redunancy_channel_id < 0) {
+ if (_channels[i].node_id < 0) {
channel = _channels + i;
break;
}
@@ -111,9 +113,9 @@ void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const
}
// Publish to the appropriate topic, abort on failure
- channel->orb_id = _orb_topics[class_instance];
- channel->redunancy_channel_id = redundancy_channel_id;
- channel->class_instance = class_instance;
+ channel->orb_id = _orb_topics[class_instance];
+ channel->node_id = node_id;
+ channel->class_instance = class_instance;
channel->orb_advert = orb_advertise(channel->orb_id, report);
if (channel->orb_advert < 0) {
@@ -123,7 +125,7 @@ void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const
return;
}
- log("channel %d class instance %d ok", channel->redunancy_channel_id, channel->class_instance);
+ log("channel %d class instance %d ok", channel->node_id, channel->class_instance);
}
assert(channel != nullptr);
@@ -134,9 +136,23 @@ unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const
{
unsigned out = 0;
for (unsigned i = 0; i < _max_channels; i++) {
- if (_channels[i].redunancy_channel_id >= 0) {
+ if (_channels[i].node_id >= 0) {
out += 1;
}
}
return out;
}
+
+void UavcanCDevSensorBridgeBase::print_status() const
+{
+ printf("devname: %s\n", _class_devname);
+
+ for (unsigned i = 0; i < _max_channels; i++) {
+ if (_channels[i].node_id >= 0) {
+ printf("channel %d: node id %d --> class instance %d\n",
+ i, _channels[i].node_id, _channels[i].class_instance);
+ } else {
+ printf("channel %d: empty\n", i);
+ }
+ }
+}
diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp
index a13d0ab35..1316f7ecc 100644
--- a/src/modules/uavcan/sensors/sensor_bridge.hpp
+++ b/src/modules/uavcan/sensors/sensor_bridge.hpp
@@ -69,6 +69,11 @@ public:
virtual unsigned get_num_redundant_channels() const = 0;
/**
+ * Prints current status in a human readable format to stdout.
+ */
+ virtual void print_status() const = 0;
+
+ /**
* Sensor bridge factory.
* Creates a bridge object by its ASCII name, e.g. "gnss", "mag".
* @return nullptr if such bridge can't be created.
@@ -84,7 +89,7 @@ class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CD
{
struct Channel
{
- int redunancy_channel_id = -1;
+ int node_id = -1;
orb_id_t orb_id = nullptr;
orb_advert_t orb_advert = -1;
int class_instance = -1;
@@ -112,13 +117,15 @@ protected:
/**
* Sends one measurement into appropriate ORB topic.
* New redundancy channels will be registered automatically.
- * @param redundancy_channel_id Redundant sensor identifier (e.g. UAVCAN Node ID)
- * @param report ORB message object
+ * @param node_id Sensor's Node ID
+ * @param report Pointer to ORB message object
*/
- void publish(const int redundancy_channel_id, const void *report);
+ void publish(const int node_id, const void *report);
public:
virtual ~UavcanCDevSensorBridgeBase();
unsigned get_num_redundant_channels() const override;
+
+ void print_status() const override;
};
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index 482fec1e0..a8485ee44 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -41,6 +41,7 @@
#include <systemlib/param/param.h>
#include <systemlib/mixer/mixer.h>
#include <systemlib/board_serial.h>
+#include <systemlib/scheduling_priorities.h>
#include <version/version.h>
#include <arch/board/board.h>
#include <arch/chip/chip.h>
@@ -175,7 +176,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
* Start the task. Normally it should never exit.
*/
static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();};
- _instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, StackSize,
+ _instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize,
static_cast<main_t>(run_trampoline), nullptr);
if (_instance->_task < 0) {
@@ -548,14 +549,16 @@ UavcanNode::print_info()
(void)pthread_mutex_lock(&_node_mutex);
// ESC mixer status
- warnx("ESC actuators control groups: sub: %u / req: %u / fds: %u",
- (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
- warnx("ESC mixer: %s", (_mixers == nullptr) ? "NONE" : "OK");
+ printf("ESC actuators control groups: sub: %u / req: %u / fds: %u\n",
+ (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
+ printf("ESC mixer: %s\n", (_mixers == nullptr) ? "NONE" : "OK");
// Sensor bridges
auto br = _sensor_bridges.getHead();
while (br != nullptr) {
- warnx("Sensor '%s': channels: %u", br->get_name(), br->get_num_redundant_channels());
+ printf("Sensor '%s':\n", br->get_name());
+ br->print_status();
+ printf("\n");
br = br->getSibling();
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-04 10:14:11 +0000