PX4模块设计之四十：FrskyTelemetry模块
Posted lida2003
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了PX4模块设计之四十：FrskyTelemetry模块相关的知识，希望对你有一定的参考价值。
PX4模块设计之四十：FrskyTelemetry模块
1. FrskyTelemetry模块简介
FrSky Telemetry support. Auto-detects D or S.PORT protocol.
frsky_telemetry <command> [arguments...]
Commands:
start
[-d <val>] Select Serial Device
values: <file:dev>, default: /dev/ttyS6
[-t <val>] Scanning timeout [s] (default: no timeout)
default: 0
[-m <val>] Select protocol (default: auto-detect)
values: sport|sport_single|sport_single_invert|dtype, default:
auto
stop
status
注1：usage函数是具体对应实现。
注2：FrskyTelemetry模块采用了纯C语言代码实现。
2. 模块入口函数
2.1 主入口frsky_telemetry_main
模块仅支持start/stop/status命令，不支持其他自定义命令。
frsky_telemetry_main
├──> <argc < 2>
│ ├──> PX4_ERR("missing command")
│ ├──> usage()
│ └──> return -1
├──> <!strcmp(argv[1], "start")>
│ ├──> <thread_running>
│ │ ├──> PX4_INFO("frsky_telemetry already running")
│ │ └──> return 0
│ ├──> thread_should_exit = false
│ ├──> frsky_task = px4_task_spawn_cmd("frsky_telemetry",SCHED_DEFAULT,SCHED_PRIORITY_DEFAULT + 4,1400,frsky_telemetry_thread_main,(char *const *)argv)
│ ├──> <while (!thread_running)>
│ │ └──> usleep(200)
│ └──> return 0
├──> <!strcmp(argv[1], "stop")>
│ ├──> <!thread_running>
│ │ ├──> PX4_WARN("frsky_telemetry already stopped")
│ │ └──> return 0
│ ├──> thread_should_exit = true
│ ├──> <while (thread_running)>
│ │ ├──> usleep(1000000)
│ │ └──> PX4_INFO(".")
│ ├──> PX4_INFO("terminated.")
│ ├──> device_name = NULL
│ └──> return 0
├──> <!strcmp(argv[1], "status")><thread_running><switch (frsky_state)>
│ ├──> <case SCANNING>
│ │ ├──> PX4_INFO("running: SCANNING")
│ │ ├──> PX4_INFO("port: %s", device_name)
│ │ └──> break
│ ├──> <case SPORT>
│ │ ├──> PX4_INFO("running: SPORT")
│ │ ├──> PX4_INFO("port: %s", device_name)
│ │ ├──> PX4_INFO("packets sent: %ld", sentPackets)
│ │ └──> break
│ ├──> <case SPORT_SINGLE_WIRE>
│ │ ├──> PX4_INFO("running: SPORT (single wire)")
│ │ ├──> PX4_INFO("port: %s", device_name)
│ │ ├──> PX4_INFO("packets sent: %ld", sentPackets)
│ │ └──> break
│ ├──> <case SPORT_SINGLE_WIRE_INVERT>
│ │ ├──> PX4_INFO("running: SPORT (single wire, inverted)")
│ │ ├──> PX4_INFO("port: %s", device_name)
│ │ ├──> PX4_INFO("packets sent: %ld", sentPackets)
│ │ └──> break
│ ├──> <case DTYPE>
│ │ ├──> PX4_INFO("running: DTYPE")
│ │ ├──> PX4_INFO("port: %s", device_name)
│ │ ├──> PX4_INFO("packets sent: %ld", sentPackets)
│ │ └──> break
│ └──> return 0
├──> <PX4_ERR("unrecognized command")
├──> <usage()
└──> return 0
2.2 自定义子命令custom_command
注：该模块采用了纯C语言代码实现，在main函数中直接执行命令，无需ModuleBase的custom_command重载实现。
2.3 模块状态print_status【重载】
注：该模块采用了纯C语言代码实现，在main函数中直接执行usage函数，无需ModuleBase的模块状态print_status重载实现。
3. FrskyTelemetry模块重要函数
3.1 task_spawn/instantiate/Run
注：该模块采用了纯C语言代码实现，无需ModuleBase的模块状态task_spawn/instantiate/Run重载实现。
3.2 frsky_telemetry_thread_main
这个是FrskyTelemetry模块任务入口函数，在任务内部会循环执行，直到条件不满足才退出循环任务。
frsky_telemetry_thread_main
├──> device_name = "/dev/ttyS6" /* default USART8 */
├──> [frsky状态机及变量初始化]
├──> [frsky telemetry参数解析]
│ ├──> <case 'd'>
│ │ ├──> device_name = myoptarg
│ │ └──> break
│ ├──> <case 't'>
│ │ ├──> scanning_timeout_ms = strtoul(myoptarg, nullptr, 10) * 1000
│ │ └──> break
│ ├──> <case 'm'>
│ │ ├──> <!strcmp(myoptarg, "sport")>
│ │ │ └──> frsky_state = baudRate = SPORT
│ │ ├──> <!strcmp(myoptarg, "sport_single")>
│ │ │ └──> frsky_state = baudRate = SPORT_SINGLE_WIRE
│ │ ├──> <!strcmp(myoptarg, "sport_single_invert")>
│ │ │ └──> frsky_state = baudRate = SPORT_SINGLE_WIRE_INVERT
│ │ ├──> <!strcmp(myoptarg, "dtype")>
│ │ │ └──> frsky_state = baudRate = DTYPE
│ │ ├──> <!strcmp(myoptarg, "auto")>
│ │ │ └──> frsky_state = baudRate = DTYPE
│ │ ├──> < else >
│ │ │ ├──> usage()
│ │ │ └──> return -1
│ │ └──> break
│ └──> <default:
│ ├──> usage()
│ └──> return -1
├──> [Open UART]
│ ├──> const int uart = sPort_open_uart(device_name, &uart_config, &uart_config_original)
│ └──> <uart < 0>
│ ├──> device_name = NULL
│ └──> return -1
├──> [poll descriptor]
│ ├──> struct pollfd fds[1]
│ ├──> fds[0].fd = uart
│ └──> fds[0].events = POLLIN
├──> thread_running = true
├──> [Auto Detect FrSky (D8 mode, DTYPE) or SmartPort (D16 mode, SPORT/SPORT_SINGLE_WIRE/SPORT_SINGLE_WIRE_INVERT)]
├──> <frsky_state == SPORT || frsky_state == SPORT_SINGLE_WIRE || frsky_state == SPORT_SINGLE_WIRE_INVERT>
│ ├──> [UART Initialization]
│ ├──> [sPort Topic subscribe]
│ ├──> <while (!thread_should_exit)> // send S.port telemetry
│ │ ├──> [Wait bus master to put 0x7E flag]
│ │ ├──> [Read ID flag for pulling]
│ │ ├──> sPort_update_topics // update subscribed topic
│ │ └──> <switch (ID flag)>
│ │ ├──> <case SMARTPORT_POLL_1><now_ms - lastBATV_ms > 1000> //report BATV at 1Hz
│ │ │ ├──> lastBATV_ms = now_ms
│ │ │ └──> sPort_send_BATV(uart) //send battery voltage
│ │ ├──> <case SMARTPORT_POLL_2><now_ms - lastCUR_ms > 200> //report battery current at 5Hz
│ │ │ ├──> lastCUR_ms = now_ms
│ │ │ └──> sPort_send_CUR(uart) //send battery current
│ │ ├──> <case SMARTPORT_POLL_3><now_ms - lastALT_ms > 200> //report altitude at 5Hz
│ │ │ ├──> lastALT_ms = now_ms
│ │ │ └──> sPort_send_ALT(uart) //send altitude
│ │ ├──> <case SMARTPORT_POLL_4><now_ms - lastSPD_ms > 200> //report speed at 5Hz
│ │ │ ├──> lastSPD_ms = now_ms
│ │ │ └──> sPort_send_SPD(uart) //send speed
│ │ ├──> <case SMARTPORT_POLL_5><now_ms - lastFUEL_ms > 1000> //report fuel at 1Hz
│ │ │ ├──> lastFUEL_ms = now_ms
│ │ │ └──> sPort_send_FUEL(uart) //send fuel
│ │ ├──> <case SMARTPORT_POLL_6><now_ms - lastVSPD_ms > 100> //report vertical speed at 10Hz
│ │ │ ├──> uint32_t dt = now_ms - lastVSPD_ms
│ │ │ ├──> float speed = (filtered_alt - last_baro_alt) / (1e-3f * (float)dt) //estimate vertical speed using first difference and delta t
│ │ │ ├──> last_baro_alt = filtered_alt //save current alt and timestamp
│ │ │ ├──> lastVSPD_ms = now_ms
│ │ │ └──> sPort_send_VSPD(uart, speed)
│ │ ├──> <case SMARTPORT_POLL_7><now_ms - lastGPS_ms > 100><switch (elementCount)> //report GPS data elements at 5*5Hz
│ │ │ ├──> <elementCount 0>:sPort_send_GPS_LON(uart)
│ │ │ ├──> <elementCount 1>:sPort_send_GPS_LAT(uart)
│ │ │ ├──> <elementCount 2>:sPort_send_GPS_CRS(uart)
│ │ │ ├──> <elementCount 3>:sPort_send_GPS_ALT(uart)
│ │ │ ├──> <elementCount 4>:sPort_send_GPS_SPD(uart)
│ │ │ └──> <elementCount 5>:sPort_send_GPS_TIME(uart)
│ │ ├──> <case SMARTPORT_POLL_8>
│ │ │ ├──> <now_ms - lastNAV_STATE_ms > 500> // report nav_state as DIY_NAVSTATE 2Hz
│ │ │ │ ├──> lastNAV_STATE_ms = now_ms
│ │ │ │ └──> sPort_send_NAV_STATE(uart) // send T1
│ │ │ └──> <now_ms - lastGPS_FIX_ms > 500> //report satcount and fix as DIY_GPSFIX at 2Hz
│ │ │ ├──> lastGPS_FIX_ms = now_ms
│ │ │ └──> sPort_send_GPS_FIX(uart) // send T2
│ │ └──> <case SMARTPORT_SENSOR_ID_SP2UR>
│ │ ├──> <case 0>
│ │ │ └──> sPort_send_flight_mode(uart)
│ │ └──> default:
│ │ └──> sPort_send_GPS_info(uart)
│ ├──> PX4_DEBUG("freeing sPort memory")
│ └──> sPort_deinit()
├──> <frsky_state == DTYPE>
│ ├──> [detected D type telemetry: reconfigure UART]
│ ├──> [frsky_init() Topic subscribe]
│ ├──> <while (!thread_should_exit)> //send D8 mode telemetry
│ │ ├──> [frsky_parse_host, parse incoming data]
│ │ ├──> frsky_update_topics // update subscribed topic
│ │ ├──> <sentPackets++>: frsky_send_frame1(uart) //Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM
│ │ ├──> <sentPackets++>: frsky_send_frame2(uart) //Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level
│ │ └──> <sentPackets++>: frsky_send_frame3(uart) //Send frame 3 (every 5000ms): date, time
│ └──> frsky_deinit()
├──> tcsetattr(uart, TCSANOW, &uart_config_original) //Reset the UART flags to original state
├──> close(uart)
├──> device_name = NULL
├──> thread_running = false
└──> return 0C
frsky_telemetry_thread_main
├──> device_name = "/dev/ttyS6"; /* default USART8 */
├──> [frsky状态机及变量初始化]
├──> [frsky telemetry参数解析]
│ ├──> <case 'd'>
│ │ ├──> device_name = myoptarg;
│ │ └──> break;
│ ├──> <case 't'>
│ │ ├──> scanning_timeout_ms = strtoul(myoptarg, nullptr, 10) * 1000;
│ │ └──> break;
│ ├──> <case 'm'>
│ │ ├──> <!strcmp(myoptarg, "sport")>
│ │ │ └──> frsky_state = baudRate = SPORT;
│ │ ├──> <!strcmp(myoptarg, "sport_single")>
│ │ │ └──> frsky_state = baudRate = SPORT_SINGLE_WIRE;
│ │ ├──> <!strcmp(myoptarg, "sport_single_invert")>
│ │ │ └──> frsky_state = baudRate = SPORT_SINGLE_WIRE_INVERT;
│ │ ├──> <!strcmp(myoptarg, "dtype")>
│ │ │ └──> frsky_state = baudRate = DTYPE;
│ │ ├──> <!strcmp(myoptarg, "auto")>
│ │ │ └──> frsky_state = baudRate = DTYPE;
│ │ ├──> < else >
│ │ │ ├──> usage();
│ │ │ └──> return -1;
│ │ └──> break;
│ └──> <default:
│ ├──> usage();
│ └──> return -1;
├──> [Open UART]
│ ├──> const int uart = sPort_open_uart(device_name, &uart_config, &uart_config_original);
│ └──> <uart < 0>
│ ├──> device_name = NULL;
│ └──> return -1;
├──> [poll descriptor]
│ ├──> struct pollfd fds[1];
│ ├──> fds[0].fd = uart;
│ └──> fds[0].events = POLLIN;
├──> thread_running = true;
├──> [Auto Detect FrSky (D8 mode, DTYPE) or SmartPort (D16 mode, SPORT/SPORT_SINGLE_WIRE/SPORT_SINGLE_WIRE_INVERT)]
├──> <frsky_state == SPORT || frsky_state == SPORT_SINGLE_WIRE || frsky_state == SPORT_SINGLE_WIRE_INVERT>
│ ├──> [UART Initialization]
│ ├──> [sPort Topic subscribe]
│ ├──> <while (!thread_should_exit)> // send S.port telemetry
│ │ ├──> [Wait bus master to put 0x7E flag]
│ │ ├──> [Read ID flag for pulling]
│ │ ├──> sPort_update_topics // update subscribed topic
│ │ └──> <switch (ID flag)>
│ │ ├──> <case SMARTPORT_POLL_1><now_ms - lastBATV_ms > 1000> //report BATV at 1Hz
│ │ │ ├──> lastBATV_ms = now_ms;
│ │ │ └──> sPort_send_BATV(uart); //send battery voltage
│ │ ├──> <case SMARTPORT_POLL_2><now_ms - lastCUR_ms > 200> //report battery current at 5Hz
│ │ │ ├──> lastCUR_ms = now_ms;
│ │ │ └──> sPort_send_CUR(uart); //send battery current
│ │ ├──> <case SMARTPORT_POLL_3><now_ms - lastALT_ms > 200> //report altitude at 5Hz
│ │ │ ├──> lastALT_ms = now_ms;
│ │ │ └──> sPort_send_ALT(uart); //send altitude
│ │ ├──> <case SMARTPORT_POLL_4><now_ms - lastSPD_ms > 200> //report speed at 5Hz
│ │ │ ├──> lastSPD_ms = now_ms;
│ │ │ └──> sPort_send_SPD(uart); //send speed
│ │ ├──> <case SMARTPORT_POLL_5><now_ms - lastFUEL_ms > 1000> //report fuel at 1Hz
│ │ │ ├──> lastFUEL_ms = now_ms;
│ │ │ └──> sPort_send_FUEL(uart); //send fuel
│ │ ├──> <case SMARTPORT_POLL_6><now_ms - lastVSPD_ms > 100> //report vertical speed at 10Hz
│ │ │ ├──> uint32_t dt = now_ms - lastVSPD_ms;
│ │ │ ├──> float speed = (filtered_alt - last_baro_alt) / (1e-3f * (float)dt); //estimate vertical speed using first difference and delta t
│ │ │ ├──> last_baro_alt = filtered_alt; //save current alt and timestamp
│ │ │ ├──> lastVSPD_ms = now_ms;
│ │ │ └──> sPort_send_VSPD(uart, speed);
│ │ ├──> <case SMARTPORT_POLL_7><now_ms - lastGPS_ms > 100><switch (elementCount)> //report GPS data elements at 5*5Hz
│ │ │ ├──> <elementCount 0>:sPort_send_GPS_LON(uart);
│ │ │ ├──> <elementCount 1>:sPort_send_GPS_LAT(uart);
│ │ │ ├──> <elementCount 2>:sPort_send_GPS_CRS(uart);
│ │ │ ├──> <elementCount 3>:sPort_send_GPS_ALT(uart);
│ │ │ ├──> <elementCount 4>:sPort_send_GPS_SPD(uart);
│ │ │ └──> <elementCount 5>:sPort_send_GPS_TIME(uart);
│ │ ├──> <case SMARTPORT_POLL_8>
│ │ │ ├──> <now_ms - lastNAV_STATE_ms > 500> // report nav_state as DIY_NAVSTATE 2Hz
│ │ │ │ ├──> lastNAV_STATE_ms = now_ms;
│ │ │ │ └──> sPort_send_NAV_STATE(uart); // send T1
│ │ │ └──> <now_ms - lastGPS_FIX_ms > 500> //report satcount and fix as DIY_GPSFIX at 2Hz
│ │ │ ├──> lastGPS_FIX_ms = now_ms;
│ │ │ └──> sPort_send_GPS_FIX(uart); // send T2
│ │ └──> <case SMARTPORT_SENSOR_ID_SP2UR>
│ │ ├──> <case 0>
│ │ │ └──> sPort_send_flight_mode(uart);
│ │ └──> default:
│ │ └──> sPort_send_GPS_info(uart);
│ ├──> PX4_DEBUG("freeing sPort memory");
│ └──> sPort_deinit();
├──> <frsky_state == DTYPE>
│ ├──> [detected D type telemetry: reconfigure UART]
│ ├──> [frsky_init() Topic subscribe]
│ ├──> <while (!thread_should_exit)> //send D8 mode telemetry
│ │ ├──> [frsky_parse_host, parse incoming data]
│ │ ├──> frsky_update_topics // update subscribed topic
│ │ ├──> <sentPackets++>: frsky_send_frame1(uart) //Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM
│ │ ├──> <sentPackets++>: frsky_send_frame2(uart) //Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level
│ │ └──> <sentPackets++>: frsky_send_frame3(uart); //Send frame 3 (every 5000ms): date, time
│ └──> frsky_deinit();
├──> tcsetattr(uart, TCSANOW, &uart_config_original); //Reset the UART flags to original state
├──> close(uart);
├──> device_name = NULL;
├──> thread_running = false;
└──> return 0;
4. 总结
1. FrSky硬件连接图，如下所示
2. 从模块代码角度，输入输出项如下所示
- 输入
uORB::SubscriptionData<battery_status_s> battery_status_subORB_ID(battery_status);
uORB::SubscriptionData<vehicle_acceleration_s> vehicle_acceleration_subORB_ID(vehicle_acceleration);
uORB::SubscriptionData<vehicle_air_data_s> vehicle_air_data_subORB_ID(vehicle_air_data);
uORB::SubscriptionData<vehicle_local_position_s> vehicle_local_position_subORB_ID(vehicle_local_position);
uORB::SubscriptionData<vehicle_global_position_s> vehicle_global_position_subORB_ID(vehicle_global_position);
uORB::SubscriptionData<vehicle_gps_position_s> vehicle_gps_position_subORB_ID(vehicle_gps_position);
uORB::SubscriptionData<vehicle_status_s> vehicle_status_subORB_ID(vehicle_status);
- 输出
通过串行口输出到Rx模块，最终到遥控Tx模块。
void sPort_send_data(int uart, uint16_t id, uint32_t data)
3. 具体逻辑业务就是期望将飞控上的数据展示到遥控器上，如下图所示
5. 参考资料
【1】PX4开源软件框架简明简介
【2】PX4模块设计之十一：Built-In框架
【3】PX4模块设计之十二：High Resolution Timer设计
【4】PX4模块设计之十三：WorkQueue设计
【5】PX4模块设计之十七：ModuleBase模块
【6】PX4模块设计之三十：Hysteresis类
【7】PX4 modules_main
【8】frsky-protocols-made-simple
以上是关于PX4模块设计之四十：FrskyTelemetry模块的主要内容，如果未能解决你的问题，请参考以下文章