Line data    Source code

       1             : /*
       2             :   MAVLink SERIAL_CONTROL handling
       3             :  */
       4             : 
       5             : /*
       6             :    This program is free software: you can redistribute it and/or modify
       7             :    it under the terms of the GNU General Public License as published by
       8             :    the Free Software Foundation, either version 3 of the License, or
       9             :    (at your option) any later version.
      10             : 
      11             :    This program is distributed in the hope that it will be useful,
      12             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
      13             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      14             :    GNU General Public License for more details.
      15             : 
      16             :    You should have received a copy of the GNU General Public License
      17             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      18             :  */
      19             : 
      20             : #include "GCS_config.h"
      21             : 
      22             : #if AP_MAVLINK_MSG_SERIAL_CONTROL_ENABLED
      23             : 
      24             : #include <AP_HAL/AP_HAL.h>
      25             : #include "GCS.h"
      26             : #include <AP_GPS/AP_GPS.h>
      27             : 
      28             : extern const AP_HAL::HAL& hal;
      29             : 
      30             : /**
      31             :    handle a SERIAL_CONTROL message
      32             :  */
      33           0 : void GCS_MAVLINK::handle_serial_control(const mavlink_message_t &msg)
      34             : {
      35             :     mavlink_serial_control_t packet;
      36           0 :     mavlink_msg_serial_control_decode(&msg, &packet);
      37             : 
      38           0 :     AP_HAL::UARTDriver *port = nullptr;
      39           0 :     AP_HAL::BetterStream *stream = nullptr;
      40             : 
      41           0 :     if (packet.flags & SERIAL_CONTROL_FLAG_REPLY) {
      42             :         // how did this packet get to us?
      43           0 :         return;
      44             :     }
      45             : 
      46           0 :     bool exclusive = (packet.flags & SERIAL_CONTROL_FLAG_EXCLUSIVE) != 0;
      47             : 
      48           0 :     switch (packet.device) {
      49           0 :     case SERIAL_CONTROL_DEV_TELEM1: {
      50           0 :         GCS_MAVLINK *link = gcs().chan(1);
      51           0 :         if (link == nullptr) {
      52           0 :             break;
      53             :         }
      54           0 :         stream = port = link->get_uart();
      55           0 :         link->lock(exclusive);
      56           0 :         break;
      57             :     }
      58           0 :     case SERIAL_CONTROL_DEV_TELEM2: {
      59           0 :         GCS_MAVLINK *link = gcs().chan(2);
      60           0 :         if (link == nullptr) {
      61           0 :             break;
      62             :         }
      63           0 :         stream = port = link->get_uart();
      64           0 :         link->lock(exclusive);
      65           0 :         break;
      66             :     }
      67             : #if AP_GPS_ENABLED
      68           0 :     case SERIAL_CONTROL_DEV_GPS1:
      69           0 :         stream = port = hal.serial(3);
      70           0 :         AP::gps().lock_port(0, exclusive);
      71           0 :         break;
      72           0 :     case SERIAL_CONTROL_DEV_GPS2:
      73           0 :         stream = port = hal.serial(4);
      74           0 :         AP::gps().lock_port(1, exclusive);
      75           0 :         break;
      76             : #endif  // AP_GPS_ENABLED
      77           0 :     case SERIAL_CONTROL_SERIAL0 ... SERIAL_CONTROL_SERIAL9: {
      78             :         // direct access to a SERIALn port
      79           0 :         stream = port = AP::serialmanager().get_serial_by_id(packet.device - SERIAL_CONTROL_SERIAL0);
      80             : 
      81             :         // see if we need to lock mavlink
      82           0 :         for (uint8_t i=0; i<gcs().num_gcs(); i++) {
      83           0 :             GCS_MAVLINK *link = gcs().chan(i);
      84           0 :             if (link == nullptr || link->get_uart() != port) {
      85           0 :                 continue;
      86             :             }
      87           0 :             link->lock(exclusive);
      88           0 :             break;
      89             :         }
      90           0 :         break;
      91             :     }
      92             : 
      93           0 :     default:
      94             :         // not supported yet
      95           0 :         return;
      96             :     }
      97           0 :     if (stream == nullptr) {
      98             :         // this is probably very bad
      99             : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     100           0 :         AP_HAL::panic("stream is nullptr");
     101             : #endif
     102           0 :         return;
     103             :     }
     104             : 
     105           0 :     if (exclusive && port != nullptr) {
     106             :         // force flow control off for exclusive access. This protocol
     107             :         // is used to talk to bootloaders which may not have flow
     108             :         // control support
     109           0 :         port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
     110             :     }
     111             : 
     112             :     // optionally change the baudrate
     113           0 :     if (packet.baudrate != 0 && port != nullptr) {
     114           0 :         port->begin(packet.baudrate);
     115             :     }
     116             : 
     117             :     // write the data
     118           0 :     if (packet.count != 0) {
     119           0 :         if (packet.count > sizeof(packet.data)) {
     120           0 :             packet.count = sizeof(packet.data);
     121             :         }
     122             : 
     123           0 :         if ((packet.flags & SERIAL_CONTROL_FLAG_BLOCKING) == 0) {
     124           0 :             stream->write(packet.data, packet.count);
     125             :         } else {
     126           0 :             const uint8_t *data = &packet.data[0];
     127           0 :             uint8_t count = packet.count;
     128           0 :             while (count > 0) {
     129           0 :                 while (stream->txspace() <= 0) {
     130           0 :                     hal.scheduler->delay(5);
     131             :                 }
     132           0 :                 uint16_t n = stream->txspace();
     133           0 :                 if (n > count) {
     134           0 :                     n = count;
     135             :                 }
     136           0 :                 stream->write(data, n);
     137           0 :                 data += n;
     138           0 :                 count -= n;
     139             :             }
     140             :         }
     141             :     }
     142             : 
     143           0 :     if ((packet.flags & SERIAL_CONTROL_FLAG_RESPOND) == 0) {
     144             :         // no response expected
     145           0 :         return;
     146             :     }
     147             : 
     148           0 :     uint8_t flags = packet.flags;
     149             : 
     150           0 : more_data:
     151             :     // sleep for the timeout
     152           0 :     while (packet.timeout != 0 && 
     153           0 :            stream->available() < (int16_t)sizeof(packet.data)) {
     154           0 :         hal.scheduler->delay(1);
     155           0 :         packet.timeout--;
     156             :     }
     157             : 
     158           0 :     packet.flags = SERIAL_CONTROL_FLAG_REPLY;
     159             : 
     160             :     // work out how many bytes are available
     161           0 :     int16_t available = stream->available();
     162           0 :     if (available < 0) {
     163           0 :         available = 0;
     164             :     }
     165           0 :     if (available > (int16_t)sizeof(packet.data)) {
     166           0 :         available = sizeof(packet.data);
     167             :     }
     168           0 :     if (available == 0 && (flags & SERIAL_CONTROL_FLAG_BLOCKING) == 0) {
     169           0 :         return;
     170             :     }
     171             : 
     172           0 :     if (packet.flags & SERIAL_CONTROL_FLAG_BLOCKING) {
     173           0 :         while (!HAVE_PAYLOAD_SPACE(chan, SERIAL_CONTROL)) {
     174           0 :             hal.scheduler->delay(1);
     175             :         }
     176             :     } else {
     177           0 :         if (!HAVE_PAYLOAD_SPACE(chan, SERIAL_CONTROL)) {
     178             :             // no space for reply
     179           0 :             return;
     180             :         }
     181             :     }
     182             : 
     183             :     // read any reply data
     184           0 :     packet.count = 0;
     185           0 :     memset(packet.data, 0, sizeof(packet.data));
     186           0 :     while (available > 0) {
     187           0 :         packet.data[packet.count++] = (uint8_t)stream->read();
     188           0 :         available--;
     189             :     }
     190             : 
     191             :     // and send the reply
     192           0 :     _mav_finalize_message_chan_send(chan, 
     193             :                                     MAVLINK_MSG_ID_SERIAL_CONTROL,
     194             :                                     (const char *)&packet,
     195             :                                     MAVLINK_MSG_ID_SERIAL_CONTROL_MIN_LEN,
     196             :                                     MAVLINK_MSG_ID_SERIAL_CONTROL_LEN,
     197             :                                     MAVLINK_MSG_ID_SERIAL_CONTROL_CRC);
     198           0 :     if ((flags & SERIAL_CONTROL_FLAG_MULTI) && packet.count != 0) {
     199           0 :         if (flags & SERIAL_CONTROL_FLAG_BLOCKING) {
     200           0 :             hal.scheduler->delay(1);
     201             :         }
     202           0 :         goto more_data;
     203             :     }
     204             : }
     205             : 
     206             : #endif  // AP_MAVLINK_MSG_SERIAL_CONTROL_ENABLED