forked from Archive/PX4-Autopilot
Compare commits
75 Commits
pr-uorb_qu
...
main
Author | SHA1 | Date |
---|---|---|
Matthias Grob | b5f6699f2e | |
Matthias Grob | 1096384a38 | |
Matthias Grob | 999a71c4dd | |
Thomas Frans | bcbae86b9f | |
Eric Katzfey | 4a553938fb | |
Daniel Agar | c024ea396a | |
Eric Katzfey | 69028f37a9 | |
Thomas Frans | bb9f4d42f3 | |
Beat Küng | 2e12e14a23 | |
Thomas Frans | d0251b8688 | |
Eric Katzfey | 82a1aa37db | |
Eric Katzfey | 5f6dc1c5d0 | |
Øyvind Taksdal Stubhaug | 710286da72 | |
Daniel Agar | 34c19b2e5a | |
alexklimaj | af16544809 | |
Daniel Agar | 35532609c9 | |
bresch | 62b8db153b | |
bresch | 638e17d551 | |
bresch | 6d819343aa | |
bresch | cb2bb2e098 | |
bresch | c9221b91ad | |
Drone-Lab | 37caddedbb | |
muramura | 63850873eb | |
Hamish Willee | 95627ea098 | |
bresch | 2e6dd243af | |
muramura | 32aa3263a6 | |
Beat Küng | 70346a5b2f | |
Silvan Fuhrer | da39d075ac | |
Silvan Fuhrer | f6430a27d6 | |
Silvan Fuhrer | 4ae2fbd171 | |
Silvan Fuhrer | 6e15dd5328 | |
Silvan Fuhrer | d330d47495 | |
Silvan Fuhrer | 6d8273483c | |
bresch | 7c6ecd95a8 | |
bresch | b2f1122372 | |
bresch | ee63f3e664 | |
bresch | e3f67d5c1a | |
Silvan Fuhrer | 6373d8d243 | |
Silvan Fuhrer | 68fcfc43ef | |
Eric Katzfey | ae947513d7 | |
Alexander Lampalzer | ad50afda10 | |
Niklas Hauser | 23c5c0b12d | |
Daniel Agar | a1cce7e961 | |
Daniel Agar | b115d3cd44 | |
Silvan Fuhrer | 9f4ae0a85d | |
Silvan Fuhrer | 00cc68baa1 | |
Silvan Fuhrer | 7fe5ee64fe | |
Silvan Fuhrer | 7f370ac6df | |
Thomas Stauber | 1ad83a8002 | |
Eric Katzfey | 18d53c3bfd | |
Eric Katzfey | f4ebfa6130 | |
Konrad | c5fde63440 | |
Silvan Fuhrer | cb8520427c | |
Silvan Fuhrer | 86c074378f | |
Konrad | e9fda548fa | |
Konrad | 91d1342f20 | |
Konrad | 89844625b4 | |
Konrad | fde71cd15e | |
Konrad | 14e4169473 | |
KonradRudin | 8dcfcf5b9e | |
alexklimaj | a80a5a92f4 | |
alexklimaj | b81ad8841e | |
Eric Katzfey | 57df7e35b2 | |
Alexis Guijarro | 006dcfafb7 | |
Silvan Fuhrer | 85a882e1ce | |
Konrad | 04099ed483 | |
Konrad | 1aa26a5a91 | |
Konrad | acd750e033 | |
Konrad | 6c6142ba79 | |
Konrad | 7fb584adbe | |
Konrad | fb3aab1fb0 | |
Konrad | 1b03ac4d2b | |
Konrad | 815cea2abb | |
Konrad | 51321c605e | |
Konrad | a0ae073d8c |
|
@ -0,0 +1,14 @@
|
|||
root = true
|
||||
|
||||
[*]
|
||||
insert_final_newline = false
|
||||
|
||||
[{*.{c,cpp,cc,h,hpp},CMakeLists.txt,Kconfig}]
|
||||
indent_style = tab
|
||||
tab_width = 8
|
||||
# Not in the official standard, but supported by many editors
|
||||
max_line_length = 120
|
||||
|
||||
[*.yaml]
|
||||
indent_style = space
|
||||
indent_size = 2
|
|
@ -81,5 +81,5 @@
|
|||
url = https://github.com/PX4/PX4-gazebo-models.git
|
||||
branch = main
|
||||
[submodule "boards/modalai/voxl2/libfc-sensor-api"]
|
||||
path = src/modules/muorb/apps/libfc-sensor-api
|
||||
path = boards/modalai/voxl2/libfc-sensor-api
|
||||
url = https://gitlab.com/voxl-public/voxl-sdk/core-libs/libfc-sensor-api.git
|
||||
|
|
|
@ -30,5 +30,5 @@ exec find boards msg src platforms test \
|
|||
-path src/lib/cdrstream/cyclonedds -prune -o \
|
||||
-path src/lib/cdrstream/rosidl -prune -o \
|
||||
-path src/modules/zenoh/zenoh-pico -prune -o \
|
||||
-path src/modules/muorb/apps/libfc-sensor-api -prune -o \
|
||||
-path boards/modalai/voxl2/libfc-sensor-api -prune -o \
|
||||
-type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN
|
||||
|
|
|
@ -4,7 +4,7 @@
|
|||
"description": "Firmware for the ARK flow board",
|
||||
"image": "",
|
||||
"build_time": 0,
|
||||
"summary": "ARFFLOW",
|
||||
"summary": "ARKFLOW",
|
||||
"version": "0.1",
|
||||
"image_size": 0,
|
||||
"image_maxsize": 2080768,
|
||||
|
|
Binary file not shown.
|
@ -20,6 +20,7 @@ CONFIG_UAVCANNODE_RTK_DATA=y
|
|||
CONFIG_UAVCANNODE_SAFETY_BUTTON=y
|
||||
CONFIG_UAVCANNODE_STATIC_PRESSURE=y
|
||||
CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
|
||||
CONFIG_MODULES_EKF2=y
|
||||
CONFIG_MODULES_GYRO_CALIBRATION=y
|
||||
CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
|
||||
CONFIG_MODULES_SENSORS=y
|
||||
|
|
|
@ -11,3 +11,5 @@ param set-default SENS_IMU_CLPNOTI 0
|
|||
|
||||
safety_button start
|
||||
tone_alarm start
|
||||
|
||||
ekf2 start
|
||||
|
|
|
@ -50,6 +50,7 @@ CONFIG_STACK_COLORATION=y
|
|||
CONFIG_START_DAY=30
|
||||
CONFIG_START_MONTH=11
|
||||
CONFIG_STDIO_DISABLE_BUFFERING=y
|
||||
CONFIG_STM32_FLASH_CONFIG_G=y
|
||||
CONFIG_STM32_NOEXT_VECTORS=y
|
||||
CONFIG_STM32_TIM8=y
|
||||
CONFIG_TASK_NAME_SIZE=0
|
||||
|
|
|
@ -123,6 +123,7 @@ CONFIG_STM32_ADC1=y
|
|||
CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y
|
||||
CONFIG_STM32_DMA1=y
|
||||
CONFIG_STM32_DMA2=y
|
||||
CONFIG_STM32_FLASH_CONFIG_G=y
|
||||
CONFIG_STM32_FLASH_PREFETCH=y
|
||||
CONFIG_STM32_FLOWCONTROL_BROKEN=y
|
||||
CONFIG_STM32_I2C1=y
|
||||
|
|
|
@ -33,7 +33,7 @@
|
|||
*
|
||||
****************************************************************************/
|
||||
|
||||
/* The STM32F412 has 512Kb of FLASH beginning at address 0x0800:0000 and
|
||||
/* The STM32F412 has 1M of FLASH beginning at address 0x0800:0000 and
|
||||
* 256Kb of SRAM. SRAM is split up into three blocks:
|
||||
*
|
||||
* 1) 112Kb of SRAM beginning at address 0x2000:0000
|
||||
|
|
|
@ -33,7 +33,7 @@
|
|||
*
|
||||
****************************************************************************/
|
||||
|
||||
/* The STM32F412 has 512Kb of FLASH beginning at address 0x0800:0000 and
|
||||
/* The STM32F412 has 1M of FLASH beginning at address 0x0800:0000 and
|
||||
* 256Kb of SRAM. SRAM is split up into three blocks:
|
||||
*
|
||||
* 1) 112Kb of SRAM beginning at address 0x2000:0000
|
||||
|
|
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
@ -60,7 +60,7 @@ CONFIG_MODULES_NAVIGATOR=y
|
|||
CONFIG_MODULES_RC_UPDATE=y
|
||||
CONFIG_MODULES_ROVER_POS_CONTROL=y
|
||||
CONFIG_MODULES_SENSORS=y
|
||||
CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
|
||||
# CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
|
||||
CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
|
||||
CONFIG_MODULES_UXRCE_DDS_CLIENT=y
|
||||
CONFIG_MODULES_VTOL_ATT_CONTROL=y
|
||||
|
|
|
@ -4,6 +4,7 @@ CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y
|
|||
CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP101XX=y
|
||||
CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L0X=y
|
||||
CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L1X=y
|
||||
CONFIG_DRIVERS_GPS=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
|
||||
CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
|
||||
CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8308=y
|
||||
|
@ -30,3 +31,4 @@ CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y
|
|||
CONFIG_SYSTEMCMDS_UORB=y
|
||||
CONFIG_SYSTEMCMDS_PARAM=y
|
||||
CONFIG_ORB_COMMUNICATOR=y
|
||||
CONFIG_PARAM_REMOTE=y
|
||||
|
|
|
@ -62,6 +62,8 @@
|
|||
#include <uORB/topics/vehicle_odometry.h>
|
||||
#include <uORB/topics/sensor_baro.h>
|
||||
#include <uORB/topics/esc_status.h>
|
||||
#include <uORB/topics/distance_sensor.h>
|
||||
#include <uORB/topics/sensor_optical_flow.h>
|
||||
|
||||
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
|
||||
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
|
||||
|
@ -88,21 +90,22 @@ static bool _is_running = false;
|
|||
volatile bool _task_should_exit = false;
|
||||
static px4_task_t _task_handle = -1;
|
||||
int _uart_fd = -1;
|
||||
bool debug = false;
|
||||
bool _debug = false;
|
||||
std::string port = "2";
|
||||
int baudrate = 921600;
|
||||
const unsigned mode_flag_custom = 1;
|
||||
const unsigned mode_flag_armed = 128;
|
||||
bool _send_gps = false;
|
||||
bool _send_mag = false;
|
||||
bool _send_distance = false;
|
||||
|
||||
uORB::Publication<battery_status_s> _battery_pub{ORB_ID(battery_status)};
|
||||
uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
|
||||
uORB::Publication<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
|
||||
uORB::Publication<vehicle_odometry_s> _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
|
||||
uORB::Publication<vehicle_odometry_s> _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)};
|
||||
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
|
||||
uORB::Publication<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};
|
||||
uORB::Publication<distance_sensor_s> _distance_sensor_pub{ORB_ID(distance_sensor)};
|
||||
uORB::Publication<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
|
||||
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
|
||||
|
||||
int32_t _output_functions[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS] {};
|
||||
|
@ -128,13 +131,42 @@ float x_gyro = 0;
|
|||
float y_gyro = 0;
|
||||
float z_gyro = 0;
|
||||
uint64_t gyro_accel_time = 0;
|
||||
bool _use_software_mav_throttling{false};
|
||||
|
||||
int heartbeat_counter = 0;
|
||||
int imu_counter = 0;
|
||||
int hil_sensor_counter = 0;
|
||||
int vision_msg_counter = 0;
|
||||
int gps_counter = 0;
|
||||
// Status counters
|
||||
uint32_t heartbeat_received_counter = 0;
|
||||
uint32_t heartbeat_sent_counter = 0;
|
||||
uint32_t imu_counter = 0;
|
||||
uint32_t hil_sensor_counter = 0;
|
||||
uint32_t mag_counter = 0;
|
||||
uint32_t baro_counter = 0;
|
||||
uint32_t actuator_sent_counter = 0;
|
||||
uint32_t odometry_received_counter = 0;
|
||||
uint32_t odometry_sent_counter = 0;
|
||||
uint32_t gps_received_counter = 0;
|
||||
uint32_t gps_sent_counter = 0;
|
||||
uint32_t distance_received_counter = 0;
|
||||
uint32_t distance_sent_counter = 0;
|
||||
uint32_t flow_received_counter = 0;
|
||||
uint32_t flow_sent_counter = 0;
|
||||
uint32_t unknown_msg_received_counter = 0;
|
||||
|
||||
enum class position_source {GPS, VIO, FLOW, NUM_POSITION_SOURCES};
|
||||
|
||||
struct position_source_data_s {
|
||||
char label[8];
|
||||
bool send;
|
||||
bool fail;
|
||||
uint32_t failure_duration;
|
||||
uint64_t failure_duration_start;
|
||||
} position_source_data[(int) position_source::NUM_POSITION_SOURCES] = {
|
||||
{"GPS", false, false, 0, 0},
|
||||
{"VIO", false, false, 0, 0},
|
||||
{"FLOW", false, false, 0, 0}
|
||||
};
|
||||
|
||||
uint64_t first_sensor_msg_timestamp = 0;
|
||||
uint64_t first_sensor_report_timestamp = 0;
|
||||
uint64_t last_sensor_report_timestamp = 0;
|
||||
|
||||
vehicle_status_s _vehicle_status{};
|
||||
vehicle_control_mode_s _control_mode{};
|
||||
|
@ -144,7 +176,6 @@ battery_status_s _battery_status{};
|
|||
sensor_accel_fifo_s accel_fifo{};
|
||||
sensor_gyro_fifo_s gyro_fifo{};
|
||||
|
||||
|
||||
int openPort(const char *dev, speed_t speed);
|
||||
int closePort();
|
||||
|
||||
|
@ -153,7 +184,8 @@ int writeResponse(void *buf, size_t len);
|
|||
|
||||
int start(int argc, char *argv[]);
|
||||
int stop();
|
||||
int get_status();
|
||||
void print_status();
|
||||
void clear_status_counters();
|
||||
bool isOpen() { return _uart_fd >= 0; };
|
||||
|
||||
void usage();
|
||||
|
@ -163,50 +195,65 @@ void *send_actuator(void *);
|
|||
void send_actuator_data();
|
||||
|
||||
void handle_message_hil_sensor_dsp(mavlink_message_t *msg);
|
||||
void handle_message_hil_optical_flow(mavlink_message_t *msg);
|
||||
void handle_message_distance_sensor(mavlink_message_t *msg);
|
||||
void handle_message_hil_gps_dsp(mavlink_message_t *msg);
|
||||
void handle_message_odometry_dsp(mavlink_message_t *msg);
|
||||
void handle_message_vision_position_estimate_dsp(mavlink_message_t *msg);
|
||||
void handle_message_command_long_dsp(mavlink_message_t *msg);
|
||||
|
||||
void handle_message_dsp(mavlink_message_t *msg);
|
||||
void actuator_controls_from_outputs_dsp(mavlink_hil_actuator_controls_t *msg);
|
||||
void send_esc_telemetry_dsp(mavlink_hil_actuator_controls_t hil_act_control);
|
||||
void send_esc_status(mavlink_hil_actuator_controls_t hil_act_control);
|
||||
|
||||
void
|
||||
handle_message_dsp(mavlink_message_t *msg)
|
||||
{
|
||||
switch (msg->msgid) {
|
||||
case MAVLINK_MSG_ID_HIL_SENSOR:
|
||||
hil_sensor_counter++;
|
||||
handle_message_hil_sensor_dsp(msg);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_HIL_GPS:
|
||||
if (_send_gps) { handle_message_hil_gps_dsp(msg); }
|
||||
gps_received_counter++;
|
||||
|
||||
break;
|
||||
if (position_source_data[(int) position_source::GPS].send) { handle_message_hil_gps_dsp(msg); }
|
||||
|
||||
case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
|
||||
handle_message_vision_position_estimate_dsp(msg);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_ODOMETRY:
|
||||
handle_message_odometry_dsp(msg);
|
||||
break;
|
||||
odometry_received_counter++;
|
||||
|
||||
if (position_source_data[(int) position_source::VIO].send) { handle_message_odometry_dsp(msg); }
|
||||
|
||||
case MAVLINK_MSG_ID_COMMAND_LONG:
|
||||
handle_message_command_long_dsp(msg);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_HEARTBEAT:
|
||||
PX4_DEBUG("Heartbeat msg received");
|
||||
heartbeat_received_counter++;
|
||||
|
||||
if (_debug) { PX4_INFO("Heartbeat msg received"); }
|
||||
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_SYSTEM_TIME:
|
||||
PX4_DEBUG("MAVLINK SYSTEM TIME");
|
||||
case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW:
|
||||
flow_received_counter++;
|
||||
|
||||
if (position_source_data[(int) position_source::FLOW].send) { handle_message_hil_optical_flow(msg); }
|
||||
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_DISTANCE_SENSOR:
|
||||
distance_received_counter++;
|
||||
|
||||
if (_send_distance) { handle_message_distance_sensor(msg); }
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
PX4_DEBUG("Unknown msg ID: %d", msg->msgid);
|
||||
unknown_msg_received_counter++;
|
||||
|
||||
if (_debug) { PX4_INFO("Unknown msg ID: %d", msg->msgid); }
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
@ -228,7 +275,6 @@ void send_actuator_data()
|
|||
bool first_sent = false;
|
||||
|
||||
while (true) {
|
||||
|
||||
bool controls_updated = false;
|
||||
(void) orb_check(_vehicle_control_mode_sub_, &controls_updated);
|
||||
|
||||
|
@ -239,45 +285,50 @@ void send_actuator_data()
|
|||
bool actuator_updated = false;
|
||||
(void) orb_check(_actuator_outputs_sub, &actuator_updated);
|
||||
|
||||
uint8_t newBuf[512];
|
||||
uint16_t newBufLen = 0;
|
||||
|
||||
mavlink_hil_actuator_controls_t hil_act_control;
|
||||
actuator_controls_from_outputs_dsp(&hil_act_control);
|
||||
|
||||
mavlink_message_t message{};
|
||||
mavlink_msg_hil_actuator_controls_encode(1, 1, &message, &hil_act_control);
|
||||
|
||||
if (actuator_updated) {
|
||||
orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuator_outputs);
|
||||
px4_lockstep_wait_for_components();
|
||||
|
||||
if (_actuator_outputs.timestamp > 0) {
|
||||
mavlink_hil_actuator_controls_t hil_act_control;
|
||||
actuator_controls_from_outputs_dsp(&hil_act_control);
|
||||
|
||||
mavlink_message_t message{};
|
||||
mavlink_msg_hil_actuator_controls_encode(1, 1, &message, &hil_act_control);
|
||||
previous_timestamp = _actuator_outputs.timestamp;
|
||||
previous_uorb_timestamp = _actuator_outputs.timestamp;
|
||||
uint8_t newBuf[512];
|
||||
uint16_t newBufLen = 0;
|
||||
|
||||
newBufLen = mavlink_msg_to_send_buffer(newBuf, &message);
|
||||
int writeRetval = writeResponse(&newBuf, newBufLen);
|
||||
PX4_DEBUG("Succesful write of actuator back to jMAVSim: %d at %llu", writeRetval, hrt_absolute_time());
|
||||
|
||||
actuator_sent_counter++;
|
||||
|
||||
if (_debug) { PX4_INFO("Succesful write of actuator back to jMAVSim: %d at %llu", writeRetval, hrt_absolute_time()); }
|
||||
|
||||
first_sent = true;
|
||||
send_esc_telemetry_dsp(hil_act_control);
|
||||
|
||||
send_esc_status(hil_act_control);
|
||||
}
|
||||
|
||||
} else if (!actuator_updated && first_sent && differential > 4000) {
|
||||
mavlink_hil_actuator_controls_t hil_act_control;
|
||||
actuator_controls_from_outputs_dsp(&hil_act_control);
|
||||
} else if (! actuator_updated && first_sent && differential > 4000) {
|
||||
previous_timestamp = hrt_absolute_time();
|
||||
|
||||
mavlink_message_t message{};
|
||||
mavlink_msg_hil_actuator_controls_encode(1, 1, &message, &hil_act_control);
|
||||
uint8_t newBuf[512];
|
||||
uint16_t newBufLen = 0;
|
||||
newBufLen = mavlink_msg_to_send_buffer(newBuf, &message);
|
||||
int writeRetval = writeResponse(&newBuf, newBufLen);
|
||||
//PX4_INFO("Sending from NOT UPDTE AND TIMEOUT: %i", differential);
|
||||
|
||||
PX4_DEBUG("Succesful write of actuator back to jMAVSim: %d at %llu", writeRetval, hrt_absolute_time());
|
||||
send_esc_telemetry_dsp(hil_act_control);
|
||||
actuator_sent_counter++;
|
||||
|
||||
if (_debug) { PX4_INFO("Succesful write of actuator back to jMAVSim: %d at %llu", writeRetval, hrt_absolute_time()); }
|
||||
|
||||
send_esc_status(hil_act_control);
|
||||
}
|
||||
|
||||
differential = hrt_absolute_time() - previous_timestamp;
|
||||
|
||||
px4_usleep(1000);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -287,14 +338,10 @@ void task_main(int argc, char *argv[])
|
|||
int myoptind = 1;
|
||||
const char *myoptarg = nullptr;
|
||||
|
||||
while ((ch = px4_getopt(argc, argv, "vsdcmgp:b:", &myoptind, &myoptarg)) != EOF) {
|
||||
while ((ch = px4_getopt(argc, argv, "odmghfp:b:", &myoptind, &myoptarg)) != EOF) {
|
||||
switch (ch) {
|
||||
case 's':
|
||||
_use_software_mav_throttling = true;
|
||||
break;
|
||||
|
||||
case 'd':
|
||||
debug = true;
|
||||
_debug = true;
|
||||
break;
|
||||
|
||||
case 'p':
|
||||
|
@ -310,7 +357,19 @@ void task_main(int argc, char *argv[])
|
|||
break;
|
||||
|
||||
case 'g':
|
||||
_send_gps = true;
|
||||
position_source_data[(int) position_source::GPS].send = true;
|
||||
break;
|
||||
|
||||
case 'o':
|
||||
position_source_data[(int) position_source::VIO].send = true;
|
||||
break;
|
||||
|
||||
case 'h':
|
||||
_send_distance = true;
|
||||
break;
|
||||
|
||||
case 'f':
|
||||
position_source_data[(int) position_source::FLOW].send = true;
|
||||
break;
|
||||
|
||||
default:
|
||||
|
@ -319,11 +378,13 @@ void task_main(int argc, char *argv[])
|
|||
}
|
||||
|
||||
const char *charport = port.c_str();
|
||||
int openRetval = openPort(charport, (speed_t) baudrate);
|
||||
int open = isOpen();
|
||||
(void) openPort(charport, (speed_t) baudrate);
|
||||
|
||||
if (open) {
|
||||
PX4_ERR("Port is open: %d", openRetval);
|
||||
if ((_debug) && (isOpen())) { PX4_INFO("DSP HITL serial port initialized. Baudrate: %d", baudrate); }
|
||||
|
||||
if (! isOpen()) {
|
||||
PX4_ERR("DSP HITL failed to open serial port");
|
||||
return;
|
||||
}
|
||||
|
||||
uint64_t last_heartbeat_timestamp = hrt_absolute_time();
|
||||
|
@ -342,14 +403,11 @@ void task_main(int argc, char *argv[])
|
|||
pthread_attr_destroy(&sender_thread_attr);
|
||||
|
||||
int _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
PX4_INFO("Got %d from orb_subscribe", _vehicle_status_sub);
|
||||
|
||||
_is_running = true;
|
||||
|
||||
while (!_task_should_exit) {
|
||||
|
||||
uint8_t rx_buf[1024];
|
||||
//rx_buf[511] = '\0';
|
||||
|
||||
uint64_t timestamp = hrt_absolute_time();
|
||||
|
||||
|
@ -357,8 +415,8 @@ void task_main(int argc, char *argv[])
|
|||
if (got_first_sensor_msg) {
|
||||
uint64_t delta_time = timestamp - last_imu_update_timestamp;
|
||||
|
||||
if (delta_time > 15000) {
|
||||
PX4_ERR("Sending updates at %llu, delta %llu", timestamp, delta_time);
|
||||
if ((imu_counter) && (delta_time > 15000)) {
|
||||
PX4_WARN("Sending updates at %llu, delta %llu", timestamp, delta_time);
|
||||
}
|
||||
|
||||
uint64_t _px4_gyro_accel_timestamp = hrt_absolute_time();
|
||||
|
@ -396,7 +454,7 @@ void task_main(int argc, char *argv[])
|
|||
hb_newBufLen = mavlink_msg_to_send_buffer(hb_newBuf, &hb_message);
|
||||
(void) writeResponse(&hb_newBuf, hb_newBufLen);
|
||||
last_heartbeat_timestamp = timestamp;
|
||||
heartbeat_counter++;
|
||||
heartbeat_sent_counter++;
|
||||
}
|
||||
|
||||
bool vehicle_updated = false;
|
||||
|
@ -416,7 +474,7 @@ void task_main(int argc, char *argv[])
|
|||
_is_running = false;
|
||||
}
|
||||
|
||||
void send_esc_telemetry_dsp(mavlink_hil_actuator_controls_t hil_act_control)
|
||||
void send_esc_status(mavlink_hil_actuator_controls_t hil_act_control)
|
||||
{
|
||||
esc_status_s esc_status{};
|
||||
esc_status.timestamp = hrt_absolute_time();
|
||||
|
@ -448,17 +506,13 @@ void send_esc_telemetry_dsp(mavlink_hil_actuator_controls_t hil_act_control)
|
|||
_esc_status_pub.publish(esc_status);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
handle_message_command_long_dsp(mavlink_message_t *msg)
|
||||
{
|
||||
/* command */
|
||||
mavlink_command_long_t cmd_mavlink;
|
||||
mavlink_msg_command_long_decode(msg, &cmd_mavlink);
|
||||
|
||||
if (debug) {
|
||||
PX4_INFO("Value of command_long.command: %d", cmd_mavlink.command);
|
||||
}
|
||||
if (_debug) { PX4_INFO("Value of command_long.command: %d", cmd_mavlink.command); }
|
||||
|
||||
mavlink_command_ack_t ack = {};
|
||||
ack.result = MAV_RESULT_UNSUPPORTED;
|
||||
|
@ -470,46 +524,140 @@ handle_message_command_long_dsp(mavlink_message_t *msg)
|
|||
uint16_t acknewBufLen = 0;
|
||||
acknewBufLen = mavlink_msg_to_send_buffer(acknewBuf, &ack_message);
|
||||
int writeRetval = writeResponse(&acknewBuf, acknewBufLen);
|
||||
PX4_INFO("Succesful write of ACK back over UART: %d at %llu", writeRetval, hrt_absolute_time());
|
||||
|
||||
if (_debug) { PX4_INFO("Succesful write of ACK back over UART: %d at %llu", writeRetval, hrt_absolute_time()); }
|
||||
}
|
||||
|
||||
int flow_debug_counter = 0;
|
||||
|
||||
void
|
||||
handle_message_vision_position_estimate_dsp(mavlink_message_t *msg)
|
||||
handle_message_hil_optical_flow(mavlink_message_t *msg)
|
||||
{
|
||||
mavlink_vision_position_estimate_t vpe;
|
||||
mavlink_msg_vision_position_estimate_decode(msg, &vpe);
|
||||
mavlink_hil_optical_flow_t flow;
|
||||
mavlink_msg_hil_optical_flow_decode(msg, &flow);
|
||||
|
||||
// fill vehicle_odometry from Mavlink VISION_POSITION_ESTIMATE
|
||||
vehicle_odometry_s odom{};
|
||||
uint64_t timestamp = hrt_absolute_time();
|
||||
odom.timestamp_sample = timestamp;
|
||||
if ((_debug) && (!(flow_debug_counter % 10))) {
|
||||
PX4_INFO("optflow: time: %llu, quality %d", flow.time_usec, (int) flow.quality);
|
||||
PX4_INFO("optflow: x: %.2f y: %.2f", (double) flow.integrated_x, (double) flow.integrated_y);
|
||||
}
|
||||
|
||||
odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED;
|
||||
odom.position[0] = vpe.x;
|
||||
odom.position[1] = vpe.y;
|
||||
odom.position[2] = vpe.z;
|
||||
flow_debug_counter++;
|
||||
|
||||
const matrix::Quatf q(matrix::Eulerf(vpe.roll, vpe.pitch, vpe.yaw));
|
||||
q.copyTo(odom.q);
|
||||
device::Device::DeviceId device_id;
|
||||
device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
|
||||
device_id.devid_s.bus = 1;
|
||||
device_id.devid_s.address = msg->sysid;
|
||||
device_id.devid_s.devtype = DRV_FLOW_DEVTYPE_SIM;
|
||||
|
||||
// VISION_POSITION_ESTIMATE covariance
|
||||
// Row-major representation of pose 6x6 cross-covariance matrix upper right triangle
|
||||
// (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.).
|
||||
// If unknown, assign NaN value to first element in the array.
|
||||
odom.position_variance[0] = vpe.covariance[0]; // X row 0, col 0
|
||||
odom.position_variance[1] = vpe.covariance[6]; // Y row 1, col 1
|
||||
odom.position_variance[2] = vpe.covariance[11]; // Z row 2, col 2
|
||||
sensor_optical_flow_s sensor_optical_flow{};
|
||||
|
||||
odom.orientation_variance[0] = vpe.covariance[15]; // R row 3, col 3
|
||||
odom.orientation_variance[1] = vpe.covariance[18]; // P row 4, col 4
|
||||
odom.orientation_variance[2] = vpe.covariance[20]; // Y row 5, col 5
|
||||
sensor_optical_flow.timestamp_sample = hrt_absolute_time();
|
||||
sensor_optical_flow.device_id = device_id.devid;
|
||||
|
||||
odom.reset_counter = vpe.reset_counter;
|
||||
sensor_optical_flow.pixel_flow[0] = flow.integrated_x;
|
||||
sensor_optical_flow.pixel_flow[1] = flow.integrated_y;
|
||||
|
||||
odom.timestamp = hrt_absolute_time();
|
||||
sensor_optical_flow.integration_timespan_us = flow.integration_time_us;
|
||||
sensor_optical_flow.quality = flow.quality;
|
||||
|
||||
_visual_odometry_pub.publish(odom);
|
||||
vision_msg_counter++;
|
||||
int index = (int) position_source::FLOW;
|
||||
|
||||
if (position_source_data[index].fail) {
|
||||
uint32_t duration = position_source_data[index].failure_duration;
|
||||
hrt_abstime start = position_source_data[index].failure_duration_start;
|
||||
|
||||
if (duration) {
|
||||
if (hrt_elapsed_time(&start) > (duration * 1000000)) {
|
||||
PX4_INFO("Optical flow failure ending");
|
||||
position_source_data[index].fail = false;
|
||||
position_source_data[index].failure_duration = 0;
|
||||
position_source_data[index].failure_duration_start = 0;
|
||||
|
||||
} else {
|
||||
sensor_optical_flow.quality = 0;
|
||||
}
|
||||
|
||||
} else {
|
||||
sensor_optical_flow.quality = 0;
|
||||
}
|
||||
}
|
||||
|
||||
const matrix::Vector3f integrated_gyro(flow.integrated_xgyro, flow.integrated_ygyro, flow.integrated_zgyro);
|
||||
|
||||
if (integrated_gyro.isAllFinite()) {
|
||||
integrated_gyro.copyTo(sensor_optical_flow.delta_angle);
|
||||
sensor_optical_flow.delta_angle_available = true;
|
||||
}
|
||||
|
||||
sensor_optical_flow.max_flow_rate = NAN;
|
||||
sensor_optical_flow.min_ground_distance = NAN;
|
||||
sensor_optical_flow.max_ground_distance = NAN;
|
||||
|
||||
// Use distance value for distance sensor topic
|
||||
// if (PX4_ISFINITE(flow.distance) && (flow.distance >= 0.f)) {
|
||||
// // Positive value (including zero): distance known. Negative value: Unknown distance.
|
||||
// sensor_optical_flow.distance_m = flow.distance;
|
||||
// sensor_optical_flow.distance_available = true;
|
||||
// }
|
||||
|
||||
// Emulate voxl-flow-server where distance comes in a separate
|
||||
// distance sensor topic message
|
||||
sensor_optical_flow.distance_m = 0.0f;
|
||||
sensor_optical_flow.distance_available = false;
|
||||
|
||||
sensor_optical_flow.timestamp = hrt_absolute_time();
|
||||
|
||||
_sensor_optical_flow_pub.publish(sensor_optical_flow);
|
||||
|
||||
flow_sent_counter++;
|
||||
}
|
||||
|
||||
int distance_debug_counter = 0;
|
||||
|
||||
void handle_message_distance_sensor(mavlink_message_t *msg)
|
||||
{
|
||||
mavlink_distance_sensor_t dist_sensor;
|
||||
mavlink_msg_distance_sensor_decode(msg, &dist_sensor);
|
||||
|
||||
if ((_debug) && (!(distance_debug_counter % 10))) {
|
||||
PX4_INFO("distance: time: %u, quality: %u, height: %u",
|
||||
dist_sensor.time_boot_ms, dist_sensor.signal_quality,
|
||||
dist_sensor.current_distance);
|
||||
}
|
||||
|
||||
distance_debug_counter++;
|
||||
|
||||
distance_sensor_s ds{};
|
||||
|
||||
device::Device::DeviceId device_id;
|
||||
device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
|
||||
device_id.devid_s.bus = 1;
|
||||
device_id.devid_s.address = msg->sysid;
|
||||
device_id.devid_s.devtype = DRV_DIST_DEVTYPE_MAVLINK;
|
||||
|
||||
ds.timestamp = hrt_absolute_time(); /* Use system time for now, don't trust sender to attach correct timestamp */
|
||||
ds.min_distance = static_cast<float>(dist_sensor.min_distance) * 1e-2f; /* cm to m */
|
||||
ds.max_distance = static_cast<float>(dist_sensor.max_distance) * 1e-2f; /* cm to m */
|
||||
ds.current_distance = static_cast<float>(dist_sensor.current_distance) * 1e-2f; /* cm to m */
|
||||
ds.variance = dist_sensor.covariance * 1e-4f; /* cm^2 to m^2 */
|
||||
ds.h_fov = dist_sensor.horizontal_fov;
|
||||
ds.v_fov = dist_sensor.vertical_fov;
|
||||
ds.q[0] = dist_sensor.quaternion[0];
|
||||
ds.q[1] = dist_sensor.quaternion[1];
|
||||
ds.q[2] = dist_sensor.quaternion[2];
|
||||
ds.q[3] = dist_sensor.quaternion[3];
|
||||
ds.type = dist_sensor.type;
|
||||
ds.device_id = device_id.devid;
|
||||
ds.orientation = dist_sensor.orientation;
|
||||
|
||||
// MAVLink DISTANCE_SENSOR signal_quality value of 0 means unset/unknown
|
||||
// quality value. Also it comes normalised between 1 and 100 while the uORB
|
||||
// signal quality is normalised between 0 and 100.
|
||||
ds.signal_quality = dist_sensor.signal_quality == 0 ? -1 : 100 * (dist_sensor.signal_quality - 1) / 99;
|
||||
|
||||
_distance_sensor_pub.publish(ds);
|
||||
|
||||
distance_sent_counter++;
|
||||
}
|
||||
|
||||
void
|
||||
|
@ -518,6 +666,8 @@ handle_message_odometry_dsp(mavlink_message_t *msg)
|
|||
mavlink_odometry_t odom_in;
|
||||
mavlink_msg_odometry_decode(msg, &odom_in);
|
||||
|
||||
odometry_sent_counter++;
|
||||
|
||||
// fill vehicle_odometry from Mavlink ODOMETRY
|
||||
vehicle_odometry_s odom{};
|
||||
uint64_t timestamp = hrt_absolute_time();
|
||||
|
@ -699,6 +849,28 @@ handle_message_odometry_dsp(mavlink_message_t *msg)
|
|||
odom.reset_counter = odom_in.reset_counter;
|
||||
odom.quality = odom_in.quality;
|
||||
|
||||
int index = (int) position_source::VIO;
|
||||
|
||||
if (position_source_data[index].fail) {
|
||||
uint32_t duration = position_source_data[index].failure_duration;
|
||||
hrt_abstime start = position_source_data[index].failure_duration_start;
|
||||
|
||||
if (duration) {
|
||||
if (hrt_elapsed_time(&start) > (duration * 1000000)) {
|
||||
PX4_INFO("VIO failure ending");
|
||||
position_source_data[index].fail = false;
|
||||
position_source_data[index].failure_duration = 0;
|
||||
position_source_data[index].failure_duration_start = 0;
|
||||
|
||||
} else {
|
||||
odom.quality = 0;
|
||||
}
|
||||
|
||||
} else {
|
||||
odom.quality = 0;
|
||||
}
|
||||
}
|
||||
|
||||
switch (odom_in.estimator_type) {
|
||||
case MAV_ESTIMATOR_TYPE_UNKNOWN: // accept MAV_ESTIMATOR_TYPE_UNKNOWN for legacy support
|
||||
case MAV_ESTIMATOR_TYPE_NAIVE:
|
||||
|
@ -745,10 +917,6 @@ void actuator_controls_from_outputs_dsp(mavlink_hil_actuator_controls_t *msg)
|
|||
msg->mode = mode_flag_custom;
|
||||
msg->mode |= (armed) ? mode_flag_armed : 0;
|
||||
msg->flags = 0;
|
||||
|
||||
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
|
||||
msg->flags |= 1;
|
||||
#endif
|
||||
}
|
||||
|
||||
int openPort(const char *dev, speed_t speed)
|
||||
|
@ -759,7 +927,8 @@ int openPort(const char *dev, speed_t speed)
|
|||
}
|
||||
|
||||
_uart_fd = qurt_uart_open(dev, speed);
|
||||
PX4_DEBUG("qurt_uart_opened");
|
||||
|
||||
if (_debug) { PX4_INFO("qurt_uart_opened"); }
|
||||
|
||||
if (_uart_fd < 0) {
|
||||
PX4_ERR("Error opening port: %s (%i)", dev, errno);
|
||||
|
@ -840,25 +1009,50 @@ int stop()
|
|||
|
||||
void usage()
|
||||
{
|
||||
PX4_INFO("Usage: dsp_hitl {start|info|status|stop}");
|
||||
PX4_INFO("Usage: dsp_hitl {start|status|clear|failure|stop}");
|
||||
PX4_INFO(" failure <source> <duration>");
|
||||
PX4_INFO(" source: gps, vio, flow");
|
||||
PX4_INFO(" duration: 0 (toggle state), else seconds");
|
||||
}
|
||||
|
||||
int get_status()
|
||||
void print_status()
|
||||
{
|
||||
PX4_INFO("Running: %s", _is_running ? "yes" : "no");
|
||||
PX4_INFO("Status of IMU_Data counter: %i", imu_counter);
|
||||
PX4_INFO("Value of current accel x, y, z data: %f, %f, %f", double(x_accel), double(y_accel), double(z_accel));
|
||||
PX4_INFO("Value of current gyro x, y, z data: %f, %f, %f", double(x_gyro), double(y_gyro), double(z_gyro));
|
||||
PX4_INFO("Value of HIL_Sensor counter: %i", hil_sensor_counter);
|
||||
PX4_INFO("Value of Heartbeat counter: %i", heartbeat_counter);
|
||||
PX4_INFO("Value of Vision data counter: %i", vision_msg_counter);
|
||||
PX4_INFO("Value of GPS Data counter: %i", gps_counter);
|
||||
return 0;
|
||||
PX4_INFO("HIL Sensor received: %i", hil_sensor_counter);
|
||||
PX4_INFO("IMU updates: %i", imu_counter);
|
||||
PX4_INFO("\tCurrent accel x, y, z: %f, %f, %f", double(x_accel), double(y_accel), double(z_accel));
|
||||
PX4_INFO("\tCurrent gyro x, y, z: %f, %f, %f", double(x_gyro), double(y_gyro), double(z_gyro));
|
||||
PX4_INFO("Magnetometer sent: %i", mag_counter);
|
||||
PX4_INFO("Barometer sent: %i", baro_counter);
|
||||
PX4_INFO("Heartbeat received: %i, sent: %i", heartbeat_received_counter, heartbeat_sent_counter);
|
||||
PX4_INFO("Odometry received: %i, sent: %i", odometry_received_counter, odometry_sent_counter);
|
||||
PX4_INFO("GPS received: %i, sent: %i", gps_received_counter, gps_sent_counter);
|
||||
PX4_INFO("Distance sensor received: %i, sent: %i", distance_received_counter, distance_sent_counter);
|
||||
PX4_INFO("Optical flow received: %i, sent: %i", flow_received_counter, flow_sent_counter);
|
||||
PX4_INFO("Actuator updates sent: %i", actuator_sent_counter);
|
||||
PX4_INFO("Unknown messages received: %i", unknown_msg_received_counter);
|
||||
}
|
||||
|
||||
uint64_t first_sensor_msg_timestamp = 0;
|
||||
uint64_t first_sensor_report_timestamp = 0;
|
||||
uint64_t last_sensor_report_timestamp = 0;
|
||||
void
|
||||
clear_status_counters()
|
||||
{
|
||||
heartbeat_received_counter = 0;
|
||||
heartbeat_sent_counter = 0;
|
||||
imu_counter = 0;
|
||||
hil_sensor_counter = 0;
|
||||
mag_counter = 0;
|
||||
baro_counter = 0;
|
||||
actuator_sent_counter = 0;
|
||||
odometry_received_counter = 0;
|
||||
odometry_sent_counter = 0;
|
||||
gps_received_counter = 0;
|
||||
gps_sent_counter = 0;
|
||||
distance_received_counter = 0;
|
||||
distance_sent_counter = 0;
|
||||
flow_received_counter = 0;
|
||||
flow_sent_counter = 0;
|
||||
unknown_msg_received_counter = 0;
|
||||
}
|
||||
|
||||
void
|
||||
handle_message_hil_sensor_dsp(mavlink_message_t *msg)
|
||||
|
@ -928,6 +1122,8 @@ handle_message_hil_sensor_dsp(mavlink_message_t *msg)
|
|||
}
|
||||
|
||||
_px4_mag->update(gyro_accel_time, hil_sensor.xmag, hil_sensor.ymag, hil_sensor.zmag);
|
||||
|
||||
mag_counter++;
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -942,17 +1138,8 @@ handle_message_hil_sensor_dsp(mavlink_message_t *msg)
|
|||
sensor_baro.error_count = 0;
|
||||
sensor_baro.timestamp = hrt_absolute_time();
|
||||
_sensor_baro_pub.publish(sensor_baro);
|
||||
}
|
||||
|
||||
// differential pressure
|
||||
if ((hil_sensor.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS) {
|
||||
differential_pressure_s report{};
|
||||
report.timestamp_sample = gyro_accel_time;
|
||||
report.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION
|
||||
report.temperature = hil_sensor.temperature;
|
||||
report.differential_pressure_pa = hil_sensor.diff_pressure * 100.0f; // hPa to Pa
|
||||
report.timestamp = hrt_absolute_time();
|
||||
_differential_pressure_pub.publish(report);
|
||||
baro_counter++;
|
||||
}
|
||||
|
||||
// battery status
|
||||
|
@ -970,7 +1157,6 @@ handle_message_hil_sensor_dsp(mavlink_message_t *msg)
|
|||
|
||||
_battery_pub.publish(hil_battery_status);
|
||||
}
|
||||
hil_sensor_counter++;
|
||||
}
|
||||
|
||||
void
|
||||
|
@ -989,15 +1175,41 @@ handle_message_hil_gps_dsp(mavlink_message_t *msg)
|
|||
|
||||
gps.device_id = device_id.devid;
|
||||
|
||||
gps.latitude_deg = hil_gps.lat;
|
||||
gps.longitude_deg = hil_gps.lon;
|
||||
gps.altitude_msl_m = hil_gps.alt;
|
||||
gps.altitude_ellipsoid_m = hil_gps.alt;
|
||||
gps.latitude_deg = hil_gps.lat * 1e-7;
|
||||
gps.longitude_deg = hil_gps.lon * 1e-7;
|
||||
gps.altitude_msl_m = hil_gps.alt * 1e-3;
|
||||
gps.altitude_ellipsoid_m = hil_gps.alt * 1e-3;
|
||||
|
||||
gps.s_variance_m_s = 0.25f;
|
||||
gps.c_variance_rad = 0.5f;
|
||||
|
||||
gps.satellites_used = hil_gps.satellites_visible;
|
||||
gps.fix_type = hil_gps.fix_type;
|
||||
|
||||
int index = (int) position_source::GPS;
|
||||
|
||||
if (position_source_data[index].fail) {
|
||||
uint32_t duration = position_source_data[index].failure_duration;
|
||||
hrt_abstime start = position_source_data[index].failure_duration_start;
|
||||
|
||||
if (duration) {
|
||||
if (hrt_elapsed_time(&start) > (duration * 1000000)) {
|
||||
PX4_INFO("GPS failure ending");
|
||||
position_source_data[index].fail = false;
|
||||
position_source_data[index].failure_duration = 0;
|
||||
position_source_data[index].failure_duration_start = 0;
|
||||
|
||||
} else {
|
||||
gps.satellites_used = 1;
|
||||
gps.fix_type = 0;
|
||||
}
|
||||
|
||||
} else {
|
||||
gps.satellites_used = 1;
|
||||
gps.fix_type = 0;
|
||||
}
|
||||
}
|
||||
|
||||
gps.eph = (float)hil_gps.eph * 1e-2f; // cm -> m
|
||||
gps.epv = (float)hil_gps.epv * 1e-2f; // cm -> m
|
||||
|
||||
|
@ -1021,7 +1233,6 @@ handle_message_hil_gps_dsp(mavlink_message_t *msg)
|
|||
gps.timestamp_time_relative = 0;
|
||||
gps.time_utc_usec = hil_gps.time_usec;
|
||||
|
||||
gps.satellites_used = hil_gps.satellites_visible;
|
||||
|
||||
gps.heading = NAN;
|
||||
gps.heading_offset = NAN;
|
||||
|
@ -1029,10 +1240,51 @@ handle_message_hil_gps_dsp(mavlink_message_t *msg)
|
|||
gps.timestamp = hrt_absolute_time();
|
||||
|
||||
_sensor_gps_pub.publish(gps);
|
||||
gps_counter++;
|
||||
|
||||
gps_sent_counter++;
|
||||
}
|
||||
|
||||
int
|
||||
process_failure(dsp_hitl::position_source src, int duration)
|
||||
{
|
||||
if (src >= position_source::NUM_POSITION_SOURCES) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
int index = (int) src;
|
||||
|
||||
if (position_source_data[index].send) {
|
||||
if (duration <= 0) {
|
||||
// Toggle state
|
||||
if (position_source_data[index].fail) {
|
||||
PX4_INFO("Ending indefinite %s failure", position_source_data[index].label);
|
||||
position_source_data[index].fail = false;
|
||||
|
||||
} else {
|
||||
PX4_INFO("Starting indefinite %s failure", position_source_data[index].label);
|
||||
position_source_data[index].fail = true;
|
||||
}
|
||||
|
||||
position_source_data[index].failure_duration = 0;
|
||||
position_source_data[index].failure_duration_start = 0;
|
||||
|
||||
} else {
|
||||
PX4_INFO("%s failure for %d seconds", position_source_data[index].label, duration);
|
||||
position_source_data[index].fail = true;
|
||||
position_source_data[index].failure_duration = duration;
|
||||
position_source_data[index].failure_duration_start = hrt_absolute_time();
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("%s not active, cannot create failure", position_source_data[index].label);
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
} // End dsp_hitl namespace
|
||||
|
||||
int dsp_hitl_main(int argc, char *argv[])
|
||||
{
|
||||
int myoptind = 1;
|
||||
|
@ -1044,20 +1296,47 @@ int dsp_hitl_main(int argc, char *argv[])
|
|||
|
||||
const char *verb = argv[myoptind];
|
||||
|
||||
|
||||
if (!strcmp(verb, "start")) {
|
||||
return dsp_hitl::start(argc - 1, argv + 1);
|
||||
}
|
||||
|
||||
else if (!strcmp(verb, "stop")) {
|
||||
} else if (!strcmp(verb, "stop")) {
|
||||
return dsp_hitl::stop();
|
||||
}
|
||||
|
||||
else if (!strcmp(verb, "status")) {
|
||||
return dsp_hitl::get_status();
|
||||
}
|
||||
} else if (!strcmp(verb, "status")) {
|
||||
dsp_hitl::print_status();
|
||||
return 0;
|
||||
|
||||
else {
|
||||
} else if (!strcmp(verb, "clear")) {
|
||||
dsp_hitl::clear_status_counters();
|
||||
return 0;
|
||||
|
||||
} else if (!strcmp(verb, "failure")) {
|
||||
if (argc != 4) {
|
||||
dsp_hitl::usage();
|
||||
return 1;
|
||||
}
|
||||
|
||||
const char *source = argv[myoptind + 1];
|
||||
int duration = atoi(argv[myoptind + 2]);
|
||||
|
||||
if (!strcmp(source, "gps")) {
|
||||
return dsp_hitl::process_failure(dsp_hitl::position_source::GPS, duration);
|
||||
|
||||
} else if (!strcmp(source, "vio")) {
|
||||
return dsp_hitl::process_failure(dsp_hitl::position_source::VIO, duration);
|
||||
|
||||
} else if (!strcmp(source, "flow")) {
|
||||
return dsp_hitl::process_failure(dsp_hitl::position_source::FLOW, duration);
|
||||
|
||||
} else {
|
||||
PX4_ERR("Unknown failure source %s, duration %d", source, duration);
|
||||
dsp_hitl::usage();
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
|
||||
} else {
|
||||
dsp_hitl::usage();
|
||||
return 1;
|
||||
}
|
||||
|
|
|
@ -0,0 +1,34 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2024 ModalAI, Inc. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
include_directories(${PX4_BOARD_DIR}/libfc-sensor-api/inc)
|
|
@ -1,7 +1,7 @@
|
|||
|
||||
# Link against the public stub version of the proprietary fc sensor library
|
||||
target_link_libraries(px4 PRIVATE
|
||||
${PX4_SOURCE_DIR}/src//modules/muorb/apps/libfc-sensor-api/build/libfc_sensor.so
|
||||
${PX4_BOARD_DIR}/libfc-sensor-api/build/libfc_sensor.so
|
||||
px4_layer
|
||||
${module_libraries}
|
||||
)
|
||||
|
|
|
@ -11,7 +11,6 @@ CONFIG_DRIVERS_VOXL2_IO=y
|
|||
CONFIG_MODULES_COMMANDER=y
|
||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||
CONFIG_MODULES_DATAMAN=y
|
||||
CONFIG_MODULES_LOAD_MON=y
|
||||
CONFIG_MODULES_LOGGER=y
|
||||
CONFIG_MODULES_MAVLINK=y
|
||||
CONFIG_MODULES_MUORB_APPS=y
|
||||
|
@ -26,3 +25,4 @@ CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
|
|||
CONFIG_SYSTEMCMDS_UORB=y
|
||||
CONFIG_SYSTEMCMDS_VER=y
|
||||
CONFIG_ORB_COMMUNICATOR=y
|
||||
CONFIG_PARAM_PRIMARY=y
|
||||
|
|
|
@ -1,10 +1,9 @@
|
|||
#!/bin/bash
|
||||
|
||||
cd src/modules/muorb/apps/libfc-sensor-api
|
||||
cd boards/modalai/voxl2/libfc-sensor-api
|
||||
rm -fR build
|
||||
mkdir build
|
||||
cd build
|
||||
CC=/home/4.1.0.4/tools/linaro64/bin/aarch64-linux-gnu-gcc cmake ..
|
||||
make
|
||||
cd ../../../../../..
|
||||
|
||||
cd ../../../../..
|
||||
|
|
|
@ -76,12 +76,13 @@ qshell flight_mode_manager start
|
|||
# Start all of the processing modules on the applications processor
|
||||
dataman start
|
||||
navigator start
|
||||
load_mon start
|
||||
|
||||
# Start microdds_client for ros2 offboard messages from agent over localhost
|
||||
microdds_client start -t udp -h 127.0.0.1 -p 8888
|
||||
|
||||
qshell pwm_out_sim start -m hil
|
||||
# g = gps, m = mag, o = odometry (vio), h = distance sensor, f = optic flow
|
||||
# qshell dsp_hitl start -g -m -o -h -f
|
||||
qshell dsp_hitl start -g -m
|
||||
|
||||
# start the onboard fast link to connect to voxl-mavlink-server
|
||||
|
|
|
@ -207,7 +207,6 @@ qshell flight_mode_manager start
|
|||
# Start all of the processing modules on the applications processor
|
||||
dataman start
|
||||
navigator start
|
||||
load_mon start
|
||||
|
||||
# This bridge allows raw data packets to be sent over UART to the ESC
|
||||
voxl2_io_bridge start
|
||||
|
|
|
@ -1,9 +1,11 @@
|
|||
CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
|
||||
CONFIG_BOARD_ARCHITECTURE="cortex-m7"
|
||||
CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2"
|
||||
CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0"
|
||||
CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1"
|
||||
CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS4"
|
||||
CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3"
|
||||
CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS4"
|
||||
CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1"
|
||||
CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS2"
|
||||
CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS5"
|
||||
CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS0"
|
||||
CONFIG_DRIVERS_ADC_ADS1115=y
|
||||
CONFIG_DRIVERS_ADC_BOARD_ADC=y
|
||||
CONFIG_DRIVERS_BAROMETER_DPS310=y
|
||||
|
@ -25,6 +27,7 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
|
|||
CONFIG_DRIVERS_POWER_MONITOR_INA226=y
|
||||
CONFIG_DRIVERS_PWM_OUT=y
|
||||
CONFIG_DRIVERS_RC_INPUT=y
|
||||
CONFIG_DRIVERS_SAFETY_BUTTON=y
|
||||
CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
|
||||
CONFIG_COMMON_TELEMETRY=y
|
||||
CONFIG_DRIVERS_TONE_ALARM=y
|
||||
|
@ -46,7 +49,6 @@ CONFIG_MODULES_FW_POS_CONTROL=y
|
|||
CONFIG_MODULES_FW_RATE_CONTROL=y
|
||||
CONFIG_MODULES_GIMBAL=y
|
||||
CONFIG_MODULES_GYRO_CALIBRATION=y
|
||||
CONFIG_MODULES_GYRO_FFT=y
|
||||
CONFIG_MODULES_LAND_DETECTOR=y
|
||||
CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
|
||||
CONFIG_MODULES_LOAD_MON=y
|
||||
|
@ -96,4 +98,3 @@ CONFIG_SYSTEMCMDS_UORB=y
|
|||
CONFIG_SYSTEMCMDS_USB_CONNECTED=y
|
||||
CONFIG_SYSTEMCMDS_VER=y
|
||||
CONFIG_SYSTEMCMDS_WORK_QUEUE=y
|
||||
CONFIG_EXAMPLES_FAKE_GPS=y
|
||||
|
|
|
@ -6,4 +6,4 @@
|
|||
param set-default BAT1_V_DIV 10.1
|
||||
param set-default BAT1_A_PER_V 17
|
||||
|
||||
safety_button start
|
||||
param set-default TEL_FRSKY_CONFIG 103
|
||||
|
|
|
@ -16,3 +16,5 @@ icm20948 -s -b 1 -R 8 -M start
|
|||
|
||||
# Interal DPS310 (barometer)
|
||||
dps310 -s -b 2 start
|
||||
|
||||
safety_button start
|
||||
|
|
|
@ -15,7 +15,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mro/ctrl-zero-classic/nuttx-con
|
|||
CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
|
||||
CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
|
||||
CONFIG_ARCH_CHIP="stm32h7"
|
||||
CONFIG_ARCH_CHIP_STM32H743II=y
|
||||
CONFIG_ARCH_CHIP_STM32H743ZI=y
|
||||
CONFIG_ARCH_CHIP_STM32H7=y
|
||||
CONFIG_ARCH_INTERRUPTSTACK=768
|
||||
CONFIG_ARMV7M_BASEPRI_WAR=y
|
||||
|
|
|
@ -222,6 +222,9 @@
|
|||
|
||||
|
||||
/* UART/USART */
|
||||
#define GPIO_USART1_TX GPIO_USART1_TX_3 /* PB6 */
|
||||
#define GPIO_USART1_RX GPIO_USART1_RX_3 /* PB7 */
|
||||
|
||||
#define GPIO_USART2_TX GPIO_USART2_TX_2 /* PD5 */
|
||||
#define GPIO_USART2_RX GPIO_USART2_RX_2 /* PD6 */
|
||||
#define GPIO_USART2_CTS GPIO_USART2_CTS_NSS_2 /* PD3 */
|
||||
|
@ -235,9 +238,6 @@
|
|||
#define GPIO_UART4_TX GPIO_UART4_TX_2 /* PA0 */
|
||||
#define GPIO_UART4_RX GPIO_UART4_RX_2 /* PA1 */
|
||||
|
||||
#define GPIO_USART6_TX 0 /* USART6 is RX-only */
|
||||
#define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC7 */
|
||||
|
||||
#define GPIO_UART7_TX GPIO_UART7_TX_3 /* PE8 */
|
||||
#define GPIO_UART7_RX GPIO_UART7_RX_3 /* PE7 */
|
||||
|
||||
|
@ -268,13 +268,14 @@
|
|||
#define GPIO_SPI5_MISO GPIO_SPI5_MISO_1 /* PF8 */
|
||||
#define GPIO_SPI5_MOSI GPIO_SPI5_MOSI_2 /* PF9 */
|
||||
|
||||
#define GPIO_SPI6_SCK ADJ_SLEW_RATE(GPIO_SPI6_SCK_1) /* PG13 */
|
||||
#define GPIO_SPI6_MISO GPIO_SPI6_MISO_1 /* PG12 */
|
||||
#define GPIO_SPI6_MOSI GPIO_SPI6_MOSI_1 /* PG14 */
|
||||
|
||||
|
||||
/* I2C */
|
||||
#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 /* PB8 */
|
||||
#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 /* PB9 */
|
||||
|
||||
#define GPIO_I2C3_SCL GPIO_I2C3_SCL_2 /* PH7 */
|
||||
#define GPIO_I2C3_SDA GPIO_I2C3_SDA_2 /* PH8 */
|
||||
|
||||
#define GPIO_I2C4_SCL GPIO_I2C4_SCL_4 /* PB6 */
|
||||
#define GPIO_I2C4_SDA GPIO_I2C4_SDA_4 /* PB7 */
|
||||
#define GPIO_I2C4_SCL GPIO_I2C4_SCL_2 /* PF14 */
|
||||
#define GPIO_I2C4_SDA GPIO_I2C4_SDA_2 /* PF15 */
|
||||
|
|
|
@ -56,7 +56,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mro/ctrl-zero-classic/nuttx-con
|
|||
CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
|
||||
CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
|
||||
CONFIG_ARCH_CHIP="stm32h7"
|
||||
CONFIG_ARCH_CHIP_STM32H743II=y
|
||||
CONFIG_ARCH_CHIP_STM32H743ZI=y
|
||||
CONFIG_ARCH_CHIP_STM32H7=y
|
||||
CONFIG_ARCH_INTERRUPTSTACK=768
|
||||
CONFIG_ARCH_STACKDUMP=y
|
||||
|
@ -192,7 +192,6 @@ CONFIG_STM32H7_DMA2=y
|
|||
CONFIG_STM32H7_DMACAPABLE=y
|
||||
CONFIG_STM32H7_FLOWCONTROL_BROKEN=y
|
||||
CONFIG_STM32H7_I2C1=y
|
||||
CONFIG_STM32H7_I2C3=y
|
||||
CONFIG_STM32H7_I2C4=y
|
||||
CONFIG_STM32H7_I2C_DYNTIMEO=y
|
||||
CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10
|
||||
|
@ -212,17 +211,20 @@ CONFIG_STM32H7_SPI2=y
|
|||
CONFIG_STM32H7_SPI5=y
|
||||
CONFIG_STM32H7_SPI5_DMA=y
|
||||
CONFIG_STM32H7_SPI5_DMA_BUFFER=1024
|
||||
CONFIG_STM32H7_SPI6=y
|
||||
CONFIG_STM32H7_TIM15=y
|
||||
CONFIG_STM32H7_TIM16=y
|
||||
CONFIG_STM32H7_TIM1=y
|
||||
CONFIG_STM32H7_TIM2=y
|
||||
CONFIG_STM32H7_TIM3=y
|
||||
CONFIG_STM32H7_TIM4=y
|
||||
CONFIG_STM32H7_TIM8=y
|
||||
CONFIG_STM32H7_USART1=y
|
||||
CONFIG_STM32H7_USART2=y
|
||||
CONFIG_STM32H7_USART3=y
|
||||
CONFIG_STM32H7_UART4=y
|
||||
CONFIG_STM32H7_UART7=y
|
||||
CONFIG_STM32H7_UART8=y
|
||||
CONFIG_STM32H7_USART2=y
|
||||
CONFIG_STM32H7_USART3=y
|
||||
CONFIG_STM32H7_USART6=y
|
||||
CONFIG_STM32H7_USART_BREAKS=y
|
||||
CONFIG_STM32H7_USART_INVERT=y
|
||||
CONFIG_STM32H7_USART_SINGLEWIRE=y
|
||||
|
@ -252,9 +254,6 @@ CONFIG_USART3_IFLOWCONTROL=y
|
|||
CONFIG_USART3_OFLOWCONTROL=y
|
||||
CONFIG_USART3_RXBUFSIZE=600
|
||||
CONFIG_USART3_TXBUFSIZE=3000
|
||||
CONFIG_USART6_BAUD=57600
|
||||
CONFIG_USART6_RXBUFSIZE=600
|
||||
CONFIG_USART6_TXBUFSIZE=1500
|
||||
CONFIG_USBDEV=y
|
||||
CONFIG_USBDEV_BUSPOWERED=y
|
||||
CONFIG_USBDEV_MAXPOWER=500
|
||||
|
|
|
@ -45,95 +45,111 @@
|
|||
#include <stm32_gpio.h>
|
||||
|
||||
/* LEDs are driven with push open drain to support Anode to 5V or 3.3V */
|
||||
#define GPIO_nLED_RED /* PB11 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11)
|
||||
#define GPIO_nLED_GREEN /* PB1 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN1)
|
||||
#define GPIO_nLED_RED /* PB4 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4)
|
||||
#define GPIO_nLED_GREEN /* PB5 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN5)
|
||||
#define GPIO_nLED_BLUE /* PB3 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN3)
|
||||
|
||||
#define GPIO_LED_SAFETY /* PE6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN6)
|
||||
|
||||
#define BOARD_HAS_CONTROL_STATUS_LEDS 1
|
||||
#define BOARD_OVERLOAD_LED LED_RED
|
||||
#define BOARD_ARMED_STATE_LED LED_BLUE
|
||||
|
||||
/* ADC channels */
|
||||
#define PX4_ADC_GPIO \
|
||||
/* PA2 */ GPIO_ADC12_INP14, \
|
||||
/* PC4 */ GPIO_ADC12_INP4, \
|
||||
/* PA3 */ GPIO_ADC12_INP15, \
|
||||
/* PA4 */ GPIO_ADC12_INP18, \
|
||||
/* PC1 */ GPIO_ADC123_INP11
|
||||
/* PC0 */ GPIO_ADC123_INP10, \
|
||||
/* PC5 */ GPIO_ADC12_INP8, \
|
||||
/* PB0 */ GPIO_ADC12_INP9, \
|
||||
/* PB1 */ GPIO_ADC12_INP5
|
||||
|
||||
/* Define Channel numbers must match above GPIO pins */
|
||||
#define ADC_BATTERY_VOLTAGE_CHANNEL 14 /* PA2 BATT_VOLT_SENS */
|
||||
#define ADC_BATTERY_VOLTAGE_CHANNEL 4 /* PC4 BATT_VOLT_SENS */
|
||||
#define ADC_BATTERY_CURRENT_CHANNEL 15 /* PA3 BATT_CURRENT_SENS */
|
||||
#define ADC_SCALED_V5_CHANNEL 18 /* PA4 VDD_5V_SENS */
|
||||
#define ADC_RC_RSSI_CHANNEL 11 /* PC1 */
|
||||
#define ADC_RC_RSSI_CHANNEL 10 /* PC0 */
|
||||
#define ADC_AIRSPEED_VOLTAGE_CHANNEL 8 /* PC5 */
|
||||
#define ADC_AUX1_VOLTAGE_CHANNEL 9 /* PB0 */
|
||||
#define ADC_AUX2_VOLTAGE_CHANNEL 5 /* PB1 */
|
||||
|
||||
#define ADC_CHANNELS \
|
||||
((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \
|
||||
(1 << ADC_BATTERY_CURRENT_CHANNEL) | \
|
||||
(1 << ADC_SCALED_V5_CHANNEL) | \
|
||||
(1 << ADC_RC_RSSI_CHANNEL))
|
||||
(1 << ADC_RC_RSSI_CHANNEL) | \
|
||||
(1 << ADC_AIRSPEED_VOLTAGE_CHANNEL) | \
|
||||
(1 << ADC_AUX1_VOLTAGE_CHANNEL) | \
|
||||
(1 << ADC_AUX2_VOLTAGE_CHANNEL))
|
||||
|
||||
/* HW has to large of R termination on ADC todo:change when HW value is chosen */
|
||||
#define BOARD_ADC_OPEN_CIRCUIT_V (5.6f)
|
||||
|
||||
/* CAN Silence: Silent mode control */
|
||||
#define GPIO_CAN1_SILENT_S0 /* PF5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5)
|
||||
#define GPIO_CAN2_SILENT_S0 /* PF5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5)
|
||||
#define GPIO_CAN1_SILENT_S0 /* PF11 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN11)
|
||||
#define GPIO_CAN2_SILENT_S0 /* PF12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN12)
|
||||
|
||||
/* PWM */
|
||||
#define DIRECT_PWM_OUTPUT_CHANNELS 12
|
||||
|
||||
/* Power supply control and monitoring GPIOs */
|
||||
#define GPIO_nPOWER_IN_A /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5)
|
||||
#define GPIO_nPOWER_IN_A /* PC1 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1)
|
||||
|
||||
#define GPIO_VDD_BRICK1_VALID GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */
|
||||
#define BOARD_NUMBER_BRICKS 1
|
||||
|
||||
#define GPIO_VDD_3V3_SPEKTRUM_POWER_EN /* PE4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4)
|
||||
|
||||
#define GPIO_LEVEL_SHIFTER_OE /* PC13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
|
||||
|
||||
/* Define True logic Power Control in arch agnostic form */
|
||||
#define VDD_3V3_SPEKTRUM_POWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SPEKTRUM_POWER_EN, (!on_true))
|
||||
#define READ_VDD_3V3_SPEKTRUM_POWER_EN() (px4_arch_gpioread(GPIO_VDD_3V3_SPEKTRUM_POWER_EN) == 0)
|
||||
|
||||
/* Tone alarm output */
|
||||
#define TONE_ALARM_TIMER 2 /* timer 2 */
|
||||
#define TONE_ALARM_CHANNEL 1 /* PA15 TIM2_CH1 */
|
||||
#define TONE_ALARM_TIMER 16 /* timer 16 */
|
||||
#define TONE_ALARM_CHANNEL 1 /* PF6 TIM16_CH1 */
|
||||
|
||||
#define GPIO_BUZZER_1 /* PA15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15)
|
||||
#define GPIO_BUZZER_1 /* PF6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN6)
|
||||
|
||||
#define GPIO_TONE_ALARM_IDLE GPIO_BUZZER_1
|
||||
#define GPIO_TONE_ALARM GPIO_TIM2_CH1OUT_2
|
||||
#define GPIO_TONE_ALARM GPIO_TIM16_CH1OUT_2
|
||||
|
||||
/* USB OTG FS */
|
||||
#define GPIO_OTGFS_VBUS /* PA9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN9)
|
||||
|
||||
/* High-resolution timer */
|
||||
#define HRT_TIMER 3 /* use timer3 for the HRT */
|
||||
#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 1 */
|
||||
#define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 1 */
|
||||
|
||||
#define HRT_PPM_CHANNEL /* T3C3 */ 3 /* use capture/compare channel 3 */
|
||||
#define GPIO_PPM_IN /* PB0 T3C3 */ GPIO_TIM3_CH3IN_1
|
||||
#define HRT_PPM_CHANNEL /* T3C2 */ 2 /* use capture/compare channel 2 */
|
||||
#define GPIO_PPM_IN /* PC7 T3C2 */ GPIO_TIM3_CH2IN_3
|
||||
|
||||
/* RC Serial port */
|
||||
#define RC_SERIAL_PORT "/dev/ttyS3"
|
||||
#define RC_SERIAL_PORT "/dev/ttyS5"
|
||||
|
||||
#define GPIO_RSSI_IN /* PC1 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1)
|
||||
#define GPIO_RSSI_IN /* PC0 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN0)
|
||||
|
||||
/* Safety Switch: Enable the FMU to control it if there is no px4io fixme:This should be BOARD_SAFETY_LED(__ontrue) */
|
||||
#define GPIO_SAFETY_SWITCH_IN /* PC4 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4)
|
||||
/* Enable the FMU to use the switch it if there is no px4io fixme:This should be BOARD_SAFTY_BUTTON() */
|
||||
#define GPIO_SAFETY_SWITCH_IN /* PA10 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10)
|
||||
/* Enable the FMU to use the switch it if there is no px4io fixme:This should be BOARD_SAFETY_BUTTON() */
|
||||
#define GPIO_BTN_SAFETY GPIO_SAFETY_SWITCH_IN /* Enable the FMU to control it if there is no px4io */
|
||||
|
||||
/* No PX4IO processor present */
|
||||
#define PX4_MFT_HW_SUPPORTED_PX4_MFT_PX4IO 0
|
||||
|
||||
/* Power switch controls ******************************************************/
|
||||
#define SPEKTRUM_POWER(_on_true) VDD_3V3_SPEKTRUM_POWER_EN(_on_true)
|
||||
|
||||
/*
|
||||
* Board has a separate RC_IN
|
||||
*
|
||||
* GPIO PPM_IN on PB0 T3CH3
|
||||
* GPIO PPM_IN on PC7 T3CH2
|
||||
* SPEKTRUM_RX (it's TX or RX in Bind) on UART6 PC7
|
||||
* Inversion is possible in the UART and can drive GPIO_PPM_IN as an output
|
||||
*/
|
||||
#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
|
||||
#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN7)
|
||||
#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
|
||||
#define SPEKTRUM_RX_AS_UART() /* Can be left as uart */
|
||||
#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))
|
||||
|
@ -156,7 +172,7 @@
|
|||
#define BOARD_HAS_STATIC_MANIFEST 1
|
||||
|
||||
|
||||
#define BOARD_NUM_IO_TIMERS 5
|
||||
#define BOARD_NUM_IO_TIMERS 6
|
||||
|
||||
|
||||
#define BOARD_ENABLE_CONSOLE_BUFFER
|
||||
|
@ -171,9 +187,12 @@
|
|||
GPIO_CAN2_SILENT_S0, \
|
||||
GPIO_nPOWER_IN_A, \
|
||||
GPIO_VDD_3V3_SPEKTRUM_POWER_EN, \
|
||||
GPIO_LEVEL_SHIFTER_OE, \
|
||||
GPIO_TONE_ALARM_IDLE, \
|
||||
GPIO_SAFETY_SWITCH_IN, \
|
||||
GPIO_OTGFS_VBUS, \
|
||||
GPIO_BTN_SAFETY, \
|
||||
GPIO_LED_SAFETY, \
|
||||
}
|
||||
|
||||
__BEGIN_DECLS
|
||||
|
|
|
@ -35,6 +35,5 @@
|
|||
|
||||
constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = {
|
||||
initI2CBusExternal(1),
|
||||
initI2CBusExternal(3),
|
||||
initI2CBusExternal(4),
|
||||
};
|
||||
|
|
|
@ -37,7 +37,7 @@
|
|||
|
||||
constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
|
||||
initSPIBus(SPI::Bus::SPI1, {
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}),
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortG, GPIO::Pin3}, SPI::DRDY{GPIO::PortG, GPIO::Pin2}),
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ICM20948, SPI::CS{GPIO::PortE, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin12}),
|
||||
}, {GPIO::PortE, GPIO::Pin3}),
|
||||
initSPIBus(SPI::Bus::SPI2, {
|
||||
|
@ -46,7 +46,10 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
|
|||
}),
|
||||
initSPIBus(SPI::Bus::SPI5, {
|
||||
initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin10}, SPI::DRDY{GPIO::PortF, GPIO::Pin3}),
|
||||
initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin6}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}),
|
||||
initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin0}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}),
|
||||
}),
|
||||
initSPIBusExternal(SPI::Bus::SPI6, {
|
||||
initSPIConfigExternal(SPI::CS{GPIO::PortG, GPIO::Pin9}),
|
||||
}),
|
||||
};
|
||||
|
||||
|
|
|
@ -33,6 +33,28 @@
|
|||
|
||||
#include <px4_arch/io_timer_hw_description.h>
|
||||
|
||||
/* Timer allocation
|
||||
*
|
||||
* TIM1_CH4 T FMU_CH1
|
||||
* TIM1_CH3 T FMU_CH2
|
||||
* TIM1_CH2 T FMU_CH3
|
||||
* TIM1_CH1 T FMU_CH4
|
||||
*
|
||||
* TIM4_CH2 T FMU_CH5
|
||||
* TIM4_CH3 T FMU_CH6
|
||||
* TIM2_CH3 T FMU_CH7
|
||||
* TIM2_CH1 T FMU_CH8
|
||||
*
|
||||
* TIM2_CH4 T FMU_CH9
|
||||
* TIM15_CH1 T FMU_CH10
|
||||
*
|
||||
* TIM8_CH1 T FMU_CH11
|
||||
*
|
||||
* TIM4_CH4 T FMU_CH12
|
||||
*
|
||||
* TIM16_CH1 T BUZZER - Driven by other driver
|
||||
*/
|
||||
|
||||
constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
|
||||
initIOTimer(Timer::Timer1, DMA{DMA::Index1}),
|
||||
initIOTimer(Timer::Timer4, DMA{DMA::Index1}),
|
||||
|
|
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
@ -81,7 +81,6 @@ CONFIG_MODULES_NAVIGATOR=y
|
|||
CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y
|
||||
CONFIG_MODULES_PAYLOAD_DELIVERER=y
|
||||
CONFIG_MODULES_RC_UPDATE=y
|
||||
CONFIG_MODULES_ROVER_POS_CONTROL=y
|
||||
CONFIG_MODULES_SENSORS=y
|
||||
CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
|
||||
CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
|
||||
|
@ -102,9 +101,7 @@ CONFIG_SYSTEMCMDS_NSHTERM=y
|
|||
CONFIG_SYSTEMCMDS_PARAM=y
|
||||
CONFIG_SYSTEMCMDS_PERF=y
|
||||
CONFIG_SYSTEMCMDS_REBOOT=y
|
||||
CONFIG_SYSTEMCMDS_REFLECT=y
|
||||
CONFIG_SYSTEMCMDS_SD_BENCH=y
|
||||
CONFIG_SYSTEMCMDS_SD_STRESS=y
|
||||
CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
|
||||
CONFIG_SYSTEMCMDS_TOP=y
|
||||
CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
|
||||
|
|
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
@ -58,6 +58,7 @@ set(msg_files
|
|||
CameraCapture.msg
|
||||
CameraStatus.msg
|
||||
CameraTrigger.msg
|
||||
CanInterfaceStatus.msg
|
||||
CellularStatus.msg
|
||||
CollisionConstraints.msg
|
||||
CollisionReport.msg
|
||||
|
@ -99,6 +100,7 @@ set(msg_files
|
|||
FollowTargetStatus.msg
|
||||
GeneratorStatus.msg
|
||||
GeofenceResult.msg
|
||||
GeofenceStatus.msg
|
||||
GimbalControls.msg
|
||||
GimbalDeviceAttitudeStatus.msg
|
||||
GimbalDeviceInformation.msg
|
||||
|
@ -153,6 +155,10 @@ set(msg_files
|
|||
OrbTest.msg
|
||||
OrbTestLarge.msg
|
||||
OrbTestMedium.msg
|
||||
ParameterResetRequest.msg
|
||||
ParameterSetUsedRequest.msg
|
||||
ParameterSetValueRequest.msg
|
||||
ParameterSetValueResponse.msg
|
||||
ParameterUpdate.msg
|
||||
Ping.msg
|
||||
PositionControllerLandingStatus.msg
|
||||
|
@ -284,6 +290,9 @@ foreach(msg_file ${msg_files})
|
|||
list(APPEND uorb_json_files ${msg_source_out_path}/${msg}.json)
|
||||
endforeach()
|
||||
|
||||
# set parent scope msg_files for ROS
|
||||
set(msg_files ${msg_files} PARENT_SCOPE)
|
||||
|
||||
# Generate uORB headers
|
||||
add_custom_command(
|
||||
OUTPUT
|
||||
|
|
|
@ -0,0 +1,6 @@
|
|||
uint64 timestamp # time since system start (microseconds)
|
||||
uint8 interface
|
||||
|
||||
uint64 io_errors
|
||||
uint64 frames_tx
|
||||
uint64 frames_rx
|
|
@ -0,0 +1,7 @@
|
|||
uint64 timestamp # time since system start (microseconds)
|
||||
|
||||
uint32 geofence_id # loaded geofence id
|
||||
uint8 status # Current geofence status
|
||||
|
||||
uint8 GF_STATUS_LOADING = 0
|
||||
uint8 GF_STATUS_READY = 1
|
|
@ -1,8 +1,8 @@
|
|||
uint64 timestamp # time since system start (microseconds)
|
||||
|
||||
uint16 mission_id # Id for the mission for which the result was generated
|
||||
uint16 geofence_id # Id for the corresponding geofence for which the result was generated (used for mission feasibility)
|
||||
uint64 home_position_counter # Counter of the home position for which the result was generated (used for mission feasibility)
|
||||
uint32 mission_id # Id for the mission for which the result was generated
|
||||
uint32 geofence_id # Id for the corresponding geofence for which the result was generated (used for mission feasibility)
|
||||
uint32 home_position_counter # Counter of the home position for which the result was generated (used for mission feasibility)
|
||||
|
||||
int32 seq_reached # Sequence of the mission item which has been reached, default -1
|
||||
uint16 seq_current # Sequence of the current mission item
|
||||
|
|
|
@ -0,0 +1,8 @@
|
|||
# ParameterResetRequest : Used by the primary to reset one or all parameter value(s) on the remote
|
||||
|
||||
uint64 timestamp
|
||||
uint16 parameter_index
|
||||
|
||||
bool reset_all # If this is true then ignore parameter_index
|
||||
|
||||
uint8 ORB_QUEUE_LENGTH = 4
|
|
@ -0,0 +1,6 @@
|
|||
# ParameterSetUsedRequest : Used by a remote to update the used flag for a parameter on the primary
|
||||
|
||||
uint64 timestamp
|
||||
uint16 parameter_index
|
||||
|
||||
uint8 ORB_QUEUE_LENGTH = 64
|
|
@ -0,0 +1,11 @@
|
|||
# ParameterSetValueRequest : Used by a remote or primary to update the value for a parameter at the other end
|
||||
|
||||
uint64 timestamp
|
||||
uint16 parameter_index
|
||||
|
||||
int32 int_value # Optional value for an integer parameter
|
||||
float32 float_value # Optional value for a float parameter
|
||||
|
||||
uint8 ORB_QUEUE_LENGTH = 32
|
||||
|
||||
# TOPICS parameter_set_value_request parameter_remote_set_value_request parameter_primary_set_value_request
|
|
@ -0,0 +1,9 @@
|
|||
# ParameterSetValueResponse : Response to a set value request by either primary or secondary
|
||||
|
||||
uint64 timestamp
|
||||
uint64 request_timestamp
|
||||
uint16 parameter_index
|
||||
|
||||
uint8 ORB_QUEUE_LENGTH = 4
|
||||
|
||||
# TOPICS parameter_set_value_response parameter_remote_set_value_response parameter_primary_set_value_response
|
|
@ -47,4 +47,4 @@ uint16 ADSB_EMITTER_TYPE_SERVICE_SURFACE=18
|
|||
uint16 ADSB_EMITTER_TYPE_POINT_OBSTACLE=19
|
||||
uint16 ADSB_EMITTER_TYPE_ENUM_END=20
|
||||
|
||||
uint8 ORB_QUEUE_LENGTH = 8
|
||||
uint8 ORB_QUEUE_LENGTH = 16
|
||||
|
|
|
@ -52,6 +52,7 @@ add_library(px4_platform STATIC
|
|||
shutdown.cpp
|
||||
spi.cpp
|
||||
pab_manifest.c
|
||||
Serial.cpp
|
||||
${SRCS}
|
||||
)
|
||||
target_link_libraries(px4_platform prebuild_targets px4_work_queue)
|
||||
|
|
|
@ -0,0 +1,138 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2023 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
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <px4_platform_common/Serial.hpp>
|
||||
|
||||
namespace device
|
||||
{
|
||||
|
||||
Serial::Serial(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
|
||||
FlowControl flowcontrol) :
|
||||
_impl(port, baudrate, bytesize, parity, stopbits, flowcontrol)
|
||||
{
|
||||
// If no baudrate was specified then set it to a reasonable default value
|
||||
if (baudrate == 0) {
|
||||
(void) _impl.setBaudrate(9600);
|
||||
}
|
||||
}
|
||||
|
||||
Serial::~Serial()
|
||||
{
|
||||
}
|
||||
|
||||
bool Serial::open()
|
||||
{
|
||||
return _impl.open();
|
||||
}
|
||||
|
||||
bool Serial::isOpen() const
|
||||
{
|
||||
return _impl.isOpen();
|
||||
}
|
||||
|
||||
bool Serial::close()
|
||||
{
|
||||
return _impl.close();
|
||||
}
|
||||
|
||||
ssize_t Serial::read(uint8_t *buffer, size_t buffer_size)
|
||||
{
|
||||
return _impl.read(buffer, buffer_size);
|
||||
}
|
||||
|
||||
ssize_t Serial::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
|
||||
{
|
||||
return _impl.readAtLeast(buffer, buffer_size, character_count, timeout_us);
|
||||
}
|
||||
|
||||
ssize_t Serial::write(const void *buffer, size_t buffer_size)
|
||||
{
|
||||
return _impl.write(buffer, buffer_size);
|
||||
}
|
||||
|
||||
uint32_t Serial::getBaudrate() const
|
||||
{
|
||||
return _impl.getBaudrate();
|
||||
}
|
||||
|
||||
bool Serial::setBaudrate(uint32_t baudrate)
|
||||
{
|
||||
return _impl.setBaudrate(baudrate);
|
||||
}
|
||||
|
||||
ByteSize Serial::getBytesize() const
|
||||
{
|
||||
return _impl.getBytesize();
|
||||
}
|
||||
|
||||
bool Serial::setBytesize(ByteSize bytesize)
|
||||
{
|
||||
return _impl.setBytesize(bytesize);
|
||||
}
|
||||
|
||||
Parity Serial::getParity() const
|
||||
{
|
||||
return _impl.getParity();
|
||||
}
|
||||
|
||||
bool Serial::setParity(Parity parity)
|
||||
{
|
||||
return _impl.setParity(parity);
|
||||
}
|
||||
|
||||
StopBits Serial::getStopbits() const
|
||||
{
|
||||
return _impl.getStopbits();
|
||||
}
|
||||
|
||||
bool Serial::setStopbits(StopBits stopbits)
|
||||
{
|
||||
return _impl.setStopbits(stopbits);
|
||||
}
|
||||
|
||||
FlowControl Serial::getFlowcontrol() const
|
||||
{
|
||||
return _impl.getFlowcontrol();
|
||||
}
|
||||
|
||||
bool Serial::setFlowcontrol(FlowControl flowcontrol)
|
||||
{
|
||||
return _impl.setFlowcontrol(flowcontrol);
|
||||
}
|
||||
|
||||
const char *Serial::getPort() const
|
||||
{
|
||||
return _impl.getPort();
|
||||
}
|
||||
|
||||
} // namespace device
|
|
@ -0,0 +1,97 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2023 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
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <SerialImpl.hpp>
|
||||
|
||||
#include <px4_platform_common/SerialCommon.hpp>
|
||||
|
||||
using device::SerialConfig::ByteSize;
|
||||
using device::SerialConfig::Parity;
|
||||
using device::SerialConfig::StopBits;
|
||||
using device::SerialConfig::FlowControl;
|
||||
|
||||
namespace device __EXPORT
|
||||
{
|
||||
|
||||
class Serial
|
||||
{
|
||||
public:
|
||||
Serial(const char *port, uint32_t baudrate = 57600,
|
||||
ByteSize bytesize = ByteSize::EightBits, Parity parity = Parity::None,
|
||||
StopBits stopbits = StopBits::One, FlowControl flowcontrol = FlowControl::Disabled);
|
||||
virtual ~Serial();
|
||||
|
||||
// Open sets up the port and gets it configured based on desired configuration
|
||||
bool open();
|
||||
bool isOpen() const;
|
||||
|
||||
bool close();
|
||||
|
||||
ssize_t read(uint8_t *buffer, size_t buffer_size);
|
||||
ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
|
||||
|
||||
ssize_t write(const void *buffer, size_t buffer_size);
|
||||
|
||||
// If port is already open then the following configuration functions
|
||||
// will reconfigure the port. If the port is not yet open then they will
|
||||
// simply store the configuration in preparation for the port to be opened.
|
||||
|
||||
uint32_t getBaudrate() const;
|
||||
bool setBaudrate(uint32_t baudrate);
|
||||
|
||||
ByteSize getBytesize() const;
|
||||
bool setBytesize(ByteSize bytesize);
|
||||
|
||||
Parity getParity() const;
|
||||
bool setParity(Parity parity);
|
||||
|
||||
StopBits getStopbits() const;
|
||||
bool setStopbits(StopBits stopbits);
|
||||
|
||||
FlowControl getFlowcontrol() const;
|
||||
bool setFlowcontrol(FlowControl flowcontrol);
|
||||
|
||||
const char *getPort() const;
|
||||
|
||||
private:
|
||||
// Disable copy constructors
|
||||
Serial(const Serial &);
|
||||
Serial &operator=(const Serial &);
|
||||
|
||||
// platform implementation
|
||||
SerialImpl _impl;
|
||||
};
|
||||
|
||||
} // namespace device
|
|
@ -0,0 +1,70 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2023 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
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
namespace device
|
||||
{
|
||||
namespace SerialConfig
|
||||
{
|
||||
|
||||
|
||||
// ByteSize: number of data bits
|
||||
enum class ByteSize {
|
||||
FiveBits = 5,
|
||||
SixBits = 6,
|
||||
SevenBits = 7,
|
||||
EightBits = 8,
|
||||
};
|
||||
|
||||
// Parity: enable parity checking
|
||||
enum class Parity {
|
||||
None = 0,
|
||||
Odd = 1,
|
||||
Even = 2,
|
||||
};
|
||||
|
||||
// StopBits: number of stop bits
|
||||
enum class StopBits {
|
||||
One = 1,
|
||||
Two = 2
|
||||
};
|
||||
|
||||
// FlowControl: enable flow control
|
||||
enum class FlowControl {
|
||||
Disabled = 0,
|
||||
Enabled = 1,
|
||||
};
|
||||
|
||||
} // namespace SerialConfig
|
||||
} // namespace device
|
|
@ -13,11 +13,21 @@ __END_DECLS
|
|||
#define px4_clock_gettime system_clock_gettime
|
||||
#endif
|
||||
|
||||
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
|
||||
#if defined(ENABLE_LOCKSTEP_SCHEDULER) || defined(__PX4_QURT)
|
||||
|
||||
__BEGIN_DECLS
|
||||
__EXPORT int px4_clock_settime(clockid_t clk_id, const struct timespec *tp);
|
||||
__END_DECLS
|
||||
|
||||
#else
|
||||
|
||||
#define px4_clock_settime system_clock_settime
|
||||
|
||||
#endif
|
||||
|
||||
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
|
||||
|
||||
__BEGIN_DECLS
|
||||
__EXPORT int px4_usleep(useconds_t usec);
|
||||
__EXPORT unsigned int px4_sleep(unsigned int seconds);
|
||||
__EXPORT int px4_pthread_cond_timedwait(pthread_cond_t *cond,
|
||||
|
@ -27,7 +37,6 @@ __END_DECLS
|
|||
|
||||
#else
|
||||
|
||||
#define px4_clock_settime system_clock_settime
|
||||
#define px4_usleep system_usleep
|
||||
#define px4_sleep system_sleep
|
||||
#define px4_pthread_cond_timedwait system_pthread_cond_timedwait
|
||||
|
|
|
@ -56,31 +56,6 @@ public:
|
|||
SubscriptionBlocking(const orb_metadata *meta, uint32_t interval_us = 0, uint8_t instance = 0) :
|
||||
SubscriptionCallback(meta, interval_us, instance)
|
||||
{
|
||||
// pthread_mutexattr_init
|
||||
pthread_mutexattr_t attr;
|
||||
int ret_attr_init = pthread_mutexattr_init(&attr);
|
||||
|
||||
if (ret_attr_init != 0) {
|
||||
PX4_ERR("pthread_mutexattr_init failed, status=%d", ret_attr_init);
|
||||
}
|
||||
|
||||
#if defined(PTHREAD_PRIO_NONE)
|
||||
// pthread_mutexattr_settype
|
||||
// PTHREAD_PRIO_NONE not available on cygwin
|
||||
int ret_mutexattr_settype = pthread_mutexattr_settype(&attr, PTHREAD_PRIO_NONE);
|
||||
|
||||
if (ret_mutexattr_settype != 0) {
|
||||
PX4_ERR("pthread_mutexattr_settype failed, status=%d", ret_mutexattr_settype);
|
||||
}
|
||||
|
||||
#endif // PTHREAD_PRIO_NONE
|
||||
|
||||
// pthread_mutex_init
|
||||
int ret_mutex_init = pthread_mutex_init(&_mutex, &attr);
|
||||
|
||||
if (ret_mutex_init != 0) {
|
||||
PX4_ERR("pthread_mutex_init failed, status=%d", ret_mutex_init);
|
||||
}
|
||||
}
|
||||
|
||||
virtual ~SubscriptionBlocking()
|
||||
|
|
|
@ -216,7 +216,7 @@ const char *orb_get_c_type(unsigned char short_type)
|
|||
return nullptr;
|
||||
}
|
||||
|
||||
uint8_t orb_get_queue_depth(const struct orb_metadata *meta)
|
||||
uint8_t orb_get_queue_size(const struct orb_metadata *meta)
|
||||
{
|
||||
if (meta) {
|
||||
return meta->o_queue;
|
||||
|
|
|
@ -234,10 +234,10 @@ extern int orb_get_interval(int handle, unsigned *interval) __EXPORT;
|
|||
const char *orb_get_c_type(unsigned char short_type);
|
||||
|
||||
/**
|
||||
* Returns the queue depth of a topic
|
||||
* Returns the queue size of a topic
|
||||
* @param meta orb topic metadata
|
||||
*/
|
||||
extern uint8_t orb_get_queue_depth(const struct orb_metadata *meta);
|
||||
extern uint8_t orb_get_queue_size(const struct orb_metadata *meta);
|
||||
|
||||
/**
|
||||
* Print a topic to console. Do not call this directly, use print_message() instead.
|
||||
|
|
|
@ -48,31 +48,6 @@
|
|||
|
||||
static uORB::SubscriptionInterval *filp_to_subscription(cdev::file_t *filp) { return static_cast<uORB::SubscriptionInterval *>(filp->f_priv); }
|
||||
|
||||
// round up to nearest power of two
|
||||
// Such as 0 => 1, 1 => 1, 2 => 2 ,3 => 4, 10 => 16, 60 => 64, 65...255 => 128
|
||||
// Note: When the input value > 128, the output is always 128
|
||||
static inline uint8_t round_pow_of_two_8(uint8_t n)
|
||||
{
|
||||
if (n == 0) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
// Avoid is already a power of 2
|
||||
uint8_t value = n - 1;
|
||||
|
||||
// Fill 1
|
||||
value |= value >> 1U;
|
||||
value |= value >> 2U;
|
||||
value |= value >> 4U;
|
||||
|
||||
// Unable to round-up, take the value of round-down
|
||||
if (value == UINT8_MAX) {
|
||||
value >>= 1U;
|
||||
}
|
||||
|
||||
return value + 1;
|
||||
}
|
||||
|
||||
uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path) :
|
||||
CDev(strdup(path)), // success is checked in CDev::init
|
||||
_meta(meta),
|
||||
|
@ -437,7 +412,10 @@ int16_t uORB::DeviceNode::process_add_subscription()
|
|||
uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
|
||||
|
||||
if (_data != nullptr && ch != nullptr) { // _data will not be null if there is a publisher.
|
||||
ch->send_message(_meta->o_name, _meta->o_size, _data);
|
||||
// Only send the most recent data to initialize the remote end.
|
||||
if (_data_valid) {
|
||||
ch->send_message(_meta->o_name, _meta->o_size, _data + (_meta->o_size * ((_generation.load() - 1) % _meta->o_queue)));
|
||||
}
|
||||
}
|
||||
|
||||
return PX4_OK;
|
||||
|
|
|
@ -630,7 +630,7 @@ int16_t uORB::Manager::process_remote_topic(const char *topic_name)
|
|||
// We didn't find a node so we need to create it via an advertisement
|
||||
PX4_DEBUG("Advertising remote topic %s", topic_name);
|
||||
_remote_topics.insert(topic_name);
|
||||
orb_advertise(topic_ptr, nullptr, topic_ptr->o_queue);
|
||||
orb_advertise(topic_ptr, nullptr);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
|
@ -574,7 +574,7 @@ int uORBTest::UnitTest::test_wrap_around()
|
|||
bool updated{false};
|
||||
|
||||
// Advertise but not publish topics, only generate device_node, which is convenient for modifying DeviceNode::_generation
|
||||
const int queue_size = orb_test_medium_s::ORB_QUEUE_LENGTH;
|
||||
const int queue_size = orb_get_queue_size(ORB_ID(orb_test_medium_wrap_around));
|
||||
ptopic = orb_advertise(ORB_ID(orb_test_medium_wrap_around), nullptr);
|
||||
|
||||
if (ptopic == nullptr) {
|
||||
|
@ -828,7 +828,7 @@ int uORBTest::UnitTest::test_queue()
|
|||
return test_fail("subscribe failed: %d", errno);
|
||||
}
|
||||
|
||||
const int queue_size = orb_test_medium_s::ORB_QUEUE_LENGTH;
|
||||
const int queue_size = orb_get_queue_size(ORB_ID(orb_test_medium_queue));
|
||||
orb_test_medium_s t{};
|
||||
ptopic = orb_advertise(ORB_ID(orb_test_medium_queue), &t);
|
||||
|
||||
|
@ -935,7 +935,7 @@ int uORBTest::UnitTest::pub_test_queue_main()
|
|||
{
|
||||
orb_test_medium_s t{};
|
||||
orb_advert_t ptopic{nullptr};
|
||||
const int queue_size = orb_test_medium_s::ORB_QUEUE_LENGTH;
|
||||
const int queue_size = orb_get_queue_size(ORB_ID(orb_test_medium_queue_poll));
|
||||
|
||||
if ((ptopic = orb_advertise(ORB_ID(orb_test_medium_queue_poll), &t)) == nullptr) {
|
||||
_thread_should_exit = true;
|
||||
|
|
|
@ -0,0 +1,394 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2023 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
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <SerialImpl.hpp>
|
||||
#include <string.h> // strncpy
|
||||
#include <termios.h>
|
||||
#include <px4_log.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
#include <poll.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#define MODULE_NAME "SerialImpl"
|
||||
|
||||
namespace device
|
||||
{
|
||||
|
||||
SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
|
||||
FlowControl flowcontrol) :
|
||||
_baudrate(baudrate),
|
||||
_bytesize(bytesize),
|
||||
_parity(parity),
|
||||
_stopbits(stopbits),
|
||||
_flowcontrol(flowcontrol)
|
||||
{
|
||||
if (port) {
|
||||
strncpy(_port, port, sizeof(_port) - 1);
|
||||
_port[sizeof(_port) - 1] = '\0';
|
||||
|
||||
} else {
|
||||
_port[0] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
SerialImpl::~SerialImpl()
|
||||
{
|
||||
if (isOpen()) {
|
||||
close();
|
||||
}
|
||||
}
|
||||
|
||||
bool SerialImpl::validateBaudrate(uint32_t baudrate)
|
||||
{
|
||||
return ((baudrate == 9600) ||
|
||||
(baudrate == 19200) ||
|
||||
(baudrate == 38400) ||
|
||||
(baudrate == 57600) ||
|
||||
(baudrate == 115200) ||
|
||||
(baudrate == 230400) ||
|
||||
(baudrate == 460800) ||
|
||||
(baudrate == 921600));
|
||||
}
|
||||
|
||||
bool SerialImpl::configure()
|
||||
{
|
||||
/* process baud rate */
|
||||
int speed;
|
||||
|
||||
if (! validateBaudrate(_baudrate)) {
|
||||
PX4_ERR("ERR: unknown baudrate: %lu", _baudrate);
|
||||
return false;
|
||||
}
|
||||
|
||||
switch (_baudrate) {
|
||||
case 9600: speed = B9600; break;
|
||||
|
||||
case 19200: speed = B19200; break;
|
||||
|
||||
case 38400: speed = B38400; break;
|
||||
|
||||
case 57600: speed = B57600; break;
|
||||
|
||||
case 115200: speed = B115200; break;
|
||||
|
||||
case 230400: speed = B230400; break;
|
||||
|
||||
#ifndef B460800
|
||||
#define B460800 460800
|
||||
#endif
|
||||
|
||||
case 460800: speed = B460800; break;
|
||||
|
||||
#ifndef B921600
|
||||
#define B921600 921600
|
||||
#endif
|
||||
|
||||
case 921600: speed = B921600; break;
|
||||
|
||||
default:
|
||||
PX4_ERR("ERR: unknown baudrate: %lu", _baudrate);
|
||||
return false;
|
||||
}
|
||||
|
||||
struct termios uart_config;
|
||||
|
||||
int termios_state;
|
||||
|
||||
/* fill the struct for the new configuration */
|
||||
if ((termios_state = tcgetattr(_serial_fd, &uart_config)) < 0) {
|
||||
PX4_ERR("ERR: %d (tcgetattr)", termios_state);
|
||||
return false;
|
||||
}
|
||||
|
||||
/* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */
|
||||
|
||||
//
|
||||
// Input flags - Turn off input processing
|
||||
//
|
||||
// convert break to null byte, no CR to NL translation,
|
||||
// no NL to CR translation, don't mark parity errors or breaks
|
||||
// no input parity check, don't strip high bit off,
|
||||
// no XON/XOFF software flow control
|
||||
//
|
||||
uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
|
||||
INLCR | PARMRK | INPCK | ISTRIP | IXON);
|
||||
|
||||
//
|
||||
// Output flags - Turn off output processing
|
||||
//
|
||||
// no CR to NL translation, no NL to CR-NL translation,
|
||||
// no NL to CR translation, no column 0 CR suppression,
|
||||
// no Ctrl-D suppression, no fill characters, no case mapping,
|
||||
// no local output processing
|
||||
//
|
||||
// config.c_oflag &= ~(OCRNL | ONLCR | ONLRET |
|
||||
// ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
|
||||
uart_config.c_oflag = 0;
|
||||
|
||||
//
|
||||
// No line processing
|
||||
//
|
||||
// echo off, echo newline off, canonical mode off,
|
||||
// extended input processing off, signal chars off
|
||||
//
|
||||
uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
|
||||
|
||||
/* no parity, one stop bit, disable flow control */
|
||||
uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
|
||||
|
||||
/* set baud rate */
|
||||
if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
|
||||
PX4_ERR("ERR: %d (cfsetispeed)", termios_state);
|
||||
return false;
|
||||
}
|
||||
|
||||
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
|
||||
PX4_ERR("ERR: %d (cfsetospeed)", termios_state);
|
||||
return false;
|
||||
}
|
||||
|
||||
if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) {
|
||||
PX4_ERR("ERR: %d (tcsetattr)", termios_state);
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool SerialImpl::open()
|
||||
{
|
||||
if (isOpen()) {
|
||||
return true;
|
||||
}
|
||||
|
||||
// Open the serial port
|
||||
int serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
|
||||
|
||||
if (serial_fd < 0) {
|
||||
PX4_ERR("failed to open %s err: %d", _port, errno);
|
||||
return false;
|
||||
}
|
||||
|
||||
_serial_fd = serial_fd;
|
||||
|
||||
// Configure the serial port
|
||||
if (! configure()) {
|
||||
PX4_ERR("failed to configure %s err: %d", _port, errno);
|
||||
return false;
|
||||
}
|
||||
|
||||
_open = true;
|
||||
|
||||
return _open;
|
||||
}
|
||||
|
||||
bool SerialImpl::isOpen() const
|
||||
{
|
||||
return _open;
|
||||
}
|
||||
|
||||
bool SerialImpl::close()
|
||||
{
|
||||
|
||||
if (_serial_fd >= 0) {
|
||||
::close(_serial_fd);
|
||||
}
|
||||
|
||||
_serial_fd = -1;
|
||||
_open = false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size)
|
||||
{
|
||||
if (!_open) {
|
||||
PX4_ERR("Cannot read from serial device until it has been opened");
|
||||
return -1;
|
||||
}
|
||||
|
||||
int ret = ::read(_serial_fd, buffer, buffer_size);
|
||||
|
||||
if (ret < 0) {
|
||||
PX4_DEBUG("%s read error %d", _port, ret);
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
|
||||
{
|
||||
if (!_open) {
|
||||
PX4_ERR("Cannot readAtLeast from serial device until it has been opened");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (buffer_size < character_count) {
|
||||
PX4_ERR("%s: Buffer not big enough to hold desired amount of read data", __FUNCTION__);
|
||||
return -1;
|
||||
}
|
||||
|
||||
const hrt_abstime start_time_us = hrt_absolute_time();
|
||||
int total_bytes_read = 0;
|
||||
|
||||
while ((total_bytes_read < (int) character_count) && (hrt_elapsed_time(&start_time_us) < timeout_us)) {
|
||||
// Poll for incoming UART data.
|
||||
pollfd fds[1];
|
||||
fds[0].fd = _serial_fd;
|
||||
fds[0].events = POLLIN;
|
||||
|
||||
hrt_abstime remaining_time = timeout_us - hrt_elapsed_time(&start_time_us);
|
||||
|
||||
if (remaining_time <= 0) { break; }
|
||||
|
||||
int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), remaining_time);
|
||||
|
||||
if (ret > 0) {
|
||||
if (fds[0].revents & POLLIN) {
|
||||
const unsigned sleeptime = character_count * 1000000 / (_baudrate / 10);
|
||||
|
||||
int err = 0;
|
||||
int bytes_available = 0;
|
||||
err = ::ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available);
|
||||
|
||||
if (err != 0 || bytes_available < (int)character_count) {
|
||||
px4_usleep(sleeptime);
|
||||
}
|
||||
|
||||
ret = read(&buffer[total_bytes_read], buffer_size - total_bytes_read);
|
||||
|
||||
if (ret > 0) {
|
||||
total_bytes_read += ret;
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("Got a poll error");
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return total_bytes_read;
|
||||
}
|
||||
|
||||
ssize_t SerialImpl::write(const void *buffer, size_t buffer_size)
|
||||
{
|
||||
if (!_open) {
|
||||
PX4_ERR("Cannot write to serial device until it has been opened");
|
||||
return -1;
|
||||
}
|
||||
|
||||
int written = ::write(_serial_fd, buffer, buffer_size);
|
||||
::fsync(_serial_fd);
|
||||
|
||||
if (written < 0) {
|
||||
PX4_ERR("%s write error %d", _port, written);
|
||||
}
|
||||
|
||||
return written;
|
||||
}
|
||||
|
||||
const char *SerialImpl::getPort() const
|
||||
{
|
||||
return _port;
|
||||
}
|
||||
|
||||
uint32_t SerialImpl::getBaudrate() const
|
||||
{
|
||||
return _baudrate;
|
||||
}
|
||||
|
||||
bool SerialImpl::setBaudrate(uint32_t baudrate)
|
||||
{
|
||||
if (! validateBaudrate(baudrate)) {
|
||||
PX4_ERR("ERR: invalid baudrate: %lu", baudrate);
|
||||
return false;
|
||||
}
|
||||
|
||||
// check if already configured
|
||||
if ((baudrate == _baudrate) && _open) {
|
||||
return true;
|
||||
}
|
||||
|
||||
_baudrate = baudrate;
|
||||
|
||||
// process baud rate change now if port is already open
|
||||
if (_open) {
|
||||
return configure();
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
ByteSize SerialImpl::getBytesize() const
|
||||
{
|
||||
return _bytesize;
|
||||
}
|
||||
|
||||
bool SerialImpl::setBytesize(ByteSize bytesize)
|
||||
{
|
||||
return bytesize == ByteSize::EightBits;
|
||||
}
|
||||
|
||||
Parity SerialImpl::getParity() const
|
||||
{
|
||||
return _parity;
|
||||
}
|
||||
|
||||
bool SerialImpl::setParity(Parity parity)
|
||||
{
|
||||
return parity == Parity::None;
|
||||
}
|
||||
|
||||
StopBits SerialImpl::getStopbits() const
|
||||
{
|
||||
return _stopbits;
|
||||
}
|
||||
|
||||
bool SerialImpl::setStopbits(StopBits stopbits)
|
||||
{
|
||||
return stopbits == StopBits::One;
|
||||
}
|
||||
|
||||
FlowControl SerialImpl::getFlowcontrol() const
|
||||
{
|
||||
return _flowcontrol;
|
||||
}
|
||||
|
||||
bool SerialImpl::setFlowcontrol(FlowControl flowcontrol)
|
||||
{
|
||||
return flowcontrol == FlowControl::Disabled;
|
||||
}
|
||||
|
||||
} // namespace device
|
|
@ -0,0 +1,104 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2023 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
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <px4_platform_common/SerialCommon.hpp>
|
||||
|
||||
using device::SerialConfig::ByteSize;
|
||||
using device::SerialConfig::Parity;
|
||||
using device::SerialConfig::StopBits;
|
||||
using device::SerialConfig::FlowControl;
|
||||
|
||||
namespace device
|
||||
{
|
||||
|
||||
class SerialImpl
|
||||
{
|
||||
public:
|
||||
|
||||
SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
|
||||
FlowControl flowcontrol);
|
||||
virtual ~SerialImpl();
|
||||
|
||||
bool open();
|
||||
bool isOpen() const;
|
||||
|
||||
bool close();
|
||||
|
||||
ssize_t read(uint8_t *buffer, size_t buffer_size);
|
||||
ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
|
||||
|
||||
ssize_t write(const void *buffer, size_t buffer_size);
|
||||
|
||||
const char *getPort() const;
|
||||
|
||||
uint32_t getBaudrate() const;
|
||||
bool setBaudrate(uint32_t baudrate);
|
||||
|
||||
ByteSize getBytesize() const;
|
||||
bool setBytesize(ByteSize bytesize);
|
||||
|
||||
Parity getParity() const;
|
||||
bool setParity(Parity parity);
|
||||
|
||||
StopBits getStopbits() const;
|
||||
bool setStopbits(StopBits stopbits);
|
||||
|
||||
FlowControl getFlowcontrol() const;
|
||||
bool setFlowcontrol(FlowControl flowcontrol);
|
||||
|
||||
private:
|
||||
|
||||
int _serial_fd{-1};
|
||||
|
||||
bool _open{false};
|
||||
|
||||
char _port[32] {};
|
||||
|
||||
uint32_t _baudrate{0};
|
||||
|
||||
ByteSize _bytesize{ByteSize::EightBits};
|
||||
Parity _parity{Parity::None};
|
||||
StopBits _stopbits{StopBits::One};
|
||||
FlowControl _flowcontrol{FlowControl::Disabled};
|
||||
|
||||
bool validateBaudrate(uint32_t baudrate);
|
||||
bool configure();
|
||||
|
||||
};
|
||||
|
||||
} // namespace device
|
|
@ -3,6 +3,8 @@
|
|||
add_library(px4_layer
|
||||
${KERNEL_SRCS}
|
||||
cdc_acm_check.cpp
|
||||
${PX4_SOURCE_DIR}/platforms/common/Serial.cpp
|
||||
SerialImpl.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(px4_layer
|
||||
|
|
|
@ -15,6 +15,8 @@ add_library(px4_layer
|
|||
usr_board_ctrl.c
|
||||
usr_hrt.cpp
|
||||
usr_mcu_version.cpp
|
||||
${PX4_SOURCE_DIR}/platforms/common/Serial.cpp
|
||||
SerialImpl.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(px4_layer
|
||||
|
|
|
@ -0,0 +1,103 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2023 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
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <px4_platform_common/SerialCommon.hpp>
|
||||
|
||||
using device::SerialConfig::ByteSize;
|
||||
using device::SerialConfig::Parity;
|
||||
using device::SerialConfig::StopBits;
|
||||
using device::SerialConfig::FlowControl;
|
||||
|
||||
namespace device
|
||||
{
|
||||
|
||||
class SerialImpl
|
||||
{
|
||||
public:
|
||||
|
||||
SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
|
||||
FlowControl flowcontrol);
|
||||
virtual ~SerialImpl();
|
||||
|
||||
bool open();
|
||||
bool isOpen() const;
|
||||
|
||||
bool close();
|
||||
|
||||
ssize_t read(uint8_t *buffer, size_t buffer_size);
|
||||
ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
|
||||
|
||||
ssize_t write(const void *buffer, size_t buffer_size);
|
||||
|
||||
const char *getPort() const;
|
||||
|
||||
uint32_t getBaudrate() const;
|
||||
bool setBaudrate(uint32_t baudrate);
|
||||
|
||||
ByteSize getBytesize() const;
|
||||
bool setBytesize(ByteSize bytesize);
|
||||
|
||||
Parity getParity() const;
|
||||
bool setParity(Parity parity);
|
||||
|
||||
StopBits getStopbits() const;
|
||||
bool setStopbits(StopBits stopbits);
|
||||
|
||||
FlowControl getFlowcontrol() const;
|
||||
bool setFlowcontrol(FlowControl flowcontrol);
|
||||
|
||||
private:
|
||||
|
||||
int _serial_fd{-1};
|
||||
|
||||
bool _open{false};
|
||||
|
||||
char _port[32] {};
|
||||
|
||||
uint32_t _baudrate{0};
|
||||
|
||||
ByteSize _bytesize{ByteSize::EightBits};
|
||||
Parity _parity{Parity::None};
|
||||
StopBits _stopbits{StopBits::One};
|
||||
FlowControl _flowcontrol{FlowControl::Disabled};
|
||||
|
||||
bool validateBaudrate(uint32_t baudrate);
|
||||
bool configure();
|
||||
};
|
||||
|
||||
} // namespace device
|
|
@ -46,6 +46,8 @@ add_library(px4_layer
|
|||
drv_hrt.cpp
|
||||
cpuload.cpp
|
||||
print_load.cpp
|
||||
${PX4_SOURCE_DIR}/platforms/common/Serial.cpp
|
||||
SerialImpl.cpp
|
||||
)
|
||||
target_compile_definitions(px4_layer PRIVATE MODULE_NAME="px4")
|
||||
target_compile_options(px4_layer PRIVATE -Wno-cast-align) # TODO: fix and enable
|
||||
|
|
|
@ -0,0 +1,387 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2023 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
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <SerialImpl.hpp>
|
||||
#include <string.h> // strncpy
|
||||
#include <termios.h>
|
||||
#include <px4_log.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
#include <poll.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
namespace device
|
||||
{
|
||||
|
||||
SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
|
||||
FlowControl flowcontrol) :
|
||||
_baudrate(baudrate),
|
||||
_bytesize(bytesize),
|
||||
_parity(parity),
|
||||
_stopbits(stopbits),
|
||||
_flowcontrol(flowcontrol)
|
||||
{
|
||||
if (port) {
|
||||
strncpy(_port, port, sizeof(_port) - 1);
|
||||
_port[sizeof(_port) - 1] = '\0';
|
||||
|
||||
} else {
|
||||
_port[0] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
SerialImpl::~SerialImpl()
|
||||
{
|
||||
if (isOpen()) {
|
||||
close();
|
||||
}
|
||||
}
|
||||
|
||||
bool SerialImpl::validateBaudrate(uint32_t baudrate)
|
||||
{
|
||||
return ((baudrate == 9600) ||
|
||||
(baudrate == 19200) ||
|
||||
(baudrate == 38400) ||
|
||||
(baudrate == 57600) ||
|
||||
(baudrate == 115200) ||
|
||||
(baudrate == 230400) ||
|
||||
(baudrate == 460800) ||
|
||||
(baudrate == 921600));
|
||||
}
|
||||
|
||||
bool SerialImpl::configure()
|
||||
{
|
||||
/* process baud rate */
|
||||
int speed;
|
||||
|
||||
if (! validateBaudrate(_baudrate)) {
|
||||
PX4_ERR("ERR: unknown baudrate: %u", _baudrate);
|
||||
return false;
|
||||
}
|
||||
|
||||
switch (_baudrate) {
|
||||
case 9600: speed = B9600; break;
|
||||
|
||||
case 19200: speed = B19200; break;
|
||||
|
||||
case 38400: speed = B38400; break;
|
||||
|
||||
case 57600: speed = B57600; break;
|
||||
|
||||
case 115200: speed = B115200; break;
|
||||
|
||||
case 230400: speed = B230400; break;
|
||||
|
||||
#ifndef B460800
|
||||
#define B460800 460800
|
||||
#endif
|
||||
|
||||
case 460800: speed = B460800; break;
|
||||
|
||||
#ifndef B921600
|
||||
#define B921600 921600
|
||||
#endif
|
||||
|
||||
case 921600: speed = B921600; break;
|
||||
|
||||
default:
|
||||
PX4_ERR("ERR: unknown baudrate: %d", _baudrate);
|
||||
return false;
|
||||
}
|
||||
|
||||
struct termios uart_config;
|
||||
|
||||
int termios_state;
|
||||
|
||||
/* fill the struct for the new configuration */
|
||||
if ((termios_state = tcgetattr(_serial_fd, &uart_config)) < 0) {
|
||||
PX4_ERR("ERR: %d (tcgetattr)", termios_state);
|
||||
return false;
|
||||
}
|
||||
|
||||
/* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */
|
||||
|
||||
//
|
||||
// Input flags - Turn off input processing
|
||||
//
|
||||
// convert break to null byte, no CR to NL translation,
|
||||
// no NL to CR translation, don't mark parity errors or breaks
|
||||
// no input parity check, don't strip high bit off,
|
||||
// no XON/XOFF software flow control
|
||||
//
|
||||
uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
|
||||
INLCR | PARMRK | INPCK | ISTRIP | IXON);
|
||||
|
||||
//
|
||||
// Output flags - Turn off output processing
|
||||
//
|
||||
// no CR to NL translation, no NL to CR-NL translation,
|
||||
// no NL to CR translation, no column 0 CR suppression,
|
||||
// no Ctrl-D suppression, no fill characters, no case mapping,
|
||||
// no local output processing
|
||||
//
|
||||
// config.c_oflag &= ~(OCRNL | ONLCR | ONLRET |
|
||||
// ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
|
||||
uart_config.c_oflag = 0;
|
||||
|
||||
//
|
||||
// No line processing
|
||||
//
|
||||
// echo off, echo newline off, canonical mode off,
|
||||
// extended input processing off, signal chars off
|
||||
//
|
||||
uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
|
||||
|
||||
/* no parity, one stop bit, disable flow control */
|
||||
uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
|
||||
|
||||
/* set baud rate */
|
||||
if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
|
||||
PX4_ERR("ERR: %d (cfsetispeed)", termios_state);
|
||||
return false;
|
||||
}
|
||||
|
||||
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
|
||||
PX4_ERR("ERR: %d (cfsetospeed)", termios_state);
|
||||
return false;
|
||||
}
|
||||
|
||||
if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) {
|
||||
PX4_ERR("ERR: %d (tcsetattr)", termios_state);
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool SerialImpl::open()
|
||||
{
|
||||
if (isOpen()) {
|
||||
return true;
|
||||
}
|
||||
|
||||
// Open the serial port
|
||||
int serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
|
||||
|
||||
if (serial_fd < 0) {
|
||||
PX4_ERR("failed to open %s err: %d", _port, errno);
|
||||
return false;
|
||||
}
|
||||
|
||||
_serial_fd = serial_fd;
|
||||
|
||||
// Configure the serial port
|
||||
if (! configure()) {
|
||||
PX4_ERR("failed to configure %s err: %d", _port, errno);
|
||||
return false;
|
||||
}
|
||||
|
||||
_open = true;
|
||||
|
||||
return _open;
|
||||
}
|
||||
|
||||
bool SerialImpl::isOpen() const
|
||||
{
|
||||
return _open;
|
||||
}
|
||||
|
||||
bool SerialImpl::close()
|
||||
{
|
||||
|
||||
if (_serial_fd >= 0) {
|
||||
::close(_serial_fd);
|
||||
}
|
||||
|
||||
_serial_fd = -1;
|
||||
_open = false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size)
|
||||
{
|
||||
if (!_open) {
|
||||
PX4_ERR("Cannot read from serial device until it has been opened");
|
||||
return -1;
|
||||
}
|
||||
|
||||
int ret = ::read(_serial_fd, buffer, buffer_size);
|
||||
|
||||
if (ret < 0) {
|
||||
PX4_DEBUG("%s read error %d", _port, ret);
|
||||
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
|
||||
{
|
||||
if (!_open) {
|
||||
PX4_ERR("Cannot readAtLeast from serial device until it has been opened");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (buffer_size < character_count) {
|
||||
PX4_ERR("%s: Buffer not big enough to hold desired amount of read data", __FUNCTION__);
|
||||
return -1;
|
||||
}
|
||||
|
||||
const hrt_abstime start_time_us = hrt_absolute_time();
|
||||
int total_bytes_read = 0;
|
||||
|
||||
while ((total_bytes_read < (int) character_count) && (hrt_elapsed_time(&start_time_us) < timeout_us)) {
|
||||
// Poll for incoming UART data.
|
||||
pollfd fds[1];
|
||||
fds[0].fd = _serial_fd;
|
||||
fds[0].events = POLLIN;
|
||||
|
||||
hrt_abstime remaining_time = timeout_us - hrt_elapsed_time(&start_time_us);
|
||||
|
||||
if (remaining_time <= 0) { break; }
|
||||
|
||||
int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), remaining_time);
|
||||
|
||||
if (ret > 0) {
|
||||
if (fds[0].revents & POLLIN) {
|
||||
const unsigned sleeptime = character_count * 1000000 / (_baudrate / 10);
|
||||
px4_usleep(sleeptime);
|
||||
|
||||
ret = read(&buffer[total_bytes_read], buffer_size - total_bytes_read);
|
||||
|
||||
if (ret > 0) {
|
||||
total_bytes_read += ret;
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("Got a poll error");
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return total_bytes_read;
|
||||
}
|
||||
|
||||
ssize_t SerialImpl::write(const void *buffer, size_t buffer_size)
|
||||
{
|
||||
if (!_open) {
|
||||
PX4_ERR("Cannot write to serial device until it has been opened");
|
||||
return -1;
|
||||
}
|
||||
|
||||
int written = ::write(_serial_fd, buffer, buffer_size);
|
||||
::fsync(_serial_fd);
|
||||
|
||||
if (written < 0) {
|
||||
PX4_ERR("%s write error %d", _port, written);
|
||||
|
||||
}
|
||||
|
||||
return written;
|
||||
}
|
||||
|
||||
const char *SerialImpl::getPort() const
|
||||
{
|
||||
return _port;
|
||||
}
|
||||
|
||||
uint32_t SerialImpl::getBaudrate() const
|
||||
{
|
||||
return _baudrate;
|
||||
}
|
||||
|
||||
bool SerialImpl::setBaudrate(uint32_t baudrate)
|
||||
{
|
||||
if (! validateBaudrate(baudrate)) {
|
||||
PX4_ERR("ERR: invalid baudrate: %u", baudrate);
|
||||
return false;
|
||||
}
|
||||
|
||||
// check if already configured
|
||||
if ((baudrate == _baudrate) && _open) {
|
||||
return true;
|
||||
}
|
||||
|
||||
_baudrate = baudrate;
|
||||
|
||||
// process baud rate change now if port is already open
|
||||
if (_open) {
|
||||
return configure();
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
ByteSize SerialImpl::getBytesize() const
|
||||
{
|
||||
return _bytesize;
|
||||
}
|
||||
|
||||
bool SerialImpl::setBytesize(ByteSize bytesize)
|
||||
{
|
||||
return bytesize == ByteSize::EightBits;
|
||||
}
|
||||
|
||||
Parity SerialImpl::getParity() const
|
||||
{
|
||||
return _parity;
|
||||
}
|
||||
|
||||
bool SerialImpl::setParity(Parity parity)
|
||||
{
|
||||
return parity == Parity::None;
|
||||
}
|
||||
|
||||
StopBits SerialImpl::getStopbits() const
|
||||
{
|
||||
return _stopbits;
|
||||
}
|
||||
|
||||
bool SerialImpl::setStopbits(StopBits stopbits)
|
||||
{
|
||||
return stopbits == StopBits::One;
|
||||
}
|
||||
|
||||
FlowControl SerialImpl::getFlowcontrol() const
|
||||
{
|
||||
return _flowcontrol;
|
||||
}
|
||||
|
||||
bool SerialImpl::setFlowcontrol(FlowControl flowcontrol)
|
||||
{
|
||||
return flowcontrol == FlowControl::Disabled;
|
||||
}
|
||||
|
||||
} // namespace device
|
|
@ -51,6 +51,11 @@
|
|||
#include <errno.h>
|
||||
#include "hrt_work.h"
|
||||
|
||||
// Voxl2 board specific API definitions to get time offset
|
||||
#if defined(CONFIG_MUORB_APPS_SYNC_TIMESTAMP)
|
||||
#include "fc_sensor.h"
|
||||
#endif
|
||||
|
||||
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
|
||||
#include <lockstep_scheduler/lockstep_scheduler.h>
|
||||
static LockstepScheduler lockstep_scheduler {true};
|
||||
|
@ -107,6 +112,29 @@ hrt_abstime hrt_absolute_time()
|
|||
#else // defined(ENABLE_LOCKSTEP_SCHEDULER)
|
||||
struct timespec ts;
|
||||
px4_clock_gettime(CLOCK_MONOTONIC, &ts);
|
||||
|
||||
# if defined(CONFIG_MUORB_APPS_SYNC_TIMESTAMP)
|
||||
hrt_abstime temp_abstime = ts_to_abstime(&ts);
|
||||
int apps_time_offset = fc_sensor_get_time_offset();
|
||||
|
||||
if (apps_time_offset < 0) {
|
||||
hrt_abstime temp_offset = -apps_time_offset;
|
||||
|
||||
if (temp_offset >= temp_abstime) {
|
||||
temp_abstime = 0;
|
||||
|
||||
} else {
|
||||
temp_abstime -= temp_offset;
|
||||
}
|
||||
|
||||
} else {
|
||||
temp_abstime += (hrt_abstime) apps_time_offset;
|
||||
}
|
||||
|
||||
ts.tv_sec = temp_abstime / 1000000;
|
||||
ts.tv_nsec = (temp_abstime % 1000000) * 1000;
|
||||
# endif // defined(CONFIG_MUORB_APPS_SYNC_TIMESTAMP)
|
||||
|
||||
return ts_to_abstime(&ts);
|
||||
#endif // defined(ENABLE_LOCKSTEP_SCHEDULER)
|
||||
}
|
||||
|
@ -449,6 +477,7 @@ int px4_clock_gettime(clockid_t clk_id, struct timespec *tp)
|
|||
|
||||
#endif // defined(ENABLE_LOCKSTEP_SCHEDULER)
|
||||
return system_clock_gettime(clk_id, tp);
|
||||
|
||||
}
|
||||
|
||||
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
|
||||
|
|
|
@ -50,5 +50,4 @@ add_library(px4 SHARED
|
|||
target_link_libraries(px4
|
||||
modules__muorb__slpi
|
||||
${module_libraries}
|
||||
px4_layer
|
||||
)
|
||||
|
|
|
@ -0,0 +1,108 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2023 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
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <unistd.h>
|
||||
|
||||
#include <px4_platform_common/SerialCommon.hpp>
|
||||
|
||||
using device::SerialConfig::ByteSize;
|
||||
using device::SerialConfig::Parity;
|
||||
using device::SerialConfig::StopBits;
|
||||
using device::SerialConfig::FlowControl;
|
||||
|
||||
namespace device
|
||||
{
|
||||
|
||||
class SerialImpl
|
||||
{
|
||||
public:
|
||||
|
||||
SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
|
||||
FlowControl flowcontrol);
|
||||
virtual ~SerialImpl();
|
||||
|
||||
bool open();
|
||||
bool isOpen() const;
|
||||
|
||||
bool close();
|
||||
|
||||
ssize_t read(uint8_t *buffer, size_t buffer_size);
|
||||
ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
|
||||
|
||||
ssize_t write(const void *buffer, size_t buffer_size);
|
||||
|
||||
const char *getPort() const;
|
||||
bool setPort(const char *port);
|
||||
|
||||
uint32_t getBaudrate() const;
|
||||
bool setBaudrate(uint32_t baudrate);
|
||||
|
||||
ByteSize getBytesize() const;
|
||||
bool setBytesize(ByteSize bytesize);
|
||||
|
||||
Parity getParity() const;
|
||||
bool setParity(Parity parity);
|
||||
|
||||
StopBits getStopbits() const;
|
||||
bool setStopbits(StopBits stopbits);
|
||||
|
||||
FlowControl getFlowcontrol() const;
|
||||
bool setFlowcontrol(FlowControl flowcontrol);
|
||||
|
||||
private:
|
||||
|
||||
int _serial_fd{-1};
|
||||
|
||||
bool _open{false};
|
||||
|
||||
char _port[32] {};
|
||||
|
||||
uint32_t _baudrate{0};
|
||||
|
||||
ByteSize _bytesize{ByteSize::EightBits};
|
||||
Parity _parity{Parity::None};
|
||||
StopBits _stopbits{StopBits::One};
|
||||
FlowControl _flowcontrol{FlowControl::Disabled};
|
||||
|
||||
bool validateBaudrate(uint32_t baudrate);
|
||||
|
||||
// Mutex used to lock the read functions
|
||||
//pthread_mutex_t read_mutex;
|
||||
|
||||
// Mutex used to lock the write functions
|
||||
//pthread_mutex_t write_mutex;
|
||||
};
|
||||
|
||||
} // namespace device
|
|
@ -38,6 +38,7 @@ set(QURT_LAYER_SRCS
|
|||
px4_qurt_impl.cpp
|
||||
main.cpp
|
||||
qurt_log.cpp
|
||||
SerialImpl.cpp
|
||||
)
|
||||
|
||||
add_library(px4_layer
|
||||
|
|
|
@ -0,0 +1,326 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2023 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
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <SerialImpl.hpp>
|
||||
#include <string.h> // strncpy
|
||||
#include <px4_log.h>
|
||||
#include <drivers/device/qurt/uart.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
namespace device
|
||||
{
|
||||
|
||||
SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
|
||||
FlowControl flowcontrol) :
|
||||
_baudrate(baudrate),
|
||||
_bytesize(bytesize),
|
||||
_parity(parity),
|
||||
_stopbits(stopbits),
|
||||
_flowcontrol(flowcontrol)
|
||||
{
|
||||
if (port) {
|
||||
strncpy(_port, port, sizeof(_port) - 1);
|
||||
_port[sizeof(_port) - 1] = '\0';
|
||||
|
||||
} else {
|
||||
_port[0] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
SerialImpl::~SerialImpl()
|
||||
{
|
||||
if (isOpen()) {
|
||||
close();
|
||||
}
|
||||
}
|
||||
|
||||
bool SerialImpl::validateBaudrate(uint32_t baudrate)
|
||||
{
|
||||
if ((baudrate != 9600) &&
|
||||
(baudrate != 38400) &&
|
||||
(baudrate != 57600) &&
|
||||
(baudrate != 115200) &&
|
||||
(baudrate != 230400) &&
|
||||
(baudrate != 250000) &&
|
||||
(baudrate != 420000) &&
|
||||
(baudrate != 460800) &&
|
||||
(baudrate != 921600) &&
|
||||
(baudrate != 1000000) &&
|
||||
(baudrate != 1843200) &&
|
||||
(baudrate != 2000000)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool SerialImpl::open()
|
||||
{
|
||||
// There's no harm in calling open multiple times on the same port.
|
||||
// In fact, that's the only way to change the baudrate
|
||||
|
||||
_open = false;
|
||||
_serial_fd = -1;
|
||||
|
||||
if (! validateBaudrate(_baudrate)) {
|
||||
PX4_ERR("Invalid baudrate: %u", _baudrate);
|
||||
return false;
|
||||
}
|
||||
|
||||
if (_bytesize != ByteSize::EightBits) {
|
||||
PX4_ERR("Qurt platform only supports ByteSize::EightBits");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (_parity != Parity::None) {
|
||||
PX4_ERR("Qurt platform only supports Parity::None");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (_stopbits != StopBits::One) {
|
||||
PX4_ERR("Qurt platform only supports StopBits::One");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (_flowcontrol != FlowControl::Disabled) {
|
||||
PX4_ERR("Qurt platform only supports FlowControl::Disabled");
|
||||
return false;
|
||||
}
|
||||
|
||||
// qurt_uart_open will check validity of port and baudrate
|
||||
int serial_fd = qurt_uart_open(_port, _baudrate);
|
||||
|
||||
if (serial_fd < 0) {
|
||||
PX4_ERR("failed to open %s, fd returned: %d", _port, serial_fd);
|
||||
return false;
|
||||
|
||||
} else {
|
||||
PX4_INFO("Successfully opened UART %s with baudrate %u", _port, _baudrate);
|
||||
}
|
||||
|
||||
_serial_fd = serial_fd;
|
||||
_open = true;
|
||||
|
||||
return _open;
|
||||
}
|
||||
|
||||
bool SerialImpl::isOpen() const
|
||||
{
|
||||
return _open;
|
||||
}
|
||||
|
||||
bool SerialImpl::close()
|
||||
{
|
||||
// No close defined for qurt uart yet
|
||||
// if (_serial_fd >= 0) {
|
||||
// qurt_uart_close(_serial_fd);
|
||||
// }
|
||||
|
||||
_serial_fd = -1;
|
||||
_open = false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size)
|
||||
{
|
||||
if (!_open) {
|
||||
PX4_ERR("Cannot read from serial device until it has been opened");
|
||||
return -1;
|
||||
}
|
||||
|
||||
int ret_read = qurt_uart_read(_serial_fd, (char *) buffer, buffer_size, 500);
|
||||
|
||||
if (ret_read < 0) {
|
||||
PX4_DEBUG("%s read error %d", _port, ret_read);
|
||||
|
||||
}
|
||||
|
||||
return ret_read;
|
||||
}
|
||||
|
||||
ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
|
||||
{
|
||||
if (!_open) {
|
||||
PX4_ERR("Cannot readAtLeast from serial device until it has been opened");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (buffer_size < character_count) {
|
||||
PX4_ERR("%s: Buffer not big enough to hold desired amount of read data", __FUNCTION__);
|
||||
return -1;
|
||||
}
|
||||
|
||||
const hrt_abstime start_time_us = hrt_absolute_time();
|
||||
int total_bytes_read = 0;
|
||||
|
||||
while (total_bytes_read < (int) character_count) {
|
||||
|
||||
if (timeout_us > 0) {
|
||||
const uint64_t elapsed_us = hrt_elapsed_time(&start_time_us);
|
||||
|
||||
if (elapsed_us >= timeout_us) {
|
||||
// If there was a partial read but not enough to satisfy the minimum then they will be lost
|
||||
// but this really should never happen when everything is working normally.
|
||||
// PX4_WARN("%s timeout %d bytes read (%llu us elapsed)", __FUNCTION__, total_bytes_read, elapsed_us);
|
||||
// Or, instead of returning an error, should we return the number of bytes read (assuming it is greater than zero)?
|
||||
return total_bytes_read;
|
||||
}
|
||||
}
|
||||
|
||||
int current_bytes_read = read(&buffer[total_bytes_read], buffer_size - total_bytes_read);
|
||||
|
||||
if (current_bytes_read < 0) {
|
||||
// Again, if there was a partial read but not enough to satisfy the minimum then they will be lost
|
||||
// but this really should never happen when everything is working normally.
|
||||
PX4_ERR("%s failed to read uart", __FUNCTION__);
|
||||
// Or, instead of returning an error, should we return the number of bytes read (assuming it is greater than zero)?
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Current bytes read could be zero
|
||||
total_bytes_read += current_bytes_read;
|
||||
|
||||
// If we have at least reached our desired minimum number of characters
|
||||
// then we can return now
|
||||
if (total_bytes_read >= (int) character_count) {
|
||||
return total_bytes_read;
|
||||
}
|
||||
|
||||
// Wait a set amount of time before trying again or the remaining time
|
||||
// until the timeout if we are getting close
|
||||
const uint64_t elapsed_us = hrt_elapsed_time(&start_time_us);
|
||||
int64_t time_until_timeout = timeout_us - elapsed_us;
|
||||
uint64_t time_to_sleep = 5000;
|
||||
|
||||
if ((time_until_timeout >= 0) &&
|
||||
(time_until_timeout < (int64_t) time_to_sleep)) {
|
||||
time_to_sleep = time_until_timeout;
|
||||
}
|
||||
|
||||
px4_usleep(time_to_sleep);
|
||||
}
|
||||
|
||||
return -1;
|
||||
}
|
||||
|
||||
ssize_t SerialImpl::write(const void *buffer, size_t buffer_size)
|
||||
{
|
||||
if (!_open) {
|
||||
PX4_ERR("Cannot write to serial device until it has been opened");
|
||||
return -1;
|
||||
}
|
||||
|
||||
int ret_write = qurt_uart_write(_serial_fd, (const char *) buffer, buffer_size);
|
||||
|
||||
if (ret_write < 0) {
|
||||
PX4_ERR("%s write error %d", _port, ret_write);
|
||||
|
||||
}
|
||||
|
||||
return ret_write;
|
||||
}
|
||||
|
||||
const char *SerialImpl::getPort() const
|
||||
{
|
||||
return _port;
|
||||
}
|
||||
|
||||
uint32_t SerialImpl::getBaudrate() const
|
||||
{
|
||||
return _baudrate;
|
||||
}
|
||||
|
||||
bool SerialImpl::setBaudrate(uint32_t baudrate)
|
||||
{
|
||||
if (! validateBaudrate(baudrate)) {
|
||||
PX4_ERR("Invalid baudrate: %u", baudrate);
|
||||
return false;
|
||||
}
|
||||
|
||||
// check if already configured
|
||||
if (baudrate == _baudrate) {
|
||||
return true;
|
||||
}
|
||||
|
||||
_baudrate = baudrate;
|
||||
|
||||
// process baud rate change now if port is already open
|
||||
if (_open) {
|
||||
return open();
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
ByteSize SerialImpl::getBytesize() const
|
||||
{
|
||||
return _bytesize;
|
||||
}
|
||||
|
||||
bool SerialImpl::setBytesize(ByteSize bytesize)
|
||||
{
|
||||
return bytesize == ByteSize::EightBits;
|
||||
}
|
||||
|
||||
Parity SerialImpl::getParity() const
|
||||
{
|
||||
return _parity;
|
||||
}
|
||||
|
||||
bool SerialImpl::setParity(Parity parity)
|
||||
{
|
||||
return parity == Parity::None;
|
||||
}
|
||||
|
||||
StopBits SerialImpl::getStopbits() const
|
||||
{
|
||||
return _stopbits;
|
||||
}
|
||||
|
||||
bool SerialImpl::setStopbits(StopBits stopbits)
|
||||
{
|
||||
return stopbits == StopBits::One;
|
||||
}
|
||||
|
||||
FlowControl SerialImpl::getFlowcontrol() const
|
||||
{
|
||||
return _flowcontrol;
|
||||
}
|
||||
|
||||
bool SerialImpl::setFlowcontrol(FlowControl flowcontrol)
|
||||
{
|
||||
return flowcontrol == FlowControl::Disabled;
|
||||
}
|
||||
|
||||
} // namespace device
|
|
@ -81,7 +81,7 @@ static void hrt_unlock()
|
|||
px4_sem_post(&_hrt_lock);
|
||||
}
|
||||
|
||||
int px4_clock_settime(clockid_t clk_id, struct timespec *tp)
|
||||
int px4_clock_settime(clockid_t clk_id, const struct timespec *tp)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
|
|
@ -160,6 +160,11 @@ status_t Argus_InitMode(argus_hnd_t *hnd, s2pi_slave_t spi_slave, argus_mode_t m
|
|||
* Also refer to #Argus_ReinitMode, which uses a specified measurement
|
||||
* mode instead of the currently active measurement mode.
|
||||
*
|
||||
* @note If a full re-initialization is not desired, refer to the
|
||||
* #Argus_RestoreDeviceState function that will only re-write the
|
||||
* register map to the device to restore its state after an power
|
||||
* cycle.
|
||||
*
|
||||
* @param hnd The API handle; contains all internal states and data.
|
||||
*
|
||||
* @return Returns the \link #status_t status\endlink (#STATUS_OK on success).
|
||||
|
@ -182,6 +187,11 @@ status_t Argus_Reinit(argus_hnd_t *hnd);
|
|||
* Also refer to #Argus_Reinit, which re-uses the currently active
|
||||
* measurement mode instead of an user specified measurement mode.
|
||||
*
|
||||
* @note If a full re-initialization is not desired, refer to the
|
||||
* #Argus_RestoreDeviceState function that will only re-write the
|
||||
* register map to the device to restore its state after an power
|
||||
* cycle.
|
||||
*
|
||||
* @param hnd The API handle; contains all internal states and data.
|
||||
*
|
||||
* @param mode The specified measurement mode to be initialized.
|
||||
|
@ -274,6 +284,69 @@ argus_hnd_t *Argus_CreateHandle(void);
|
|||
*****************************************************************************/
|
||||
status_t Argus_DestroyHandle(argus_hnd_t *hnd);
|
||||
|
||||
/*!***************************************************************************
|
||||
* @brief Restores the device state with a re-write of all register values.
|
||||
*
|
||||
* @details The function invalidates and restores the device state by executing
|
||||
* a re-write of the full register map.
|
||||
*
|
||||
* The purpose of this function is to recover from known external
|
||||
* events like power cycles, for example due to sleep / wake-up
|
||||
* functionality. This can be implemented by cutting off the external
|
||||
* power supply of the device (e.g. via a MOSFET switch controlled by
|
||||
* a GPIB pin). By calling this function, the expected state of the
|
||||
* API is written to the device without the need to fully re-initialize
|
||||
* the device. Thus, the API can resume where it has stopped as if
|
||||
* there has never been a power cycle.
|
||||
*
|
||||
* The internal state machines like the dynamic configuration adaption
|
||||
* (DCA) algorithm will not be reseted. The API/sensor will immediately
|
||||
* resume at the last state that was optimized for the given
|
||||
* environmental conditions.
|
||||
*
|
||||
* The use case of sleep / wake-up can be implemented as follows:
|
||||
*
|
||||
* 1. In case of ongoing measurements, stop the measurements via
|
||||
* the #Argus_StopMeasurementTimer function (if started by the
|
||||
* #Argus_StartMeasurementTimer function).
|
||||
*
|
||||
* 2. Shut down the device by removing the 5V power supply, e.g.
|
||||
* via a GPIO pin that switches a MOSFET circuit.
|
||||
*
|
||||
* 3. After the desired sleep period, power the device by switching
|
||||
* the 5V power supply on again. Wait until the power-on-reset
|
||||
* (POR) is finished (approx. 1 ms) or just repeat step 4 until
|
||||
* it succeeds.
|
||||
*
|
||||
* 4. Call the #Argus_RestoreDeviceState function to trigger the
|
||||
* restoration of the device state in the API. Note that the
|
||||
* function will return an error code if it fails. One can repeat
|
||||
* the execution of that function a few times until it succeeds.
|
||||
*
|
||||
* 6. Continue with measurements via #Argus_StartMeasurementTimer
|
||||
* of #Argus_TriggerMeasurement functions as desired.
|
||||
*
|
||||
* @note If a complete re-initialization (= soft-reset) is desired, see
|
||||
* the #Argus_Reinit functionality.
|
||||
*
|
||||
* @note Changing a configuration or calibration parameter will always
|
||||
* invalidate the device state as well as the state machine of the
|
||||
* dynamic configuration adaption (DCA) algorithm. In that case, the
|
||||
* device/API needs a few measurements to adopt to the present
|
||||
* environmental conditions before the first valid measurement result
|
||||
* can be obtained. This is almost similar to re-initializing the
|
||||
* device (see #Argus_Reinit) which would also re-read the EEPROM.
|
||||
* On the other hand, the #Argus_RestoreDeviceState does not reset
|
||||
* or re-initialize anything. It just makes sure that the device
|
||||
* register map (which has changed to its reset values after the
|
||||
* power cycle) is what the API expects upon the next measurement.
|
||||
*
|
||||
* @param hnd The device handle object to be invalidated.
|
||||
*
|
||||
* @return Returns the \link #status_t status\endlink (#STATUS_OK on success).
|
||||
*****************************************************************************/
|
||||
status_t Argus_RestoreDeviceState(argus_hnd_t *hnd);
|
||||
|
||||
/*!**************************************************************************
|
||||
* Generic API
|
||||
****************************************************************************/
|
||||
|
@ -726,7 +799,7 @@ status_t Argus_ExecuteXtalkCalibrationSequence(argus_hnd_t *hnd);
|
|||
* After calibration has finished successfully, the obtained data is
|
||||
* applied immediately and can be read from the API using the
|
||||
* #Argus_GetCalibrationPixelRangeOffsets or
|
||||
* #Argus_GetCalibrationGlobalRangeOffset function.
|
||||
* #Argus_GetCalibrationGlobalRangeOffsets function.
|
||||
*
|
||||
* @param hnd The API handle; contains all internal states and data.
|
||||
* @return Returns the \link #status_t status\endlink (#STATUS_OK on success).
|
||||
|
@ -775,7 +848,7 @@ status_t Argus_ExecuteRelativeRangeOffsetCalibrationSequence(argus_hnd_t *hnd);
|
|||
* After calibration has finished successfully, the obtained data is
|
||||
* applied immediately and can be read from the API using the
|
||||
* #Argus_GetCalibrationPixelRangeOffsets or
|
||||
* #Argus_GetCalibrationGlobalRangeOffset function.
|
||||
* #Argus_GetCalibrationGlobalRangeOffsets function.
|
||||
*
|
||||
* @param hnd The API handle; contains all internal states and data.
|
||||
* @param targetRange The absolute range between the reference plane and the
|
||||
|
@ -1043,28 +1116,40 @@ status_t Argus_GetConfigurationUnambiguousRange(argus_hnd_t *hnd,
|
|||
****************************************************************************/
|
||||
|
||||
/*!***************************************************************************
|
||||
* @brief Sets the global range offset value to a specified device.
|
||||
* @brief Sets the global range offset values to a specified device.
|
||||
*
|
||||
* @details The global range offset is subtracted from the raw range values.
|
||||
* @details The global range offsets are subtracted from the raw range values.
|
||||
* There are two distinct values that are applied in low or high
|
||||
* power stage setting respectively.
|
||||
*
|
||||
* @param hnd The API handle; contains all internal states and data.
|
||||
* @param value The new global range offset in meter and Q0.15 format.
|
||||
* @param offset_low The new global range offset for the low power stage in
|
||||
* meter and Q0.15 format.
|
||||
* @param offset_high The new global range offset for the high power stage in
|
||||
* meter and Q0.15 format.
|
||||
* @return Returns the \link #status_t status\endlink (#STATUS_OK on success).
|
||||
*****************************************************************************/
|
||||
status_t Argus_SetCalibrationGlobalRangeOffset(argus_hnd_t *hnd,
|
||||
q0_15_t value);
|
||||
status_t Argus_SetCalibrationGlobalRangeOffsets(argus_hnd_t *hnd,
|
||||
q0_15_t offset_low,
|
||||
q0_15_t offset_high);
|
||||
|
||||
/*!***************************************************************************
|
||||
* @brief Gets the global range offset value from a specified device.
|
||||
* @brief Gets the global range offset values from a specified device.
|
||||
*
|
||||
* @details The global range offset is subtracted from the raw range values.
|
||||
* @details The global range offsets are subtracted from the raw range values.
|
||||
* There are two distinct values that are applied in low or high
|
||||
* power stage setting respectively.
|
||||
*
|
||||
* @param hnd The API handle; contains all internal states and data.
|
||||
* @param value The current global range offset in meter and Q0.15 format.
|
||||
* @param offset_low The current range offset for the low power stage in
|
||||
* meter and Q0.15 format.
|
||||
* @param offset_high The current global range offset for the high power stage
|
||||
* in meter and Q0.15 format.
|
||||
* @return Returns the \link #status_t status\endlink (#STATUS_OK on success).
|
||||
*****************************************************************************/
|
||||
status_t Argus_GetCalibrationGlobalRangeOffset(argus_hnd_t *hnd,
|
||||
q0_15_t *value);
|
||||
status_t Argus_GetCalibrationGlobalRangeOffsets(argus_hnd_t *hnd,
|
||||
q0_15_t *offset_low,
|
||||
q0_15_t *offset_high);
|
||||
|
||||
/*!***************************************************************************
|
||||
* @brief Sets the relative pixel offset table to a specified device.
|
||||
|
|
|
@ -210,9 +210,13 @@ typedef enum argus_dca_gain_t {
|
|||
* - [9]: #ARGUS_STATE_LASER_ERROR
|
||||
* - [10]: #ARGUS_STATE_HAS_DATA
|
||||
* - [11]: #ARGUS_STATE_HAS_AUX_DATA
|
||||
* - [12]: #ARGUS_STATE_DCA_MAX
|
||||
* - [12]: #ARGUS_STATE_SATURATED_PIXELS
|
||||
* - [13]: DCA Power Stage
|
||||
* - [14-15]: DCA Gain Stages
|
||||
* - [16]: #ARGUS_STATE_DCA_MIN
|
||||
* - [17]: #ARGUS_STATE_DCA_MAX
|
||||
* - [18]: #ARGUS_STATE_DCA_RESET
|
||||
* - [18-31]: not used
|
||||
* .
|
||||
*****************************************************************************/
|
||||
typedef enum argus_state_t {
|
||||
|
@ -229,36 +233,35 @@ typedef enum argus_state_t {
|
|||
* - 1: Enabled: measurement with detuned frequency. */
|
||||
ARGUS_STATE_DUAL_FREQ_MODE = 1U << 1U,
|
||||
|
||||
/*! 0x0004: Measurement Frequency for Dual Frequency Mode
|
||||
/*! 0x0004: Measurement Frequency for Dual Frequency Mode \n
|
||||
* (only if #ARGUS_STATE_DUAL_FREQ_MODE flag is set).
|
||||
* - 0: A-Frame w/ detuned frequency,
|
||||
* - 1: B-Frame w/ detuned frequency */
|
||||
ARGUS_STATE_MEASUREMENT_FREQ = 1U << 2U,
|
||||
|
||||
/*! 0x0008: Debug Mode. If set, the range value of erroneous pixels
|
||||
/*! 0x0008: Debug Mode. \n
|
||||
* If set, the range value of erroneous pixels
|
||||
* are not cleared or reset.
|
||||
* - 0: Disabled (default).
|
||||
* - 1: Enabled. */
|
||||
ARGUS_STATE_DEBUG_MODE = 1U << 3U,
|
||||
|
||||
/*! 0x0010: Weak Signal Flag.
|
||||
/*! 0x0010: Weak Signal Flag. \n
|
||||
* Set whenever the Pixel Binning Algorithm is detecting a
|
||||
* weak signal, i.e. if the amplitude dies not reach its
|
||||
* (absolute) threshold. If the Golden Pixel is enabled,
|
||||
* this also indicates that the Pixel Binning Algorithm
|
||||
* falls back to the Golden Pixel.
|
||||
* (absolute) threshold.
|
||||
* - 0: Normal Signal.
|
||||
* - 1: Weak Signal or Golden Pixel Mode. */
|
||||
* - 1: Weak Signal. */
|
||||
ARGUS_STATE_WEAK_SIGNAL = 1U << 4U,
|
||||
|
||||
/*! 0x0020: Background Light Warning Flag.
|
||||
/*! 0x0020: Background Light Warning Flag. \n
|
||||
* Set whenever the background light is very high and the
|
||||
* measurement data might be unreliable.
|
||||
* - 0: No Warning: Background Light is within valid range.
|
||||
* - 1: Warning: Background Light is very high. */
|
||||
ARGUS_STATE_BGL_WARNING = 1U << 5U,
|
||||
|
||||
/*! 0x0040: Background Light Error Flag.
|
||||
/*! 0x0040: Background Light Error Flag. \n
|
||||
* Set whenever the background light is too high and the
|
||||
* measurement data is unreliable or invalid.
|
||||
* - 0: No Error: Background Light is within valid range.
|
||||
|
@ -270,7 +273,7 @@ typedef enum argus_state_t {
|
|||
* - 1: PLL locked at start of integration. */
|
||||
ARGUS_STATE_PLL_LOCKED = 1U << 7U,
|
||||
|
||||
/*! 0x0100: Laser Failure Warning Flag.
|
||||
/*! 0x0100: Laser Failure Warning Flag. \n
|
||||
* Set whenever the an invalid system condition is detected.
|
||||
* (i.e. DCA at max state but no amplitude on any (incl. reference)
|
||||
* pixel, not amplitude but any saturated pixel).
|
||||
|
@ -279,7 +282,7 @@ typedef enum argus_state_t {
|
|||
* condition stays, a laser malfunction error is raised. */
|
||||
ARGUS_STATE_LASER_WARNING = 1U << 8U,
|
||||
|
||||
/*! 0x0200: Laser Failure Error Flag.
|
||||
/*! 0x0200: Laser Failure Error Flag. \n
|
||||
* Set whenever a laser malfunction error is raised and the
|
||||
* system is put into a safe state.
|
||||
* - 0: No Error: Laser is operating properly.
|
||||
|
@ -297,13 +300,12 @@ typedef enum argus_state_t {
|
|||
* - 1: Auxiliary data is available and correctly evaluated. */
|
||||
ARGUS_STATE_HAS_AUX_DATA = 1U << 11U,
|
||||
|
||||
/*! 0x1000: DCA Maximum State Flag.
|
||||
* Set whenever the DCA has extended all its parameters to their
|
||||
* maximum values and can not increase the integration energy any
|
||||
* further.
|
||||
* - 0: DCA has not yet reached its maximum state.
|
||||
* - 1: DCA has reached its maximum state and can not increase any further. */
|
||||
ARGUS_STATE_DCA_MAX = 1U << 12U,
|
||||
/*! 0x0100: Pixel Saturation Flag. \n
|
||||
* Set whenever any pixel is saturated, i.e. its pixel state is
|
||||
* #PIXEL_SAT
|
||||
* - 0: No saturated pixels.
|
||||
* - 1: Any saturated pixels. */
|
||||
ARGUS_STATE_SATURATED_PIXELS = 1U << 12U,
|
||||
|
||||
/*! 0x2000: DCA is in high Optical Output Power stage. */
|
||||
ARGUS_STATE_DCA_POWER_HIGH = DCA_POWER_HIGH << ARGUS_STATE_DCA_POWER_SHIFT,
|
||||
|
@ -320,6 +322,31 @@ typedef enum argus_state_t {
|
|||
/*! 0xC000: DCA is in high Pixel Input Gain stage. */
|
||||
ARGUS_STATE_DCA_GAIN_HIGH = DCA_GAIN_HIGH << ARGUS_STATE_DCA_GAIN_SHIFT,
|
||||
|
||||
/*! 0x10000: DCA Minimum State Flag. \n
|
||||
* Set whenever the DCA has reduced all its parameters to their
|
||||
* minimum values and it can not decrease the integration energy
|
||||
* any further.
|
||||
* - 0: DCA has not yet reached its minimum state.
|
||||
* - 1: DCA has reached its minimum state and can not decrease
|
||||
* its parameters any further. */
|
||||
ARGUS_STATE_DCA_MIN = 1U << 16U,
|
||||
|
||||
/*! 0x20000: DCA Maximum State Flag. \n
|
||||
* Set whenever the DCA has extended all its parameters to their
|
||||
* maximum values and it can not increase the integration energy
|
||||
* any further.
|
||||
* - 0: DCA has not yet reached its maximum state.
|
||||
* - 1: DCA has reached its maximum state and can not increase
|
||||
* its parameters any further. */
|
||||
ARGUS_STATE_DCA_MAX = 1U << 17U,
|
||||
|
||||
/*! 0x20000: DCA Reset State Flag. \n
|
||||
* Set whenever the DCA is resetting all its parameters to their
|
||||
* minimum values because it has detected too many saturated pixels.
|
||||
* - 0: DCA is operating in normal mode.
|
||||
* - 1: DCA is performing a reset. */
|
||||
ARGUS_STATE_DCA_RESET = 1U << 18U,
|
||||
|
||||
} argus_state_t;
|
||||
|
||||
/*!***************************************************************************
|
||||
|
|
|
@ -58,6 +58,7 @@ extern "C" {
|
|||
*****************************************************************************/
|
||||
|
||||
#include "utility/int_math.h"
|
||||
#include <stdbool.h>
|
||||
#include <assert.h>
|
||||
|
||||
|
||||
|
@ -138,6 +139,13 @@ extern "C" {
|
|||
#define PIXEL_CH2N(c) (((((c) << 1U) ^ 0x1CU) & 0x1CU) | (((c) >> 3U) & 0x02U) | ((c) & 0x01U))
|
||||
|
||||
|
||||
/*!*****************************************************************************
|
||||
* @brief Macro to create a pixel mask given by the pixels n-index.
|
||||
* @param n n-index of the pixel.
|
||||
* @return The pixel mask with only n-index pixel set.
|
||||
******************************************************************************/
|
||||
#define PIXELN_MASK(n) (0x01U << (n))
|
||||
|
||||
/*!*****************************************************************************
|
||||
* @brief Macro to determine if a pixel given by the n-index is enabled in a pixel mask.
|
||||
* @param msk 32-bit pixel mask
|
||||
|
@ -151,16 +159,23 @@ extern "C" {
|
|||
* @param msk 32-bit pixel mask
|
||||
* @param n n-index of the pixel to enable.
|
||||
******************************************************************************/
|
||||
#define PIXELN_ENABLE(msk, n) ((msk) |= (0x01U << (n)))
|
||||
#define PIXELN_ENABLE(msk, n) ((msk) |= (PIXELN_MASK(n)))
|
||||
|
||||
/*!*****************************************************************************
|
||||
* @brief Macro disable a pixel given by the n-index in a pixel mask.
|
||||
* @param msk 32-bit pixel mask
|
||||
* @param n n-index of the pixel to disable.
|
||||
******************************************************************************/
|
||||
#define PIXELN_DISABLE(msk, n) ((msk) &= (~(0x01U << (n))))
|
||||
#define PIXELN_DISABLE(msk, n) ((msk) &= (~PIXELN_MASK(n)))
|
||||
|
||||
|
||||
/*!*****************************************************************************
|
||||
* @brief Macro to create a pixel mask given by the pixels ADC channel number.
|
||||
* @param c The ADC channel number of the pixel.
|
||||
* @return The 32-bit pixel mask with only pixel ADC channel set.
|
||||
******************************************************************************/
|
||||
#define PIXELCH_MASK(c) (0x01U << (PIXEL_CH2N(c)))
|
||||
|
||||
/*!*****************************************************************************
|
||||
* @brief Macro to determine if an ADC pixel channel is enabled from a pixel mask.
|
||||
* @param msk The 32-bit pixel mask
|
||||
|
@ -184,6 +199,14 @@ extern "C" {
|
|||
#define PIXELCH_DISABLE(msk, c) (PIXELN_DISABLE(msk, PIXEL_CH2N(c)))
|
||||
|
||||
|
||||
/*!*****************************************************************************
|
||||
* @brief Macro to create a pixel mask given by the pixel x-y-indices.
|
||||
* @param x x-index of the pixel.
|
||||
* @param y y-index of the pixel.
|
||||
* @return The 32-bit pixel mask with only pixel ADC channel set.
|
||||
******************************************************************************/
|
||||
#define PIXELXY_MASK(x, y) (0x01U << (PIXEL_XY2N(x, y)))
|
||||
|
||||
/*!*****************************************************************************
|
||||
* @brief Macro to determine if a pixel given by the x-y-indices is enabled in a pixel mask.
|
||||
* @param msk 32-bit pixel mask
|
||||
|
@ -337,10 +360,10 @@ static inline uint32_t ShiftSelectedPixels(const uint32_t pixel_mask,
|
|||
|
||||
uint32_t shifted_mask = 0;
|
||||
|
||||
for (uint8_t x = 0; x < ARGUS_PIXELS_X; ++x) {
|
||||
for (uint8_t y = 0; y < ARGUS_PIXELS_Y; ++y) {
|
||||
int8_t x_src = x - dx;
|
||||
int8_t y_src = y - dy;
|
||||
for (int8_t x = 0; x < ARGUS_PIXELS_X; ++x) {
|
||||
for (int8_t y = 0; y < ARGUS_PIXELS_Y; ++y) {
|
||||
int8_t x_src = (int8_t)(x - dx);
|
||||
int8_t y_src = (int8_t)(y - dy);
|
||||
|
||||
if (dy & 0x1) {
|
||||
/* Compensate for hexagonal pixel shape. */
|
||||
|
@ -409,8 +432,8 @@ static inline uint32_t FillPixelMask(uint32_t pixel_mask,
|
|||
int8_t min_y = -1;
|
||||
|
||||
/* Find nearest not selected pixel. */
|
||||
for (uint8_t x = 0; x < ARGUS_PIXELS_X; ++x) {
|
||||
for (uint8_t y = 0; y < ARGUS_PIXELS_Y; ++y) {
|
||||
for (int8_t x = 0; x < ARGUS_PIXELS_X; ++x) {
|
||||
for (int8_t y = 0; y < ARGUS_PIXELS_Y; ++y) {
|
||||
if (!PIXELXY_ISENABLED(pixel_mask, x, y)) {
|
||||
int32_t distx = (x - center_x) << 1;
|
||||
|
||||
|
@ -423,8 +446,8 @@ static inline uint32_t FillPixelMask(uint32_t pixel_mask,
|
|||
|
||||
if (dist < min_dist) {
|
||||
min_dist = dist;
|
||||
min_x = x;
|
||||
min_y = y;
|
||||
min_x = (int8_t)x;
|
||||
min_y = (int8_t)y;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -438,6 +461,64 @@ static inline uint32_t FillPixelMask(uint32_t pixel_mask,
|
|||
|
||||
return pixel_mask;
|
||||
}
|
||||
|
||||
/*!*****************************************************************************
|
||||
* @brief Fills a pixel mask with the direct neighboring pixels around a pixel.
|
||||
*
|
||||
* @details The pixel mask is iteratively filled with the direct neighbors of the
|
||||
* specified center pixel.
|
||||
*
|
||||
* Note that the function is able to handle corner and edge pixels and
|
||||
* also to handle odd/even lines (which have different layouts)
|
||||
*
|
||||
* @param x The selected pixel x-index.
|
||||
* @param y The selected pixel y-index.
|
||||
* @return The filled pixel mask with all direct neighbors of the selected pixel.
|
||||
******************************************************************************/
|
||||
static inline uint32_t GetAdjacentPixelsMask(const uint_fast8_t x,
|
||||
const uint_fast8_t y)
|
||||
{
|
||||
assert(x < ARGUS_PIXELS_X);
|
||||
assert(y < ARGUS_PIXELS_Y);
|
||||
|
||||
uint32_t mask = 0u;
|
||||
|
||||
bool isXEdgeLow = (x == 0);
|
||||
bool isXEdgeHigh = (x == (ARGUS_PIXELS_X - 1));
|
||||
bool isYEdgeLow = (y == 0);
|
||||
bool isYEdgeHigh = (y == (ARGUS_PIXELS_Y - 1));
|
||||
|
||||
if (y % 2 == 0) {
|
||||
if (!isYEdgeLow) { PIXELXY_ENABLE(mask, x, y - 1); }
|
||||
|
||||
if ((!isXEdgeHigh) && (!isYEdgeLow)) { PIXELXY_ENABLE(mask, x + 1, y - 1); }
|
||||
|
||||
if (!isXEdgeHigh) { PIXELXY_ENABLE(mask, x + 1, y); }
|
||||
|
||||
if ((!isXEdgeHigh) && (!isYEdgeHigh)) { PIXELXY_ENABLE(mask, x + 1, y + 1); }
|
||||
|
||||
if (!isYEdgeHigh) { PIXELXY_ENABLE(mask, x, y + 1); }
|
||||
|
||||
if (!isXEdgeLow) { PIXELXY_ENABLE(mask, x - 1, y); }
|
||||
|
||||
} else {
|
||||
if ((!isXEdgeLow) && (!isYEdgeLow)) { PIXELXY_ENABLE(mask, x - 1, y - 1); }
|
||||
|
||||
if (!isYEdgeLow) { PIXELXY_ENABLE(mask, x, y - 1); }
|
||||
|
||||
if (!isXEdgeHigh) { PIXELXY_ENABLE(mask, x + 1, y); }
|
||||
|
||||
if (!isYEdgeHigh) { PIXELXY_ENABLE(mask, x, y + 1); }
|
||||
|
||||
if ((!isXEdgeLow) && (!isYEdgeHigh)) { PIXELXY_ENABLE(mask, x - 1, y + 1); }
|
||||
|
||||
if (!isXEdgeLow) { PIXELXY_ENABLE(mask, x - 1, y); }
|
||||
}
|
||||
|
||||
return mask;
|
||||
}
|
||||
|
||||
|
||||
/*! @} */
|
||||
#ifdef __cplusplus
|
||||
} // extern "C"
|
||||
|
|
|
@ -1,170 +0,0 @@
|
|||
/*************************************************************************//**
|
||||
* @file
|
||||
* @brief This file is part of the AFBR-S50 API.
|
||||
* @details Defines macros to work with pixel and ADC channel masks.
|
||||
*
|
||||
* @copyright
|
||||
*
|
||||
* Copyright (c) 2021, Broadcom Inc
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||
* list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* 3. Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived from
|
||||
* this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*****************************************************************************/
|
||||
|
||||
#ifndef ARGUS_MSK_H
|
||||
#define ARGUS_MSK_H
|
||||
|
||||
/*!***************************************************************************
|
||||
* @defgroup argusmap ADC Channel Mapping
|
||||
* @ingroup argusres
|
||||
*
|
||||
* @brief Pixel ADC Channel (n) to x-y-Index Mapping
|
||||
*
|
||||
* @details The ADC Channels of each pixel or auxiliary channel on the device
|
||||
* is numbered in a way that is convenient on the chip. The macros
|
||||
* in this module are defined in order to obtain the x-y-indices of
|
||||
* each channel and vice versa.
|
||||
*
|
||||
* @addtogroup argusmap
|
||||
* @{
|
||||
*****************************************************************************/
|
||||
|
||||
#include "api/argus_def.h"
|
||||
#include "utility/int_math.h"
|
||||
|
||||
/*!*****************************************************************************
|
||||
* @brief Macro to determine the channel number of an specified Pixel.
|
||||
* @param x The x index of the pixel.
|
||||
* @param y The y index of the pixel.
|
||||
* @return The channel number n of the pixel.
|
||||
******************************************************************************/
|
||||
#define PIXEL_XY2N(x, y) ((((x) ^ 7) << 1) | ((y) & 2) << 3 | ((y) & 1))
|
||||
|
||||
/*!*****************************************************************************
|
||||
* @brief Macro to determine the x index of an specified Pixel channel.
|
||||
* @param n The channel number of the pixel.
|
||||
* @return The x index number of the pixel.
|
||||
******************************************************************************/
|
||||
#define PIXEL_N2X(n) ((((n) >> 1U) & 7) ^ 7)
|
||||
|
||||
/*!*****************************************************************************
|
||||
* @brief Macro to determine the y index of an specified Pixel channel.
|
||||
* @param n The channel number of the pixel.
|
||||
* @return The y index number of the pixel.
|
||||
******************************************************************************/
|
||||
#define PIXEL_N2Y(n) (((n) & 1U) | (((n) >> 3) & 2U))
|
||||
|
||||
/*!*****************************************************************************
|
||||
* @brief Macro to determine if a ADC Pixel channel was enabled from a pixel mask.
|
||||
* @param msk The 32-bit pixel mask
|
||||
* @param ch The channel number of the pixel.
|
||||
* @return True if the pixel channel n was enabled, false elsewise.
|
||||
******************************************************************************/
|
||||
#define PIXELN_ISENABLED(msk, ch) (((msk) >> (ch)) & 0x01U)
|
||||
|
||||
/*!*****************************************************************************
|
||||
* @brief Macro enables an ADC Pixel channel in a pixel mask.
|
||||
* @param msk The 32-bit pixel mask
|
||||
* @param ch The channel number of the pixel.
|
||||
******************************************************************************/
|
||||
#define PIXELN_ENABLE(msk, ch) ((msk) |= (0x01U << (ch)))
|
||||
|
||||
/*!*****************************************************************************
|
||||
* @brief Macro disables an ADC Pixel channel in a pixel mask.
|
||||
* @param msk The 32-bit pixel mask
|
||||
* @param ch The channel number of the pixel.
|
||||
******************************************************************************/
|
||||
#define PIXELN_DISABLE(msk, ch) ((msk) &= (~(0x01U << (ch))))
|
||||
|
||||
/*!*****************************************************************************
|
||||
* @brief Macro to determine if an ADC Pixel channel was enabled from a pixel mask.
|
||||
* @param msk 32-bit pixel mask
|
||||
* @param x x index of the pixel.
|
||||
* @param y y index of the pixel.
|
||||
* @return True if the pixel (x,y) was enabled, false elsewise.
|
||||
******************************************************************************/
|
||||
#define PIXELXY_ISENABLED(msk, x, y) (PIXELN_ISENABLED(msk, PIXEL_XY2N(x, y)))
|
||||
|
||||
/*!*****************************************************************************
|
||||
* @brief Macro enables an ADC Pixel channel in a pixel mask.
|
||||
* @param msk 32-bit pixel mask
|
||||
* @param x x index of the pixel.
|
||||
* @param y y index of the pixel.
|
||||
******************************************************************************/
|
||||
#define PIXELXY_ENABLE(msk, x, y) (PIXELN_ENABLE(msk, PIXEL_XY2N(x, y)))
|
||||
|
||||
/*!*****************************************************************************
|
||||
* @brief Macro disables an ADC Pixel channel in a pixel mask.
|
||||
* @param msk 32-bit pixel mask
|
||||
* @param x x index of the pixel.
|
||||
* @param y y index of the pixel.
|
||||
******************************************************************************/
|
||||
#define PIXELXY_DISABLE(msk, x, y) (PIXELN_DISABLE(msk, PIXEL_XY2N(x, y)))
|
||||
|
||||
/*!*****************************************************************************
|
||||
* @brief Macro to determine if a ADC channel was enabled from a channel mask.
|
||||
* @param msk 32-bit channel mask
|
||||
* @param ch channel number of the ADC channel.
|
||||
* @return True if the ADC channel n was enabled, false elsewise.
|
||||
******************************************************************************/
|
||||
#define CHANNELN_ISENABLED(msk, ch) (((msk) >> ((ch) - 32U)) & 0x01U)
|
||||
|
||||
/*!*****************************************************************************
|
||||
* @brief Macro to determine if a ADC channel was enabled from a channel mask.
|
||||
* @param msk 32-bit channel mask
|
||||
* @param ch channel number of the ADC channel.
|
||||
* @return True if the ADC channel n was enabled, false elsewise.
|
||||
******************************************************************************/
|
||||
#define CHANNELN_ENABLE(msk, ch) ((msk) |= (0x01U << ((ch) - 32U)))
|
||||
|
||||
/*!*****************************************************************************
|
||||
* @brief Macro to determine if a ADC channel was enabled from a channel mask.
|
||||
* @param msk 32-bit channel mask
|
||||
* @param ch channel number of the ADC channel.
|
||||
* @return True if the ADC channel n was enabled, false elsewise.
|
||||
******************************************************************************/
|
||||
#define CHANNELN_DISABLE(msk, ch) ((msk) &= (~(0x01U << ((ch) - 32U))))
|
||||
|
||||
|
||||
/*!*****************************************************************************
|
||||
* @brief Macro to determine the number of enabled pixel channels via a popcount
|
||||
* algorithm.
|
||||
* @param pxmsk 32-bit pixel mask
|
||||
* @return The count of enabled pixel channels.
|
||||
******************************************************************************/
|
||||
#define PIXEL_COUNT(pxmsk) popcount(pxmsk)
|
||||
|
||||
/*!*****************************************************************************
|
||||
* @brief Macro to determine the number of enabled channels via a popcount
|
||||
* algorithm.
|
||||
* @param pxmsk 32-bit pixel mask
|
||||
* @param chmsk 32-bit channel mask
|
||||
* @return The count of enabled ADC channels.
|
||||
******************************************************************************/
|
||||
#define CHANNEL_COUNT(pxmsk, chmsk) (popcount(pxmsk) + popcount(chmsk))
|
||||
|
||||
/*! @} */
|
||||
#endif /* ARGUS_MSK_H */
|
|
@ -36,6 +36,9 @@
|
|||
|
||||
#ifndef ARGUS_OFFSET_H
|
||||
#define ARGUS_OFFSET_H
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/*!***************************************************************************
|
||||
* @addtogroup argus_cal
|
||||
|
@ -48,12 +51,26 @@
|
|||
* @brief Pixel Range Offset Table.
|
||||
* @details Contains pixel range offset values for all 32 active pixels.
|
||||
*****************************************************************************/
|
||||
typedef struct argus_cal_offset_table_t {
|
||||
/*! The offset values per pixel in meter and Q0.15 format. */
|
||||
q0_15_t Table[ARGUS_PIXELS_X][ARGUS_PIXELS_Y];
|
||||
typedef union argus_cal_offset_table_t {
|
||||
struct {
|
||||
/*! The offset values table for Low Power Stage of all 32 pixels.
|
||||
* Unit: meter; Format: Q0.15 */
|
||||
q0_15_t LowPower[ARGUS_PIXELS_X][ARGUS_PIXELS_Y];
|
||||
|
||||
/*! The offset values table for High Power Stage of all 32 pixels.
|
||||
* Unit: meter; Format: Q0.15 */
|
||||
q0_15_t HighPower[ARGUS_PIXELS_X][ARGUS_PIXELS_Y];
|
||||
};
|
||||
|
||||
/*! The offset values table for Low/High Power Stages of all 32 pixels.
|
||||
* Unit: meter; Format: Q0.15 */
|
||||
q0_15_t Table[ARGUS_DCA_POWER_STAGE_COUNT][ARGUS_PIXELS_X][ARGUS_PIXELS_Y];
|
||||
|
||||
} argus_cal_offset_table_t;
|
||||
|
||||
|
||||
/*! @} */
|
||||
#ifdef __cplusplus
|
||||
} // extern "C"
|
||||
#endif
|
||||
#endif /* ARGUS_OFFSET_T */
|
||||
|
|
|
@ -55,11 +55,11 @@ extern "C" {
|
|||
* information from the filtered pixels by averaging them in a
|
||||
* specified way.
|
||||
*
|
||||
* The Pixel Binning Algorithm is a three-stage filter with a
|
||||
* fallback value:
|
||||
* Basically, the Pixel Binning Algorithm is a multi-stage filter:
|
||||
*
|
||||
* -# A fixed pre-filter mask is applied to statically disable
|
||||
* specified pixels.
|
||||
*
|
||||
* -# A relative and absolute amplitude filter is applied in the
|
||||
* second stage. The relative filter is determined by a ratio
|
||||
* of the maximum amplitude off all available (i.e. not filtered
|
||||
|
@ -75,12 +75,28 @@ extern "C" {
|
|||
* selected and considered for the final 1D distance. The
|
||||
* absolute threshold is used to dismiss pixels that are below
|
||||
* the noise level. The latter would be considered for the 1D
|
||||
* result if the maximum amplitude is already very low.
|
||||
* result if the maximum amplitude is already very low.\n
|
||||
* Those threshold are implemented using a hysteresis behavior.
|
||||
* For its configuration, see the following parameters:
|
||||
* - #argus_cfg_pba_t::RelativeAmplitudeInclusion
|
||||
* - #argus_cfg_pba_t::RelativeAmplitudeExclusion
|
||||
* - #argus_cfg_pba_t::AbsoluteAmplitudeInclusion
|
||||
* - #argus_cfg_pba_t::AbsoluteAmplitudeExclusion
|
||||
* .
|
||||
*
|
||||
* -# An absolute minimum distance filter is applied in addition
|
||||
* to the amplitude filter. This removes all pixel that have
|
||||
* a lower distance than the specified threshold. This is used
|
||||
* to remove invalid pixels that can be detected by a physically
|
||||
* not correct negative distance.
|
||||
* not correct negative distance.\n
|
||||
* For its configuration, see the following parameters:
|
||||
* - #PBA_ENABLE_MIN_DIST_SCOPE
|
||||
* - #argus_cfg_pba_t::AbsoluteDistanceScopeInclusion
|
||||
* - #argus_cfg_pba_t::AbsoluteDistanceScopeExclusion
|
||||
* - #argus_cfg_pba_t::RelativeDistanceScopeInclusion
|
||||
* - #argus_cfg_pba_t::RelativeDistanceScopeExclusion
|
||||
* .
|
||||
*
|
||||
* -# A distance filter is used to distinguish pixels that target
|
||||
* the actual object from pixels that see the brighter background,
|
||||
* e.g. white walls. Thus, the pixel with the minimum distance
|
||||
|
@ -90,11 +106,31 @@ extern "C" {
|
|||
* determined by an relative (to the current minimum distance)
|
||||
* and an absolute value. The larger scope value is the
|
||||
* relevant one, i.e. the relative distance scope can be used
|
||||
* to heed the increasing noise at larger distances.
|
||||
* to heed the increasing noise at larger distances.\n
|
||||
* For its configuration, see the following parameters:
|
||||
* - #argus_cfg_pba_t::AbsoluteMinimumDistanceThreshold
|
||||
* .
|
||||
*
|
||||
* -# If all of the above filters fail to determine a single valid
|
||||
* pixel, the Golden Pixel is used as a fallback value. The
|
||||
* Golden Pixel is the pixel that sits right at the focus point
|
||||
* of the optics at large distances.
|
||||
* of the optics at large distances. Thus, it is expected to
|
||||
* have the best signal at large distances.\n
|
||||
* For its configuration, see the following parameters:
|
||||
* - #PBA_ENABLE_GOLDPX_FALLBACK_MODE
|
||||
* .
|
||||
*
|
||||
* -# In order to avoid unwanted effects from "out-of-focus" pixels
|
||||
* in application that require a smaller focus, the Golden Pixel
|
||||
* Priority Mode prioritizes a valid signal on the central
|
||||
* Golden Pixel over other pixels. That is, while the Golden
|
||||
* Pixel has a reasonable signal strength, it is the only pixel
|
||||
* considered for the 1D result.\n
|
||||
* For its configuration, see the following parameters:
|
||||
* - #PBA_ENABLE_GOLDPX_FALLBACK_MODE
|
||||
* - #argus_cfg_pba_t::GoldenPixelPriorityAmplitudeInclusion
|
||||
* - #argus_cfg_pba_t::GoldenPixelPriorityAmplitudeExclusion
|
||||
* .
|
||||
* .
|
||||
*
|
||||
* After filtering is done, there may be more than a single pixel
|
||||
|
@ -113,14 +149,17 @@ extern "C" {
|
|||
* @brief Enable flags for the pixel binning algorithm.
|
||||
*
|
||||
* @details Determines the pixel binning algorithm feature enable status.
|
||||
*
|
||||
* - [0]: #PBA_ENABLE: Enables the pixel binning feature.
|
||||
* - [1]: reserved
|
||||
* - [2]: reserved
|
||||
* - [3]: reserved
|
||||
* - [4]: reserved
|
||||
* - [5]: #PBA_ENABLE_GOLDPX: Enables the Golden Pixel feature.
|
||||
* - [6]: #PBA_ENABLE_MIN_DIST_SCOPE: Enables the minimum distance scope
|
||||
* feature.
|
||||
* - [4]: #PBA_ENABLE_GOLDPX_PRIORITY_MODE: Enables the Golden Pixel
|
||||
* priority mode feature.
|
||||
* - [5]: #PBA_ENABLE_GOLDPX_FALLBACK_MODE: Enables the Golden Pixel
|
||||
* fallback mode feature.
|
||||
* - [6]: #PBA_ENABLE_MIN_DIST_SCOPE: Enables the minimum distance
|
||||
* scope feature.
|
||||
* - [7]: reserved
|
||||
* .
|
||||
*****************************************************************************/
|
||||
|
@ -128,8 +167,17 @@ typedef enum argus_pba_flags_t {
|
|||
/*! Enables the pixel binning feature. */
|
||||
PBA_ENABLE = 1U << 0U,
|
||||
|
||||
/*! Enables the Golden Pixel. */
|
||||
PBA_ENABLE_GOLDPX = 1U << 5U,
|
||||
/*! Enables the Golden Pixel Priority Mode.
|
||||
* If enabled, the Golden Pixel is prioritized over other Pixels as long
|
||||
* as it has a good signal (determined by # */
|
||||
PBA_ENABLE_GOLDPX_PRIORITY_MODE = 1U << 4U,
|
||||
|
||||
/*! Enables the Golden Pixel Fallback Mode.
|
||||
* If enabled, the Golden Pixel is used as a last fallback pixel to obtain
|
||||
* a valid signal from. This is recommended for all non-multi pixel
|
||||
* devices whose TX field-of-view is aligned to target the Golden Pixel in
|
||||
* factory calibration. */
|
||||
PBA_ENABLE_GOLDPX_FALLBACK_MODE = 1U << 5U,
|
||||
|
||||
/*! Enables the minimum distance scope filter. */
|
||||
PBA_ENABLE_MIN_DIST_SCOPE = 1U << 6U,
|
||||
|
@ -168,65 +216,297 @@ typedef struct {
|
|||
* about the individual evaluation modes. */
|
||||
argus_pba_averaging_mode_t AveragingMode;
|
||||
|
||||
/*! The Relative amplitude threshold value (in %) of the max. amplitude.
|
||||
/*! The relative amplitude inclusion threshold (in %) of the max. amplitude.
|
||||
*
|
||||
* Pixels with amplitude below this threshold value are dismissed.
|
||||
* Pixels, whose amplitudes raise above this inclusion threshold, are
|
||||
* added to the pixel binning. The amplitude must fall below the
|
||||
* exclusion (#RelativeAmplitudeExclusion) threshold to be removed from
|
||||
* the pixel binning again.
|
||||
*
|
||||
* All available values from the 8-bit representation are valid.
|
||||
* The actual percentage value is determined by 100%/256*x.
|
||||
*
|
||||
* Use 0 to disable the relative amplitude threshold. */
|
||||
uq0_8_t RelAmplThreshold;
|
||||
|
||||
/*! The relative minimum distance scope value in %.
|
||||
* Note: in addition to the relative criteria, there is also the absolute
|
||||
* criteria (#AbsoluteAmplitudeInclusion, #AbsoluteAmplitudeExclusion).
|
||||
* The pixels are added to the pixel binning if their respective amplitude
|
||||
* is larger than the absolute AND relative inclusion values. On the other
|
||||
* hand, they are removed if their amplitude falls below the absolute OR
|
||||
* relative exclusion threshold.
|
||||
*
|
||||
* Pixels that have a range value within [x0, x0 + dx] are considered
|
||||
* for the pixel binning, where x0 is the minimum distance of all
|
||||
* amplitude picked pixels and dx is the minimum distance scope value.
|
||||
* The minimum distance scope value will be the maximum of relative
|
||||
* and absolute value.
|
||||
* Must be greater than or equal to the #RelativeAmplitudeExclusion.
|
||||
*
|
||||
* Use #RelativeAmplitudeExclusion == #RelativeAmplitudeInclusion to
|
||||
* disable the hysteresis behavior and use it as a threshold only.
|
||||
*
|
||||
* Use 0 (for both, #RelativeAmplitudeExclusion and
|
||||
* #RelativeAmplitudeInclusion) to disable the relative amplitude
|
||||
* hysteresis. */
|
||||
uq0_8_t RelativeAmplitudeInclusion;
|
||||
|
||||
/*! The relative amplitude exclusion threshold (in %) of the max. amplitude.
|
||||
*
|
||||
* Pixels, whose amplitudes fall below this exclusion threshold, are
|
||||
* removed from the pixel binning. The amplitude must raise above the
|
||||
* inclusion (#RelativeAmplitudeInclusion) threshold to be added back
|
||||
* to be pixel binning again.
|
||||
*
|
||||
* All available values from the 8-bit representation are valid.
|
||||
* The actual percentage value is determined by 100%/256*x.
|
||||
*
|
||||
* Special values:
|
||||
* - 0: Use 0 for absolute value only or to choose the pixel with the
|
||||
* minimum distance only (of also the absolute value is 0)! */
|
||||
uq0_8_t RelMinDistanceScope;
|
||||
* Note: in addition to the relative criteria, there is also the absolute
|
||||
* criteria (#AbsoluteAmplitudeInclusion, #AbsoluteAmplitudeExclusion).
|
||||
* The pixels are added to the pixel binning if their respective amplitude
|
||||
* is larger than the absolute AND relative inclusion values. On the other
|
||||
* hand, they are removed if their amplitude falls below the absolute OR
|
||||
* relative exclusion threshold.
|
||||
*
|
||||
* Must be less than or equal to #RelativeAmplitudeInclusion.
|
||||
*
|
||||
* Use #RelativeAmplitudeExclusion == #RelativeAmplitudeInclusion to
|
||||
* disable the hysteresis behavior and use it as a threshold only.
|
||||
*
|
||||
* Use 0 (for both, #RelativeAmplitudeExclusion and
|
||||
* #RelativeAmplitudeInclusion) to disable the relative amplitude
|
||||
* hysteresis. */
|
||||
uq0_8_t RelativeAmplitudeExclusion;
|
||||
|
||||
/*! The absolute amplitude threshold value in LSB.
|
||||
/*! The absolute amplitude inclusion threshold in LSB.
|
||||
*
|
||||
* Pixels with amplitude below this threshold value are dismissed.
|
||||
* Pixels, whose amplitudes raise above this inclusion threshold, are
|
||||
* added to the pixel binning. The amplitude must fall below the
|
||||
* exclusion (#RelativeAmplitudeExclusion) threshold to be removed from
|
||||
* the pixel binning again.
|
||||
*
|
||||
* The absolute amplitude threshold is only valid if the Golden Pixel
|
||||
* mode is enabled. Otherwise, the threshold is set to 0 LSB internally.
|
||||
* The absolute amplitude hysteresis is only valid if the Golden Pixel
|
||||
* mode is enabled. Otherwise, the thresholds are set to 0 LSB internally
|
||||
* which disables the absolute criteria.
|
||||
*
|
||||
* All available values from the 16-bit representation are valid.
|
||||
* The actual LSB value is determined by x/16.
|
||||
*
|
||||
* Use 0 to disable the absolute amplitude threshold. */
|
||||
uq12_4_t AbsAmplThreshold;
|
||||
|
||||
/*! The absolute minimum distance scope value in m.
|
||||
* Note: in addition to the absolute criteria, there is also the relative
|
||||
* criteria (#RelativeAmplitudeInclusion, #RelativeAmplitudeExclusion).
|
||||
* The pixels are added to the pixel binning if their respective amplitude
|
||||
* is larger than the absolute AND relative inclusion values. On the other
|
||||
* hand, they are removed if their amplitude falls below the absolute OR
|
||||
* relative exclusion threshold.
|
||||
*
|
||||
* Pixels that have a range value within [x0, x0 + dx] are considered
|
||||
* for the pixel binning, where x0 is the minimum distance of all
|
||||
* amplitude picked pixels and dx is the minimum distance scope value.
|
||||
* The minimum distance scope value will be the maximum of relative
|
||||
* and absolute value.
|
||||
* Must be greater than or equal to #AbsoluteAmplitudeExclusion.
|
||||
*
|
||||
* Use #AbsoluteAmplitudeExclusion == #AbsoluteAmplitudeInclusion to
|
||||
* disable the hysteresis behavior and use it as a threshold only.
|
||||
*
|
||||
* Use 0 (for both, #AbsoluteAmplitudeExclusion and
|
||||
* #AbsoluteAmplitudeInclusion) to disable the absolute amplitude
|
||||
* hysteresis. */
|
||||
uq12_4_t AbsoluteAmplitudeInclusion;
|
||||
|
||||
/*! The absolute amplitude exclusion threshold in LSB.
|
||||
*
|
||||
* Pixels, whose amplitudes fall below this exclusion threshold, are
|
||||
* removed from the pixel binning. The amplitude must raise above the
|
||||
* inclusion (#RelativeAmplitudeInclusion) threshold to be added back
|
||||
* to be pixel binning again.
|
||||
*
|
||||
* The absolute amplitude hysteresis is only valid if the Golden Pixel
|
||||
* mode is enabled. Otherwise, the thresholds are set to 0 LSB internally
|
||||
* which disables the absolute criteria.
|
||||
*
|
||||
* All available values from the 16-bit representation are valid.
|
||||
* The actual LSB value is determined by x/16.
|
||||
*
|
||||
* Note: in addition to the absolute criteria, there is also the relative
|
||||
* criteria (#RelativeAmplitudeInclusion, #RelativeAmplitudeExclusion).
|
||||
* The pixels are added to the pixel binning if their respective amplitude
|
||||
* is larger than the absolute AND relative inclusion values. On the other
|
||||
* hand, they are removed if their amplitude falls below the absolute OR
|
||||
* relative exclusion threshold.
|
||||
*
|
||||
* Must be less than or equal to #AbsoluteAmplitudeInclusion.
|
||||
*
|
||||
* Use #AbsoluteAmplitudeExclusion == #AbsoluteAmplitudeInclusion to
|
||||
* disable the hysteresis behavior and use it as a threshold only.
|
||||
*
|
||||
* Use 0 (for both, #AbsoluteAmplitudeExclusion and
|
||||
* #AbsoluteAmplitudeInclusion) to disable the absolute amplitude
|
||||
* hysteresis. */
|
||||
uq12_4_t AbsoluteAmplitudeExclusion;
|
||||
|
||||
/*! The Golden Pixel Priority Mode inclusion threshold in LSB.
|
||||
*
|
||||
* The Golden Pixel Priority Mode prioritizes a valid signal on the
|
||||
* Golden Pixel over other pixel to avoid unwanted effects from
|
||||
* "out-of-focus" pixels in application that require a smaller focus.
|
||||
*
|
||||
* If the Golden Pixel priority mode is enabled (see
|
||||
* #PBA_ENABLE_GOLDPX_PRIORITY_MODE) and the Golden Pixel has a valid signal
|
||||
* with amplitude higher than this inclusion threshold, its priority state
|
||||
* is enabled and the binning exits early by dismissing all other pixels
|
||||
* regardless of their respective amplitude or state. The Golden Pixel
|
||||
* priority state is disabled if the Golden Pixel amplitude falls below
|
||||
* the exclusion threshold (#GoldenPixelPriorityAmplitudeExclusion) or its
|
||||
* state becomes invalid (e.g. #PIXEL_SAT).
|
||||
*
|
||||
* All available values from the 16-bit representation are valid.
|
||||
* The actual LSB value is determined by x/16.
|
||||
*
|
||||
* Use 0 to disable the Golden Pixel priority mode hysteresis. */
|
||||
uq12_4_t GoldenPixelPriorityAmplitudeInclusion;
|
||||
|
||||
/*! The Golden Pixel Priority Mode exclusion threshold in LSB.
|
||||
*
|
||||
* The Golden Pixel Priority Mode prioritizes a valid signal on the
|
||||
* Golden Pixel over other pixel to avoid unwanted effects from
|
||||
* "out-of-focus" pixels in application that require a smaller focus.
|
||||
*
|
||||
* If the Golden Pixel priority mode is enabled (see
|
||||
* #PBA_ENABLE_GOLDPX_PRIORITY_MODE) and the Golden Pixel has a valid
|
||||
* signal with amplitude higher than the exclusion threshold
|
||||
* (#GoldenPixelPriorityAmplitudeInclusion), its priority state is enabled
|
||||
* and the binning exits early by dismissing all other pixels regardless
|
||||
* of their respective amplitude or state. The Golden Pixel priority state
|
||||
* is disabled if the Golden Pixel amplitude falls below this exclusion
|
||||
* threshold or its state becomes invalid (e.g. #PIXEL_SAT).
|
||||
*
|
||||
* All available values from the 16-bit representation are valid.
|
||||
* The actual LSB value is determined by x/16.
|
||||
*
|
||||
* Use 0 to disable the Golden Pixel priority mode hysteresis. */
|
||||
uq12_4_t GoldenPixelPriorityAmplitudeExclusion;
|
||||
|
||||
/*! The relative minimum distance scope inclusion threshold (in %).
|
||||
*
|
||||
* Pixels, whose range is smaller than the minimum distance inclusion
|
||||
* threshold (x_min + dx_incl) are added to the pixel binning. The
|
||||
* range must raise above the exclusion
|
||||
* (#RelativeDistanceScopeExclusion) threshold to be removed
|
||||
* from the pixel binning again. The relative value is determined
|
||||
* by multiplying the percentage with the minimum distance.
|
||||
*
|
||||
* The distance scope determines an interval within that pixels
|
||||
* are considered valid, originating at the minimum distance (x_min).
|
||||
* The width of the interval is specified by the relative and absolute
|
||||
* minimum distance scope thresholds. The actual values it the
|
||||
* maximum of both, the relative and absolute inclusion values
|
||||
* (#AbsoluteDistanceScopeInclusion).
|
||||
*
|
||||
* All available values from the 8-bit representation are valid.
|
||||
* The actual percentage value is determined by 100%/256*x.
|
||||
*
|
||||
* Must be smaller than or equal to the #RelativeDistanceScopeExclusion.
|
||||
*
|
||||
* Use #RelativeDistanceScopeExclusion == #RelativeDistanceScopeInclusion to
|
||||
* disable the hysteresis behavior and use it as a threshold only. */
|
||||
uq0_8_t RelativeDistanceScopeInclusion;
|
||||
|
||||
/*! The relative distance scope exclusion threshold (in %).
|
||||
*
|
||||
* Pixels, whose range is larger than the minimum distance exclusion
|
||||
* threshold (x_min + dx_excl) are removed from the pixel binning. The
|
||||
* range must fall below the inclusion
|
||||
* (#RelativeDistanceScopeInclusion) threshold to be added
|
||||
* to the pixel binning again. The relative value is determined
|
||||
* by multiplying the percentage with the minimum distance.
|
||||
*
|
||||
* The distance scope determines an interval within that pixels
|
||||
* are considered valid, originating at the minimum distance (x_min).
|
||||
* The width of the interval is specified by the relative and absolute
|
||||
* minimum distance scope thresholds. The actual values it the
|
||||
* maximum of both, the relative and absolute exclusion values
|
||||
* (#AbsoluteDistanceScopeExclusion).
|
||||
*
|
||||
* All available values from the 8-bit representation are valid.
|
||||
* The actual percentage value is determined by 100%/256*x.
|
||||
*
|
||||
* Must be larger than or equal to the #RelativeDistanceScopeInclusion.
|
||||
*
|
||||
* Use #RelativeDistanceScopeExclusion == #RelativeDistanceScopeInclusion to
|
||||
* disable the hysteresis behavior and use it as a threshold only. */
|
||||
uq0_8_t RelativeDistanceScopeExclusion;
|
||||
|
||||
/*! The absolute minimum distance scope inclusion threshold (in m).
|
||||
*
|
||||
* Pixels, whose range is smaller than the minimum distance inclusion
|
||||
* threshold (x_min + dx_incl) are added to the pixel binning. The
|
||||
* range must raise above the exclusion
|
||||
* (#AbsoluteDistanceScopeExclusion) threshold to be added
|
||||
* to the pixel binning again.
|
||||
*
|
||||
* The distance scope determines an interval within that pixels
|
||||
* are considered valid, originating at the minimum distance (x_min).
|
||||
* The width of the interval is specified by the relative and absolute
|
||||
* minimum distance scope thresholds. The actual values it the
|
||||
* maximum of both, the relative and absolute exclusion values
|
||||
* (#RelativeDistanceScopeInclusion).
|
||||
*
|
||||
* All available values from the 16-bit representation are valid.
|
||||
* The actual LSB value is determined by x/2^15.
|
||||
*
|
||||
* Special values:
|
||||
* - 0: Use 0 for relative value only or to choose the pixel with the
|
||||
* minimum distance only (of also the relative value is 0)! */
|
||||
uq1_15_t AbsMinDistanceScope;
|
||||
* Must be smaller than or equal to the #AbsoluteDistanceScopeExclusion.
|
||||
*
|
||||
* Use #AbsoluteDistanceScopeExclusion == #AbsoluteDistanceScopeInclusion to
|
||||
* disable the hysteresis behavior and use it as a threshold only. */
|
||||
uq1_15_t AbsoluteDistanceScopeInclusion;
|
||||
|
||||
/*! The absolute minimum distance scope exclusion threshold (in m).
|
||||
*
|
||||
* Pixels, whose range is larger than the minimum distance exclusion
|
||||
* threshold (x_min + dx_excl) are removed from the pixel binning. The
|
||||
* range must fall below the inclusion
|
||||
* (#AbsoluteDistanceScopeInclusion) threshold to be added
|
||||
* to the pixel binning again.
|
||||
*
|
||||
* The distance scope determines an interval within that pixels
|
||||
* are considered valid, originating at the minimum distance (x_min).
|
||||
* The width of the interval is specified by the relative and absolute
|
||||
* minimum distance scope thresholds. The actual values it the
|
||||
* maximum of both, the relative and absolute exclusion values
|
||||
* (#RelativeDistanceScopeExclusion).
|
||||
*
|
||||
* All available values from the 16-bit representation are valid.
|
||||
* The actual LSB value is determined by x/2^15.
|
||||
*
|
||||
* Must be larger than or equal to the #AbsoluteDistanceScopeInclusion.
|
||||
*
|
||||
* Use #AbsoluteDistanceScopeExclusion == #AbsoluteDistanceScopeInclusion to
|
||||
* disable the hysteresis behavior and use it as a threshold only. */
|
||||
uq1_15_t AbsoluteDistanceScopeExclusion;
|
||||
|
||||
/*! The Golden Pixel Saturation Filter Pixel Threshold.
|
||||
*
|
||||
* The Golden Pixel Saturation Filter will evaluate the status of the
|
||||
* Golden Pixel to #PIXEL_INVALID if a certain number of active pixels,
|
||||
* i.e. pixels that are not removed by the static pre-filter mask
|
||||
* (#PrefilterMask), are saturated (#PIXEL_SAT).
|
||||
*
|
||||
* The purpose of this filter is to avoid erroneous situations with highly
|
||||
* reflective targets (e.g. retro-reflectors) that can invalidate the
|
||||
* Golden Pixel such that it would not show the correct saturation state.
|
||||
* In order to avoid using the Golden Pixel in that scenario, this filter
|
||||
* mechanism can be used to remove the Golden Pixel if a specified number
|
||||
* of other pixels show saturation state.
|
||||
*
|
||||
* Use 0 to disable the Golden Pixel Saturation Filter. */
|
||||
uint8_t GoldenPixelSaturationFilterPixelThreshold;
|
||||
|
||||
/*! The Golden Pixel out-of-sync age limit for the GPPM.
|
||||
*
|
||||
* The Golden Pixel out-of-sync age is the number of consecutive frames
|
||||
* where the Golden Pixel is out-of-sync. This parameters is the threshold
|
||||
* to distinguish between temporary and permanent out-of-sync states.
|
||||
*
|
||||
* Temporary out-of-sync states happen when the target rapidly changes. In
|
||||
* this case, the Golden Pixel Priority Mode (GPPM) is not exited. Only if
|
||||
* the out-of-sync age exceeds the specified threshold, the Golden Pixel is
|
||||
* considered erroneous and the GPPM is exited.
|
||||
*
|
||||
* Use 0 to disable the Golden Pixel out-of-sync aging (= infinity). */
|
||||
uint8_t GoldenPixelOutOfSyncAgeThreshold;
|
||||
|
||||
/*! The absolute minimum distance threshold value in m.
|
||||
*
|
||||
* Pixels with distance below this threshold value are dismissed. */
|
||||
q9_22_t AbsMinDistanceThreshold;
|
||||
q9_22_t AbsoluteMinimumDistanceThreshold;
|
||||
|
||||
/*! The pre-filter pixel mask determines the pixel channels that are
|
||||
* statically excluded from the pixel binning (i.e. 1D distance) result.
|
||||
|
|
|
@ -55,6 +55,9 @@ extern "C" {
|
|||
* Also used as a special value to determine no object detected or infinity range. */
|
||||
#define ARGUS_RANGE_MAX (Q9_22_MAX)
|
||||
|
||||
/*! Minimum range value in Q9.22 format. */
|
||||
#define ARGUS_RANGE_MIN (Q9_22_MIN)
|
||||
|
||||
/*!***************************************************************************
|
||||
* @brief Status flags for the evaluated pixel structure.
|
||||
*
|
||||
|
|
|
@ -227,12 +227,19 @@ enum Status {
|
|||
|
||||
/*! -114: AFBR-S50 Error: Register data integrity is lost (e.g. due to unexpected
|
||||
* power-on-reset cycle or invalid write cycle of SPI. System tries to
|
||||
* reset the values. */
|
||||
* reset the values.
|
||||
*
|
||||
* @note If this error occurs after intentionally cycling the power supply
|
||||
* of the device, use the #Argus_RestoreDeviceState API function to properly
|
||||
* recover the current API state into the device to avoid that issue. */
|
||||
ERROR_ARGUS_DATA_INTEGRITY_LOST = -114,
|
||||
|
||||
/*! -115: AFBR-S50 Error: The range offsets calibration failed! */
|
||||
ERROR_ARGUS_RANGE_OFFSET_CALIBRATION_FAILED = -115,
|
||||
|
||||
/*! -116: AFBR-S50 Error: The VSUB calibration failed! */
|
||||
ERROR_ARGUS_VSUB_CALIBRATION_FAILED = -116,
|
||||
|
||||
/*! -191: AFBR-S50 Error: The device is currently busy and cannot execute the
|
||||
* requested command. */
|
||||
ERROR_ARGUS_BUSY = -191,
|
||||
|
|
|
@ -56,13 +56,13 @@ extern "C" {
|
|||
#define ARGUS_API_VERSION_MAJOR 1
|
||||
|
||||
/*! Minor version number of the AFBR-S50 API. */
|
||||
#define ARGUS_API_VERSION_MINOR 4
|
||||
#define ARGUS_API_VERSION_MINOR 5
|
||||
|
||||
/*! Bugfix version number of the AFBR-S50 API. */
|
||||
#define ARGUS_API_VERSION_BUGFIX 4
|
||||
#define ARGUS_API_VERSION_BUGFIX 6
|
||||
|
||||
/*! Build version number of the AFBR-S50 API. */
|
||||
#define ARGUS_API_VERSION_BUILD "20230327150535"
|
||||
#define ARGUS_API_VERSION_BUILD "20240208081753"
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
|
|
|
@ -72,30 +72,28 @@ typedef struct xtalk_t {
|
|||
* @details Contains crosstalk vector values for all 32 active pixels,
|
||||
* separated for A/B-Frames.
|
||||
*****************************************************************************/
|
||||
typedef struct argus_cal_xtalk_table_t {
|
||||
union {
|
||||
struct {
|
||||
/*! The crosstalk vector table for A-Frames. */
|
||||
xtalk_t FrameA[ARGUS_PIXELS_X][ARGUS_PIXELS_Y];
|
||||
typedef union argus_cal_xtalk_table_t {
|
||||
struct {
|
||||
/*! The crosstalk vector table for A-Frames. */
|
||||
xtalk_t FrameA[ARGUS_PIXELS_X][ARGUS_PIXELS_Y];
|
||||
|
||||
/*! The crosstalk vector table for B-Frames. */
|
||||
xtalk_t FrameB[ARGUS_PIXELS_X][ARGUS_PIXELS_Y];
|
||||
};
|
||||
|
||||
/*! The crosstalk vector table for A/B-Frames of all 32 pixels.*/
|
||||
xtalk_t Table[ARGUS_DFM_FRAME_COUNT][ARGUS_PIXELS_X][ARGUS_PIXELS_Y];
|
||||
/*! The crosstalk vector table for B-Frames. */
|
||||
xtalk_t FrameB[ARGUS_PIXELS_X][ARGUS_PIXELS_Y];
|
||||
};
|
||||
|
||||
/*! The crosstalk vector table for A/B-Frames of all 32 pixels.*/
|
||||
xtalk_t Table[ARGUS_DFM_FRAME_COUNT][ARGUS_PIXELS_X][ARGUS_PIXELS_Y];
|
||||
|
||||
} argus_cal_xtalk_table_t;
|
||||
|
||||
|
||||
/*!***************************************************************************
|
||||
* @brief Pixel-To-Pixel Crosstalk Compensation Parameters.
|
||||
* @details Contains calibration data that belongs to the pixel-to-pixel
|
||||
* crosstalk compensation feature.
|
||||
* @brief Electrical Pixel-To-Pixel Crosstalk Compensation Parameters.
|
||||
* @details Contains calibration data that belongs to the electrical
|
||||
* pixel-to-pixel crosstalk compensation feature.
|
||||
*****************************************************************************/
|
||||
typedef struct argus_cal_p2pxtalk_t {
|
||||
/*! Pixel-To-Pixel Compensation on/off. */
|
||||
typedef struct argus_cal_electrical_p2pxtalk_t {
|
||||
/*! Electrical Pixel-To-Pixel Compensation on/off. */
|
||||
bool Enabled;
|
||||
|
||||
/*! The relative threshold determines when the compensation is active for
|
||||
|
@ -134,8 +132,39 @@ typedef struct argus_cal_p2pxtalk_t {
|
|||
* Higher values determine more influence on the reference pixel signal. */
|
||||
q3_12_t KcFactorCRefPx;
|
||||
|
||||
} argus_cal_p2pxtalk_t;
|
||||
} argus_cal_electrical_p2pxtalk_t;
|
||||
|
||||
/*!***************************************************************************
|
||||
* @brief Optical Pixel-To-Pixel Crosstalk Compensation Parameters.
|
||||
* @details Contains calibration data that belongs to the optical
|
||||
* pixel-to-pixel crosstalk compensation feature.
|
||||
*****************************************************************************/
|
||||
typedef struct argus_cal_optical_p2pxtalk_t {
|
||||
/*! Optical Pixel-To-Pixel Compensation on/off. */
|
||||
bool Enabled;
|
||||
|
||||
/*! The sine component of the coupling coefficient that determines the amount
|
||||
* of a neighbour pixel signal that influences the raw signal of certain pixel.
|
||||
* Higher values determine more influence on the individual pixel signal. */
|
||||
q3_12_t CouplingCoeffS;
|
||||
|
||||
/*! The cosine component of the coupling coefficient that determines the amount
|
||||
* of a neighbour pixel signal that influences the raw signal of a certain pixel.
|
||||
* Higher values determine more influence on the individual pixel signal. */
|
||||
q3_12_t CouplingCoeffC;
|
||||
|
||||
} argus_cal_optical_p2pxtalk_t;
|
||||
|
||||
/*!***************************************************************************
|
||||
* @brief Pixel-To-Pixel Crosstalk Compensation Parameters.
|
||||
* @details Contains combined calibration data for electrical and
|
||||
* optical pixel-to-pixel crosstalk compensation feature.
|
||||
*****************************************************************************/
|
||||
typedef struct argus_cal_p2pxtalk_t {
|
||||
argus_cal_electrical_p2pxtalk_t Electrical;
|
||||
|
||||
argus_cal_optical_p2pxtalk_t Optical;
|
||||
} argus_cal_p2pxtalk_t;
|
||||
|
||||
/*! @} */
|
||||
#ifdef __cplusplus
|
||||
|
|
|
@ -61,7 +61,7 @@ extern "C" {
|
|||
* @details Algorithm to evaluate a/b, where b is in Q15.16 format, on a 32-bit
|
||||
* architecture with maximum precision.
|
||||
* The result is correctly rounded and given as the input format.
|
||||
* Division by 0 yields max. values determined by signa of numerator.
|
||||
* Division by 0 yields max. values determined by signs of numerator.
|
||||
* Too high/low results are truncated to max/min values.
|
||||
*
|
||||
* Depending on the architecture, the division is implemented with a 64-bit
|
||||
|
@ -89,14 +89,14 @@ inline int32_t fp_div16(int32_t a, q15_16_t b)
|
|||
|
||||
if (c > 0x80000000U) { return INT32_MIN; }
|
||||
|
||||
return -c;
|
||||
return (int32_t) - c;
|
||||
|
||||
} else {
|
||||
c = ((c / b) + (1 << 13U)) >> 14U;
|
||||
|
||||
if (c > (int64_t)INT32_MAX) { return INT32_MAX; }
|
||||
|
||||
return c;
|
||||
return (int32_t)c;
|
||||
}
|
||||
|
||||
#else
|
||||
|
@ -159,10 +159,16 @@ inline int32_t fp_div16(int32_t a, q15_16_t b)
|
|||
|
||||
/* Figure out the sign of result */
|
||||
if ((uint32_t)(a ^ b) & 0x80000000U) {
|
||||
result = -result;
|
||||
return (int32_t) - result;
|
||||
|
||||
} else {
|
||||
// fix 05.10.2023; the corner case, when result == INT32_MAX + 1:
|
||||
// Catch the wraparound (to INT32_MIN) and truncate instead.
|
||||
if (quotient > INT32_MAX) { return INT32_MAX; }
|
||||
|
||||
return (int32_t)result;
|
||||
}
|
||||
|
||||
return (int32_t)result;
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
|
@ -118,7 +118,7 @@ inline uint32_t fp_mulu(uint32_t u, uint32_t v, uint_fast8_t shift)
|
|||
assert(shift <= 32);
|
||||
#if USE_64BIT_MUL
|
||||
const uint64_t w = (uint64_t)u * (uint64_t)v;
|
||||
return (w >> shift) + ((w >> (shift - 1)) & 1U);
|
||||
return (uint32_t)((w >> shift) + ((w >> (shift - 1)) & 1U));
|
||||
#else
|
||||
uint32_t tmp[2] = { 0 };
|
||||
muldwu(tmp, u, v);
|
||||
|
@ -158,15 +158,15 @@ inline int32_t fp_muls(int32_t u, int32_t v, uint_fast8_t shift)
|
|||
|
||||
uint32_t u2, v2;
|
||||
|
||||
if (u < 0) { u2 = -u; sign = -sign; } else { u2 = u; }
|
||||
if (u < 0) { u2 = (uint32_t) - u; sign = -sign; } else { u2 = (uint32_t)u; }
|
||||
|
||||
if (v < 0) { v2 = -v; sign = -sign; } else { v2 = v; }
|
||||
if (v < 0) { v2 = (uint32_t) - v; sign = -sign; } else { v2 = (uint32_t)v; }
|
||||
|
||||
const uint32_t res = fp_mulu(u2, v2, shift);
|
||||
|
||||
assert(sign > 0 ? res <= 0x7FFFFFFFU : res <= 0x80000000U);
|
||||
|
||||
return sign > 0 ? res : -res;
|
||||
return sign > 0 ? (int32_t)res : -(int32_t)res;
|
||||
}
|
||||
|
||||
|
||||
|
@ -225,7 +225,9 @@ inline uint32_t fp_mul_u32_u16(uint32_t u, uint16_t v, uint_fast8_t shift)
|
|||
*****************************************************************************/
|
||||
inline int32_t fp_mul_s32_u16(int32_t u, uint16_t v, uint_fast8_t shift)
|
||||
{
|
||||
return u >= 0 ? fp_mul_u32_u16(u, v, shift) : - fp_mul_u32_u16(-u, v, shift);
|
||||
return u >= 0 ?
|
||||
(int32_t)fp_mul_u32_u16((uint32_t)u, v, shift) :
|
||||
-(int32_t)fp_mul_u32_u16((uint32_t) - u, v, shift);
|
||||
}
|
||||
|
||||
/*! @} */
|
||||
|
|
|
@ -80,7 +80,7 @@ inline uint32_t fp_rndu(uint32_t Q, uint_fast8_t n)
|
|||
*****************************************************************************/
|
||||
inline int32_t fp_rnds(int32_t Q, uint_fast8_t n)
|
||||
{
|
||||
return (Q < 0) ? -fp_rndu(-Q, n) : fp_rndu(Q, n);
|
||||
return (Q < 0) ? -(int32_t)fp_rndu((uint32_t)(-Q), n) : (int32_t)fp_rndu((uint32_t)Q, n);
|
||||
}
|
||||
|
||||
/*!***************************************************************************
|
||||
|
@ -108,7 +108,7 @@ inline uint32_t fp_truncu(uint32_t Q, uint_fast8_t n)
|
|||
*****************************************************************************/
|
||||
inline int32_t fp_truncs(int32_t Q, uint_fast8_t n)
|
||||
{
|
||||
return (Q < 0) ? -fp_truncu(-Q, n) : fp_truncu(Q, n);
|
||||
return (Q < 0) ? -(int32_t)fp_truncu((uint32_t)(-Q), n) : (int32_t)fp_truncu((uint32_t)Q, n);
|
||||
}
|
||||
|
||||
/*! @} */
|
||||
|
|
|
@ -66,7 +66,7 @@ inline uint32_t log2i(uint32_t x)
|
|||
{
|
||||
assert(x != 0);
|
||||
#if 1
|
||||
return 31 - __builtin_clz(x);
|
||||
return (uint32_t)(31 - __builtin_clz(x));
|
||||
#else
|
||||
#define S(k) if (x >= (1 << k)) { i += k; x >>= k; }
|
||||
int i = 0; S(16); S(8); S(4); S(2); S(1); return i;
|
||||
|
|
Binary file not shown.
Binary file not shown.
|
@ -162,7 +162,16 @@ static inline void abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
|
|||
*/
|
||||
static inline hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
|
||||
{
|
||||
return hrt_absolute_time() - *then;
|
||||
hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
// Cannot allow a negative elapsed time as this would appear
|
||||
// to be a huge positive elapsed time when represented as an
|
||||
// unsigned value!
|
||||
if (*then > now) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
return now - *then;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
@ -45,7 +45,6 @@
|
|||
#include <poll.h>
|
||||
#endif
|
||||
|
||||
#include <termios.h>
|
||||
#include <cstring>
|
||||
|
||||
#include <drivers/drv_sensor.h>
|
||||
|
@ -57,6 +56,8 @@
|
|||
#include <px4_platform_common/cli.h>
|
||||
#include <px4_platform_common/getopt.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/time.h>
|
||||
#include <px4_platform_common/Serial.hpp>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
|
@ -81,6 +82,7 @@
|
|||
#include <linux/spi/spidev.h>
|
||||
#endif /* __PX4_LINUX */
|
||||
|
||||
using namespace device;
|
||||
using namespace time_literals;
|
||||
|
||||
#define TIMEOUT_1HZ 1300 //!< Timeout time in mS, 1000 mS (1Hz) + 300 mS delta for error
|
||||
|
@ -169,7 +171,10 @@ public:
|
|||
void reset_if_scheduled();
|
||||
|
||||
private:
|
||||
int _serial_fd{-1}; ///< serial interface to GPS
|
||||
#ifdef __PX4_LINUX
|
||||
int _spi_fd {-1}; ///< SPI interface to GPS
|
||||
#endif
|
||||
Serial *_uart = nullptr; ///< UART interface to GPS
|
||||
unsigned _baudrate{0}; ///< current baudrate
|
||||
const unsigned _configured_baudrate{0}; ///< configured baudrate (0=auto-detect)
|
||||
char _port[20] {}; ///< device / serial port path
|
||||
|
@ -329,8 +334,11 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac
|
|||
char c = _port[strlen(_port) - 1]; // last digit of path (eg /dev/ttyS2)
|
||||
set_device_bus(c - 48); // sub 48 to convert char to integer
|
||||
|
||||
#ifdef __PX4_LINUX
|
||||
|
||||
} else if (_interface == GPSHelper::Interface::SPI) {
|
||||
set_device_bus_type(device::Device::DeviceBusType::DeviceBusType_SPI);
|
||||
#endif
|
||||
}
|
||||
|
||||
if (_mode == gps_driver_mode_t::None) {
|
||||
|
@ -403,10 +411,23 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
|
|||
return num_read;
|
||||
}
|
||||
|
||||
case GPSCallbackType::writeDeviceData:
|
||||
gps->dumpGpsData((uint8_t *)data1, (size_t)data2, gps_dump_comm_mode_t::Full, true);
|
||||
case GPSCallbackType::writeDeviceData: {
|
||||
gps->dumpGpsData((uint8_t *)data1, (size_t)data2, gps_dump_comm_mode_t::Full, true);
|
||||
|
||||
return ::write(gps->_serial_fd, data1, (size_t)data2);
|
||||
int ret = 0;
|
||||
|
||||
if (gps->_uart) {
|
||||
ret = gps->_uart->write((void *) data1, (size_t) data2);
|
||||
|
||||
#ifdef __PX4_LINUX
|
||||
|
||||
} else if (gps->_spi_fd >= 0) {
|
||||
ret = ::write(gps->_spi_fd, data1, (size_t)data2);
|
||||
#endif
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
case GPSCallbackType::setBaudrate:
|
||||
return gps->setBaudrate(data2);
|
||||
|
@ -449,72 +470,64 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
|
|||
|
||||
int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout)
|
||||
{
|
||||
int ret = 0;
|
||||
const unsigned character_count = 32; // minimum bytes that we want to read
|
||||
const int max_timeout = 50;
|
||||
int timeout_adjusted = math::min(max_timeout, timeout);
|
||||
|
||||
handleInjectDataTopic();
|
||||
|
||||
#if !defined(__PX4_QURT)
|
||||
if ((_interface == GPSHelper::Interface::UART) && (_uart)) {
|
||||
ret = _uart->readAtLeast(buf, buf_length, character_count, timeout_adjusted);
|
||||
|
||||
/* For non QURT, use the usual polling. */
|
||||
// SPI is only supported on LInux
|
||||
#if defined(__PX4_LINUX)
|
||||
|
||||
//Poll only for the serial data. In the same thread we also need to handle orb messages,
|
||||
//so ideally we would poll on both, the serial fd and orb subscription. Unfortunately the
|
||||
//two pollings use different underlying mechanisms (at least under posix), which makes this
|
||||
//impossible. Instead we limit the maximum polling interval and regularly check for new orb
|
||||
//messages.
|
||||
//FIXME: add a unified poll() API
|
||||
const int max_timeout = 50;
|
||||
} else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)) {
|
||||
|
||||
pollfd fds[1];
|
||||
fds[0].fd = _serial_fd;
|
||||
fds[0].events = POLLIN;
|
||||
//Poll only for the SPI data. In the same thread we also need to handle orb messages,
|
||||
//so ideally we would poll on both, the SPI fd and orb subscription. Unfortunately the
|
||||
//two pollings use different underlying mechanisms (at least under posix), which makes this
|
||||
//impossible. Instead we limit the maximum polling interval and regularly check for new orb
|
||||
//messages.
|
||||
//FIXME: add a unified poll() API
|
||||
|
||||
int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), math::min(max_timeout, timeout));
|
||||
pollfd fds[1];
|
||||
fds[0].fd = _spi_fd;
|
||||
fds[0].events = POLLIN;
|
||||
|
||||
if (ret > 0) {
|
||||
/* if we have new data from GPS, go handle it */
|
||||
if (fds[0].revents & POLLIN) {
|
||||
/*
|
||||
* We are here because poll says there is some data, so this
|
||||
* won't block even on a blocking device. But don't read immediately
|
||||
* by 1-2 bytes, wait for some more data to save expensive read() calls.
|
||||
* If we have all requested data available, read it without waiting.
|
||||
* If more bytes are available, we'll go back to poll() again.
|
||||
*/
|
||||
const unsigned character_count = 32; // minimum bytes that we want to read
|
||||
unsigned baudrate = _baudrate == 0 ? 115200 : _baudrate;
|
||||
const unsigned sleeptime = character_count * 1000000 / (baudrate / 10);
|
||||
ret = poll(fds, sizeof(fds) / sizeof(fds[0]), timeout_adjusted);
|
||||
|
||||
#ifdef __PX4_NUTTX
|
||||
int err = 0;
|
||||
int bytes_available = 0;
|
||||
err = ::ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available);
|
||||
if (ret > 0) {
|
||||
/* if we have new data from GPS, go handle it */
|
||||
if (fds[0].revents & POLLIN) {
|
||||
/*
|
||||
* We are here because poll says there is some data, so this
|
||||
* won't block even on a blocking device. But don't read immediately
|
||||
* by 1-2 bytes, wait for some more data to save expensive read() calls.
|
||||
* If we have all requested data available, read it without waiting.
|
||||
* If more bytes are available, we'll go back to poll() again.
|
||||
*/
|
||||
unsigned baudrate = _baudrate == 0 ? 115200 : _baudrate;
|
||||
const unsigned sleeptime = character_count * 1000000 / (baudrate / 10);
|
||||
|
||||
if (err != 0 || bytes_available < (int)character_count) {
|
||||
px4_usleep(sleeptime);
|
||||
|
||||
ret = ::read(_spi_fd, buf, buf_length);
|
||||
|
||||
if (ret > 0) {
|
||||
_num_bytes_read += ret;
|
||||
}
|
||||
|
||||
} else {
|
||||
ret = -1;
|
||||
}
|
||||
|
||||
#else
|
||||
px4_usleep(sleeptime);
|
||||
#endif
|
||||
|
||||
ret = ::read(_serial_fd, buf, buf_length);
|
||||
|
||||
if (ret > 0) {
|
||||
_num_bytes_read += ret;
|
||||
}
|
||||
|
||||
} else {
|
||||
ret = -1;
|
||||
}
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
return ret;
|
||||
|
||||
#else
|
||||
/* For QURT, just use read for now, since this doesn't block, we need to slow it down
|
||||
* just a bit. */
|
||||
px4_usleep(10000);
|
||||
return ::read(_serial_fd, buf, buf_length);
|
||||
#endif
|
||||
}
|
||||
|
||||
void GPS::handleInjectDataTopic()
|
||||
|
@ -583,105 +596,38 @@ bool GPS::injectData(uint8_t *data, size_t len)
|
|||
{
|
||||
dumpGpsData(data, len, gps_dump_comm_mode_t::Full, true);
|
||||
|
||||
size_t written = ::write(_serial_fd, data, len);
|
||||
::fsync(_serial_fd);
|
||||
size_t written = 0;
|
||||
|
||||
if ((_interface == GPSHelper::Interface::UART) && (_uart)) {
|
||||
written = _uart->write((const void *) data, len);
|
||||
|
||||
#ifdef __PX4_LINUX
|
||||
|
||||
} else if (_interface == GPSHelper::Interface::SPI) {
|
||||
written = ::write(_spi_fd, data, len);
|
||||
::fsync(_spi_fd);
|
||||
#endif
|
||||
}
|
||||
|
||||
return written == len;
|
||||
}
|
||||
|
||||
int GPS::setBaudrate(unsigned baud)
|
||||
{
|
||||
/* process baud rate */
|
||||
int speed;
|
||||
if (_interface == GPSHelper::Interface::UART) {
|
||||
if ((_uart) && (_uart->setBaudrate(baud))) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
switch (baud) {
|
||||
case 9600: speed = B9600; break;
|
||||
#ifdef __PX4_LINUX
|
||||
|
||||
case 19200: speed = B19200; break;
|
||||
|
||||
case 38400: speed = B38400; break;
|
||||
|
||||
case 57600: speed = B57600; break;
|
||||
|
||||
case 115200: speed = B115200; break;
|
||||
|
||||
case 230400: speed = B230400; break;
|
||||
|
||||
#ifndef B460800
|
||||
#define B460800 460800
|
||||
} else if (_interface == GPSHelper::Interface::SPI) {
|
||||
// Can't set the baudrate on a SPI port but just return a success
|
||||
return 0;
|
||||
#endif
|
||||
|
||||
case 460800: speed = B460800; break;
|
||||
|
||||
#ifndef B921600
|
||||
#define B921600 921600
|
||||
#endif
|
||||
|
||||
case 921600: speed = B921600; break;
|
||||
|
||||
default:
|
||||
PX4_ERR("ERR: unknown baudrate: %d", baud);
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
struct termios uart_config;
|
||||
|
||||
int termios_state;
|
||||
|
||||
/* fill the struct for the new configuration */
|
||||
tcgetattr(_serial_fd, &uart_config);
|
||||
|
||||
/* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */
|
||||
|
||||
//
|
||||
// Input flags - Turn off input processing
|
||||
//
|
||||
// convert break to null byte, no CR to NL translation,
|
||||
// no NL to CR translation, don't mark parity errors or breaks
|
||||
// no input parity check, don't strip high bit off,
|
||||
// no XON/XOFF software flow control
|
||||
//
|
||||
uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
|
||||
INLCR | PARMRK | INPCK | ISTRIP | IXON);
|
||||
//
|
||||
// Output flags - Turn off output processing
|
||||
//
|
||||
// no CR to NL translation, no NL to CR-NL translation,
|
||||
// no NL to CR translation, no column 0 CR suppression,
|
||||
// no Ctrl-D suppression, no fill characters, no case mapping,
|
||||
// no local output processing
|
||||
//
|
||||
// config.c_oflag &= ~(OCRNL | ONLCR | ONLRET |
|
||||
// ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
|
||||
uart_config.c_oflag = 0;
|
||||
|
||||
//
|
||||
// No line processing
|
||||
//
|
||||
// echo off, echo newline off, canonical mode off,
|
||||
// extended input processing off, signal chars off
|
||||
//
|
||||
uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
|
||||
|
||||
/* no parity, one stop bit, disable flow control */
|
||||
uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
|
||||
|
||||
/* set baud rate */
|
||||
if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
|
||||
GPS_ERR("ERR: %d (cfsetispeed)", termios_state);
|
||||
return -1;
|
||||
}
|
||||
|
||||
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
|
||||
GPS_ERR("ERR: %d (cfsetospeed)", termios_state);
|
||||
return -1;
|
||||
}
|
||||
|
||||
if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) {
|
||||
GPS_ERR("ERR: %d (tcsetattr)", termios_state);
|
||||
return -1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
return -1;
|
||||
}
|
||||
|
||||
void GPS::initializeCommunicationDump()
|
||||
|
@ -840,31 +786,58 @@ GPS::run()
|
|||
_helper = nullptr;
|
||||
}
|
||||
|
||||
if (_serial_fd < 0) {
|
||||
/* open the serial port */
|
||||
_serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
|
||||
if ((_interface == GPSHelper::Interface::UART) && (_uart == nullptr)) {
|
||||
|
||||
if (_serial_fd < 0) {
|
||||
PX4_ERR("failed to open %s err: %d", _port, errno);
|
||||
// Create the UART port instance
|
||||
_uart = new Serial(_port);
|
||||
|
||||
if (_uart == nullptr) {
|
||||
PX4_ERR("Error creating serial device %s", _port);
|
||||
px4_sleep(1);
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
if ((_interface == GPSHelper::Interface::UART) && (! _uart->isOpen())) {
|
||||
// Configure the desired baudrate if one was specified by the user.
|
||||
// Otherwise the default baudrate will be used.
|
||||
if (_configured_baudrate) {
|
||||
if (! _uart->setBaudrate(_configured_baudrate)) {
|
||||
PX4_ERR("Error setting baudrate to %u on %s", _configured_baudrate, _port);
|
||||
px4_sleep(1);
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
// Open the UART. If this is successful then the UART is ready to use.
|
||||
if (! _uart->open()) {
|
||||
PX4_ERR("Error opening serial device %s", _port);
|
||||
px4_sleep(1);
|
||||
continue;
|
||||
}
|
||||
|
||||
#ifdef __PX4_LINUX
|
||||
|
||||
if (_interface == GPSHelper::Interface::SPI) {
|
||||
int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi)
|
||||
int status_value = ::ioctl(_serial_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
|
||||
} else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd < 0)) {
|
||||
_spi_fd = ::open(_port, O_RDWR | O_NOCTTY);
|
||||
|
||||
if (status_value < 0) {
|
||||
PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
|
||||
}
|
||||
if (_spi_fd < 0) {
|
||||
PX4_ERR("failed to open SPI port %s err: %d", _port, errno);
|
||||
px4_sleep(1);
|
||||
continue;
|
||||
}
|
||||
|
||||
status_value = ::ioctl(_serial_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed);
|
||||
int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi)
|
||||
int status_value = ::ioctl(_spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
|
||||
|
||||
if (status_value < 0) {
|
||||
PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
|
||||
}
|
||||
if (status_value < 0) {
|
||||
PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
|
||||
}
|
||||
|
||||
status_value = ::ioctl(_spi_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed);
|
||||
|
||||
if (status_value < 0) {
|
||||
PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
|
||||
}
|
||||
|
||||
#endif /* __PX4_LINUX */
|
||||
|
@ -1056,9 +1029,17 @@ GPS::run()
|
|||
}
|
||||
}
|
||||
|
||||
if (_serial_fd >= 0) {
|
||||
::close(_serial_fd);
|
||||
_serial_fd = -1;
|
||||
if ((_interface == GPSHelper::Interface::UART) && (_uart)) {
|
||||
(void) _uart->close();
|
||||
delete _uart;
|
||||
_uart = nullptr;
|
||||
|
||||
#ifdef __PX4_LINUX
|
||||
|
||||
} else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)) {
|
||||
::close(_spi_fd);
|
||||
_spi_fd = -1;
|
||||
#endif
|
||||
}
|
||||
|
||||
if (_mode_auto) {
|
||||
|
@ -1406,7 +1387,7 @@ int GPS::task_spawn(int argc, char *argv[], Instance instance)
|
|||
entry_point, (char *const *)argv);
|
||||
|
||||
if (task_id < 0) {
|
||||
task_id = -1;
|
||||
_task_id = -1;
|
||||
return -errno;
|
||||
}
|
||||
|
||||
|
@ -1477,12 +1458,12 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
|
|||
break;
|
||||
|
||||
case 'i':
|
||||
if (!strcmp(myoptarg, "spi")) {
|
||||
interface = GPSHelper::Interface::SPI;
|
||||
|
||||
} else if (!strcmp(myoptarg, "uart")) {
|
||||
if (!strcmp(myoptarg, "uart")) {
|
||||
interface = GPSHelper::Interface::UART;
|
||||
|
||||
#ifdef __PX4_LINUX
|
||||
} else if (!strcmp(myoptarg, "spi")) {
|
||||
interface = GPSHelper::Interface::SPI;
|
||||
#endif
|
||||
} else {
|
||||
PX4_ERR("unknown interface: %s", myoptarg);
|
||||
error_flag = true;
|
||||
|
@ -1490,12 +1471,12 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
|
|||
break;
|
||||
|
||||
case 'j':
|
||||
if (!strcmp(myoptarg, "spi")) {
|
||||
interface_secondary = GPSHelper::Interface::SPI;
|
||||
|
||||
} else if (!strcmp(myoptarg, "uart")) {
|
||||
if (!strcmp(myoptarg, "uart")) {
|
||||
interface_secondary = GPSHelper::Interface::UART;
|
||||
|
||||
#ifdef __PX4_LINUX
|
||||
} else if (!strcmp(myoptarg, "spi")) {
|
||||
interface_secondary = GPSHelper::Interface::SPI;
|
||||
#endif
|
||||
} else {
|
||||
PX4_ERR("unknown interface for secondary: %s", myoptarg);
|
||||
error_flag = true;
|
||||
|
|
|
@ -364,6 +364,13 @@ PX4IO::~PX4IO()
|
|||
bool PX4IO::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
|
||||
unsigned num_outputs, unsigned num_control_groups_updated)
|
||||
{
|
||||
for (size_t i = 0; i < num_outputs; i++) {
|
||||
if (!_mixing_output.isFunctionSet(i)) {
|
||||
// do not run any signal on disabled channels
|
||||
outputs[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
if (!_test_fmu_fail) {
|
||||
/* output to the servos */
|
||||
io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, outputs, num_outputs);
|
||||
|
|
|
@ -75,6 +75,7 @@ add_definitions(
|
|||
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_${OS_UPPER}=1
|
||||
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_NUM_IFACES=${config_uavcan_num_ifaces}
|
||||
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_TIMER_NUMBER=${UAVCAN_TIMER}
|
||||
-DUAVCAN_NUM_IFACES=${config_uavcan_num_ifaces}
|
||||
-DUAVCAN_CPP_VERSION=UAVCAN_CPP03
|
||||
-DUAVCAN_DRIVER=uavcan_${UAVCAN_DRIVER}
|
||||
-DUAVCAN_IMPLEMENT_PLACEMENT_NEW=1
|
||||
|
|
|
@ -744,6 +744,41 @@ UavcanNode::Run()
|
|||
|
||||
_node.spinOnce(); // expected to be non-blocking
|
||||
|
||||
// Publish status
|
||||
constexpr hrt_abstime status_pub_interval = 100_ms;
|
||||
|
||||
if (hrt_absolute_time() - _last_can_status_pub >= status_pub_interval) {
|
||||
_last_can_status_pub = hrt_absolute_time();
|
||||
|
||||
for (int i = 0; i < _node.getDispatcher().getCanIOManager().getCanDriver().getNumIfaces(); i++) {
|
||||
if (i > UAVCAN_NUM_IFACES) {
|
||||
break;
|
||||
}
|
||||
|
||||
auto iface = _node.getDispatcher().getCanIOManager().getCanDriver().getIface(i);
|
||||
|
||||
if (!iface) {
|
||||
continue;
|
||||
}
|
||||
|
||||
auto iface_perf_cnt = _node.getDispatcher().getCanIOManager().getIfacePerfCounters(i);
|
||||
can_interface_status_s status{
|
||||
.timestamp = hrt_absolute_time(),
|
||||
.io_errors = iface_perf_cnt.errors,
|
||||
.frames_tx = iface_perf_cnt.frames_tx,
|
||||
.frames_rx = iface_perf_cnt.frames_rx,
|
||||
.interface = static_cast<uint8_t>(i),
|
||||
};
|
||||
|
||||
if (_can_status_pub_handles[i] == nullptr) {
|
||||
int instance{0};
|
||||
_can_status_pub_handles[i] = orb_advertise_multi(ORB_ID(can_interface_status), nullptr, &instance);
|
||||
}
|
||||
|
||||
(void)orb_publish(ORB_ID(can_interface_status), _can_status_pub_handles[i], &status);
|
||||
}
|
||||
}
|
||||
|
||||
// check for parameter updates
|
||||
if (_parameter_update_sub.updated()) {
|
||||
// clear update
|
||||
|
|
|
@ -96,6 +96,7 @@
|
|||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionInterval.hpp>
|
||||
#include <uORB/topics/can_interface_status.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/uavcan_parameter_request.h>
|
||||
#include <uORB/topics/uavcan_parameter_value.h>
|
||||
|
@ -309,6 +310,10 @@ private:
|
|||
|
||||
uORB::Publication<uavcan_parameter_value_s> _param_response_pub{ORB_ID(uavcan_parameter_value)};
|
||||
uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
|
||||
uORB::PublicationMulti<can_interface_status_s> _can_status_pub{ORB_ID(can_interface_status)};
|
||||
|
||||
hrt_abstime _last_can_status_pub{0};
|
||||
orb_advert_t _can_status_pub_handles[UAVCAN_NUM_IFACES] = {nullptr};
|
||||
|
||||
/*
|
||||
* The MAVLink parameter bridge needs to know the maximum parameter index
|
||||
|
|
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue