Merge branch 'radioconf' into beta

This commit is contained in:
Lorenz Meier
2015-06-30 15:26:26 +02:00
4 changed files with 72 additions and 10 deletions
+1 -1
View File
@@ -483,7 +483,7 @@ then
fi
if param compare SYS_COMPANION 57600
then
mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 60000 -x
mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 5000 -x
fi
if param compare SYS_COMPANION 157600
then
+21 -9
View File
@@ -33,9 +33,9 @@
/**
* @file mavlink.c
* Adapter functions expected by the protocol library
* Define MAVLink specific parameters
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Lorenz Meier <lorenz@px4.io>
*/
#include <nuttx/config.h>
@@ -46,7 +46,6 @@
#include "mavlink_bridge_header.h"
#include <systemlib/param/param.h>
/* define MAVLink specific parameters */
/**
* MAVLink system ID
* @group MAVLink
@@ -63,19 +62,32 @@ PARAM_DEFINE_INT32(MAV_SYS_ID, 1);
*/
PARAM_DEFINE_INT32(MAV_COMP_ID, 50);
/**
* MAVLink Radio ID
*
* When non-zero the MAVLink app will attempt to configure the
* radio to this ID and re-set the parameter to 0. If the value
* is negative it will reset the complete radio config to
* factory defaults.
*
* @group MAVLink
* @min -1
* @max 240
*/
PARAM_DEFINE_INT32(MAV_RADIO_ID, 0);
/**
* MAVLink airframe type
*
*
* @min 0
* @min 1
* @group MAVLink
*/
PARAM_DEFINE_INT32(MAV_TYPE, 1);
/**
* Use/Accept HIL GPS message (even if not in HIL mode)
* Use/Accept HIL GPS message even if not in HIL mode
*
* If set to 1 incomming HIL GPS messages are parsed.
* If set to 1 incoming HIL GPS messages are parsed.
*
* @group MAVLink
*/
@@ -84,8 +96,8 @@ PARAM_DEFINE_INT32(MAV_USEHILGPS, 0);
/**
* Forward external setpoint messages
*
* If set to 1 incomming external setpoint messages will be directly forwarded to the controllers if in offboard
* control mode
* If set to 1 incoming external setpoint messages will be directly forwarded
* to the controllers if in offboard control mode
*
* @group MAVLink
*/
+48
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,50 @@ 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);
// 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("open fd %d 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);
+2
View File
@@ -324,6 +324,7 @@ private:
MAVLINK_MODE _mode;
mavlink_channel_t _channel;
int32_t _radio_id;
struct mavlink_logbuffer _logbuffer;
unsigned int _total_counter;
@@ -381,6 +382,7 @@ private:
bool _param_initialized;
param_t _param_system_id;
param_t _param_component_id;
param_t _param_radio_id;
param_t _param_system_type;
param_t _param_use_hil_gps;
param_t _param_forward_externalsp;