From df7c060bba7087b62bcfba3968e92561699835e6 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Thu, 25 Nov 2010 23:41:05 -0500 Subject: [PATCH] Added datarate spin box's --- src/uas/UAS.cc | 60 ++++++++-------- src/uas/UAS.h | 20 +++--- src/uas/UASInterface.h | 2 +- src/ui/QGCSensorSettingsWidget.cc | 17 ++--- src/ui/QGCSensorSettingsWidget.ui | 110 +++++++++++++++++++++--------- 5 files changed, 128 insertions(+), 81 deletions(-) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 1ba2516360..30ce495af6 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -883,7 +883,7 @@ void UAS::readParametersFromStorage() sendMessage(msg); } -void UAS::enableAllDataTransmission(bool enabled) +void UAS::enableAllDataTransmission(int rate) { // Buffers to write data to mavlink_message_t msg; @@ -893,9 +893,11 @@ void UAS::enableAllDataTransmission(bool enabled) stream.req_stream_id = MAV_DATA_STREAM_ALL; // Select the update rate in Hz the message should be send // All messages will be send with their default rate + // TODO: use 0 to turn off and get rid of enable/disable? will require + // a different magic flag for turning on defaults, possibly something really high like 1111 ? stream.req_message_rate = 0; // Start / stop the message - stream.start_stop = (enabled) ? 1 : 0; + stream.start_stop = (rate) ? 1 : 0; // The system which should take this command stream.target_system = uasId; // The component / subsystem which should take this command @@ -907,7 +909,7 @@ void UAS::enableAllDataTransmission(bool enabled) sendMessage(msg); } -void UAS::enableRawSensorDataTransmission(bool enabled) +void UAS::enableRawSensorDataTransmission(int rate) { // Buffers to write data to mavlink_message_t msg; @@ -915,9 +917,9 @@ void UAS::enableRawSensorDataTransmission(bool enabled) // Select the message to request from now on stream.req_stream_id = MAV_DATA_STREAM_RAW_SENSORS; // Select the update rate in Hz the message should be send - stream.req_message_rate = 200; + stream.req_message_rate = rate; // Start / stop the message - stream.start_stop = (enabled) ? 1 : 0; + stream.start_stop = (rate) ? 1 : 0; // The system which should take this command stream.target_system = uasId; // The component / subsystem which should take this command @@ -929,7 +931,7 @@ void UAS::enableRawSensorDataTransmission(bool enabled) sendMessage(msg); } -void UAS::enableExtendedSystemStatusTransmission(bool enabled) +void UAS::enableExtendedSystemStatusTransmission(int rate) { // Buffers to write data to mavlink_message_t msg; @@ -937,9 +939,9 @@ void UAS::enableExtendedSystemStatusTransmission(bool enabled) // Select the message to request from now on stream.req_stream_id = MAV_DATA_STREAM_EXTENDED_STATUS; // Select the update rate in Hz the message should be send - stream.req_message_rate = 10; + stream.req_message_rate = rate; // Start / stop the message - stream.start_stop = (enabled) ? 1 : 0; + stream.start_stop = (rate) ? 1 : 0; // The system which should take this command stream.target_system = uasId; // The component / subsystem which should take this command @@ -951,7 +953,7 @@ void UAS::enableExtendedSystemStatusTransmission(bool enabled) sendMessage(msg); } -void UAS::enableRCChannelDataTransmission(bool enabled) +void UAS::enableRCChannelDataTransmission(int rate) { #if defined(MAVLINK_ENABLED_UALBERTA_MESSAGES) mavlink_message_t msg; @@ -963,9 +965,9 @@ void UAS::enableRCChannelDataTransmission(bool enabled) // Select the message to request from now on stream.req_stream_id = MAV_DATA_STREAM_RC_CHANNELS; // Select the update rate in Hz the message should be send - stream.req_message_rate = 200; + stream.req_message_rate = rate; // Start / stop the message - stream.start_stop = (enabled) ? 1 : 0; + stream.start_stop = (rate) ? 1 : 0; // The system which should take this command stream.target_system = uasId; // The component / subsystem which should take this command @@ -978,7 +980,7 @@ void UAS::enableRCChannelDataTransmission(bool enabled) #endif } -void UAS::enableRawControllerDataTransmission(bool enabled) +void UAS::enableRawControllerDataTransmission(int rate) { // Buffers to write data to mavlink_message_t msg; @@ -986,9 +988,9 @@ void UAS::enableRawControllerDataTransmission(bool enabled) // Select the message to request from now on stream.req_stream_id = MAV_DATA_STREAM_RAW_CONTROLLER; // Select the update rate in Hz the message should be send - stream.req_message_rate = 200; + stream.req_message_rate = rate; // Start / stop the message - stream.start_stop = (enabled) ? 1 : 0; + stream.start_stop = (rate) ? 1 : 0; // The system which should take this command stream.target_system = uasId; // The component / subsystem which should take this command @@ -1000,7 +1002,7 @@ void UAS::enableRawControllerDataTransmission(bool enabled) sendMessage(msg); } -void UAS::enableRawSensorFusionTransmission(bool enabled) +void UAS::enableRawSensorFusionTransmission(int rate) { // Buffers to write data to mavlink_message_t msg; @@ -1008,9 +1010,9 @@ void UAS::enableRawSensorFusionTransmission(bool enabled) // Select the message to request from now on stream.req_stream_id = MAV_DATA_STREAM_RAW_SENSOR_FUSION; // Select the update rate in Hz the message should be send - stream.req_message_rate = 200; + stream.req_message_rate = rate; // Start / stop the message - stream.start_stop = (enabled) ? 1 : 0; + stream.start_stop = (rate) ? 1 : 0; // The system which should take this command stream.target_system = uasId; // The component / subsystem which should take this command @@ -1022,7 +1024,7 @@ void UAS::enableRawSensorFusionTransmission(bool enabled) sendMessage(msg); } -void UAS::enablePositionTransmission(bool enabled) +void UAS::enablePositionTransmission(int rate) { // Buffers to write data to mavlink_message_t msg; @@ -1030,9 +1032,9 @@ void UAS::enablePositionTransmission(bool enabled) // Select the message to request from now on stream.req_stream_id = MAV_DATA_STREAM_POSITION; // Select the update rate in Hz the message should be send - stream.req_message_rate = 200; + stream.req_message_rate = rate; // Start / stop the message - stream.start_stop = (enabled) ? 1 : 0; + stream.start_stop = (rate) ? 1 : 0; // The system which should take this command stream.target_system = uasId; // The component / subsystem which should take this command @@ -1044,7 +1046,7 @@ void UAS::enablePositionTransmission(bool enabled) sendMessage(msg); } -void UAS::enableExtra1Transmission(bool enabled) +void UAS::enableExtra1Transmission(int rate) { // Buffers to write data to mavlink_message_t msg; @@ -1052,9 +1054,9 @@ void UAS::enableExtra1Transmission(bool enabled) // Select the message to request from now on stream.req_stream_id = MAV_DATA_STREAM_EXTRA1; // Select the update rate in Hz the message should be send - stream.req_message_rate = 200; + stream.req_message_rate = rate; // Start / stop the message - stream.start_stop = (enabled) ? 1 : 0; + stream.start_stop = (rate) ? 1 : 0; // The system which should take this command stream.target_system = uasId; // The component / subsystem which should take this command @@ -1066,7 +1068,7 @@ void UAS::enableExtra1Transmission(bool enabled) sendMessage(msg); } -void UAS::enableExtra2Transmission(bool enabled) +void UAS::enableExtra2Transmission(int rate) { // Buffers to write data to mavlink_message_t msg; @@ -1074,9 +1076,9 @@ void UAS::enableExtra2Transmission(bool enabled) // Select the message to request from now on stream.req_stream_id = MAV_DATA_STREAM_EXTRA2; // Select the update rate in Hz the message should be send - stream.req_message_rate = 200; + stream.req_message_rate = rate; // Start / stop the message - stream.start_stop = (enabled) ? 1 : 0; + stream.start_stop = (rate) ? 1 : 0; // The system which should take this command stream.target_system = uasId; // The component / subsystem which should take this command @@ -1088,7 +1090,7 @@ void UAS::enableExtra2Transmission(bool enabled) sendMessage(msg); } -void UAS::enableExtra3Transmission(bool enabled) +void UAS::enableExtra3Transmission(int rate) { // Buffers to write data to mavlink_message_t msg; @@ -1096,9 +1098,9 @@ void UAS::enableExtra3Transmission(bool enabled) // Select the message to request from now on stream.req_stream_id = MAV_DATA_STREAM_EXTRA3; // Select the update rate in Hz the message should be send - stream.req_message_rate = 200; + stream.req_message_rate = rate; // Start / stop the message - stream.start_stop = (enabled) ? 1 : 0; + stream.start_stop = (rate) ? 1 : 0; // The system which should take this command stream.target_system = uasId; // The component / subsystem which should take this command diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 9c2ef9a3be..da8c44c0e8 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -223,16 +223,16 @@ public slots: /** @brief Read parameters from permanent storage */ void readParametersFromStorage(); - void enableAllDataTransmission(bool enabled); - void enableRawSensorDataTransmission(bool enabled); - void enableExtendedSystemStatusTransmission(bool enabled); - void enableRCChannelDataTransmission(bool enabled); - void enableRawControllerDataTransmission(bool enabled); - void enableRawSensorFusionTransmission(bool enabled); - void enablePositionTransmission(bool enabled); - void enableExtra1Transmission(bool enabled); - void enableExtra2Transmission(bool enabled); - void enableExtra3Transmission(bool enabled); + void enableAllDataTransmission(int rate); + void enableRawSensorDataTransmission(int rate); + void enableExtendedSystemStatusTransmission(int rate); + void enableRCChannelDataTransmission(int rate); + void enableRawControllerDataTransmission(int rate); + void enableRawSensorFusionTransmission(int rate); + void enablePositionTransmission(int rate); + void enableExtra1Transmission(int rate); + void enableExtra2Transmission(int rate); + void enableExtra3Transmission(int rate); /** @brief Update the system state */ void updateState(); diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index c044997aad..eb10fcaf4a 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -221,7 +221,7 @@ public slots: virtual void setSelected() = 0; virtual void enableAllDataTransmission(bool enabled) = 0; - virtual void enableRawSensorDataTransmission(bool enabled) = 0; + virtual void enableRawSensorDataTransmission(bool enabled, int rate = 0) = 0; virtual void enableExtendedSystemStatusTransmission(bool enabled) = 0; virtual void enableRCChannelDataTransmission(bool enabled) = 0; virtual void enableRawControllerDataTransmission(bool enabled) = 0; diff --git a/src/ui/QGCSensorSettingsWidget.cc b/src/ui/QGCSensorSettingsWidget.cc index 2842116349..68b55acfc8 100644 --- a/src/ui/QGCSensorSettingsWidget.cc +++ b/src/ui/QGCSensorSettingsWidget.cc @@ -37,14 +37,15 @@ QGCSensorSettingsWidget::QGCSensorSettingsWidget(UASInterface* uas, QWidget *par ui(new Ui::QGCSensorSettingsWidget) { ui->setupUi(this); - connect(ui->sendRawCheckBox, SIGNAL(toggled(bool)), mav, SLOT(enableRawSensorDataTransmission(bool))); - connect(ui->sendControllerCheckBox, SIGNAL(toggled(bool)), mav, SLOT(enableRawControllerDataTransmission(bool))); - connect(ui->sendExtendedCheckBox, SIGNAL(toggled(bool)), mav, SLOT(enableExtendedSystemStatusTransmission(bool))); - connect(ui->sendRCCheckBox, SIGNAL(toggled(bool)), mav, SLOT(enableRCChannelDataTransmission(bool))); - connect(ui->sendPositionCheckBox, SIGNAL(toggled(bool)), mav, SLOT(enablePositionTransmission(bool))); - connect(ui->sendExtra1CheckBox, SIGNAL(toggled(bool)), mav, SLOT(enableExtra1Transmission(bool))); - connect(ui->sendExtra2CheckBox, SIGNAL(toggled(bool)), mav, SLOT(enableExtra2Transmission(bool))); - connect(ui->sendExtra3CheckBox, SIGNAL(toggled(bool)), mav, SLOT(enableExtra3Transmission(bool))); + // XXX: This might be a bad idea sending a message every time the value changes + connect(ui->spinBox_rawSensor, SIGNAL(valueChanged(int)), mav, SLOT(enableRawSensorDataTransmission(int))); + connect(ui->spinBox_controller, SIGNAL(valueChanged(int)), mav, SLOT(enableRawControllerDataTransmission(int))); + connect(ui->spinBox_extended, SIGNAL(valueChanged(int)), mav, SLOT(enableExtendedSystemStatusTransmission(int))); + connect(ui->spinBox_rc, SIGNAL(valueChanged(int)), mav, SLOT(enableRCChannelDataTransmission(int))); + connect(ui->spinBox_position, SIGNAL(valueChanged(int)), mav, SLOT(enablePositionTransmission(int))); + connect(ui->spinBox_extra1, SIGNAL(valueChanged(int)), mav, SLOT(enableExtra1Transmission(int))); + connect(ui->spinBox_extra2, SIGNAL(valueChanged(int)), mav, SLOT(enableExtra2Transmission(int))); + connect(ui->spinBox_extra3, SIGNAL(valueChanged(int)), mav, SLOT(enableExtra3Transmission(int))); // Calibration connect(ui->rcCalButton, SIGNAL(clicked()), mav, SLOT(startRadioControlCalibration())); diff --git a/src/ui/QGCSensorSettingsWidget.ui b/src/ui/QGCSensorSettingsWidget.ui index a80d959e30..b475dda88e 100644 --- a/src/ui/QGCSensorSettingsWidget.ui +++ b/src/ui/QGCSensorSettingsWidget.ui @@ -6,7 +6,7 @@ 0 0 - 231 + 237 221 @@ -75,7 +75,7 @@ - Activate Extended Output + Data Stream Rates (Hz) @@ -97,70 +97,114 @@ 2 - - - true + + + 10000 + + + + - RAW Sensors + Raw Sensor - - false + + + + + + 10000 - - false + + + + + + 10000 - - - - true + + + + 10000 + + + + - RC Values + Extended - - + + - Position CTRL + Position - - + + - Attitude CTRL + Controller - - + + + + 10000 + + + + + + + 10000 + + + + + + + 10000 + + + + + + + 10000 + + + + + - Send Extra1 + RC Values - - + + - Attitude + Extra 1 - - + + - Send Extra2 + Extra 2 - - + + - Send Extra3 + Extra 3 -- GitLab