yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

MAVLink app: Support rudimentary radio config.

Browse Source
This commit is contained in:
Lorenz Meier
2015-06-30 13:21:09 +02:00
parent 319f9d820f
commit 963972721d
3 changed files with 67 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

49
src/modules/mavlink/mavlink_main.cpp
View File

@@ -137,6 +137,7 @@ Mavlink::Mavlink() :
_mavlink_ftp(nullptr),
_mode(MAVLINK_MODE_NORMAL),
_channel(MAVLINK_COMM_0),
_radio_id(0),
_logbuffer {},
_total_counter(0),
_receive_thread {},
@@ -170,6 +171,7 @@ Mavlink::Mavlink() :
_param_initialized(false),
_param_system_id(0),
_param_component_id(0),
_param_radio_id(0),
_param_system_type(MAV_TYPE_FIXED_WING),
_param_use_hil_gps(0),
_param_forward_externalsp(0),
@@ -489,6 +491,7 @@ void Mavlink::mavlink_update_system(void)
if (!_param_initialized) {
_param_system_id = param_find("MAV_SYS_ID");
_param_component_id = param_find("MAV_COMP_ID");
_param_radio_id = param_find("MAV_RADIO_ID");
_param_system_type = param_find("MAV_TYPE");
_param_use_hil_gps = param_find("MAV_USEHILGPS");
_param_forward_externalsp = param_find("MAV_FWDEXTSP");
@@ -504,6 +507,7 @@ void Mavlink::mavlink_update_system(void)
int32_t component_id;
param_get(_param_component_id, &component_id);
param_get(_param_radio_id, &_radio_id);
/* only allow system ID and component ID updates
* after reboot - not during operation */
@@ -1508,6 +1512,51 @@ Mavlink::task_main(int argc, char *argv[])
mavlink_update_system();
}
/* radio config check */
if (_radio_id != 0 && _rstatus.type == TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) {
/* request to configure radio and radio is present */
FILE *fs = fdopen(_uart_fd, "w");
if (fs) {
/* switch to AT command mode */
usleep(1200000);
fprintf(fs, "+++\n");
usleep(1200000);
if (_radio_id > 0) {
/* set channel */
fprintf(fs, "ATS3=%u\n", _radio_id);
usleep(200000);
} else {
/* reset to factory defaults */
fprintf(fs, "AT&F\n");
usleep(200000);
}
/* write config */
fprintf(fs, "AT&W");
usleep(200000);
/* reboot */
fprintf(fs, "ATZ");
usleep(200000);
warnx("configured radio");
// XXX NuttX suffers from a bug where
// fclose() also closes the fd, not just
// the file stream. Since this is a one-time
// config thing, we leave the file struct
// allocated.
//fclose(fs);
} else {
warnx("opening %d as file failed", _uart_fd);
}
/* reset param and save */
_radio_id = 0;
param_set(_param_radio_id, &_radio_id);
}
if (status_sub->update(&status_time, &status)) {
/* switch HIL mode if required */
set_hil_enabled(status.hil_state == vehicle_status_s::HIL_STATE_ON);