mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-01 05:21:36 +08:00
Fix format
This commit is contained in:
committed by
JaeyoungLim
parent
042b48aadf
commit
4f87cb0b76
@@ -85,7 +85,8 @@ si7210_main(int argc, char *argv[])
|
|||||||
cli.support_keep_running = true;
|
cli.support_keep_running = true;
|
||||||
|
|
||||||
int ch;
|
int ch;
|
||||||
while ((ch = cli.getopt(argc, argv, "")) != EOF) {}
|
|
||||||
|
while ((ch = cli.getopt(argc, argv, "")) != EOF) {}
|
||||||
|
|
||||||
const char *verb = cli.parseDefaultArguments(argc, argv);
|
const char *verb = cli.parseDefaultArguments(argc, argv);
|
||||||
|
|
||||||
|
|||||||
@@ -348,7 +348,7 @@ static const StreamListItem streams_list[] = {
|
|||||||
create_stream_list_item<MavlinkStreamScaledPressure3>(),
|
create_stream_list_item<MavlinkStreamScaledPressure3>(),
|
||||||
#endif // SCALED_PRESSURE3
|
#endif // SCALED_PRESSURE3
|
||||||
#if defined(SENSOR_AIRFLOW_ANGLES_HPP)
|
#if defined(SENSOR_AIRFLOW_ANGLES_HPP)
|
||||||
create_stream_list_item<MavlinkStreamSensorAirflowAngles>(),
|
create_stream_list_item<MavlinkStreamSensorAirflowAngles>(),
|
||||||
#endif // SENSOR_AIRFLOW_ANGLES
|
#endif // SENSOR_AIRFLOW_ANGLES
|
||||||
#if defined(ACTUATOR_OUTPUT_STATUS_HPP)
|
#if defined(ACTUATOR_OUTPUT_STATUS_HPP)
|
||||||
create_stream_list_item<MavlinkStreamActuatorOutputStatus>(),
|
create_stream_list_item<MavlinkStreamActuatorOutputStatus>(),
|
||||||
|
|||||||
@@ -50,11 +50,11 @@ public:
|
|||||||
|
|
||||||
unsigned get_size() override
|
unsigned get_size() override
|
||||||
{
|
{
|
||||||
if (_airflow_aoa_sub.advertised() || _airflow_slip_sub.advertised()) {
|
if (_airflow_aoa_sub.advertised() || _airflow_slip_sub.advertised()) {
|
||||||
return MAVLINK_MSG_ID_SENSOR_AIRFLOW_ANGLES_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
return MAVLINK_MSG_ID_SENSOR_AIRFLOW_ANGLES_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
||||||
}
|
}
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
@@ -66,34 +66,40 @@ private:
|
|||||||
bool send() override
|
bool send() override
|
||||||
{
|
{
|
||||||
if (_airflow_aoa_sub.updated() || _airflow_slip_sub.updated()) {
|
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;
|
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_slip_s airflow_slip;
|
if (_airflow_aoa_sub.copy(&airflow_aoa)) {
|
||||||
if (_airflow_slip_sub.copy(&airflow_slip)) {
|
msg.timestamp = airflow_aoa.timestamp / 1000;
|
||||||
const uint64_t timestamp = airflow_slip.timestamp / 1000;
|
msg.angleofattack = math::degrees(airflow_aoa.aoa_rad);
|
||||||
if (timestamp > msg.timestamp) {
|
msg.angleofattack_valid = airflow_aoa.valid;
|
||||||
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);
|
} 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;
|
return true;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -132,15 +132,15 @@ private:
|
|||||||
uORB::Subscription _diff_pres_sub{ORB_ID(differential_pressure)};
|
uORB::Subscription _diff_pres_sub{ORB_ID(differential_pressure)};
|
||||||
uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)};
|
uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)};
|
||||||
uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
|
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
|
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), 0},
|
||||||
{ORB_ID(sensor_hall), 1},
|
{ORB_ID(sensor_hall), 1},
|
||||||
{ORB_ID(sensor_hall), 2},
|
{ORB_ID(sensor_hall), 2},
|
||||||
{ORB_ID(sensor_hall), 3},
|
{ORB_ID(sensor_hall), 3},
|
||||||
};
|
};
|
||||||
|
|
||||||
int _av_aoa_hall_sub_index = -1; // AoA 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
|
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<airspeed_s> _airspeed_pub{ORB_ID(airspeed)};
|
||||||
uORB::Publication<sensor_combined_s> _sensor_pub{ORB_ID(sensor_combined)};
|
uORB::Publication<sensor_combined_s> _sensor_pub{ORB_ID(sensor_combined)};
|
||||||
@@ -442,7 +442,7 @@ void Sensors::diff_pres_poll()
|
|||||||
} else {
|
} else {
|
||||||
// differential pressure temperature invalid, check barometer
|
// differential pressure temperature invalid, check barometer
|
||||||
if ((air_data.timestamp != 0) && PX4_ISFINITE(air_data.baro_temp_celcius)
|
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
|
// TODO: review PCB_TEMP_ESTIMATE_DEG, ignore for external baro
|
||||||
air_temperature_celsius = air_data.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG;
|
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 */
|
/* don't risk to feed negative airspeed into the system */
|
||||||
airspeed.indicated_airspeed_m_s = calc_IAS_corrected((enum AIRSPEED_COMPENSATION_MODEL)
|
airspeed.indicated_airspeed_m_s = calc_IAS_corrected((enum AIRSPEED_COMPENSATION_MODEL)
|
||||||
_parameters.air_cmodel,
|
_parameters.air_cmodel,
|
||||||
smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm,
|
smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm,
|
||||||
diff_pres.differential_pressure_filtered_pa, air_data.baro_pressure_pa,
|
diff_pres.differential_pressure_filtered_pa, air_data.baro_pressure_pa,
|
||||||
air_temperature_celsius);
|
air_temperature_celsius);
|
||||||
|
|
||||||
airspeed.true_airspeed_m_s = calc_TAS_from_CAS(airspeed.indicated_airspeed_m_s, air_data.baro_pressure_pa,
|
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;
|
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)) {
|
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
|
// A hall sensor with selected driver ID for the AoA vane has been found/set
|
||||||
struct sensor_hall_s sensor_hall;
|
struct sensor_hall_s sensor_hall;
|
||||||
|
|
||||||
if (_sensor_hall_subs[_av_aoa_hall_sub_index].update(&sensor_hall)) {
|
if (_sensor_hall_subs[_av_aoa_hall_sub_index].update(&sensor_hall)) {
|
||||||
airflow_aoa_s airflow_aoa;
|
airflow_aoa_s airflow_aoa;
|
||||||
airflow_aoa.timestamp = hrt_absolute_time();
|
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)) {
|
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
|
// A hall sensor with selected driver ID for the slip vane has been found/set
|
||||||
struct sensor_hall_s sensor_hall;
|
struct sensor_hall_s sensor_hall;
|
||||||
|
|
||||||
if (_sensor_hall_subs[_av_slip_hall_sub_index].update(&sensor_hall)) {
|
if (_sensor_hall_subs[_av_slip_hall_sub_index].update(&sensor_hall)) {
|
||||||
airflow_slip_s airflow_slip;
|
airflow_slip_s airflow_slip;
|
||||||
airflow_slip.timestamp = hrt_absolute_time();
|
airflow_slip.timestamp = hrt_absolute_time();
|
||||||
@@ -663,7 +665,7 @@ void Sensors::hall_poll()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// find the Slip hall sensor index if not set yet
|
// 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);
|
_av_slip_hall_sub_index = getHallSubIndex(_parameters.CAL_AV_SLIP_ID);
|
||||||
|
|
||||||
if (_av_slip_hall_sub_index >= 0) {
|
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++) {
|
for (unsigned i = 0; i < MAX_SENSOR_COUNT; i++) {
|
||||||
sensor_hall_s sensor_hall;
|
sensor_hall_s sensor_hall;
|
||||||
|
|
||||||
if (_sensor_hall_subs[i].copy(&sensor_hall)) {
|
if (_sensor_hall_subs[i].copy(&sensor_hall)) {
|
||||||
if (sensor_hall.instance == av_driver_id) {
|
if (sensor_hall.instance == av_driver_id) {
|
||||||
return i;
|
return i;
|
||||||
|
|||||||
Reference in New Issue
Block a user