APM:Libraries
Util.cpp
Go to the documentation of this file.
1 
2 #include <AP_HAL/AP_HAL.h>
3 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4
4 #include <stdio.h>
5 #include <stdarg.h>
6 #include <unistd.h>
7 #include <stdlib.h>
8 #include <errno.h>
9 #include <apps/nsh.h>
10 #include <fcntl.h>
11 #include "UARTDriver.h"
12 #include <uORB/uORB.h>
13 #include <uORB/topics/safety.h>
14 #include <systemlib/board_serial.h>
15 #include <drivers/drv_gpio.h>
16 #include <AP_Math/AP_Math.h>
17 
18 extern const AP_HAL::HAL& hal;
19 
20 #include "Util.h"
21 using namespace PX4;
22 
23 extern bool _px4_thread_should_exit;
24 
25 /*
26  constructor
27  */
28 PX4Util::PX4Util(void) : Util()
29 {
30  _safety_handle = orb_subscribe(ORB_ID(safety));
31 }
32 
33 
34 /*
35  start an instance of nsh
36  */
38 {
39  PX4UARTDriver *uart = (PX4UARTDriver *)stream;
40  int fd;
41 
42  // trigger exit in the other threads. This stops use of the
43  // various driver handles, and especially the px4io handle,
44  // which otherwise would cause a crash if px4io is stopped in
45  // the shell
47 
48  // take control of stream fd
49  fd = uart->_get_fd();
50 
51  // mark it blocking (nsh expects a blocking fd)
52  unsigned v;
53  v = fcntl(fd, F_GETFL, 0);
54  fcntl(fd, F_SETFL, v & ~O_NONBLOCK);
55 
56  // setup the UART on stdin/stdout/stderr
57  close(0);
58  close(1);
59  close(2);
60  dup2(fd, 0);
61  dup2(fd, 1);
62  dup2(fd, 2);
63 
64  nsh_consolemain(0, nullptr);
65 
66  // this shouldn't happen
67  hal.console->printf("shell exited\n");
68  return true;
69 }
70 
71 /*
72  return state of safety switch
73  */
75 {
76 #if !HAL_HAVE_SAFETY_SWITCH
78 #endif
79 
80  if (_safety_handle == -1) {
81  _safety_handle = orb_subscribe(ORB_ID(safety));
82  }
83  if (_safety_handle == -1) {
85  }
86  struct safety_s safety;
87  if (orb_copy(ORB_ID(safety), _safety_handle, &safety) != OK) {
89  }
90  if (!safety.safety_switch_available) {
92  }
93  if (safety.safety_off) {
95  }
97 }
98 
99 void PX4Util::set_system_clock(uint64_t time_utc_usec)
100 {
101  timespec ts;
102  ts.tv_sec = time_utc_usec/1000000ULL;
103  ts.tv_nsec = (time_utc_usec % 1000000ULL) * 1000ULL;
104  clock_settime(CLOCK_REALTIME, &ts);
105 }
106 
107 /*
108  display PX4 system identifer - board type and serial number
109  */
110 bool PX4Util::get_system_id(char buf[40])
111 {
112  uint8_t serialid[12];
113  memset(serialid, 0, sizeof(serialid));
114  get_board_serial(serialid);
115 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
116  const char *board_type = "PX4v1";
117 #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V3)
118  const char *board_type = "PX4v3";
119 #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
120  const char *board_type = "PX4v2";
121 #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
122  const char *board_type = "PX4v4";
123 #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V4PRO)
124  const char *board_type = "PX4v4PRO";
125 #elif defined(CONFIG_ARCH_BOARD_AEROFC_V1)
126  const char *board_type = "AEROFCv1";
127 #else
128  const char *board_type = "PX4v?";
129 #endif
130  // this format is chosen to match the human_readable_serial()
131  // function in auth.c
132  snprintf(buf, 40, "%s %02X%02X%02X%02X %02X%02X%02X%02X %02X%02X%02X%02X",
133  board_type,
134  (unsigned)serialid[0], (unsigned)serialid[1], (unsigned)serialid[2], (unsigned)serialid[3],
135  (unsigned)serialid[4], (unsigned)serialid[5], (unsigned)serialid[6], (unsigned)serialid[7],
136  (unsigned)serialid[8], (unsigned)serialid[9], (unsigned)serialid[10],(unsigned)serialid[11]);
137  return true;
138 }
139 
144 {
145  return mallinfo().fordblks;
146 }
147 
148 /*
149  AP_HAL wrapper around PX4 perf counters
150  */
152 {
153  ::perf_counter_type px4_t;
154  switch (t) {
155  case PX4Util::PC_COUNT:
156  px4_t = ::PC_COUNT;
157  break;
158  case PX4Util::PC_ELAPSED:
159  px4_t = ::PC_ELAPSED;
160  break;
162  px4_t = ::PC_INTERVAL;
163  break;
164  default:
165  return nullptr;
166  }
167  return (perf_counter_t)::perf_alloc(px4_t, name);
168 }
169 
171 {
173 }
174 
176 {
178 }
179 
181 {
183 }
184 
185 void PX4Util::set_imu_temp(float current)
186 {
187  if (!_heater.target || *_heater.target == -1) {
188  return;
189  }
190 
191  // average over temperatures to remove noise
192  _heater.count++;
193  _heater.sum += current;
194 
195  // update once a second
196  uint32_t now = AP_HAL::millis();
197  if (now - _heater.last_update_ms < 1000) {
198  return;
199  }
200  _heater.last_update_ms = now;
201 
202  current = _heater.sum / _heater.count;
203  _heater.sum = 0;
204  _heater.count = 0;
205 
206  // experimentally tweaked for Pixhawk2
207  const float kI = 0.3f;
208  const float kP = 200.0f;
209  float target = (float)(*_heater.target);
210 
211  // limit to 65 degrees to prevent damage
212  target = constrain_float(target, 0, 65);
213 
214  float err = target - current;
215 
216  _heater.integrator += kI * err;
217  _heater.integrator = constrain_float(_heater.integrator, 0, 70);
218 
219  float output = constrain_float(kP * err + _heater.integrator, 0, 100);
220 
221  // hal.console->printf("integrator %.1f out=%.1f temp=%.2f err=%.2f\n", _heater.integrator, output, current, err);
222 
223  if (_heater.fd == -1) {
224  _heater.fd = open("/dev/px4io", O_RDWR);
225  }
226  if (_heater.fd != -1) {
227  ioctl(_heater.fd, GPIO_SET_HEATER_DUTY_CYCLE, (unsigned)output);
228  }
229 
230 }
231 
233 {
234  _heater.target = target;
235 }
236 
237 
238 extern "C" {
239  extern void *fat_dma_alloc(size_t);
240  extern void fat_dma_free(void *, size_t);
241 }
242 
243 /*
244  allocate DMA-capable memory if possible. Otherwise return normal
245  memory.
246 */
248 {
249 #if !defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
250  if (mem_type == AP_HAL::Util::MEM_DMA_SAFE) {
251  return fat_dma_alloc(size);
252  } else {
253  return calloc(1, size);
254  }
255 #else
256  return calloc(1, size);
257 #endif
258 }
259 void PX4Util::free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type)
260 {
261 #if !defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
262  if (mem_type == AP_HAL::Util::MEM_DMA_SAFE) {
263  return fat_dma_free(ptr, size);
264  } else {
265  return free(ptr);
266  }
267 #else
268  return free(ptr);
269 #endif
270 }
271 
272 #endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4
int open(const char *pathname, int flags)
POSIX Open a file with integer mode flags.
Definition: posix.c:885
bool run_debug_shell(AP_HAL::BetterStream *stream)
Definition: Util.cpp:37
AP_HAL::UARTDriver * console
Definition: HAL.h:110
void get_board_serial(uint8_t *serialid)
Definition: syscalls.c:217
bool _px4_thread_should_exit
void perf_end(perf_counter_t) override
Definition: Util.cpp:175
const char * name
Definition: BusTest.cpp:11
void free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type) override
Definition: Util.cpp:259
void * calloc(size_t nmemb, size_t size)
Definition: malloc.c:76
int fd
Definition: Util.h:78
void set_imu_target_temp(int8_t *target) override
Definition: Util.cpp:232
struct PX4::PX4Util::@109 _heater
void perf_count(perf_counter_t) override
Definition: Util.cpp:180
virtual void printf(const char *,...) FMT_PRINTF(2
Definition: BetterStream.cpp:5
void * perf_counter_t
Definition: Util.h:101
uint32_t available_memory(void) override
Definition: Util.cpp:143
void * fat_dma_alloc(size_t)
perf_counter_type
Definition: Util.h:96
perf_counter_t perf_alloc(perf_counter_type t, const char *name) override
Definition: Util.cpp:151
uint32_t millis()
Definition: system.cpp:157
int8_t * target
Definition: Util.h:73
int close(int fileno)
POSIX Close a file with fileno handel.
Definition: posix.c:675
PX4Util(void)
Definition: Util.cpp:28
void perf_begin(perf_counter_t) override
Definition: Util.cpp:170
enum safety_state safety_switch_state(void)
Definition: Util.cpp:74
float v
Definition: Printf.cpp:15
safety_state
Definition: Util.h:35
const AP_HAL::HAL & hal
Definition: AC_PID_test.cpp:14
void free(void *ptr)
Definition: malloc.c:81
float constrain_float(const float amt, const float low, const float high)
Definition: AP_Math.h:142
int _safety_handle
Definition: Util.h:69
void * malloc_type(size_t size, AP_HAL::Util::Memory_Type mem_type) override
Definition: Util.cpp:247
int snprintf(char *str, size_t size, const char *format,...)
Definition: Util.cpp:44
void fat_dma_free(void *, size_t)
bool get_system_id(char buf[40])
Definition: Util.cpp:110
void set_imu_temp(float current) override
Definition: Util.cpp:185
void set_system_clock(uint64_t time_utc_usec)
Definition: Util.cpp:99
int _get_fd(void)
Definition: UARTDriver.h:36