PX4 Firmware
PX4 Autopilot Software http://px4.io
px4io_serial.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file px4io_serial.cpp
36  *
37  * Serial interface for PX4IO
38  */
39 
40 #include "px4io_driver.h"
41 
42 #include <px4_arch/px4io_serial.h>
43 
44 static PX4IO_serial *g_interface;
45 
48 {
49  return new ArchPX4IOSerial();
50 }
51 
52 PX4IO_serial::PX4IO_serial() :
53  Device("PX4IO_serial"),
54  _pc_txns(perf_alloc(PC_ELAPSED, "io_txns")),
55 #if 0
56  _pc_retries(perf_alloc(PC_COUNT, "io_retries ")),
57  _pc_timeouts(perf_alloc(PC_COUNT, "io_timeouts ")),
58  _pc_crcerrs(perf_alloc(PC_COUNT, "io_crcerrs ")),
59  _pc_protoerrs(perf_alloc(PC_COUNT, "io_protoerrs")),
60  _pc_uerrs(perf_alloc(PC_COUNT, "io_uarterrs ")),
61  _pc_idle(perf_alloc(PC_COUNT, "io_idle ")),
62  _pc_badidle(perf_alloc(PC_COUNT, "io_badidle ")),
63 #else
64  _pc_retries(nullptr),
65  _pc_timeouts(nullptr),
66  _pc_crcerrs(nullptr),
67  _pc_protoerrs(nullptr),
68  _pc_uerrs(nullptr),
69  _pc_idle(nullptr),
70  _pc_badidle(nullptr),
71 #endif
72  _bus_semaphore(SEM_INITIALIZER(0))
73 {
74  g_interface = this;
75 }
76 
77 PX4IO_serial::~PX4IO_serial()
78 {
79  /* kill our semaphores */
80  px4_sem_destroy(&_bus_semaphore);
81 
82  perf_free(_pc_txns);
83  perf_free(_pc_retries);
84  perf_free(_pc_timeouts);
85  perf_free(_pc_crcerrs);
86  perf_free(_pc_protoerrs);
87  perf_free(_pc_uerrs);
88  perf_free(_pc_idle);
89  perf_free(_pc_badidle);
90 
91  if (g_interface == this) {
92  g_interface = nullptr;
93  }
94 }
95 
96 int
97 PX4IO_serial::init(IOPacket *io_buffer)
98 {
99  _io_buffer_ptr = io_buffer;
100  /* create semaphores */
101  // in case the sub-class impl fails, the semaphore is cleaned up by destructor.
102  px4_sem_init(&_bus_semaphore, 0, 1);
103 
104  return 0;
105 }
106 
107 int
108 PX4IO_serial::write(unsigned address, void *data, unsigned count)
109 {
110  uint8_t page = address >> 8;
111  uint8_t offset = address & 0xff;
112  const uint16_t *values = reinterpret_cast<const uint16_t *>(data);
113 
114  if (count > PKT_MAX_REGS) {
115  return -EINVAL;
116  }
117 
118  px4_sem_wait(&_bus_semaphore);
119 
120  int result;
121 
122  for (unsigned retries = 0; retries < 3; retries++) {
123  _io_buffer_ptr->count_code = count | PKT_CODE_WRITE;
124  _io_buffer_ptr->page = page;
125  _io_buffer_ptr->offset = offset;
126  memcpy((void *)&_io_buffer_ptr->regs[0], (void *)values, (2 * count));
127 
128  for (unsigned i = count; i < PKT_MAX_REGS; i++) {
129  _io_buffer_ptr->regs[i] = 0x55aa;
130  }
131 
132  _io_buffer_ptr->crc = 0;
133  _io_buffer_ptr->crc = crc_packet(_io_buffer_ptr);
134 
135  /* start the transaction and wait for it to complete */
136  result = _bus_exchange(_io_buffer_ptr);
137 
138  /* successful transaction? */
139  if (result == OK) {
140 
141  /* check result in packet */
142  if (PKT_CODE(*_io_buffer_ptr) == PKT_CODE_ERROR) {
143 
144  /* IO didn't like it - no point retrying */
145  result = -EINVAL;
146  perf_count(_pc_protoerrs);
147  }
148 
149  break;
150  }
151 
152  perf_count(_pc_retries);
153  }
154 
155  px4_sem_post(&_bus_semaphore);
156 
157  if (result == OK) {
158  result = count;
159  }
160 
161  return result;
162 }
163 
164 int
165 PX4IO_serial::read(unsigned address, void *data, unsigned count)
166 {
167  uint8_t page = address >> 8;
168  uint8_t offset = address & 0xff;
169  uint16_t *values = reinterpret_cast<uint16_t *>(data);
170 
171  if (count > PKT_MAX_REGS) {
172  return -EINVAL;
173  }
174 
175  px4_sem_wait(&_bus_semaphore);
176 
177  int result;
178 
179  for (unsigned retries = 0; retries < 3; retries++) {
180 
181  _io_buffer_ptr->count_code = count | PKT_CODE_READ;
182  _io_buffer_ptr->page = page;
183  _io_buffer_ptr->offset = offset;
184 
185  _io_buffer_ptr->crc = 0;
186  _io_buffer_ptr->crc = crc_packet(_io_buffer_ptr);
187 
188  /* start the transaction and wait for it to complete */
189  result = _bus_exchange(_io_buffer_ptr);
190 
191  /* successful transaction? */
192  if (result == OK) {
193 
194  /* check result in packet */
195  if (PKT_CODE(*_io_buffer_ptr) == PKT_CODE_ERROR) {
196 
197  /* IO didn't like it - no point retrying */
198  result = -EINVAL;
199  perf_count(_pc_protoerrs);
200 
201  /* compare the received count with the expected count */
202 
203  } else if (PKT_COUNT(*_io_buffer_ptr) != count) {
204 
205  /* IO returned the wrong number of registers - no point retrying */
206  result = -EIO;
207  perf_count(_pc_protoerrs);
208 
209  /* successful read */
210 
211  } else {
212 
213  /* copy back the result */
214  memcpy(values, &_io_buffer_ptr->regs[0], (2 * count));
215  }
216 
217  break;
218  }
219 
220  perf_count(_pc_retries);
221  }
222 
223  px4_sem_post(&_bus_semaphore);
224 
225  if (result == OK) {
226  result = count;
227  }
228 
229  return result;
230 }
measure the time elapsed performing an event
Definition: perf_counter.h:56
Interface for PX4IO.
count the number of times an event occurs
Definition: perf_counter.h:55
static void read(bootloader_app_shared_t *pshared)
#define PKT_CODE_ERROR
Definition: protocol.h:348
void perf_count(perf_counter_t handle)
Count a performance event.
#define PKT_CODE(_p)
Definition: protocol.h:354
void perf_free(perf_counter_t handle)
Free a counter.
void init()
Activates/configures the hardware registers.
#define PKT_COUNT(_p)
Definition: protocol.h:353
#define perf_alloc(a, b)
Definition: px4io.h:59
uint8_t * data
Definition: dataman.cpp:149
static uint8_t crc_packet(struct IOPacket *pkt) __attribute__((unused))
Definition: protocol.h:394
#define PKT_CODE_READ
Definition: protocol.h:344
#define PKT_MAX_REGS
Serial protocol encapsulation.
Definition: protocol.h:328
static void write(bootloader_app_shared_t *pshared)
static PX4IO_serial * g_interface
#define PKT_CODE_WRITE
Definition: protocol.h:345
Fundamental base class for all physical drivers (I2C, SPI).
Definition: Device.hpp:65
device::Device * PX4IO_serial_interface()
#define OK
Definition: uavcan_main.cpp:71