Skip to content

Commit

Permalink
add second diff pressure sensor, perform port ID update in real time …
Browse files Browse the repository at this point in the history 
…and add simple logging when run via launch file
  • Loading branch information
@PonomarevDA
PonomarevDA committed Nov 19, 2023
1 parent b25bc5b commit 163e8b1
Show file tree
Hide file tree
Showing 7 changed files with 113 additions and 58 deletions.
2 changes: 1 addition & 1 deletion Libs/cyphal_application
Submodule cyphal_application updated 2 files
+5 −5 Udral/battery.cpp
+7 −7 Udral/battery.hpp
2 changes: 1 addition & 1 deletion launch/cyphal_communicator.launch
View file
Original file line number Diff line number Diff line change
@@ -1,3 +1,3 @@
<launch>
<node pkg="cyphal_communicator" type="cyphal_communicator" name="cyphal_communicator" required="true" />
<node pkg="cyphal_communicator" type="cyphal_communicator" name="cyphal_communicator" output="screen" required="true" />
</launch>
30 changes: 29 additions & 1 deletion src/autopilot_interface/cyphal_hitl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@ int CyphalHitlInterface::init() {
}

void CyphalHitlInterface::process() {
_update_port_identifiers();
cyphal.process();
cyphal.process();
}
Expand Down Expand Up @@ -114,7 +115,8 @@ void CyphalHitlInterface::publish_battery(float voltage, float current, float te
}

void CyphalHitlInterface::publish_diff_pressure(float pressure) {
diff_pressure.publish(pressure);
diff_pressure_0.publish(pressure);
diff_pressure_1.publish(pressure);
}

