fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fix format

Browse Source
This commit is contained in:
Florian Achermann 2021-06-09 11:05:28 +02:00 committed by JaeyoungLim
parent 042b48aadf
commit 4f87cb0b76
4 changed files with 56 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/drivers/si7210/si7210_main.cpp
View File

@ -85,7 +85,8 @@ si7210_main(int argc, char *argv[])
cli.support_keep_running = true;
int ch;
while ((ch = cli.getopt(argc, argv, "")) != EOF) {}
while ((ch = cli.getopt(argc, argv, "")) != EOF) {}
const char *verb = cli.parseDefaultArguments(argc, argv);

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

@ -348,7 +348,7 @@ static const StreamListItem streams_list[] = {
create_stream_list_item<MavlinkStreamScaledPressure3>(),
#endif // SCALED_PRESSURE3
#if defined(SENSOR_AIRFLOW_ANGLES_HPP)
create_stream_list_item<MavlinkStreamSensorAirflowAngles>(),
create_stream_list_item<MavlinkStreamSensorAirflowAngles>(),
#endif // SENSOR_AIRFLOW_ANGLES
#if defined(ACTUATOR_OUTPUT_STATUS_HPP)
create_stream_list_item<MavlinkStreamActuatorOutputStatus>(),

64
src/modules/mavlink/streams/SENSOR_AIRFLOW_ANGLES.hpp
View File

@ -50,11 +50,11 @@ public:
unsigned get_size() override
{
if (_airflow_aoa_sub.advertised() || _airflow_slip_sub.advertised()) {
return MAVLINK_MSG_ID_SENSOR_AIRFLOW_ANGLES_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
if (_airflow_aoa_sub.advertised() || _airflow_slip_sub.advertised()) {
return MAVLINK_MSG_ID_SENSOR_AIRFLOW_ANGLES_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
return 0;
return 0;
}
private:
@ -66,34 +66,40 @@ private:
bool send() override
{
if (_airflow_aoa_sub.updated() || _airflow_slip_sub.updated()) {
mavlink_sensor_airflow_angles_t msg{};
mavlink_sensor_airflow_angles_t msg{};
struct airflow_aoa_s airflow_aoa;
if (_airflow_aoa_sub.copy(&airflow_aoa)) {
msg.timestamp = airflow_aoa.timestamp / 1000;
msg.angleofattack = math::degrees(airflow_aoa.aoa_rad);
msg.angleofattack_valid = airflow_aoa.valid;
} else {
msg.timestamp = 0;
msg.angleofattack = 0.0;
msg.angleofattack_valid = false;
}
struct airflow_aoa_s airflow_aoa;
struct airflow_slip_s airflow_slip;
if (_airflow_slip_sub.copy(&airflow_slip)) {
const uint64_t timestamp = airflow_slip.timestamp / 1000;
if (timestamp > msg.timestamp) {
msg.timestamp = timestamp;
}
msg.sideslip = math::degrees(airflow_slip.slip_rad);
msg.sideslip_valid = airflow_slip.valid;
} else {
msg.timestamp = 0;
msg.sideslip = 0.0;
msg.sideslip_valid = false;
}
if (_airflow_aoa_sub.copy(&airflow_aoa)) {
msg.timestamp = airflow_aoa.timestamp / 1000;
msg.angleofattack = math::degrees(airflow_aoa.aoa_rad);
msg.angleofattack_valid = airflow_aoa.valid;
mavlink_msg_sensor_airflow_angles_send_struct(_mavlink->get_channel(), &msg);
} else {
msg.timestamp = 0;
msg.angleofattack = 0.0;
msg.angleofattack_valid = false;
}
struct airflow_slip_s airflow_slip;
if (_airflow_slip_sub.copy(&airflow_slip)) {
const uint64_t timestamp = airflow_slip.timestamp / 1000;
if (timestamp > msg.timestamp) {
msg.timestamp = timestamp;
}
msg.sideslip = math::degrees(airflow_slip.slip_rad);
msg.sideslip_valid = airflow_slip.valid;
} else {
msg.timestamp = 0;
msg.sideslip = 0.0;
msg.sideslip_valid = false;
}
mavlink_msg_sensor_airflow_angles_send_struct(_mavlink->get_channel(), &msg);
return true;
}

33
src/modules/sensors/sensors.cpp
View File

@ -132,15 +132,15 @@ private:
uORB::Subscription _diff_pres_sub{ORB_ID(differential_pressure)};
uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
uORB::Subscription _sensor_hall_subs[MAX_SENSOR_COUNT] { // could be set to ORB_MULTI_MAX_INSTANCES if needed
{ORB_ID(sensor_hall), 0},
{ORB_ID(sensor_hall), 1},
{ORB_ID(sensor_hall), 2},
{ORB_ID(sensor_hall), 3},
};
uORB::Subscription _sensor_hall_subs[MAX_SENSOR_COUNT] { // could be set to ORB_MULTI_MAX_INSTANCES if needed
{ORB_ID(sensor_hall), 0},
{ORB_ID(sensor_hall), 1},
{ORB_ID(sensor_hall), 2},
{ORB_ID(sensor_hall), 3},
};
int _av_aoa_hall_sub_index = -1; // AoA vane index for hall effect subscription
int _av_slip_hall_sub_index = -1; // Slip vane index for hall effect subscription
int _av_aoa_hall_sub_index = -1; // AoA vane index for hall effect subscription
int _av_slip_hall_sub_index = -1; // Slip vane index for hall effect subscription
uORB::Publication<airspeed_s> _airspeed_pub{ORB_ID(airspeed)};
uORB::Publication<sensor_combined_s> _sensor_pub{ORB_ID(sensor_combined)};
@ -442,7 +442,7 @@ void Sensors::diff_pres_poll()
} else {
// differential pressure temperature invalid, check barometer
if ((air_data.timestamp != 0) && PX4_ISFINITE(air_data.baro_temp_celcius)
&& (air_data.baro_temp_celcius >= -40.f) && (air_data.baro_temp_celcius <= 125.f)) {
&& (air_data.baro_temp_celcius >= -40.f) && (air_data.baro_temp_celcius <= 125.f)) {
// TODO: review PCB_TEMP_ESTIMATE_DEG, ignore for external baro
air_temperature_celsius = air_data.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG;
@ -480,13 +480,13 @@ void Sensors::diff_pres_poll()
/* don't risk to feed negative airspeed into the system */
airspeed.indicated_airspeed_m_s = calc_IAS_corrected((enum AIRSPEED_COMPENSATION_MODEL)
_parameters.air_cmodel,
smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm,
diff_pres.differential_pressure_filtered_pa, air_data.baro_pressure_pa,
air_temperature_celsius);
_parameters.air_cmodel,
smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm,
diff_pres.differential_pressure_filtered_pa, air_data.baro_pressure_pa,
air_temperature_celsius);
airspeed.true_airspeed_m_s = calc_TAS_from_CAS(airspeed.indicated_airspeed_m_s, air_data.baro_pressure_pa,
air_temperature_celsius); // assume that CAS = IAS as we don't have an CAS-scale here
air_temperature_celsius); // assume that CAS = IAS as we don't have an CAS-scale here
airspeed.air_temperature_celsius = air_temperature_celsius;
@ -593,6 +593,7 @@ void Sensors::hall_poll()
if ((_av_aoa_hall_sub_index >= 0) && (_av_aoa_hall_sub_index < MAX_SENSOR_COUNT)) {
// A hall sensor with selected driver ID for the AoA vane has been found/set
struct sensor_hall_s sensor_hall;
if (_sensor_hall_subs[_av_aoa_hall_sub_index].update(&sensor_hall)) {
airflow_aoa_s airflow_aoa;
airflow_aoa.timestamp = hrt_absolute_time();
@ -624,6 +625,7 @@ void Sensors::hall_poll()
if ((_av_slip_hall_sub_index >= 0) && (_av_slip_hall_sub_index < MAX_SENSOR_COUNT)) {
// A hall sensor with selected driver ID for the slip vane has been found/set
struct sensor_hall_s sensor_hall;
if (_sensor_hall_subs[_av_slip_hall_sub_index].update(&sensor_hall)) {
airflow_slip_s airflow_slip;
airflow_slip.timestamp = hrt_absolute_time();
@ -663,7 +665,7 @@ void Sensors::hall_poll()
}
// find the Slip hall sensor index if not set yet
if (_parameters.CAL_AV_SLIP_ID >= 0&& _av_slip_hall_sub_index < 0) {
if (_parameters.CAL_AV_SLIP_ID >= 0 && _av_slip_hall_sub_index < 0) {
_av_slip_hall_sub_index = getHallSubIndex(_parameters.CAL_AV_SLIP_ID);
if (_av_slip_hall_sub_index >= 0) {
@ -676,6 +678,7 @@ int Sensors::getHallSubIndex(const int av_driver_id)
{
for (unsigned i = 0; i < MAX_SENSOR_COUNT; i++) {
sensor_hall_s sensor_hall;
if (_sensor_hall_subs[i].copy(&sensor_hall)) {
if (sensor_hall.instance == av_driver_id) {
return i;