• PX4模块设计之四十：FrskyTelemetry模块

    PX4模块设计之四十：FrskyTelemetry模块

    1. FrskyTelemetry模块简介

    FrSky Telemetry support. Auto-detects D or S.PORT protocol.

frsky_telemetry  [arguments...]
 Commands:
   start
     [-d ]  Select Serial Device
                 values: , default: /dev/ttyS6
     [-t ]  Scanning timeout [s] (default: no timeout)
                 default: 0
     [-m ]  Select protocol (default: auto-detect)
                 values: sport|sport_single|sport_single_invert|dtype, default:
                 auto

   stop

   status

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16

    注1：usage函数是具体对应实现。
    注2：FrskyTelemetry模块采用了纯C语言代码实现。

    2. 模块入口函数

    2.1 主入口frsky_telemetry_main

    模块仅支持start/stop/status命令，不支持其他自定义命令。

    frsky_telemetry_main
 ├──> 
 │   ├──> PX4_ERR("missing command")
 │   ├──> usage()
 │   └──> return -1
 ├──> 
 │   ├──> 
 │   │   ├──> 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)
 │   ├──> 
 │   │   └──> usleep(200)
 │   └──> return 0
 ├──> 
 │   ├──> 
 │   │   ├──> PX4_WARN("frsky_telemetry already stopped")
 │   │   └──> return 0
 │   ├──> thread_should_exit = true
 │   ├──> 
 │   │   ├──> usleep(1000000)
 │   │   └──> PX4_INFO(".")
 │   ├──> PX4_INFO("terminated.")
 │   ├──> device_name = NULL
 │   └──> return 0
 ├──> 
 │   ├──> 
 │   │   ├──> PX4_INFO("running: SCANNING")
 │   │   ├──> PX4_INFO("port: %s", device_name)
 │   │   └──> break
 │   ├──> 
 │   │   ├──> PX4_INFO("running: SPORT")
 │   │   ├──> PX4_INFO("port: %s", device_name)
 │   │   ├──> PX4_INFO("packets sent: %ld", sentPackets)
 │   │   └──> break
 │   ├──> 
 │   │   ├──> PX4_INFO("running: SPORT (single wire)")
 │   │   ├──> PX4_INFO("port: %s", device_name)
 │   │   ├──> PX4_INFO("packets sent: %ld", sentPackets)
 │   │   └──> break
 │   ├──> 
 │   │   ├──> PX4_INFO("running: SPORT (single wire, inverted)")
 │   │   ├──> PX4_INFO("port: %s", device_name)
 │   │   ├──> PX4_INFO("packets sent: %ld", sentPackets)
 │   │   └──> break
 │   ├──> 
 │   │   ├──> PX4_INFO("running: DTYPE")
 │   │   ├──> PX4_INFO("port: %s", device_name)
 │   │   ├──> PX4_INFO("packets sent: %ld", sentPackets)
 │   │   └──> break
 │   └──> return 0
 ├──>   return 0

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54

    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参数解析]
 │   ├──> 
 │   │   ├──> device_name = myoptarg
 │   │   └──> break
 │   ├──> 
 │   │   ├──> scanning_timeout_ms = strtoul(myoptarg, nullptr, 10) * 1000
 │   │   └──> break
 │   ├──> 
 │   │   ├──> 
 │   │   │   └──> frsky_state = baudRate = SPORT
 │   │   ├──> 
 │   │   │   └──> frsky_state = baudRate = SPORT_SINGLE_WIRE
 │   │   ├──> 
 │   │   │   └──> frsky_state = baudRate = SPORT_SINGLE_WIRE_INVERT
 │   │   ├──> 
 │   │   │   └──> frsky_state = baudRate = DTYPE
 │   │   ├──> 
 │   │   │   └──> frsky_state = baudRate = DTYPE
 │   │   ├──> < else >
 │   │   │   ├──> usage()
 │   │   │   └──> return -1
 │   │   └──> break
 │   └──>  usage()
 │       └──> return -1
 ├──> [Open UART]
 │   ├──> const int uart = sPort_open_uart(device_name, &uart_config, &uart_config_original)
 │   └──> 
 │       ├──> 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)]
 ├──> 
 │   ├──> [UART Initialization]
 │   ├──> [sPort Topic subscribe]
 │   ├──>   // send S.port telemetry
 │   │   ├──> [Wait bus master to put 0x7E flag]
 │   │   ├──> [Read ID flag for pulling]
 │   │   ├──> sPort_update_topics  // update subscribed topic
 │   │   └──> 
 │   │       ├──>  1000> //report BATV at 1Hz
 │   │       │   ├──> lastBATV_ms = now_ms
 │   │       │   └──> sPort_send_BATV(uart)  //send battery voltage
 │   │       ├──>  200>  //report battery current at 5Hz
 │   │       │   ├──> lastCUR_ms = now_ms
 │   │       │   └──> sPort_send_CUR(uart)  //send battery current
 │   │       ├──>  200> //report altitude at 5Hz
 │   │       │   ├──> lastALT_ms = now_ms
 │   │       │   └──> sPort_send_ALT(uart) //send altitude
 │   │       ├──>  200> //report speed at 5Hz
 │   │       │   ├──> lastSPD_ms = now_ms
 │   │       │   └──> sPort_send_SPD(uart)  //send speed
 │   │       ├──>  1000> //report fuel at 1Hz
 │   │       │   ├──> lastFUEL_ms = now_ms
 │   │       │   └──> sPort_send_FUEL(uart) //send fuel
 │   │       ├──>  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)
 │   │       ├──>  100>  //report GPS data elements at 5*5Hz
 │   │       │   ├──> :sPort_send_GPS_LON(uart)
 │   │       │   ├──> :sPort_send_GPS_LAT(uart)
 │   │       │   ├──> :sPort_send_GPS_CRS(uart)
 │   │       │   ├──> :sPort_send_GPS_ALT(uart)
 │   │       │   ├──> :sPort_send_GPS_SPD(uart)
 │   │       │   └──> :sPort_send_GPS_TIME(uart)
 │   │       ├──> 
 │   │       │   ├──>  500>  // report nav_state as DIY_NAVSTATE 2Hz
 │   │       │   │   ├──> lastNAV_STATE_ms = now_ms
 │   │       │   │   └──> sPort_send_NAV_STATE(uart)  // send T1
 │   │       │   └──>  500>  //report satcount and fix as DIY_GPSFIX at 2Hz
 │   │       │       ├──> lastGPS_FIX_ms = now_ms
 │   │       │       └──> sPort_send_GPS_FIX(uart)  // send T2
 │   │       └──> 
 │   │           ├──> 
 │   │           │   └──> sPort_send_flight_mode(uart)
 │   │           └──> default:
 │   │               └──> sPort_send_GPS_info(uart)
 │   ├──> PX4_DEBUG("freeing sPort memory")
 │   └──> sPort_deinit()
 ├──> 
 │   ├──> [detected D type telemetry: reconfigure UART]
 │   ├──> [frsky_init() Topic subscribe]
 │   ├──>  //send D8 mode telemetry
 │   │   ├──> [frsky_parse_host, parse incoming data]
 │   │   ├──> frsky_update_topics // update subscribed topic
 │   │   ├──> : frsky_send_frame1(uart)  //Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM
 │   │   ├──> : frsky_send_frame2(uart)  //Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level
 │   │   └──> : 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参数解析]
 │   ├──> 
 │   │   ├──> device_name = myoptarg;
 │   │   └──> break;
 │   ├──> 
 │   │   ├──> scanning_timeout_ms = strtoul(myoptarg, nullptr, 10) * 1000;
 │   │   └──> break;
 │   ├──> 
 │   │   ├──> 
 │   │   │   └──> frsky_state = baudRate = SPORT;
 │   │   ├──> 
 │   │   │   └──> frsky_state = baudRate = SPORT_SINGLE_WIRE;
 │   │   ├──> 
 │   │   │   └──> frsky_state = baudRate = SPORT_SINGLE_WIRE_INVERT;
 │   │   ├──> 
 │   │   │   └──> frsky_state = baudRate = DTYPE;
 │   │   ├──> 
 │   │   │   └──> frsky_state = baudRate = DTYPE;
 │   │   ├──> < else >
 │   │   │   ├──> usage();
 │   │   │   └──> return -1;
 │   │   └──> break;
 │   └──>  usage();
 │       └──> return -1;
 ├──> [Open UART]
 │   ├──> const int uart = sPort_open_uart(device_name, &uart_config, &uart_config_original);
 │   └──> 
 │       ├──> 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)]
 ├──> 
 │   ├──> [UART Initialization]
 │   ├──> [sPort Topic subscribe]
 │   ├──>   // send S.port telemetry
 │   │   ├──> [Wait bus master to put 0x7E flag]
 │   │   ├──> [Read ID flag for pulling]
 │   │   ├──> sPort_update_topics  // update subscribed topic
 │   │   └──> 
 │   │       ├──>  1000> //report BATV at 1Hz
 │   │       │   ├──> lastBATV_ms = now_ms;
 │   │       │   └──> sPort_send_BATV(uart);  //send battery voltage
 │   │       ├──>  200>  //report battery current at 5Hz
 │   │       │   ├──> lastCUR_ms = now_ms;
 │   │       │   └──> sPort_send_CUR(uart);  //send battery current
 │   │       ├──>  200> //report altitude at 5Hz
 │   │       │   ├──> lastALT_ms = now_ms;
 │   │       │   └──> sPort_send_ALT(uart); //send altitude
 │   │       ├──>  200> //report speed at 5Hz
 │   │       │   ├──> lastSPD_ms = now_ms;
 │   │       │   └──> sPort_send_SPD(uart);  //send speed
 │   │       ├──>  1000> //report fuel at 1Hz
 │   │       │   ├──> lastFUEL_ms = now_ms;
 │   │       │   └──> sPort_send_FUEL(uart); //send fuel
 │   │       ├──>  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);
 │   │       ├──>  100>  //report GPS data elements at 5*5Hz
 │   │       │   ├──> :sPort_send_GPS_LON(uart);
 │   │       │   ├──> :sPort_send_GPS_LAT(uart);
 │   │       │   ├──> :sPort_send_GPS_CRS(uart);
 │   │       │   ├──> :sPort_send_GPS_ALT(uart);
 │   │       │   ├──> :sPort_send_GPS_SPD(uart);
 │   │       │   └──> :sPort_send_GPS_TIME(uart);
 │   │       ├──> 
 │   │       │   ├──>  500>  // report nav_state as DIY_NAVSTATE 2Hz
 │   │       │   │   ├──> lastNAV_STATE_ms = now_ms;
 │   │       │   │   └──> sPort_send_NAV_STATE(uart);  // send T1
 │   │       │   └──>  500>  //report satcount and fix as DIY_GPSFIX at 2Hz
 │   │       │       ├──> lastGPS_FIX_ms = now_ms;
 │   │       │       └──> sPort_send_GPS_FIX(uart);  // send T2
 │   │       └──> 
 │   │           ├──> 
 │   │           │   └──> sPort_send_flight_mode(uart);
 │   │           └──> default:
 │   │               └──> sPort_send_GPS_info(uart);
 │   ├──> PX4_DEBUG("freeing sPort memory");
 │   └──> sPort_deinit();
 ├──> 
 │   ├──> [detected D type telemetry: reconfigure UART]
 │   ├──> [frsky_init() Topic subscribe]
 │   ├──>  //send D8 mode telemetry
 │   │   ├──> [frsky_parse_host, parse incoming data]
 │   │   ├──> frsky_update_topics // update subscribed topic
 │   │   ├──> : frsky_send_frame1(uart)  //Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM
 │   │   ├──> : frsky_send_frame2(uart)  //Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level
 │   │   └──> : 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;

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58
    • 59
    • 60
    • 61
    • 62
    • 63
    • 64
    • 65
    • 66
    • 67
    • 68
    • 69
    • 70
    • 71
    • 72
    • 73
    • 74
    • 75
    • 76
    • 77
    • 78
    • 79
    • 80
    • 81
    • 82
    • 83
    • 84
    • 85
    • 86
    • 87
    • 88
    • 89
    • 90
    • 91
    • 92
    • 93
    • 94
    • 95
    • 96
    • 97
    • 98
    • 99
    • 100
    • 101
    • 102
    • 103
    • 104
    • 105
    • 106
    • 107
    • 108
    • 109
    • 110
    • 111
    • 112
    • 113
    • 114
    • 115
    • 116
    • 117
    • 118
    • 119
    • 120
    • 121
    • 122
    • 123
    • 124
    • 125
    • 126
    • 127
    • 128
    • 129
    • 130
    • 131
    • 132
    • 133
    • 134
    • 135
    • 136
    • 137
    • 138
    • 139
    • 140
    • 141
    • 142
    • 143
    • 144
    • 145
    • 146
    • 147
    • 148
    • 149
    • 150
    • 151
    • 152
    • 153
    • 154
    • 155
    • 156
    • 157
    • 158
    • 159
    • 160
    • 161
    • 162
    • 163
    • 164
    • 165
    • 166
    • 167
    • 168
    • 169
    • 170
    • 171
    • 172
    • 173
    • 174
    • 175
    • 176
    • 177
    • 178
    • 179
    • 180
    • 181
    • 182
    • 183
    • 184
    • 185
    • 186
    • 187
    • 188
    • 189
    • 190
    • 191
    • 192
    • 193
    • 194
    • 195
    • 196
    • 197
    • 198
    • 199
    • 200
    • 201
    • 202
    • 203
    • 204
    • 205
    • 206
    • 207
    • 208

    4. 总结

    1. FrSky硬件连接图，如下所示
    FrSky Rx硬件连接图

    2. 从模块代码角度，输入输出项如下所示

    • 输入
    uORB::SubscriptionData battery_status_sub{ORB_ID(battery_status)};
uORB::SubscriptionData vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
uORB::SubscriptionData vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
uORB::SubscriptionData vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::SubscriptionData vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
uORB::SubscriptionData vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
uORB::SubscriptionData vehicle_status_sub{ORB_ID(vehicle_status)};

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 输出
      通过串行口输出到Rx模块，最终到遥控Tx模块。
    void sPort_send_data(int uart, uint16_t id, uint32_t data)

    • 1

    3. 具体逻辑业务就是期望将飞控上的数据展示到遥控器上，如下图所示
    遥控Tx显示数据

    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

  • 相关阅读:
    三防加固平板在房地产行业的应用｜亿道三防onerugged
    第四代智能井盖传感器：万宾科技助力城市安全
    第二十一章《万年历》第2节：系统功能实现
    CountDownLatch 使用例子和代码流程
    （C语言）一元多项式的乘法与加法运算
    docker 安装kafka
    JavaScript中常用的输入输出语句介绍
    GBase 8c核心特性-算子下推
    排序篇--插入排序及希尔排序
    如何做好软件测试管理的工作呢？
  • 原文地址：https://blog.csdn.net/lida2003/article/details/126869090