void CyphalHitlInterface::publish_rangefinder(float range) {
Expand Down Expand Up @@ -154,3 +156,29 @@ void CyphalHitlInterface::clear_servo_pwm_counter() {
void CyphalHitlInterface::set_time_factor(double time_factor) {
_time_factor = std::clamp(time_factor, 0.7, 1.0);
}

void CyphalHitlInterface::_update_port_identifiers() {
esc_feedback_0.setPortId(paramsGetIntegerValue(PARAM_ESC_FEEDBACK_0_ID));
esc_feedback_1.setPortId(paramsGetIntegerValue(PARAM_ESC_FEEDBACK_1_ID));
esc_feedback_2.setPortId(paramsGetIntegerValue(PARAM_ESC_FEEDBACK_2_ID));
esc_feedback_3.setPortId(paramsGetIntegerValue(PARAM_ESC_FEEDBACK_3_ID));

gps_point.setPortId(paramsGetIntegerValue(PARAM_GPS_POINT_ID));
gps_sats.setPortId(paramsGetIntegerValue(PARAM_GPS_SATS_ID));
gps_status.setPortId(paramsGetIntegerValue(PARAM_GPS_STATUS_ID));
gps_pdop.setPortId(paramsGetIntegerValue(PARAM_GPS_PDOP_ID));

baro_pressure.setPortId(paramsGetIntegerValue(BAROMETER_PRESSURE_ID));
baro_temperature.setPortId(paramsGetIntegerValue(BAROMETER_TEMPERATURE_ID));
magnetometer.setPortId(paramsGetIntegerValue(PARAM_MAGNETOMETER_ID));
accel.setPortId(paramsGetIntegerValue(IMU_ACCEL_ID));
gyro.setPortId(paramsGetIntegerValue(IMU_GYRO_ID));
imu.setPortId(paramsGetIntegerValue(PARAM_IMU_IMU_ID));
diff_pressure_0.setPortId(paramsGetIntegerValue(PARAM_ASPD_DIFF_PRESSURE_0_ID));
diff_pressure_1.setPortId(paramsGetIntegerValue(PARAM_ASPD_DIFF_PRESSURE_1_ID));
rangefinder.setPortId(paramsGetIntegerValue(PARAM_RANGEFINDER_ID));

battery.source_pub.setPortId(paramsGetIntegerValue(BMS_ENERGY_SOURCE_ID));
battery.status_pub.setPortId(paramsGetIntegerValue(BMS_BATTERY_STATUS_ID));
battery.parameters_pub.setPortId(paramsGetIntegerValue(BMS_BATTERY_PARAMETERS_ID));
}
49 changes: 27 additions & 22 deletions src/autopilot_interface/cyphal_hitl.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,27 +24,29 @@

class CyphalHitlInterface {
public:
CyphalHitlInterface() : setpoint(&cyphal),
readiness(&cyphal),
rgbled(&cyphal),
esc_feedback_0(&cyphal, 2500),
esc_feedback_1(&cyphal, 2501),
esc_feedback_2(&cyphal, 2502),
esc_feedback_3(&cyphal, 2503),
esc_feedback{{&esc_feedback_0, &esc_feedback_1, &esc_feedback_2, &esc_feedback_3}},
gps_point(&cyphal, 2406),
gps_sats(&cyphal, 2407),
gps_status(&cyphal, 2408),
gps_pdop(&cyphal, 2409),
baro_pressure(&cyphal, 2404),
baro_temperature(&cyphal, 2403),
magnetometer(&cyphal, 2402),
accel(&cyphal, 2400),
gyro(&cyphal, 2401),
imu(&cyphal, 2300),
diff_pressure(&cyphal, 2600),
rangefinder(&cyphal, 2800),
battery(&cyphal, 2700, 2701, 2702) {}
CyphalHitlInterface() :
setpoint(&cyphal),
readiness(&cyphal),
rgbled(&cyphal),
esc_feedback_0(&cyphal, 0),
esc_feedback_1(&cyphal, 0),
esc_feedback_2(&cyphal, 0),
esc_feedback_3(&cyphal, 0),
esc_feedback{{&esc_feedback_0, &esc_feedback_1, &esc_feedback_2, &esc_feedback_3}},
gps_point(&cyphal, 0),
gps_sats(&cyphal, 0),
gps_status(&cyphal, 0),
gps_pdop(&cyphal, 0),
baro_pressure(&cyphal, 0),
baro_temperature(&cyphal, 0),
magnetometer(&cyphal, 0),
accel(&cyphal, 0),
gyro(&cyphal, 0),
imu(&cyphal, 0),
diff_pressure_0(&cyphal, 0),
diff_pressure_1(&cyphal, 0),
rangefinder(&cyphal, 0),
battery(&cyphal, 0, 0, 0) {}
int init();
void process();

Expand All @@ -63,6 +65,8 @@ class CyphalHitlInterface {
void clear_servo_pwm_counter();
void set_time_factor(double time_factor);
private:
void _update_port_identifiers();

Cyphal cyphal;

SetpointSubscriber setpoint;
Expand All @@ -88,7 +92,8 @@ class CyphalHitlInterface {
ImuGyroPublisher gyro;
RawImuPublisher imu;

DiffPressurePublisher diff_pressure;
DiffPressurePublisher diff_pressure_0;
DiffPressurePublisher diff_pressure_1;
RangefinderRangePublisher rangefinder;
UdralBatteryPublisher battery;

Expand Down
56 changes: 34 additions & 22 deletions src/params.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,55 +8,67 @@
IntegerDesc_t integer_desc_pool[] = {
{"id", 2, 2, 2, MUTABLE},

{"uavcan.sub.setpoint.id", 2342, 2342, 2342, MUTABLE},
{"uavcan.sub.readiness.id", 2343, 2343, 2343, MUTABLE},
{"uavcan.sub.rgbled.id", 2344, 2344, 2344, MUTABLE},
{"uavcan.sub.setpoint.id", 1, 65535, 2342, MUTABLE},
{"uavcan.sub.readiness.id", 1, 65535, 2343, MUTABLE},
{"uavcan.sub.rgbled.id", 1, 65535, 2344, MUTABLE},

{"uavcan.pub.baro.press.id", 2404, 2404, 2404, MUTABLE},
{"uavcan.pub.baro.temp.id", 2403, 2403, 2403, MUTABLE},
{"uavcan.pub.mag.id", 1, 65535, 2402, MUTABLE},

{"uavcan.pub.gps.point.id", 2406, 2406, 2406, MUTABLE},
{"uavcan.pub.gps.sats.id", 2407, 2407, 2407, MUTABLE},
{"uavcan.pub.gps.status.id", 2408, 2408, 2408, MUTABLE},
{"uavcan.pub.gps.pdop.id", 2409, 2409, 2409, MUTABLE},
{"uavcan.pub.baro.temp.id", 1, 65535, 2403, MUTABLE},
{"uavcan.pub.baro.press.id", 1, 65535, 2404, MUTABLE},

{"uavcan.pub.mag.id", 2402, 2402, 2402, MUTABLE},
{"uavcan.pub.gps.point.id", 1, 65535, 2406, MUTABLE},
{"uavcan.pub.gps.sats.id", 1, 65535, 2407, MUTABLE},
{"uavcan.pub.gps.status.id", 1, 65535, 2408, MUTABLE},
{"uavcan.pub.gps.pdop.id", 1, 65535, 2409, MUTABLE},

{"uavcan.pub.imu.accel.id", 2400, 2400, 2400, MUTABLE},
{"uavcan.pub.imu.gyro.id", 2401, 2401, 2401, MUTABLE},
{"uavcan.pub.imu.imu.id", 2300, 2300, 2300, MUTABLE},
{"uavcan.pub.imu.accel.id", 1, 65535, 2400, MUTABLE},
{"uavcan.pub.imu.gyro.id", 1, 65535, 2401, MUTABLE},
{"uavcan.pub.imu.imu.id", 1, 65535, 2300, MUTABLE},

{"uavcan.pub.aspd.dpres.id", 2600, 2600, 2600, MUTABLE},
{"uavcan.pub.esc.fb.0.id", 1, 65535, 2500, MUTABLE},
{"uavcan.pub.esc.fb.1.id", 1, 65535, 2501, MUTABLE},
{"uavcan.pub.esc.fb.2.id", 1, 65535, 2502, MUTABLE},
{"uavcan.pub.esc.fb.3.id", 1, 65535, 2503, MUTABLE},

{"uavcan.pub.energy_source.id", 2700, 2700, 2700, MUTABLE},
{"uavcan.pub.battery_status.id", 2701, 2701, 2701, MUTABLE},
{"uavcan.pub.battery_parameters.id", 2702, 2702, 2702, MUTABLE},
{"uavcan.pub.aspd.dpres.0.id", 1, 65535, 2600, MUTABLE},
{"uavcan.pub.aspd.dpres.1.id", 1, 65535, 2601, MUTABLE},

{"uavcan.pub.range.id", 2800, 2800, 2800, MUTABLE},
{"uavcan.pub.energy_source.id", 1, 65535, 2700, MUTABLE},
{"uavcan.pub.battery_status.id", 1, 65535, 2701, MUTABLE},
{"uavcan.pub.battery_parameters.id", 1, 65535, 2702, MUTABLE},

{"uavcan.pub.range.id", 1, 65535, 2800, MUTABLE},
};
IntegerParamValue_t integer_values_pool[sizeof(integer_desc_pool) / sizeof(IntegerDesc_t)];

StringDesc_t string_desc_pool[NUM_OF_STR_PARAMS] = {
{"name", "co.racconlab.cyphal_communicator", MUTABLE},
{"name", "co.racconlab.cyphal_communicator", IMMUTABLE},

{"uavcan.sub.setpoint.type", "reg.udral.service.actuator.common.sp.Vector4", IMMUTABLE},
{"uavcan.sub.readiness.type", "reg.udral.service.common.Readiness", IMMUTABLE},
{"uavcan.sub.rgbled.type", "reg.udral.physics.optics.HighColor", IMMUTABLE},

{"uavcan.pub.baro.press.type", "uavcan.si.sample.pressure.Scalar", IMMUTABLE},
{"uavcan.pub.mag.type", "uavcan.si.sample.magnetic_field_strength.Vector3", IMMUTABLE},

{"uavcan.pub.baro.temp.type", "uavcan.si.sample.temperature.Scalar", IMMUTABLE},
{"uavcan.pub.baro.press.type", "uavcan.si.sample.pressure.Scalar", IMMUTABLE},

{"uavcan.pub.gps.point.type", "reg.udral.physics.kinematics.geodetic.PointStateVarTs", IMMUTABLE},
{"uavcan.pub.gps.sats.type", "uavcan.primitive.scalar.Integer16", IMMUTABLE},
{"uavcan.pub.gps.status.type", "uavcan.primitive.scalar.Integer16", IMMUTABLE},
{"uavcan.pub.gps.pdop.type", "uavcan.primitive.scalar.Integer16", IMMUTABLE},

{"uavcan.pub.mag.type", "uavcan.si.sample.magnetic_field_strength.Vector3", IMMUTABLE},

{"uavcan.pub.imu.accel.type", "uavcan.si.sample.acceleration.Vector3", IMMUTABLE},
{"uavcan.pub.imu.gyro.type", "uavcan.si.sample.angular_velocity.Vector3", IMMUTABLE},
{"uavcan.pub.imu.imu.type", "uavcan.primitive.array.Real16", IMMUTABLE},

{"uavcan.pub.esc.fb.0.type", "zubax.telega.CompactFeedback.0.1", IMMUTABLE},
{"uavcan.pub.esc.fb.1.type", "zubax.telega.CompactFeedback.0.1", IMMUTABLE},
{"uavcan.pub.esc.fb.2.type", "zubax.telega.CompactFeedback.0.1", IMMUTABLE},
{"uavcan.pub.esc.fb.3.type", "zubax.telega.CompactFeedback.0.1", IMMUTABLE},

{"uavcan.pub.aspd.dpres.type", "uavcan.si.sample.pressure.Scalar", IMMUTABLE},
{"uavcan.pub.aspd.dpres.type", "uavcan.si.sample.pressure.Scalar", IMMUTABLE},

{"uavcan.pub.energy_source.type", "reg.udral.physics.electricity.SourceTs", IMMUTABLE},
Expand Down
29 changes: 18 additions & 11 deletions src/params.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,30 +12,37 @@ enum IntParamsIndexes : ParamIndex_t {
READINESS_ID,
RGBLED_ID,

BAROMETER_PRESSURE_ID,
BAROMETER_TEMPERATURE_ID,
PARAM_MAGNETOMETER_ID,

GPS_POINT_ID,
GPS_SATS_ID,
GPS_STATUS_ID,
GPS_PDOP_ID,
BAROMETER_TEMPERATURE_ID,
BAROMETER_PRESSURE_ID,

MAGNETOMETER_ID,
PARAM_GPS_POINT_ID,
PARAM_GPS_SATS_ID,
PARAM_GPS_STATUS_ID,
PARAM_GPS_PDOP_ID,

IMU_ACCEL_ID,
IMU_GYRO_ID,
IMU_IMU_ID,
PARAM_IMU_IMU_ID,

PARAM_ESC_FEEDBACK_0_ID,
PARAM_ESC_FEEDBACK_1_ID,
PARAM_ESC_FEEDBACK_2_ID,
PARAM_ESC_FEEDBACK_3_ID,

PARAM_ASPD_DIFF_PRESSURE_0_ID,
PARAM_ASPD_DIFF_PRESSURE_1_ID,

ASPD_DIFF_PRESSURE_ID,
BMS_ENERGY_SOURCE_ID,
BMS_BATTERY_STATUS_ID,
BMS_BATTERY_PARAMETERS_ID,

RANGEFINDER_ID,
PARAM_RANGEFINDER_ID,

INTEGER_PARAMS_AMOUNT
};

#define NUM_OF_STR_PARAMS 19
#define NUM_OF_STR_PARAMS 24

#endif // PARAMS_HPP_
3 changes: 3 additions & 0 deletions src/simulator_interface/ros_interface/ros_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,9 @@

RosInterface::RosInterface(int argc, char** argv) {
ros::init(argc, argv, "cyphal_communicator");
if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info) ) {
ros::console::notifyLoggerLevelsChanged();
}
ros_node = new ros::NodeHandle;
}

Expand Down

0 comments on commit 163e8b1

Please sign in to comment.