mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-24 15:40:31 +08:00
Merge working changes into export-build branch.
This commit is contained in:
px4dev
@@ -0,0 +1,398 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file ardrone_interface.c
|
||||
* Implementation of AR.Drone 1.0 / 2.0 motor control interface.
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <stdbool.h>
|
||||
#include <unistd.h>
|
||||
#include <math.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
#include <debug.h>
|
||||
#include <termios.h>
|
||||
#include <time.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <sys/prctl.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
|
||||
#include <systemlib/systemlib.h>
|
||||
|
||||
#include "ardrone_motor_control.h"
|
||||
|
||||
__EXPORT int ardrone_interface_main(int argc, char *argv[]);
|
||||
|
||||
|
||||
static bool thread_should_exit = false; /**< Deamon exit flag */
|
||||
static bool thread_running = false; /**< Deamon status flag */
|
||||
static int ardrone_interface_task; /**< Handle of deamon task / thread */
|
||||
static int ardrone_write; /**< UART to write AR.Drone commands to */
|
||||
|
||||
/**
|
||||
* Mainloop of ardrone_interface.
|
||||
*/
|
||||
int ardrone_interface_thread_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Open the UART connected to the motor controllers
|
||||
*/
|
||||
static int ardrone_open_uart(char *uart_name, struct termios *uart_config_original);
|
||||
|
||||
/**
|
||||
* Print the correct usage.
|
||||
*/
|
||||
static void usage(const char *reason);
|
||||
|
||||
static void
|
||||
usage(const char *reason)
|
||||
{
|
||||
if (reason)
|
||||
fprintf(stderr, "%s\n", reason);
|
||||
fprintf(stderr, "usage: ardrone_interface {start|stop|status} [-d <UART>]\n\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
/**
|
||||
* The deamon app only briefly exists to start
|
||||
* the background job. The stack size assigned in the
|
||||
* Makefile does only apply to this management task.
|
||||
*
|
||||
* The actual stack size should be set in the call
|
||||
* to task_create().
|
||||
*/
|
||||
int ardrone_interface_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 1)
|
||||
usage("missing command");
|
||||
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
if (thread_running) {
|
||||
printf("ardrone_interface already running\n");
|
||||
/* this is not an error */
|
||||
exit(0);
|
||||
}
|
||||
|
||||
thread_should_exit = false;
|
||||
ardrone_interface_task = task_spawn("ardrone_interface",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 15,
|
||||
2048,
|
||||
ardrone_interface_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
thread_should_exit = true;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
if (thread_running) {
|
||||
printf("\tardrone_interface is running\n");
|
||||
} else {
|
||||
printf("\tardrone_interface not started\n");
|
||||
}
|
||||
exit(0);
|
||||
}
|
||||
|
||||
usage("unrecognized command");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
static int ardrone_open_uart(char *uart_name, struct termios *uart_config_original)
|
||||
{
|
||||
/* baud rate */
|
||||
int speed = B115200;
|
||||
int uart;
|
||||
|
||||
/* open uart */
|
||||
uart = open(uart_name, O_RDWR | O_NOCTTY);
|
||||
|
||||
/* Try to set baud rate */
|
||||
struct termios uart_config;
|
||||
int termios_state;
|
||||
|
||||
/* Back up the original uart configuration to restore it after exit */
|
||||
if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
|
||||
fprintf(stderr, "[ardrone_interface] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state);
|
||||
close(uart);
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* Fill the struct for the new configuration */
|
||||
tcgetattr(uart, &uart_config);
|
||||
|
||||
/* Clear ONLCR flag (which appends a CR for every LF) */
|
||||
uart_config.c_oflag &= ~ONLCR;
|
||||
|
||||
/* Set baud rate */
|
||||
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
|
||||
fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
|
||||
close(uart);
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
|
||||
fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
|
||||
close(uart);
|
||||
return -1;
|
||||
}
|
||||
|
||||
return uart;
|
||||
}
|
||||
|
||||
int ardrone_interface_thread_main(int argc, char *argv[])
|
||||
{
|
||||
thread_running = true;
|
||||
|
||||
char *device = "/dev/ttyS1";
|
||||
|
||||
/* welcome user */
|
||||
printf("[ardrone_interface] Control started, taking over motors\n");
|
||||
|
||||
/* File descriptors */
|
||||
int gpios;
|
||||
|
||||
char *commandline_usage = "\tusage: ardrone_interface start|status|stop [-t for motor test (10%% thrust)]\n";
|
||||
|
||||
bool motor_test_mode = false;
|
||||
int test_motor = -1;
|
||||
|
||||
/* read commandline arguments */
|
||||
for (int i = 0; i < argc && argv[i]; i++) {
|
||||
if (strcmp(argv[i], "-t") == 0 || strcmp(argv[i], "--test") == 0) {
|
||||
motor_test_mode = true;
|
||||
}
|
||||
|
||||
if (strcmp(argv[i], "-m") == 0 || strcmp(argv[i], "--motor") == 0) {
|
||||
if (i+1 < argc) {
|
||||
int motor = atoi(argv[i+1]);
|
||||
if (motor > 0 && motor < 5) {
|
||||
test_motor = motor;
|
||||
} else {
|
||||
thread_running = false;
|
||||
errx(1, "supply a motor # between 1 and 4. Example: -m 1\n %s", commandline_usage);
|
||||
}
|
||||
} else {
|
||||
thread_running = false;
|
||||
errx(1, "missing parameter to -m 1..4\n %s", commandline_usage);
|
||||
}
|
||||
}
|
||||
if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set
|
||||
if (argc > i + 1) {
|
||||
device = argv[i + 1];
|
||||
|
||||
} else {
|
||||
thread_running = false;
|
||||
errx(1, "missing parameter to -m 1..4\n %s", commandline_usage);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
struct termios uart_config_original;
|
||||
|
||||
if (motor_test_mode) {
|
||||
printf("[ardrone_interface] Motor test mode enabled, setting 10 %% thrust.\n");
|
||||
}
|
||||
|
||||
/* Led animation */
|
||||
int counter = 0;
|
||||
int led_counter = 0;
|
||||
|
||||
/* declare and safely initialize all structs */
|
||||
struct vehicle_status_s state;
|
||||
memset(&state, 0, sizeof(state));
|
||||
struct actuator_controls_s actuator_controls;
|
||||
memset(&actuator_controls, 0, sizeof(actuator_controls));
|
||||
struct actuator_armed_s armed;
|
||||
armed.armed = false;
|
||||
|
||||
/* subscribe to attitude, motor setpoints and system state */
|
||||
int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
|
||||
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
|
||||
|
||||
printf("[ardrone_interface] Motors initialized - ready.\n");
|
||||
fflush(stdout);
|
||||
|
||||
/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
|
||||
ardrone_write = ardrone_open_uart(device, &uart_config_original);
|
||||
|
||||
/* initialize multiplexing, deactivate all outputs - must happen after UART open to claim GPIOs on PX4FMU */
|
||||
gpios = ar_multiplexing_init();
|
||||
|
||||
if (ardrone_write < 0) {
|
||||
fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
|
||||
thread_running = false;
|
||||
exit(ERROR);
|
||||
}
|
||||
|
||||
/* initialize motors */
|
||||
if (OK != ar_init_motors(ardrone_write, gpios)) {
|
||||
close(ardrone_write);
|
||||
fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n");
|
||||
thread_running = false;
|
||||
exit(ERROR);
|
||||
}
|
||||
|
||||
ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0);
|
||||
|
||||
|
||||
// XXX Re-done initialization to make sure it is accepted by the motors
|
||||
// XXX should be removed after more testing, but no harm
|
||||
|
||||
/* close uarts */
|
||||
close(ardrone_write);
|
||||
|
||||
/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
|
||||
ardrone_write = ardrone_open_uart(device, &uart_config_original);
|
||||
|
||||
/* initialize multiplexing, deactivate all outputs - must happen after UART open to claim GPIOs on PX4FMU */
|
||||
gpios = ar_multiplexing_init();
|
||||
|
||||
if (ardrone_write < 0) {
|
||||
fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
|
||||
thread_running = false;
|
||||
exit(ERROR);
|
||||
}
|
||||
|
||||
/* initialize motors */
|
||||
if (OK != ar_init_motors(ardrone_write, gpios)) {
|
||||
close(ardrone_write);
|
||||
fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n");
|
||||
thread_running = false;
|
||||
exit(ERROR);
|
||||
}
|
||||
|
||||
while (!thread_should_exit) {
|
||||
|
||||
if (motor_test_mode) {
|
||||
/* set motors to idle speed */
|
||||
if (test_motor > 0 && test_motor < 5) {
|
||||
int motors[4] = {0, 0, 0, 0};
|
||||
motors[test_motor - 1] = 10;
|
||||
ardrone_write_motor_commands(ardrone_write, motors[0], motors[1], motors[2], motors[3]);
|
||||
} else {
|
||||
ardrone_write_motor_commands(ardrone_write, 10, 10, 10, 10);
|
||||
}
|
||||
|
||||
} else {
|
||||
/* MAIN OPERATION MODE */
|
||||
|
||||
/* get a local copy of the vehicle state */
|
||||
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
|
||||
/* get a local copy of the actuator controls */
|
||||
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
|
||||
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
|
||||
|
||||
/* for now only spin if armed and immediately shut down
|
||||
* if in failsafe
|
||||
*/
|
||||
if (armed.armed && !armed.lockdown) {
|
||||
ardrone_mixing_and_output(ardrone_write, &actuator_controls);
|
||||
|
||||
} else {
|
||||
/* Silently lock down motor speeds to zero */
|
||||
ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0);
|
||||
}
|
||||
}
|
||||
|
||||
if (counter % 24 == 0) {
|
||||
if (led_counter == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0);
|
||||
|
||||
if (led_counter == 1) ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0);
|
||||
|
||||
if (led_counter == 2) ar_set_leds(ardrone_write, 1, 0, 0, 0, 0, 0, 0 , 0);
|
||||
|
||||
if (led_counter == 3) ar_set_leds(ardrone_write, 0, 0, 0, 1, 0, 0, 0 , 0);
|
||||
|
||||
if (led_counter == 4) ar_set_leds(ardrone_write, 0, 0, 1, 1, 0, 0, 0 , 0);
|
||||
|
||||
if (led_counter == 5) ar_set_leds(ardrone_write, 0, 0, 1, 0, 0, 0, 0 , 0);
|
||||
|
||||
if (led_counter == 6) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 1, 0 , 0);
|
||||
|
||||
if (led_counter == 7) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 1, 0 , 0);
|
||||
|
||||
if (led_counter == 8) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 0, 0 , 0);
|
||||
|
||||
if (led_counter == 9) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 0 , 1);
|
||||
|
||||
if (led_counter == 10) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 1);
|
||||
|
||||
if (led_counter == 11) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 0);
|
||||
|
||||
led_counter++;
|
||||
|
||||
if (led_counter == 12) led_counter = 0;
|
||||
}
|
||||
|
||||
/* run at approximately 200 Hz */
|
||||
usleep(4500);
|
||||
|
||||
counter++;
|
||||
}
|
||||
|
||||
/* restore old UART config */
|
||||
int termios_state;
|
||||
|
||||
if ((termios_state = tcsetattr(ardrone_write, TCSANOW, &uart_config_original)) < 0) {
|
||||
fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for (tcsetattr)\n");
|
||||
}
|
||||
|
||||
printf("[ardrone_interface] Restored original UART config, exiting..\n");
|
||||
|
||||
/* close uarts */
|
||||
close(ardrone_write);
|
||||
ar_multiplexing_deinit(gpios);
|
||||
|
||||
fflush(stdout);
|
||||
|
||||
thread_running = false;
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,492 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file ardrone_motor_control.c
|
||||
* Implementation of AR.Drone 1.0 / 2.0 motor control interface
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <stdio.h>
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <drivers/drv_gpio.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/actuator_controls_effective.h>
|
||||
#include <systemlib/err.h>
|
||||
|
||||
#include "ardrone_motor_control.h"
|
||||
|
||||
static unsigned long motor_gpios = GPIO_EXT_1 | GPIO_EXT_2 | GPIO_MULTI_1 | GPIO_MULTI_2;
|
||||
static unsigned long motor_gpio[4] = { GPIO_EXT_1, GPIO_EXT_2, GPIO_MULTI_1, GPIO_MULTI_2 };
|
||||
|
||||
typedef union {
|
||||
uint16_t motor_value;
|
||||
uint8_t bytes[2];
|
||||
} motor_union_t;
|
||||
|
||||
#define UART_TRANSFER_TIME_BYTE_US (9+50) /**< 9 us per byte at 115200k plus overhead */
|
||||
|
||||
/**
|
||||
* @brief Generate the 8-byte motor set packet
|
||||
*
|
||||
* @return the number of bytes (8)
|
||||
*/
|
||||
void ar_get_motor_packet(uint8_t *motor_buf, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4)
|
||||
{
|
||||
motor_buf[0] = 0x20;
|
||||
motor_buf[1] = 0x00;
|
||||
motor_buf[2] = 0x00;
|
||||
motor_buf[3] = 0x00;
|
||||
motor_buf[4] = 0x00;
|
||||
/*
|
||||
* {0x20, 0x00, 0x00, 0x00, 0x00};
|
||||
* 0x20 is start sign / motor command
|
||||
*/
|
||||
motor_union_t curr_motor;
|
||||
uint16_t nineBitMask = 0x1FF;
|
||||
|
||||
/* Set motor 1 */
|
||||
curr_motor.motor_value = (motor1 & nineBitMask) << 4;
|
||||
motor_buf[0] |= curr_motor.bytes[1];
|
||||
motor_buf[1] |= curr_motor.bytes[0];
|
||||
|
||||
/* Set motor 2 */
|
||||
curr_motor.motor_value = (motor2 & nineBitMask) << 3;
|
||||
motor_buf[1] |= curr_motor.bytes[1];
|
||||
motor_buf[2] |= curr_motor.bytes[0];
|
||||
|
||||
/* Set motor 3 */
|
||||
curr_motor.motor_value = (motor3 & nineBitMask) << 2;
|
||||
motor_buf[2] |= curr_motor.bytes[1];
|
||||
motor_buf[3] |= curr_motor.bytes[0];
|
||||
|
||||
/* Set motor 4 */
|
||||
curr_motor.motor_value = (motor4 & nineBitMask) << 1;
|
||||
motor_buf[3] |= curr_motor.bytes[1];
|
||||
motor_buf[4] |= curr_motor.bytes[0];
|
||||
}
|
||||
|
||||
void ar_enable_broadcast(int fd)
|
||||
{
|
||||
ar_select_motor(fd, 0);
|
||||
}
|
||||
|
||||
int ar_multiplexing_init()
|
||||
{
|
||||
int fd;
|
||||
|
||||
fd = open(GPIO_DEVICE_PATH, 0);
|
||||
|
||||
if (fd < 0) {
|
||||
warn("GPIO: open fail");
|
||||
return fd;
|
||||
}
|
||||
|
||||
/* deactivate all outputs */
|
||||
if (ioctl(fd, GPIO_SET, motor_gpios)) {
|
||||
warn("GPIO: clearing pins fail");
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* configure all motor select GPIOs as outputs */
|
||||
if (ioctl(fd, GPIO_SET_OUTPUT, motor_gpios) != 0) {
|
||||
warn("GPIO: output set fail");
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
return fd;
|
||||
}
|
||||
|
||||
int ar_multiplexing_deinit(int fd)
|
||||
{
|
||||
if (fd < 0) {
|
||||
printf("GPIO: no valid descriptor\n");
|
||||
return fd;
|
||||
}
|
||||
|
||||
int ret = 0;
|
||||
|
||||
/* deselect motor 1-4 */
|
||||
ret += ioctl(fd, GPIO_SET, motor_gpios);
|
||||
|
||||
if (ret != 0) {
|
||||
printf("GPIO: clear failed %d times\n", ret);
|
||||
}
|
||||
|
||||
if (ioctl(fd, GPIO_SET_INPUT, motor_gpios) != 0) {
|
||||
printf("GPIO: input set fail\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
close(fd);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int ar_select_motor(int fd, uint8_t motor)
|
||||
{
|
||||
int ret = 0;
|
||||
/*
|
||||
* Four GPIOS:
|
||||
* GPIO_EXT1
|
||||
* GPIO_EXT2
|
||||
* GPIO_UART2_CTS
|
||||
* GPIO_UART2_RTS
|
||||
*/
|
||||
|
||||
/* select motor 0 to enable broadcast */
|
||||
if (motor == 0) {
|
||||
/* select motor 1-4 */
|
||||
ret += ioctl(fd, GPIO_CLEAR, motor_gpios);
|
||||
|
||||
} else {
|
||||
/* select reqested motor */
|
||||
ret += ioctl(fd, GPIO_CLEAR, motor_gpio[motor - 1]);
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int ar_deselect_motor(int fd, uint8_t motor)
|
||||
{
|
||||
int ret = 0;
|
||||
/*
|
||||
* Four GPIOS:
|
||||
* GPIO_EXT1
|
||||
* GPIO_EXT2
|
||||
* GPIO_UART2_CTS
|
||||
* GPIO_UART2_RTS
|
||||
*/
|
||||
|
||||
if (motor == 0) {
|
||||
/* deselect motor 1-4 */
|
||||
ret += ioctl(fd, GPIO_SET, motor_gpios);
|
||||
|
||||
} else {
|
||||
/* deselect reqested motor */
|
||||
ret = ioctl(fd, GPIO_SET, motor_gpio[motor - 1]);
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int ar_init_motors(int ardrone_uart, int gpios)
|
||||
{
|
||||
/* Write ARDrone commands on UART2 */
|
||||
uint8_t initbuf[] = {0xE0, 0x91, 0xA1, 0x00, 0x40};
|
||||
uint8_t multicastbuf[] = {0xA0, 0xA0, 0xA0, 0xA0, 0xA0, 0xA0};
|
||||
|
||||
/* deselect all motors */
|
||||
ar_deselect_motor(gpios, 0);
|
||||
|
||||
/* initialize all motors
|
||||
* - select one motor at a time
|
||||
* - configure motor
|
||||
*/
|
||||
int i;
|
||||
int errcounter = 0;
|
||||
|
||||
|
||||
/* initial setup run */
|
||||
for (i = 1; i < 5; ++i) {
|
||||
/* Initialize motors 1-4 */
|
||||
errcounter += ar_select_motor(gpios, i);
|
||||
usleep(200);
|
||||
|
||||
/*
|
||||
* write 0xE0 - request status
|
||||
* receive one status byte
|
||||
*/
|
||||
write(ardrone_uart, &(initbuf[0]), 1);
|
||||
fsync(ardrone_uart);
|
||||
usleep(UART_TRANSFER_TIME_BYTE_US*1);
|
||||
|
||||
/*
|
||||
* write 0x91 - request checksum
|
||||
* receive 120 status bytes
|
||||
*/
|
||||
write(ardrone_uart, &(initbuf[1]), 1);
|
||||
fsync(ardrone_uart);
|
||||
usleep(UART_TRANSFER_TIME_BYTE_US*120);
|
||||
|
||||
/*
|
||||
* write 0xA1 - set status OK
|
||||
* receive one status byte - should be A0
|
||||
* to confirm status is OK
|
||||
*/
|
||||
write(ardrone_uart, &(initbuf[2]), 1);
|
||||
fsync(ardrone_uart);
|
||||
usleep(UART_TRANSFER_TIME_BYTE_US*1);
|
||||
|
||||
/*
|
||||
* set as motor i, where i = 1..4
|
||||
* receive nothing
|
||||
*/
|
||||
initbuf[3] = (uint8_t)i;
|
||||
write(ardrone_uart, &(initbuf[3]), 1);
|
||||
fsync(ardrone_uart);
|
||||
|
||||
/*
|
||||
* write 0x40 - check version
|
||||
* receive 11 bytes encoding the version
|
||||
*/
|
||||
write(ardrone_uart, &(initbuf[4]), 1);
|
||||
fsync(ardrone_uart);
|
||||
usleep(UART_TRANSFER_TIME_BYTE_US*11);
|
||||
|
||||
ar_deselect_motor(gpios, i);
|
||||
/* sleep 200 ms */
|
||||
usleep(200000);
|
||||
}
|
||||
|
||||
/* start the multicast part */
|
||||
errcounter += ar_select_motor(gpios, 0);
|
||||
usleep(200);
|
||||
|
||||
/*
|
||||
* first round
|
||||
* write six times A0 - enable broadcast
|
||||
* receive nothing
|
||||
*/
|
||||
write(ardrone_uart, multicastbuf, sizeof(multicastbuf));
|
||||
fsync(ardrone_uart);
|
||||
usleep(UART_TRANSFER_TIME_BYTE_US * sizeof(multicastbuf));
|
||||
|
||||
/*
|
||||
* second round
|
||||
* write six times A0 - enable broadcast
|
||||
* receive nothing
|
||||
*/
|
||||
write(ardrone_uart, multicastbuf, sizeof(multicastbuf));
|
||||
fsync(ardrone_uart);
|
||||
usleep(UART_TRANSFER_TIME_BYTE_US * sizeof(multicastbuf));
|
||||
|
||||
/* set motors to zero speed (fsync is part of the write command */
|
||||
ardrone_write_motor_commands(ardrone_uart, 0, 0, 0, 0);
|
||||
|
||||
if (errcounter != 0) {
|
||||
fprintf(stderr, "[ardrone_interface] init sequence incomplete, failed %d times", -errcounter);
|
||||
fflush(stdout);
|
||||
}
|
||||
return errcounter;
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the leds on the motor controllers, 1 turns led on, 0 off.
|
||||
*/
|
||||
void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green)
|
||||
{
|
||||
/*
|
||||
* 2 bytes are sent. The first 3 bits describe the command: 011 means led control
|
||||
* the following 4 bits are the red leds for motor 4, 3, 2, 1
|
||||
* then 4 bits with unknown function, then 4 bits for green leds for motor 4, 3, 2, 1
|
||||
* the last bit is unknown.
|
||||
*
|
||||
* The packet is therefore:
|
||||
* 011 rrrr 0000 gggg 0
|
||||
*/
|
||||
uint8_t leds[2];
|
||||
leds[0] = 0x60 | ((led4_red & 0x01) << 4) | ((led3_red & 0x01) << 3) | ((led2_red & 0x01) << 2) | ((led1_red & 0x01) << 1);
|
||||
leds[1] = ((led4_green & 0x01) << 4) | ((led3_green & 0x01) << 3) | ((led2_green & 0x01) << 2) | ((led1_green & 0x01) << 1);
|
||||
write(ardrone_uart, leds, 2);
|
||||
}
|
||||
|
||||
int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4) {
|
||||
const unsigned int min_motor_interval = 4900;
|
||||
static uint64_t last_motor_time = 0;
|
||||
|
||||
static struct actuator_outputs_s outputs;
|
||||
outputs.timestamp = hrt_absolute_time();
|
||||
outputs.output[0] = motor1;
|
||||
outputs.output[1] = motor2;
|
||||
outputs.output[2] = motor3;
|
||||
outputs.output[3] = motor4;
|
||||
static orb_advert_t pub = 0;
|
||||
if (pub == 0) {
|
||||
pub = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &outputs);
|
||||
}
|
||||
|
||||
if (hrt_absolute_time() - last_motor_time > min_motor_interval) {
|
||||
uint8_t buf[5] = {0};
|
||||
ar_get_motor_packet(buf, motor1, motor2, motor3, motor4);
|
||||
int ret;
|
||||
ret = write(ardrone_fd, buf, sizeof(buf));
|
||||
fsync(ardrone_fd);
|
||||
|
||||
/* publish just written values */
|
||||
orb_publish(ORB_ID_VEHICLE_CONTROLS, pub, &outputs);
|
||||
|
||||
if (ret == sizeof(buf)) {
|
||||
return OK;
|
||||
} else {
|
||||
return ret;
|
||||
}
|
||||
} else {
|
||||
return -ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators) {
|
||||
|
||||
float roll_control = actuators->control[0];
|
||||
float pitch_control = actuators->control[1];
|
||||
float yaw_control = actuators->control[2];
|
||||
float motor_thrust = actuators->control[3];
|
||||
|
||||
//printf("AMO: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",roll_control, pitch_control, yaw_control, motor_thrust);
|
||||
|
||||
const float min_thrust = 0.02f; /**< 2% minimum thrust */
|
||||
const float max_thrust = 1.0f; /**< 100% max thrust */
|
||||
const float scaling = 500.0f; /**< 100% thrust equals a value of 500 which works, 512 leads to cutoff */
|
||||
const float min_gas = min_thrust * scaling; /**< value range sent to motors, minimum */
|
||||
const float max_gas = max_thrust * scaling; /**< value range sent to motors, maximum */
|
||||
|
||||
/* initialize all fields to zero */
|
||||
uint16_t motor_pwm[4] = {0};
|
||||
float motor_calc[4] = {0};
|
||||
|
||||
float output_band = 0.0f;
|
||||
float band_factor = 0.75f;
|
||||
const float startpoint_full_control = 0.25f; /**< start full control at 25% thrust */
|
||||
float yaw_factor = 1.0f;
|
||||
|
||||
static bool initialized = false;
|
||||
/* publish effective outputs */
|
||||
static struct actuator_controls_effective_s actuator_controls_effective;
|
||||
static orb_advert_t actuator_controls_effective_pub;
|
||||
|
||||
if (motor_thrust <= min_thrust) {
|
||||
motor_thrust = min_thrust;
|
||||
output_band = 0.0f;
|
||||
} else if (motor_thrust < startpoint_full_control && motor_thrust > min_thrust) {
|
||||
output_band = band_factor * (motor_thrust - min_thrust);
|
||||
} else if (motor_thrust >= startpoint_full_control && motor_thrust < max_thrust - band_factor * startpoint_full_control) {
|
||||
output_band = band_factor * startpoint_full_control;
|
||||
} else if (motor_thrust >= max_thrust - band_factor * startpoint_full_control) {
|
||||
output_band = band_factor * (max_thrust - motor_thrust);
|
||||
}
|
||||
|
||||
//add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer
|
||||
|
||||
// FRONT (MOTOR 1)
|
||||
motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control);
|
||||
|
||||
// RIGHT (MOTOR 2)
|
||||
motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control);
|
||||
|
||||
// BACK (MOTOR 3)
|
||||
motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control);
|
||||
|
||||
// LEFT (MOTOR 4)
|
||||
motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control);
|
||||
|
||||
// if we are not in the output band
|
||||
if (!(motor_calc[0] < motor_thrust + output_band && motor_calc[0] > motor_thrust - output_band
|
||||
&& motor_calc[1] < motor_thrust + output_band && motor_calc[1] > motor_thrust - output_band
|
||||
&& motor_calc[2] < motor_thrust + output_band && motor_calc[2] > motor_thrust - output_band
|
||||
&& motor_calc[3] < motor_thrust + output_band && motor_calc[3] > motor_thrust - output_band)) {
|
||||
|
||||
yaw_factor = 0.5f;
|
||||
yaw_control *= yaw_factor;
|
||||
// FRONT (MOTOR 1)
|
||||
motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control);
|
||||
|
||||
// RIGHT (MOTOR 2)
|
||||
motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control);
|
||||
|
||||
// BACK (MOTOR 3)
|
||||
motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control);
|
||||
|
||||
// LEFT (MOTOR 4)
|
||||
motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control);
|
||||
}
|
||||
|
||||
for (int i = 0; i < 4; i++) {
|
||||
//check for limits
|
||||
if (motor_calc[i] < motor_thrust - output_band) {
|
||||
motor_calc[i] = motor_thrust - output_band;
|
||||
}
|
||||
|
||||
if (motor_calc[i] > motor_thrust + output_band) {
|
||||
motor_calc[i] = motor_thrust + output_band;
|
||||
}
|
||||
}
|
||||
|
||||
/* publish effective outputs */
|
||||
actuator_controls_effective.control_effective[0] = roll_control;
|
||||
actuator_controls_effective.control_effective[1] = pitch_control;
|
||||
/* yaw output after limiting */
|
||||
actuator_controls_effective.control_effective[2] = yaw_control;
|
||||
/* possible motor thrust limiting */
|
||||
actuator_controls_effective.control_effective[3] = (motor_calc[0] + motor_calc[1] + motor_calc[2] + motor_calc[3]) / 4.0f;
|
||||
|
||||
if (!initialized) {
|
||||
/* advertise and publish */
|
||||
actuator_controls_effective_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, &actuator_controls_effective);
|
||||
initialized = true;
|
||||
} else {
|
||||
/* already initialized, just publishing */
|
||||
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, actuator_controls_effective_pub, &actuator_controls_effective);
|
||||
}
|
||||
|
||||
/* set the motor values */
|
||||
|
||||
/* scale up from 0..1 to 10..512) */
|
||||
motor_pwm[0] = (uint16_t) (motor_calc[0] * ((float)max_gas - min_gas) + min_gas);
|
||||
motor_pwm[1] = (uint16_t) (motor_calc[1] * ((float)max_gas - min_gas) + min_gas);
|
||||
motor_pwm[2] = (uint16_t) (motor_calc[2] * ((float)max_gas - min_gas) + min_gas);
|
||||
motor_pwm[3] = (uint16_t) (motor_calc[3] * ((float)max_gas - min_gas) + min_gas);
|
||||
|
||||
/* Keep motors spinning while armed and prevent overflows */
|
||||
|
||||
/* Failsafe logic - should never be necessary */
|
||||
motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : 10;
|
||||
motor_pwm[1] = (motor_pwm[1] > 0) ? motor_pwm[1] : 10;
|
||||
motor_pwm[2] = (motor_pwm[2] > 0) ? motor_pwm[2] : 10;
|
||||
motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : 10;
|
||||
|
||||
/* Failsafe logic - should never be necessary */
|
||||
motor_pwm[0] = (motor_pwm[0] <= 512) ? motor_pwm[0] : 512;
|
||||
motor_pwm[1] = (motor_pwm[1] <= 512) ? motor_pwm[1] : 512;
|
||||
motor_pwm[2] = (motor_pwm[2] <= 512) ? motor_pwm[2] : 512;
|
||||
motor_pwm[3] = (motor_pwm[3] <= 512) ? motor_pwm[3] : 512;
|
||||
|
||||
/* send motors via UART */
|
||||
ardrone_write_motor_commands(ardrone_write, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
|
||||
}
|
||||
@@ -0,0 +1,93 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file ardrone_motor_control.h
|
||||
* Definition of AR.Drone 1.0 / 2.0 motor control interface
|
||||
*/
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
|
||||
/**
|
||||
* Generate the 5-byte motor set packet.
|
||||
*
|
||||
* @return the number of bytes (5)
|
||||
*/
|
||||
void ar_get_motor_packet(uint8_t *motor_buf, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4);
|
||||
|
||||
/**
|
||||
* Select a motor in the multiplexing.
|
||||
*
|
||||
* @param fd GPIO file descriptor
|
||||
* @param motor Motor number, from 1 to 4, 0 selects all
|
||||
*/
|
||||
int ar_select_motor(int fd, uint8_t motor);
|
||||
|
||||
/**
|
||||
* Deselect a motor in the multiplexing.
|
||||
*
|
||||
* @param fd GPIO file descriptor
|
||||
* @param motor Motor number, from 1 to 4, 0 deselects all
|
||||
*/
|
||||
int ar_deselect_motor(int fd, uint8_t motor);
|
||||
|
||||
void ar_enable_broadcast(int fd);
|
||||
|
||||
int ar_multiplexing_init(void);
|
||||
int ar_multiplexing_deinit(int fd);
|
||||
|
||||
/**
|
||||
* Write four motor commands to an already initialized port.
|
||||
*
|
||||
* Writing 0 stops a motor, values from 1-512 encode the full thrust range.
|
||||
* on some motor controller firmware revisions a minimum value of 10 is
|
||||
* required to spin the motors.
|
||||
*/
|
||||
int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4);
|
||||
|
||||
/**
|
||||
* Initialize the motors.
|
||||
*/
|
||||
int ar_init_motors(int ardrone_uart, int gpio);
|
||||
|
||||
/**
|
||||
* Set LED pattern.
|
||||
*/
|
||||
void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green);
|
||||
|
||||
/**
|
||||
* Mix motors and output actuators
|
||||
*/
|
||||
void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators);
|
||||
@@ -0,0 +1,40 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# AR.Drone motor driver
|
||||
#
|
||||
|
||||
MODULE_COMMAND = ardrone_interface
|
||||
SRCS = ardrone_interface.c \
|
||||
ardrone_motor_control.c
|
||||
@@ -0,0 +1,9 @@
|
||||
#
|
||||
# Board-specific startup code for the PX4FMU
|
||||
#
|
||||
|
||||
SRCS = px4fmu_can.c \
|
||||
px4fmu_init.c \
|
||||
px4fmu_pwm_servo.c \
|
||||
px4fmu_spi.c \
|
||||
px4fmu_usb.c
|
||||
@@ -0,0 +1,144 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file px4fmu_can.c
|
||||
*
|
||||
* Board-specific CAN functions.
|
||||
*/
|
||||
|
||||
/************************************************************************************
|
||||
* Included Files
|
||||
************************************************************************************/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <errno.h>
|
||||
#include <debug.h>
|
||||
|
||||
#include <nuttx/can.h>
|
||||
#include <arch/board/board.h>
|
||||
|
||||
#include "chip.h"
|
||||
#include "up_arch.h"
|
||||
|
||||
#include "stm32.h"
|
||||
#include "stm32_can.h"
|
||||
#include "px4fmu_internal.h"
|
||||
|
||||
#ifdef CONFIG_CAN
|
||||
|
||||
/************************************************************************************
|
||||
* Pre-processor Definitions
|
||||
************************************************************************************/
|
||||
/* Configuration ********************************************************************/
|
||||
|
||||
#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2)
|
||||
# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1."
|
||||
# undef CONFIG_STM32_CAN2
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_STM32_CAN1
|
||||
# define CAN_PORT 1
|
||||
#else
|
||||
# define CAN_PORT 2
|
||||
#endif
|
||||
|
||||
/* Debug ***************************************************************************/
|
||||
/* Non-standard debug that may be enabled just for testing CAN */
|
||||
|
||||
#ifdef CONFIG_DEBUG_CAN
|
||||
# define candbg dbg
|
||||
# define canvdbg vdbg
|
||||
# define canlldbg lldbg
|
||||
# define canllvdbg llvdbg
|
||||
#else
|
||||
# define candbg(x...)
|
||||
# define canvdbg(x...)
|
||||
# define canlldbg(x...)
|
||||
# define canllvdbg(x...)
|
||||
#endif
|
||||
|
||||
/************************************************************************************
|
||||
* Private Functions
|
||||
************************************************************************************/
|
||||
|
||||
/************************************************************************************
|
||||
* Public Functions
|
||||
************************************************************************************/
|
||||
|
||||
/************************************************************************************
|
||||
* Name: can_devinit
|
||||
*
|
||||
* Description:
|
||||
* All STM32 architectures must provide the following interface to work with
|
||||
* examples/can.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
int can_devinit(void)
|
||||
{
|
||||
static bool initialized = false;
|
||||
struct can_dev_s *can;
|
||||
int ret;
|
||||
|
||||
/* Check if we have already initialized */
|
||||
|
||||
if (!initialized) {
|
||||
/* Call stm32_caninitialize() to get an instance of the CAN interface */
|
||||
|
||||
can = stm32_caninitialize(CAN_PORT);
|
||||
|
||||
if (can == NULL) {
|
||||
candbg("ERROR: Failed to get CAN interface\n");
|
||||
return -ENODEV;
|
||||
}
|
||||
|
||||
/* Register the CAN driver at "/dev/can0" */
|
||||
|
||||
ret = can_register("/dev/can0", can);
|
||||
|
||||
if (ret < 0) {
|
||||
candbg("ERROR: can_register failed: %d\n", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* Now we are initialized */
|
||||
|
||||
initialized = true;
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,255 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file px4fmu_init.c
|
||||
*
|
||||
* PX4FMU-specific early startup code. This file implements the
|
||||
* nsh_archinitialize() function that is called early by nsh during startup.
|
||||
*
|
||||
* Code here is run before the rcS script is invoked; it should start required
|
||||
* subsystems and perform board-specific initialisation.
|
||||
*/
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdio.h>
|
||||
#include <debug.h>
|
||||
#include <errno.h>
|
||||
|
||||
#include <nuttx/arch.h>
|
||||
#include <nuttx/spi.h>
|
||||
#include <nuttx/i2c.h>
|
||||
#include <nuttx/mmcsd.h>
|
||||
#include <nuttx/analog/adc.h>
|
||||
|
||||
#include "stm32_internal.h"
|
||||
#include "px4fmu_internal.h"
|
||||
#include "stm32_uart.h"
|
||||
|
||||
#include <arch/board/board.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_led.h>
|
||||
|
||||
#include <systemlib/cpuload.h>
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-Processor Definitions
|
||||
****************************************************************************/
|
||||
|
||||
/* Configuration ************************************************************/
|
||||
|
||||
/* Debug ********************************************************************/
|
||||
|
||||
#ifdef CONFIG_CPP_HAVE_VARARGS
|
||||
# ifdef CONFIG_DEBUG
|
||||
# define message(...) lowsyslog(__VA_ARGS__)
|
||||
# else
|
||||
# define message(...) printf(__VA_ARGS__)
|
||||
# endif
|
||||
#else
|
||||
# ifdef CONFIG_DEBUG
|
||||
# define message lowsyslog
|
||||
# else
|
||||
# define message printf
|
||||
# endif
|
||||
#endif
|
||||
|
||||
/****************************************************************************
|
||||
* Protected Functions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
|
||||
/************************************************************************************
|
||||
* Name: stm32_boardinitialize
|
||||
*
|
||||
* Description:
|
||||
* All STM32 architectures must provide the following entry point. This entry point
|
||||
* is called early in the intitialization -- after all memory has been configured
|
||||
* and mapped but before any devices have been initialized.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
__EXPORT void stm32_boardinitialize(void)
|
||||
{
|
||||
/* configure SPI interfaces */
|
||||
stm32_spiinitialize();
|
||||
|
||||
/* configure LEDs */
|
||||
up_ledinit();
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: nsh_archinitialize
|
||||
*
|
||||
* Description:
|
||||
* Perform architecture specific initialization
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
static struct spi_dev_s *spi1;
|
||||
static struct spi_dev_s *spi2;
|
||||
static struct spi_dev_s *spi3;
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
__EXPORT int matherr(struct __exception *e)
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
#else
|
||||
__EXPORT int matherr(struct exception *e)
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
#endif
|
||||
|
||||
__EXPORT int nsh_archinitialize(void)
|
||||
{
|
||||
int result;
|
||||
|
||||
/* configure always-on ADC pins */
|
||||
stm32_configgpio(GPIO_ADC1_IN10);
|
||||
stm32_configgpio(GPIO_ADC1_IN11);
|
||||
/* IN12 and IN13 further below */
|
||||
|
||||
/* configure the high-resolution time/callout interface */
|
||||
hrt_init();
|
||||
|
||||
/* configure CPU load estimation */
|
||||
#ifdef CONFIG_SCHED_INSTRUMENTATION
|
||||
cpuload_initialize_once();
|
||||
#endif
|
||||
|
||||
/* set up the serial DMA polling */
|
||||
static struct hrt_call serial_dma_call;
|
||||
struct timespec ts;
|
||||
|
||||
/*
|
||||
* Poll at 1ms intervals for received bytes that have not triggered
|
||||
* a DMA event.
|
||||
*/
|
||||
ts.tv_sec = 0;
|
||||
ts.tv_nsec = 1000000;
|
||||
|
||||
hrt_call_every(&serial_dma_call,
|
||||
ts_to_abstime(&ts),
|
||||
ts_to_abstime(&ts),
|
||||
(hrt_callout)stm32_serial_dma_poll,
|
||||
NULL);
|
||||
|
||||
// initial LED state
|
||||
// drv_led_start();
|
||||
up_ledoff(LED_BLUE);
|
||||
up_ledoff(LED_AMBER);
|
||||
up_ledon(LED_BLUE);
|
||||
|
||||
/* Configure SPI-based devices */
|
||||
|
||||
spi1 = up_spiinitialize(1);
|
||||
|
||||
if (!spi1) {
|
||||
message("[boot] FAILED to initialize SPI port 1\r\n");
|
||||
up_ledon(LED_AMBER);
|
||||
return -ENODEV;
|
||||
}
|
||||
|
||||
/* Default SPI1 to 1MHz and de-assert the known chip selects. */
|
||||
SPI_SETFREQUENCY(spi1, 10000000);
|
||||
SPI_SETBITS(spi1, 8);
|
||||
SPI_SETMODE(spi1, SPIDEV_MODE3);
|
||||
SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);
|
||||
SPI_SELECT(spi1, PX4_SPIDEV_ACCEL, false);
|
||||
SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
|
||||
up_udelay(20);
|
||||
|
||||
message("[boot] Successfully initialized SPI port 1\r\n");
|
||||
|
||||
/*
|
||||
* If SPI2 is enabled in the defconfig, we loose some ADC pins as chip selects.
|
||||
* Keep the SPI2 init optional and conditionally initialize the ADC pins
|
||||
*/
|
||||
spi2 = up_spiinitialize(2);
|
||||
|
||||
if (!spi2) {
|
||||
message("[boot] Enabling IN12/13 instead of SPI2\n");
|
||||
/* no SPI2, use pins for ADC */
|
||||
stm32_configgpio(GPIO_ADC1_IN12);
|
||||
stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards
|
||||
} else {
|
||||
/* Default SPI2 to 1MHz and de-assert the known chip selects. */
|
||||
SPI_SETFREQUENCY(spi2, 10000000);
|
||||
SPI_SETBITS(spi2, 8);
|
||||
SPI_SETMODE(spi2, SPIDEV_MODE3);
|
||||
SPI_SELECT(spi2, PX4_SPIDEV_GYRO, false);
|
||||
SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false);
|
||||
|
||||
message("[boot] Initialized SPI port2 (ADC IN12/13 blocked)\n");
|
||||
}
|
||||
|
||||
/* Get the SPI port for the microSD slot */
|
||||
|
||||
message("[boot] Initializing SPI port 3\n");
|
||||
spi3 = up_spiinitialize(3);
|
||||
|
||||
if (!spi3) {
|
||||
message("[boot] FAILED to initialize SPI port 3\n");
|
||||
up_ledon(LED_AMBER);
|
||||
return -ENODEV;
|
||||
}
|
||||
|
||||
message("[boot] Successfully initialized SPI port 3\n");
|
||||
|
||||
/* Now bind the SPI interface to the MMCSD driver */
|
||||
result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3);
|
||||
|
||||
if (result != OK) {
|
||||
message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\n");
|
||||
up_ledon(LED_AMBER);
|
||||
return -ENODEV;
|
||||
}
|
||||
|
||||
message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
|
||||
|
||||
return OK;
|
||||
}
|
||||
@@ -0,0 +1,162 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file px4fmu_internal.h
|
||||
*
|
||||
* PX4FMU internal definitions
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
/****************************************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************************************/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <nuttx/compiler.h>
|
||||
#include <stdint.h>
|
||||
|
||||
__BEGIN_DECLS
|
||||
|
||||
/* these headers are not C++ safe */
|
||||
#include <stm32_internal.h>
|
||||
|
||||
|
||||
/****************************************************************************************************
|
||||
* Definitions
|
||||
****************************************************************************************************/
|
||||
/* Configuration ************************************************************************************/
|
||||
|
||||
//#ifdef CONFIG_STM32_SPI2
|
||||
//# error "SPI2 is not supported on this board"
|
||||
//#endif
|
||||
|
||||
#if defined(CONFIG_STM32_CAN1)
|
||||
# warning "CAN1 is not supported on this board"
|
||||
#endif
|
||||
|
||||
/* PX4FMU GPIOs ***********************************************************************************/
|
||||
/* LEDs */
|
||||
|
||||
#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15)
|
||||
#define GPIO_LED2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14)
|
||||
|
||||
/* External interrupts */
|
||||
#define GPIO_EXTI_COMPASS (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1)
|
||||
// XXX MPU6000 DRDY?
|
||||
|
||||
/* SPI chip selects */
|
||||
|
||||
#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14)
|
||||
#define GPIO_SPI_CS_ACCEL (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
|
||||
#define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
|
||||
#define GPIO_SPI_CS_SDCARD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4)
|
||||
|
||||
/* User GPIOs
|
||||
*
|
||||
* GPIO0-1 are the buffered high-power GPIOs.
|
||||
* GPIO2-5 are the USART2 pins.
|
||||
* GPIO6-7 are the CAN2 pins.
|
||||
*/
|
||||
#define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4)
|
||||
#define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN5)
|
||||
#define GPIO_GPIO2_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN0)
|
||||
#define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN1)
|
||||
#define GPIO_GPIO4_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN2)
|
||||
#define GPIO_GPIO5_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN3)
|
||||
#define GPIO_GPIO6_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN13)
|
||||
#define GPIO_GPIO7_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN2)
|
||||
#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN4)
|
||||
#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN5)
|
||||
#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
|
||||
#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1)
|
||||
#define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2)
|
||||
#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN3)
|
||||
#define GPIO_GPIO6_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN13)
|
||||
#define GPIO_GPIO7_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN12)
|
||||
#define GPIO_GPIO_DIR (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
|
||||
|
||||
/* USB OTG FS
|
||||
*
|
||||
* PA9 OTG_FS_VBUS VBUS sensing (also connected to the green LED)
|
||||
*/
|
||||
#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9)
|
||||
|
||||
/* PWM
|
||||
*
|
||||
* The PX4FMU has five PWM outputs, of which only the output on
|
||||
* pin PC8 is fixed assigned to this function. The other four possible
|
||||
* pwm sources are the TX, RX, RTS and CTS pins of USART2
|
||||
*
|
||||
* Alternate function mapping:
|
||||
* PC8 - BUZZER - TIM8_CH3/SDIO_D0 /TIM3_CH3/ USART6_CK / DCMI_D2
|
||||
*/
|
||||
|
||||
#ifdef CONFIG_PWM
|
||||
# if defined(CONFIG_STM32_TIM3_PWM)
|
||||
# define BUZZER_PWMCHANNEL 3
|
||||
# define BUZZER_PWMTIMER 3
|
||||
# elif defined(CONFIG_STM32_TIM8_PWM)
|
||||
# define BUZZER_PWMCHANNEL 3
|
||||
# define BUZZER_PWMTIMER 8
|
||||
# endif
|
||||
#endif
|
||||
|
||||
/****************************************************************************************************
|
||||
* Public Types
|
||||
****************************************************************************************************/
|
||||
|
||||
/****************************************************************************************************
|
||||
* Public data
|
||||
****************************************************************************************************/
|
||||
|
||||
#ifndef __ASSEMBLY__
|
||||
|
||||
/****************************************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************************************/
|
||||
|
||||
/****************************************************************************************************
|
||||
* Name: stm32_spiinitialize
|
||||
*
|
||||
* Description:
|
||||
* Called to configure SPI chip select GPIO pins for the PX4FMU board.
|
||||
*
|
||||
****************************************************************************************************/
|
||||
|
||||
extern void stm32_spiinitialize(void);
|
||||
|
||||
#endif /* __ASSEMBLY__ */
|
||||
|
||||
__END_DECLS
|
||||
@@ -0,0 +1,88 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file px4fmu_led.c
|
||||
*
|
||||
* PX4FMU LED backend.
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <debug.h>
|
||||
|
||||
#include <arch/board/board.h>
|
||||
|
||||
#include "chip.h"
|
||||
#include "up_arch.h"
|
||||
#include "up_internal.h"
|
||||
#include "stm32_internal.h"
|
||||
#include "px4fmu_internal.h"
|
||||
|
||||
__EXPORT void up_ledinit()
|
||||
{
|
||||
/* Configure LED1-2 GPIOs for output */
|
||||
|
||||
stm32_configgpio(GPIO_LED1);
|
||||
stm32_configgpio(GPIO_LED2);
|
||||
}
|
||||
|
||||
__EXPORT void up_ledon(int led)
|
||||
{
|
||||
if (led == 0)
|
||||
{
|
||||
/* Pull down to switch on */
|
||||
stm32_gpiowrite(GPIO_LED1, false);
|
||||
}
|
||||
if (led == 1)
|
||||
{
|
||||
/* Pull down to switch on */
|
||||
stm32_gpiowrite(GPIO_LED2, false);
|
||||
}
|
||||
}
|
||||
|
||||
__EXPORT void up_ledoff(int led)
|
||||
{
|
||||
if (led == 0)
|
||||
{
|
||||
/* Pull up to switch off */
|
||||
stm32_gpiowrite(GPIO_LED1, true);
|
||||
}
|
||||
if (led == 1)
|
||||
{
|
||||
/* Pull up to switch off */
|
||||
stm32_gpiowrite(GPIO_LED2, true);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,87 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/*
|
||||
* @file px4fmu_pwm_servo.c
|
||||
*
|
||||
* Configuration data for the stm32 pwm_servo driver.
|
||||
*
|
||||
* Note that these arrays must always be fully-sized.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include <drivers/stm32/drv_pwm_servo.h>
|
||||
|
||||
#include <arch/board/board.h>
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
|
||||
#include <stm32_internal.h>
|
||||
#include <stm32_gpio.h>
|
||||
#include <stm32_tim.h>
|
||||
|
||||
__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
|
||||
{
|
||||
.base = STM32_TIM2_BASE,
|
||||
.clock_register = STM32_RCC_APB1ENR,
|
||||
.clock_bit = RCC_APB1ENR_TIM2EN,
|
||||
.clock_freq = STM32_APB1_TIM2_CLKIN
|
||||
}
|
||||
};
|
||||
|
||||
__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
|
||||
{
|
||||
.gpio = GPIO_TIM2_CH1OUT,
|
||||
.timer_index = 0,
|
||||
.timer_channel = 1,
|
||||
.default_value = 1000,
|
||||
},
|
||||
{
|
||||
.gpio = GPIO_TIM2_CH2OUT,
|
||||
.timer_index = 0,
|
||||
.timer_channel = 2,
|
||||
.default_value = 1000,
|
||||
},
|
||||
{
|
||||
.gpio = GPIO_TIM2_CH3OUT,
|
||||
.timer_index = 0,
|
||||
.timer_channel = 3,
|
||||
.default_value = 1000,
|
||||
},
|
||||
{
|
||||
.gpio = GPIO_TIM2_CH4OUT,
|
||||
.timer_index = 0,
|
||||
.timer_channel = 4,
|
||||
.default_value = 1000,
|
||||
}
|
||||
};
|
||||
@@ -0,0 +1,154 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file px4fmu_spi.c
|
||||
*
|
||||
* Board-specific SPI functions.
|
||||
*/
|
||||
|
||||
/************************************************************************************
|
||||
* Included Files
|
||||
************************************************************************************/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <debug.h>
|
||||
|
||||
#include <nuttx/spi.h>
|
||||
#include <arch/board/board.h>
|
||||
|
||||
#include "up_arch.h"
|
||||
#include "chip.h"
|
||||
#include "stm32_internal.h"
|
||||
#include "px4fmu_internal.h"
|
||||
|
||||
/************************************************************************************
|
||||
* Public Functions
|
||||
************************************************************************************/
|
||||
|
||||
/************************************************************************************
|
||||
* Name: stm32_spiinitialize
|
||||
*
|
||||
* Description:
|
||||
* Called to configure SPI chip select GPIO pins for the PX4FMU board.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
__EXPORT void weak_function stm32_spiinitialize(void)
|
||||
{
|
||||
stm32_configgpio(GPIO_SPI_CS_GYRO);
|
||||
stm32_configgpio(GPIO_SPI_CS_ACCEL);
|
||||
stm32_configgpio(GPIO_SPI_CS_MPU);
|
||||
stm32_configgpio(GPIO_SPI_CS_SDCARD);
|
||||
|
||||
/* De-activate all peripherals,
|
||||
* required for some peripheral
|
||||
* state machines
|
||||
*/
|
||||
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
|
||||
stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1);
|
||||
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
|
||||
stm32_gpiowrite(GPIO_SPI_CS_SDCARD, 1);
|
||||
}
|
||||
|
||||
__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
|
||||
{
|
||||
/* SPI select is active low, so write !selected to select the device */
|
||||
|
||||
switch (devid) {
|
||||
case PX4_SPIDEV_GYRO:
|
||||
/* Making sure the other peripherals are not selected */
|
||||
stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
|
||||
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
|
||||
stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1);
|
||||
break;
|
||||
|
||||
case PX4_SPIDEV_ACCEL:
|
||||
/* Making sure the other peripherals are not selected */
|
||||
stm32_gpiowrite(GPIO_SPI_CS_ACCEL, !selected);
|
||||
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
|
||||
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
|
||||
break;
|
||||
|
||||
case PX4_SPIDEV_MPU:
|
||||
/* Making sure the other peripherals are not selected */
|
||||
stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1);
|
||||
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
|
||||
stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
|
||||
{
|
||||
return SPI_STATUS_PRESENT;
|
||||
}
|
||||
|
||||
__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
|
||||
{
|
||||
/* SPI select is active low, so write !selected to select the device */
|
||||
|
||||
switch (devid) {
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
|
||||
{
|
||||
return SPI_STATUS_PRESENT;
|
||||
}
|
||||
|
||||
|
||||
__EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
|
||||
{
|
||||
/* there can only be one device on this bus, so always select it */
|
||||
stm32_gpiowrite(GPIO_SPI_CS_SDCARD, !selected);
|
||||
}
|
||||
|
||||
__EXPORT uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
|
||||
{
|
||||
/* this is actually bogus, but PX4 has no way to sense the presence of an SD card */
|
||||
return SPI_STATUS_PRESENT;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,108 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file px4fmu_usb.c
|
||||
*
|
||||
* Board-specific USB functions.
|
||||
*/
|
||||
|
||||
/************************************************************************************
|
||||
* Included Files
|
||||
************************************************************************************/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <debug.h>
|
||||
|
||||
#include <nuttx/usb/usbdev.h>
|
||||
#include <nuttx/usb/usbdev_trace.h>
|
||||
|
||||
#include "up_arch.h"
|
||||
#include "stm32_internal.h"
|
||||
#include "px4fmu_internal.h"
|
||||
|
||||
/************************************************************************************
|
||||
* Definitions
|
||||
************************************************************************************/
|
||||
|
||||
/************************************************************************************
|
||||
* Private Functions
|
||||
************************************************************************************/
|
||||
|
||||
/************************************************************************************
|
||||
* Public Functions
|
||||
************************************************************************************/
|
||||
|
||||
/************************************************************************************
|
||||
* Name: stm32_usbinitialize
|
||||
*
|
||||
* Description:
|
||||
* Called to setup USB-related GPIO pins for the PX4FMU board.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
__EXPORT void stm32_usbinitialize(void)
|
||||
{
|
||||
/* The OTG FS has an internal soft pull-up */
|
||||
|
||||
/* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */
|
||||
|
||||
#ifdef CONFIG_STM32_OTGFS
|
||||
stm32_configgpio(GPIO_OTGFS_VBUS);
|
||||
/* XXX We only support device mode
|
||||
stm32_configgpio(GPIO_OTGFS_PWRON);
|
||||
stm32_configgpio(GPIO_OTGFS_OVER);
|
||||
*/
|
||||
#endif
|
||||
}
|
||||
|
||||
/************************************************************************************
|
||||
* Name: stm32_usbsuspend
|
||||
*
|
||||
* Description:
|
||||
* Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is
|
||||
* used. This function is called whenever the USB enters or leaves suspend mode.
|
||||
* This is an opportunity for the board logic to shutdown clocks, power, etc.
|
||||
* while the USB is suspended.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume)
|
||||
{
|
||||
ulldbg("resume: %d\n", resume);
|
||||
}
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,6 @@
|
||||
#
|
||||
# LSM303D accel/mag driver
|
||||
#
|
||||
|
||||
MODULE_COMMAND = l3gd20
|
||||
SRCS = l3gd20.cpp
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,6 @@
|
||||
#
|
||||
# Interface driver for the PX4FMU board
|
||||
#
|
||||
|
||||
MODULE_COMMAND = fmu
|
||||
SRCS = fmu.cpp
|
||||
@@ -0,0 +1,62 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file visibility.h
|
||||
*
|
||||
* Definitions controlling symbol naming and visibility.
|
||||
*
|
||||
* This file is normally included automatically by the build system.
|
||||
*/
|
||||
|
||||
#ifndef __SYSTEMLIB_VISIBILITY_H
|
||||
#define __SYSTEMLIB_VISIBILITY_H
|
||||
|
||||
#ifdef __EXPORT
|
||||
# undef __EXPORT
|
||||
#endif
|
||||
#define __EXPORT __attribute__ ((visibility ("default")))
|
||||
|
||||
#ifdef __PRIVATE
|
||||
# undef __PRIVATE
|
||||
#endif
|
||||
#define __PRIVATE __attribute__ ((visibility ("hidden")))
|
||||
|
||||
#ifdef __cplusplus
|
||||
# define __BEGIN_DECLS extern "C" {
|
||||
# define __END_DECLS }
|
||||
#else
|
||||
# define __BEGIN_DECLS
|
||||
# define __END_DECLS
|
||||
#endif
|
||||
#endif
|
||||
@@ -0,0 +1,472 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
* Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/*
|
||||
* @file attitude_estimator_ekf_main.c
|
||||
*
|
||||
* Extended Kalman Filter for Attitude Estimation.
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <unistd.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <stdbool.h>
|
||||
#include <poll.h>
|
||||
#include <fcntl.h>
|
||||
#include <v1.0/common/mavlink.h>
|
||||
#include <float.h>
|
||||
#include <nuttx/sched.h>
|
||||
#include <sys/prctl.h>
|
||||
#include <termios.h>
|
||||
#include <errno.h>
|
||||
#include <limits.h>
|
||||
#include <math.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/debug_key_value.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <systemlib/err.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
#include "codegen/attitudeKalmanfilter_initialize.h"
|
||||
#include "codegen/attitudeKalmanfilter.h"
|
||||
#include "attitude_estimator_ekf_params.h"
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
extern "C" __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]);
|
||||
|
||||
static bool thread_should_exit = false; /**< Deamon exit flag */
|
||||
static bool thread_running = false; /**< Deamon status flag */
|
||||
static int attitude_estimator_ekf_task; /**< Handle of deamon task / thread */
|
||||
|
||||
/**
|
||||
* Mainloop of attitude_estimator_ekf.
|
||||
*/
|
||||
int attitude_estimator_ekf_thread_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Print the correct usage.
|
||||
*/
|
||||
static void usage(const char *reason);
|
||||
|
||||
static void
|
||||
usage(const char *reason)
|
||||
{
|
||||
if (reason)
|
||||
fprintf(stderr, "%s\n", reason);
|
||||
|
||||
fprintf(stderr, "usage: attitude_estimator_ekf {start|stop|status} [-p <additional params>]\n\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
/**
|
||||
* The attitude_estimator_ekf app only briefly exists to start
|
||||
* the background job. The stack size assigned in the
|
||||
* Makefile does only apply to this management task.
|
||||
*
|
||||
* The actual stack size should be set in the call
|
||||
* to task_create().
|
||||
*/
|
||||
int attitude_estimator_ekf_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 1)
|
||||
usage("missing command");
|
||||
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
if (thread_running) {
|
||||
printf("attitude_estimator_ekf already running\n");
|
||||
/* this is not an error */
|
||||
exit(0);
|
||||
}
|
||||
|
||||
thread_should_exit = false;
|
||||
attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
12400,
|
||||
attitude_estimator_ekf_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
thread_should_exit = true;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
if (thread_running) {
|
||||
printf("\tattitude_estimator_ekf app is running\n");
|
||||
|
||||
} else {
|
||||
printf("\tattitude_estimator_ekf app not started\n");
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
usage("unrecognized command");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
/*
|
||||
* [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst)
|
||||
*/
|
||||
|
||||
/*
|
||||
* EKF Attitude Estimator main function.
|
||||
*
|
||||
* Estimates the attitude recursively once started.
|
||||
*
|
||||
* @param argc number of commandline arguments (plus command name)
|
||||
* @param argv strings containing the arguments
|
||||
*/
|
||||
int attitude_estimator_ekf_thread_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
|
||||
|
||||
float dt = 0.005f;
|
||||
/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
|
||||
float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */
|
||||
float x_aposteriori_k[12]; /**< states */
|
||||
float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f,
|
||||
}; /**< init: diagonal matrix with big values */
|
||||
|
||||
float x_aposteriori[12];
|
||||
float P_aposteriori[144];
|
||||
|
||||
/* output euler angles */
|
||||
float euler[3] = {0.0f, 0.0f, 0.0f};
|
||||
|
||||
float Rot_matrix[9] = {1.f, 0, 0,
|
||||
0, 1.f, 0,
|
||||
0, 0, 1.f
|
||||
}; /**< init: identity matrix */
|
||||
|
||||
// print text
|
||||
printf("Extended Kalman Filter Attitude Estimator initialized..\n\n");
|
||||
fflush(stdout);
|
||||
|
||||
int overloadcounter = 19;
|
||||
|
||||
/* Initialize filter */
|
||||
attitudeKalmanfilter_initialize();
|
||||
|
||||
/* store start time to guard against too slow update rates */
|
||||
uint64_t last_run = hrt_absolute_time();
|
||||
|
||||
struct sensor_combined_s raw;
|
||||
memset(&raw, 0, sizeof(raw));
|
||||
struct vehicle_attitude_s att;
|
||||
memset(&att, 0, sizeof(att));
|
||||
struct vehicle_status_s state;
|
||||
memset(&state, 0, sizeof(state));
|
||||
|
||||
uint64_t last_data = 0;
|
||||
uint64_t last_measurement = 0;
|
||||
|
||||
/* subscribe to raw data */
|
||||
int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
|
||||
/* rate-limit raw data updates to 200Hz */
|
||||
orb_set_interval(sub_raw, 4);
|
||||
|
||||
/* subscribe to param changes */
|
||||
int sub_params = orb_subscribe(ORB_ID(parameter_update));
|
||||
|
||||
/* subscribe to system state*/
|
||||
int sub_state = orb_subscribe(ORB_ID(vehicle_status));
|
||||
|
||||
/* advertise attitude */
|
||||
orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
|
||||
|
||||
|
||||
int loopcounter = 0;
|
||||
int printcounter = 0;
|
||||
|
||||
thread_running = true;
|
||||
|
||||
/* advertise debug value */
|
||||
// struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
|
||||
// orb_advert_t pub_dbg = -1;
|
||||
|
||||
float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f};
|
||||
// XXX write this out to perf regs
|
||||
|
||||
/* keep track of sensor updates */
|
||||
uint32_t sensor_last_count[3] = {0, 0, 0};
|
||||
uint64_t sensor_last_timestamp[3] = {0, 0, 0};
|
||||
|
||||
struct attitude_estimator_ekf_params ekf_params;
|
||||
|
||||
struct attitude_estimator_ekf_param_handles ekf_param_handles;
|
||||
|
||||
/* initialize parameter handles */
|
||||
parameters_init(&ekf_param_handles);
|
||||
|
||||
uint64_t start_time = hrt_absolute_time();
|
||||
bool initialized = false;
|
||||
|
||||
float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
|
||||
unsigned offset_count = 0;
|
||||
|
||||
/* register the perf counter */
|
||||
perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf");
|
||||
|
||||
/* Main loop*/
|
||||
while (!thread_should_exit) {
|
||||
|
||||
struct pollfd fds[2];
|
||||
fds[0].fd = sub_raw;
|
||||
fds[0].events = POLLIN;
|
||||
fds[1].fd = sub_params;
|
||||
fds[1].events = POLLIN;
|
||||
int ret = poll(fds, 2, 1000);
|
||||
|
||||
if (ret < 0) {
|
||||
/* XXX this is seriously bad - should be an emergency */
|
||||
} else if (ret == 0) {
|
||||
/* check if we're in HIL - not getting sensor data is fine then */
|
||||
orb_copy(ORB_ID(vehicle_status), sub_state, &state);
|
||||
|
||||
if (!state.flag_hil_enabled) {
|
||||
fprintf(stderr,
|
||||
"[att ekf] WARNING: Not getting sensors - sensor app running?\n");
|
||||
}
|
||||
|
||||
} else {
|
||||
|
||||
/* only update parameters if they changed */
|
||||
if (fds[1].revents & POLLIN) {
|
||||
/* read from param to clear updated flag */
|
||||
struct parameter_update_s update;
|
||||
orb_copy(ORB_ID(parameter_update), sub_params, &update);
|
||||
|
||||
/* update parameters */
|
||||
parameters_update(&ekf_param_handles, &ekf_params);
|
||||
}
|
||||
|
||||
/* only run filter if sensor values changed */
|
||||
if (fds[0].revents & POLLIN) {
|
||||
|
||||
/* get latest measurements */
|
||||
orb_copy(ORB_ID(sensor_combined), sub_raw, &raw);
|
||||
|
||||
if (!initialized) {
|
||||
|
||||
gyro_offsets[0] += raw.gyro_rad_s[0];
|
||||
gyro_offsets[1] += raw.gyro_rad_s[1];
|
||||
gyro_offsets[2] += raw.gyro_rad_s[2];
|
||||
offset_count++;
|
||||
|
||||
if (hrt_absolute_time() - start_time > 3000000LL) {
|
||||
initialized = true;
|
||||
gyro_offsets[0] /= offset_count;
|
||||
gyro_offsets[1] /= offset_count;
|
||||
gyro_offsets[2] /= offset_count;
|
||||
}
|
||||
|
||||
} else {
|
||||
|
||||
perf_begin(ekf_loop_perf);
|
||||
|
||||
/* Calculate data time difference in seconds */
|
||||
dt = (raw.timestamp - last_measurement) / 1000000.0f;
|
||||
last_measurement = raw.timestamp;
|
||||
uint8_t update_vect[3] = {0, 0, 0};
|
||||
|
||||
/* Fill in gyro measurements */
|
||||
if (sensor_last_count[0] != raw.gyro_counter) {
|
||||
update_vect[0] = 1;
|
||||
sensor_last_count[0] = raw.gyro_counter;
|
||||
sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
|
||||
sensor_last_timestamp[0] = raw.timestamp;
|
||||
}
|
||||
|
||||
z_k[0] = raw.gyro_rad_s[0] - gyro_offsets[0];
|
||||
z_k[1] = raw.gyro_rad_s[1] - gyro_offsets[1];
|
||||
z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2];
|
||||
|
||||
/* update accelerometer measurements */
|
||||
if (sensor_last_count[1] != raw.accelerometer_counter) {
|
||||
update_vect[1] = 1;
|
||||
sensor_last_count[1] = raw.accelerometer_counter;
|
||||
sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
|
||||
sensor_last_timestamp[1] = raw.timestamp;
|
||||
}
|
||||
|
||||
z_k[3] = raw.accelerometer_m_s2[0];
|
||||
z_k[4] = raw.accelerometer_m_s2[1];
|
||||
z_k[5] = raw.accelerometer_m_s2[2];
|
||||
|
||||
/* update magnetometer measurements */
|
||||
if (sensor_last_count[2] != raw.magnetometer_counter) {
|
||||
update_vect[2] = 1;
|
||||
sensor_last_count[2] = raw.magnetometer_counter;
|
||||
sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
|
||||
sensor_last_timestamp[2] = raw.timestamp;
|
||||
}
|
||||
|
||||
z_k[6] = raw.magnetometer_ga[0];
|
||||
z_k[7] = raw.magnetometer_ga[1];
|
||||
z_k[8] = raw.magnetometer_ga[2];
|
||||
|
||||
uint64_t now = hrt_absolute_time();
|
||||
unsigned int time_elapsed = now - last_run;
|
||||
last_run = now;
|
||||
|
||||
if (time_elapsed > loop_interval_alarm) {
|
||||
//TODO: add warning, cpu overload here
|
||||
// if (overloadcounter == 20) {
|
||||
// printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
|
||||
// overloadcounter = 0;
|
||||
// }
|
||||
|
||||
overloadcounter++;
|
||||
}
|
||||
|
||||
static bool const_initialized = false;
|
||||
|
||||
/* initialize with good values once we have a reasonable dt estimate */
|
||||
if (!const_initialized && dt < 0.05f && dt > 0.005f) {
|
||||
dt = 0.005f;
|
||||
parameters_update(&ekf_param_handles, &ekf_params);
|
||||
|
||||
x_aposteriori_k[0] = z_k[0];
|
||||
x_aposteriori_k[1] = z_k[1];
|
||||
x_aposteriori_k[2] = z_k[2];
|
||||
x_aposteriori_k[3] = 0.0f;
|
||||
x_aposteriori_k[4] = 0.0f;
|
||||
x_aposteriori_k[5] = 0.0f;
|
||||
x_aposteriori_k[6] = z_k[3];
|
||||
x_aposteriori_k[7] = z_k[4];
|
||||
x_aposteriori_k[8] = z_k[5];
|
||||
x_aposteriori_k[9] = z_k[6];
|
||||
x_aposteriori_k[10] = z_k[7];
|
||||
x_aposteriori_k[11] = z_k[8];
|
||||
|
||||
const_initialized = true;
|
||||
}
|
||||
|
||||
/* do not execute the filter if not initialized */
|
||||
if (!const_initialized) {
|
||||
continue;
|
||||
}
|
||||
|
||||
uint64_t timing_start = hrt_absolute_time();
|
||||
|
||||
attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r,
|
||||
euler, Rot_matrix, x_aposteriori, P_aposteriori);
|
||||
|
||||
/* swap values for next iteration, check for fatal inputs */
|
||||
if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {
|
||||
memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k));
|
||||
memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k));
|
||||
|
||||
} else {
|
||||
/* due to inputs or numerical failure the output is invalid, skip it */
|
||||
continue;
|
||||
}
|
||||
|
||||
if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data);
|
||||
|
||||
last_data = raw.timestamp;
|
||||
|
||||
/* send out */
|
||||
att.timestamp = raw.timestamp;
|
||||
|
||||
// XXX Apply the same transformation to the rotation matrix
|
||||
att.roll = euler[0] - ekf_params.roll_off;
|
||||
att.pitch = euler[1] - ekf_params.pitch_off;
|
||||
att.yaw = euler[2] - ekf_params.yaw_off;
|
||||
|
||||
att.rollspeed = x_aposteriori[0];
|
||||
att.pitchspeed = x_aposteriori[1];
|
||||
att.yawspeed = x_aposteriori[2];
|
||||
att.rollacc = x_aposteriori[3];
|
||||
att.pitchacc = x_aposteriori[4];
|
||||
att.yawacc = x_aposteriori[5];
|
||||
|
||||
//att.yawspeed =z_k[2] ;
|
||||
/* copy offsets */
|
||||
memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));
|
||||
|
||||
/* copy rotation matrix */
|
||||
memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix));
|
||||
att.R_valid = true;
|
||||
|
||||
if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
|
||||
// Broadcast
|
||||
orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
|
||||
|
||||
} else {
|
||||
warnx("NaN in roll/pitch/yaw estimate!");
|
||||
}
|
||||
|
||||
perf_end(ekf_loop_perf);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
loopcounter++;
|
||||
}
|
||||
|
||||
thread_running = false;
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,105 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
* Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/*
|
||||
* @file attitude_estimator_ekf_params.c
|
||||
*
|
||||
* Parameters for EKF filter
|
||||
*/
|
||||
|
||||
#include "attitude_estimator_ekf_params.h"
|
||||
|
||||
/* Extended Kalman Filter covariances */
|
||||
|
||||
/* gyro process noise */
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q0, 1e-4f);
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q1, 0.08f);
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q2, 0.009f);
|
||||
/* gyro offsets process noise */
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q3, 0.005f);
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q4, 0.0f);
|
||||
|
||||
/* gyro measurement noise */
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V2_R0, 0.0008f);
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V2_R1, 0.8f);
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V2_R2, 1.0f);
|
||||
/* accelerometer measurement noise */
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V2_R3, 0.0f);
|
||||
|
||||
/* offsets in roll, pitch and yaw of sensor plane and body */
|
||||
PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f);
|
||||
|
||||
int parameters_init(struct attitude_estimator_ekf_param_handles *h)
|
||||
{
|
||||
/* PID parameters */
|
||||
h->q0 = param_find("EKF_ATT_V2_Q0");
|
||||
h->q1 = param_find("EKF_ATT_V2_Q1");
|
||||
h->q2 = param_find("EKF_ATT_V2_Q2");
|
||||
h->q3 = param_find("EKF_ATT_V2_Q3");
|
||||
h->q4 = param_find("EKF_ATT_V2_Q4");
|
||||
|
||||
h->r0 = param_find("EKF_ATT_V2_R0");
|
||||
h->r1 = param_find("EKF_ATT_V2_R1");
|
||||
h->r2 = param_find("EKF_ATT_V2_R2");
|
||||
h->r3 = param_find("EKF_ATT_V2_R3");
|
||||
|
||||
h->roll_off = param_find("ATT_ROLL_OFFS");
|
||||
h->pitch_off = param_find("ATT_PITCH_OFFS");
|
||||
h->yaw_off = param_find("ATT_YAW_OFFS");
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p)
|
||||
{
|
||||
param_get(h->q0, &(p->q[0]));
|
||||
param_get(h->q1, &(p->q[1]));
|
||||
param_get(h->q2, &(p->q[2]));
|
||||
param_get(h->q3, &(p->q[3]));
|
||||
param_get(h->q4, &(p->q[4]));
|
||||
|
||||
param_get(h->r0, &(p->r[0]));
|
||||
param_get(h->r1, &(p->r[1]));
|
||||
param_get(h->r2, &(p->r[2]));
|
||||
param_get(h->r3, &(p->r[3]));
|
||||
|
||||
param_get(h->roll_off, &(p->roll_off));
|
||||
param_get(h->pitch_off, &(p->pitch_off));
|
||||
param_get(h->yaw_off, &(p->yaw_off));
|
||||
|
||||
return OK;
|
||||
}
|
||||
@@ -0,0 +1,68 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
* Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/*
|
||||
* @file attitude_estimator_ekf_params.h
|
||||
*
|
||||
* Parameters for EKF filter
|
||||
*/
|
||||
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
struct attitude_estimator_ekf_params {
|
||||
float r[9];
|
||||
float q[12];
|
||||
float roll_off;
|
||||
float pitch_off;
|
||||
float yaw_off;
|
||||
};
|
||||
|
||||
struct attitude_estimator_ekf_param_handles {
|
||||
param_t r0, r1, r2, r3;
|
||||
param_t q0, q1, q2, q3, q4;
|
||||
param_t roll_off, pitch_off, yaw_off;
|
||||
};
|
||||
|
||||
/**
|
||||
* Initialize all parameter handles and values
|
||||
*
|
||||
*/
|
||||
int parameters_init(struct attitude_estimator_ekf_param_handles *h);
|
||||
|
||||
/**
|
||||
* Update all parameters
|
||||
*
|
||||
*/
|
||||
int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p);
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,34 @@
|
||||
/*
|
||||
* attitudeKalmanfilter.h
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Sat Jan 19 15:25:29 2013
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __ATTITUDEKALMANFILTER_H__
|
||||
#define __ATTITUDEKALMANFILTER_H__
|
||||
/* Include files */
|
||||
#include <math.h>
|
||||
#include <stddef.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include "rt_defines.h"
|
||||
#include "rt_nonfinite.h"
|
||||
|
||||
#include "rtwtypes.h"
|
||||
#include "attitudeKalmanfilter_types.h"
|
||||
|
||||
/* Type Definitions */
|
||||
|
||||
/* Named Constants */
|
||||
|
||||
/* Variable Declarations */
|
||||
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
extern void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]);
|
||||
#endif
|
||||
/* End of code generation (attitudeKalmanfilter.h) */
|
||||
+31
@@ -0,0 +1,31 @@
|
||||
/*
|
||||
* attitudeKalmanfilter_initialize.c
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter_initialize'
|
||||
*
|
||||
* C source code generated on: Sat Jan 19 15:25:29 2013
|
||||
*
|
||||
*/
|
||||
|
||||
/* Include files */
|
||||
#include "rt_nonfinite.h"
|
||||
#include "attitudeKalmanfilter.h"
|
||||
#include "attitudeKalmanfilter_initialize.h"
|
||||
|
||||
/* Type Definitions */
|
||||
|
||||
/* Named Constants */
|
||||
|
||||
/* Variable Declarations */
|
||||
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
|
||||
/* Function Definitions */
|
||||
void attitudeKalmanfilter_initialize(void)
|
||||
{
|
||||
rt_InitInfAndNaN(8U);
|
||||
}
|
||||
|
||||
/* End of code generation (attitudeKalmanfilter_initialize.c) */
|
||||
+34
@@ -0,0 +1,34 @@
|
||||
/*
|
||||
* attitudeKalmanfilter_initialize.h
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter_initialize'
|
||||
*
|
||||
* C source code generated on: Sat Jan 19 15:25:29 2013
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __ATTITUDEKALMANFILTER_INITIALIZE_H__
|
||||
#define __ATTITUDEKALMANFILTER_INITIALIZE_H__
|
||||
/* Include files */
|
||||
#include <math.h>
|
||||
#include <stddef.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include "rt_defines.h"
|
||||
#include "rt_nonfinite.h"
|
||||
|
||||
#include "rtwtypes.h"
|
||||
#include "attitudeKalmanfilter_types.h"
|
||||
|
||||
/* Type Definitions */
|
||||
|
||||
/* Named Constants */
|
||||
|
||||
/* Variable Declarations */
|
||||
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
extern void attitudeKalmanfilter_initialize(void);
|
||||
#endif
|
||||
/* End of code generation (attitudeKalmanfilter_initialize.h) */
|
||||
+31
@@ -0,0 +1,31 @@
|
||||
/*
|
||||
* attitudeKalmanfilter_terminate.c
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter_terminate'
|
||||
*
|
||||
* C source code generated on: Sat Jan 19 15:25:29 2013
|
||||
*
|
||||
*/
|
||||
|
||||
/* Include files */
|
||||
#include "rt_nonfinite.h"
|
||||
#include "attitudeKalmanfilter.h"
|
||||
#include "attitudeKalmanfilter_terminate.h"
|
||||
|
||||
/* Type Definitions */
|
||||
|
||||
/* Named Constants */
|
||||
|
||||
/* Variable Declarations */
|
||||
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
|
||||
/* Function Definitions */
|
||||
void attitudeKalmanfilter_terminate(void)
|
||||
{
|
||||
/* (no terminate code required) */
|
||||
}
|
||||
|
||||
/* End of code generation (attitudeKalmanfilter_terminate.c) */
|
||||
+34
@@ -0,0 +1,34 @@
|
||||
/*
|
||||
* attitudeKalmanfilter_terminate.h
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter_terminate'
|
||||
*
|
||||
* C source code generated on: Sat Jan 19 15:25:29 2013
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __ATTITUDEKALMANFILTER_TERMINATE_H__
|
||||
#define __ATTITUDEKALMANFILTER_TERMINATE_H__
|
||||
/* Include files */
|
||||
#include <math.h>
|
||||
#include <stddef.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include "rt_defines.h"
|
||||
#include "rt_nonfinite.h"
|
||||
|
||||
#include "rtwtypes.h"
|
||||
#include "attitudeKalmanfilter_types.h"
|
||||
|
||||
/* Type Definitions */
|
||||
|
||||
/* Named Constants */
|
||||
|
||||
/* Variable Declarations */
|
||||
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
extern void attitudeKalmanfilter_terminate(void);
|
||||
#endif
|
||||
/* End of code generation (attitudeKalmanfilter_terminate.h) */
|
||||
@@ -0,0 +1,16 @@
|
||||
/*
|
||||
* attitudeKalmanfilter_types.h
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Sat Jan 19 15:25:29 2013
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __ATTITUDEKALMANFILTER_TYPES_H__
|
||||
#define __ATTITUDEKALMANFILTER_TYPES_H__
|
||||
|
||||
/* Type Definitions */
|
||||
|
||||
#endif
|
||||
/* End of code generation (attitudeKalmanfilter_types.h) */
|
||||
+37
@@ -0,0 +1,37 @@
|
||||
/*
|
||||
* cross.c
|
||||
*
|
||||
* Code generation for function 'cross'
|
||||
*
|
||||
* C source code generated on: Sat Jan 19 15:25:29 2013
|
||||
*
|
||||
*/
|
||||
|
||||
/* Include files */
|
||||
#include "rt_nonfinite.h"
|
||||
#include "attitudeKalmanfilter.h"
|
||||
#include "cross.h"
|
||||
|
||||
/* Type Definitions */
|
||||
|
||||
/* Named Constants */
|
||||
|
||||
/* Variable Declarations */
|
||||
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
|
||||
/* Function Definitions */
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void cross(const real32_T a[3], const real32_T b[3], real32_T c[3])
|
||||
{
|
||||
c[0] = a[1] * b[2] - a[2] * b[1];
|
||||
c[1] = a[2] * b[0] - a[0] * b[2];
|
||||
c[2] = a[0] * b[1] - a[1] * b[0];
|
||||
}
|
||||
|
||||
/* End of code generation (cross.c) */
|
||||
+34
@@ -0,0 +1,34 @@
|
||||
/*
|
||||
* cross.h
|
||||
*
|
||||
* Code generation for function 'cross'
|
||||
*
|
||||
* C source code generated on: Sat Jan 19 15:25:29 2013
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __CROSS_H__
|
||||
#define __CROSS_H__
|
||||
/* Include files */
|
||||
#include <math.h>
|
||||
#include <stddef.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include "rt_defines.h"
|
||||
#include "rt_nonfinite.h"
|
||||
|
||||
#include "rtwtypes.h"
|
||||
#include "attitudeKalmanfilter_types.h"
|
||||
|
||||
/* Type Definitions */
|
||||
|
||||
/* Named Constants */
|
||||
|
||||
/* Variable Declarations */
|
||||
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
extern void cross(const real32_T a[3], const real32_T b[3], real32_T c[3]);
|
||||
#endif
|
||||
/* End of code generation (cross.h) */
|
||||
+51
@@ -0,0 +1,51 @@
|
||||
/*
|
||||
* eye.c
|
||||
*
|
||||
* Code generation for function 'eye'
|
||||
*
|
||||
* C source code generated on: Sat Jan 19 15:25:29 2013
|
||||
*
|
||||
*/
|
||||
|
||||
/* Include files */
|
||||
#include "rt_nonfinite.h"
|
||||
#include "attitudeKalmanfilter.h"
|
||||
#include "eye.h"
|
||||
|
||||
/* Type Definitions */
|
||||
|
||||
/* Named Constants */
|
||||
|
||||
/* Variable Declarations */
|
||||
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
|
||||
/* Function Definitions */
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void b_eye(real_T I[144])
|
||||
{
|
||||
int32_T i;
|
||||
memset(&I[0], 0, 144U * sizeof(real_T));
|
||||
for (i = 0; i < 12; i++) {
|
||||
I[i + 12 * i] = 1.0;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void eye(real_T I[9])
|
||||
{
|
||||
int32_T i;
|
||||
memset(&I[0], 0, 9U * sizeof(real_T));
|
||||
for (i = 0; i < 3; i++) {
|
||||
I[i + 3 * i] = 1.0;
|
||||
}
|
||||
}
|
||||
|
||||
/* End of code generation (eye.c) */
|
||||
+35
@@ -0,0 +1,35 @@
|
||||
/*
|
||||
* eye.h
|
||||
*
|
||||
* Code generation for function 'eye'
|
||||
*
|
||||
* C source code generated on: Sat Jan 19 15:25:29 2013
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __EYE_H__
|
||||
#define __EYE_H__
|
||||
/* Include files */
|
||||
#include <math.h>
|
||||
#include <stddef.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include "rt_defines.h"
|
||||
#include "rt_nonfinite.h"
|
||||
|
||||
#include "rtwtypes.h"
|
||||
#include "attitudeKalmanfilter_types.h"
|
||||
|
||||
/* Type Definitions */
|
||||
|
||||
/* Named Constants */
|
||||
|
||||
/* Variable Declarations */
|
||||
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
extern void b_eye(real_T I[144]);
|
||||
extern void eye(real_T I[9]);
|
||||
#endif
|
||||
/* End of code generation (eye.h) */
|
||||
+357
@@ -0,0 +1,357 @@
|
||||
/*
|
||||
* mrdivide.c
|
||||
*
|
||||
* Code generation for function 'mrdivide'
|
||||
*
|
||||
* C source code generated on: Sat Jan 19 15:25:29 2013
|
||||
*
|
||||
*/
|
||||
|
||||
/* Include files */
|
||||
#include "rt_nonfinite.h"
|
||||
#include "attitudeKalmanfilter.h"
|
||||
#include "mrdivide.h"
|
||||
|
||||
/* Type Definitions */
|
||||
|
||||
/* Named Constants */
|
||||
|
||||
/* Variable Declarations */
|
||||
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
|
||||
/* Function Definitions */
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36])
|
||||
{
|
||||
real32_T b_A[9];
|
||||
int32_T rtemp;
|
||||
int32_T k;
|
||||
real32_T b_B[36];
|
||||
int32_T r1;
|
||||
int32_T r2;
|
||||
int32_T r3;
|
||||
real32_T maxval;
|
||||
real32_T a21;
|
||||
real32_T Y[36];
|
||||
for (rtemp = 0; rtemp < 3; rtemp++) {
|
||||
for (k = 0; k < 3; k++) {
|
||||
b_A[k + 3 * rtemp] = B[rtemp + 3 * k];
|
||||
}
|
||||
}
|
||||
|
||||
for (rtemp = 0; rtemp < 12; rtemp++) {
|
||||
for (k = 0; k < 3; k++) {
|
||||
b_B[k + 3 * rtemp] = A[rtemp + 12 * k];
|
||||
}
|
||||
}
|
||||
|
||||
r1 = 0;
|
||||
r2 = 1;
|
||||
r3 = 2;
|
||||
maxval = (real32_T)fabs(b_A[0]);
|
||||
a21 = (real32_T)fabs(b_A[1]);
|
||||
if (a21 > maxval) {
|
||||
maxval = a21;
|
||||
r1 = 1;
|
||||
r2 = 0;
|
||||
}
|
||||
|
||||
if ((real32_T)fabs(b_A[2]) > maxval) {
|
||||
r1 = 2;
|
||||
r2 = 1;
|
||||
r3 = 0;
|
||||
}
|
||||
|
||||
b_A[r2] /= b_A[r1];
|
||||
b_A[r3] /= b_A[r1];
|
||||
b_A[3 + r2] -= b_A[r2] * b_A[3 + r1];
|
||||
b_A[3 + r3] -= b_A[r3] * b_A[3 + r1];
|
||||
b_A[6 + r2] -= b_A[r2] * b_A[6 + r1];
|
||||
b_A[6 + r3] -= b_A[r3] * b_A[6 + r1];
|
||||
if ((real32_T)fabs(b_A[3 + r3]) > (real32_T)fabs(b_A[3 + r2])) {
|
||||
rtemp = r2;
|
||||
r2 = r3;
|
||||
r3 = rtemp;
|
||||
}
|
||||
|
||||
b_A[3 + r3] /= b_A[3 + r2];
|
||||
b_A[6 + r3] -= b_A[3 + r3] * b_A[6 + r2];
|
||||
for (k = 0; k < 12; k++) {
|
||||
Y[3 * k] = b_B[r1 + 3 * k];
|
||||
Y[1 + 3 * k] = b_B[r2 + 3 * k] - Y[3 * k] * b_A[r2];
|
||||
Y[2 + 3 * k] = (b_B[r3 + 3 * k] - Y[3 * k] * b_A[r3]) - Y[1 + 3 * k] * b_A[3
|
||||
+ r3];
|
||||
Y[2 + 3 * k] /= b_A[6 + r3];
|
||||
Y[3 * k] -= Y[2 + 3 * k] * b_A[6 + r1];
|
||||
Y[1 + 3 * k] -= Y[2 + 3 * k] * b_A[6 + r2];
|
||||
Y[1 + 3 * k] /= b_A[3 + r2];
|
||||
Y[3 * k] -= Y[1 + 3 * k] * b_A[3 + r1];
|
||||
Y[3 * k] /= b_A[r1];
|
||||
}
|
||||
|
||||
for (rtemp = 0; rtemp < 3; rtemp++) {
|
||||
for (k = 0; k < 12; k++) {
|
||||
y[k + 12 * rtemp] = Y[rtemp + 3 * k];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72])
|
||||
{
|
||||
real32_T b_A[36];
|
||||
int8_T ipiv[6];
|
||||
int32_T i3;
|
||||
int32_T iy;
|
||||
int32_T j;
|
||||
int32_T c;
|
||||
int32_T ix;
|
||||
real32_T temp;
|
||||
int32_T k;
|
||||
real32_T s;
|
||||
int32_T jy;
|
||||
int32_T ijA;
|
||||
real32_T Y[72];
|
||||
for (i3 = 0; i3 < 6; i3++) {
|
||||
for (iy = 0; iy < 6; iy++) {
|
||||
b_A[iy + 6 * i3] = B[i3 + 6 * iy];
|
||||
}
|
||||
|
||||
ipiv[i3] = (int8_T)(1 + i3);
|
||||
}
|
||||
|
||||
for (j = 0; j < 5; j++) {
|
||||
c = j * 7;
|
||||
iy = 0;
|
||||
ix = c;
|
||||
temp = (real32_T)fabs(b_A[c]);
|
||||
for (k = 2; k <= 6 - j; k++) {
|
||||
ix++;
|
||||
s = (real32_T)fabs(b_A[ix]);
|
||||
if (s > temp) {
|
||||
iy = k - 1;
|
||||
temp = s;
|
||||
}
|
||||
}
|
||||
|
||||
if (b_A[c + iy] != 0.0F) {
|
||||
if (iy != 0) {
|
||||
ipiv[j] = (int8_T)((j + iy) + 1);
|
||||
ix = j;
|
||||
iy += j;
|
||||
for (k = 0; k < 6; k++) {
|
||||
temp = b_A[ix];
|
||||
b_A[ix] = b_A[iy];
|
||||
b_A[iy] = temp;
|
||||
ix += 6;
|
||||
iy += 6;
|
||||
}
|
||||
}
|
||||
|
||||
i3 = (c - j) + 6;
|
||||
for (jy = c + 1; jy + 1 <= i3; jy++) {
|
||||
b_A[jy] /= b_A[c];
|
||||
}
|
||||
}
|
||||
|
||||
iy = c;
|
||||
jy = c + 6;
|
||||
for (k = 1; k <= 5 - j; k++) {
|
||||
temp = b_A[jy];
|
||||
if (b_A[jy] != 0.0F) {
|
||||
ix = c + 1;
|
||||
i3 = (iy - j) + 12;
|
||||
for (ijA = 7 + iy; ijA + 1 <= i3; ijA++) {
|
||||
b_A[ijA] += b_A[ix] * -temp;
|
||||
ix++;
|
||||
}
|
||||
}
|
||||
|
||||
jy += 6;
|
||||
iy += 6;
|
||||
}
|
||||
}
|
||||
|
||||
for (i3 = 0; i3 < 12; i3++) {
|
||||
for (iy = 0; iy < 6; iy++) {
|
||||
Y[iy + 6 * i3] = A[i3 + 12 * iy];
|
||||
}
|
||||
}
|
||||
|
||||
for (jy = 0; jy < 6; jy++) {
|
||||
if (ipiv[jy] != jy + 1) {
|
||||
for (j = 0; j < 12; j++) {
|
||||
temp = Y[jy + 6 * j];
|
||||
Y[jy + 6 * j] = Y[(ipiv[jy] + 6 * j) - 1];
|
||||
Y[(ipiv[jy] + 6 * j) - 1] = temp;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (j = 0; j < 12; j++) {
|
||||
c = 6 * j;
|
||||
for (k = 0; k < 6; k++) {
|
||||
iy = 6 * k;
|
||||
if (Y[k + c] != 0.0F) {
|
||||
for (jy = k + 2; jy < 7; jy++) {
|
||||
Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (j = 0; j < 12; j++) {
|
||||
c = 6 * j;
|
||||
for (k = 5; k > -1; k += -1) {
|
||||
iy = 6 * k;
|
||||
if (Y[k + c] != 0.0F) {
|
||||
Y[k + c] /= b_A[k + iy];
|
||||
for (jy = 0; jy + 1 <= k; jy++) {
|
||||
Y[jy + c] -= Y[k + c] * b_A[jy + iy];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (i3 = 0; i3 < 6; i3++) {
|
||||
for (iy = 0; iy < 12; iy++) {
|
||||
y[iy + 12 * i3] = Y[i3 + 6 * iy];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])
|
||||
{
|
||||
real32_T b_A[81];
|
||||
int8_T ipiv[9];
|
||||
int32_T i2;
|
||||
int32_T iy;
|
||||
int32_T j;
|
||||
int32_T c;
|
||||
int32_T ix;
|
||||
real32_T temp;
|
||||
int32_T k;
|
||||
real32_T s;
|
||||
int32_T jy;
|
||||
int32_T ijA;
|
||||
real32_T Y[108];
|
||||
for (i2 = 0; i2 < 9; i2++) {
|
||||
for (iy = 0; iy < 9; iy++) {
|
||||
b_A[iy + 9 * i2] = B[i2 + 9 * iy];
|
||||
}
|
||||
|
||||
ipiv[i2] = (int8_T)(1 + i2);
|
||||
}
|
||||
|
||||
for (j = 0; j < 8; j++) {
|
||||
c = j * 10;
|
||||
iy = 0;
|
||||
ix = c;
|
||||
temp = (real32_T)fabs(b_A[c]);
|
||||
for (k = 2; k <= 9 - j; k++) {
|
||||
ix++;
|
||||
s = (real32_T)fabs(b_A[ix]);
|
||||
if (s > temp) {
|
||||
iy = k - 1;
|
||||
temp = s;
|
||||
}
|
||||
}
|
||||
|
||||
if (b_A[c + iy] != 0.0F) {
|
||||
if (iy != 0) {
|
||||
ipiv[j] = (int8_T)((j + iy) + 1);
|
||||
ix = j;
|
||||
iy += j;
|
||||
for (k = 0; k < 9; k++) {
|
||||
temp = b_A[ix];
|
||||
b_A[ix] = b_A[iy];
|
||||
b_A[iy] = temp;
|
||||
ix += 9;
|
||||
iy += 9;
|
||||
}
|
||||
}
|
||||
|
||||
i2 = (c - j) + 9;
|
||||
for (jy = c + 1; jy + 1 <= i2; jy++) {
|
||||
b_A[jy] /= b_A[c];
|
||||
}
|
||||
}
|
||||
|
||||
iy = c;
|
||||
jy = c + 9;
|
||||
for (k = 1; k <= 8 - j; k++) {
|
||||
temp = b_A[jy];
|
||||
if (b_A[jy] != 0.0F) {
|
||||
ix = c + 1;
|
||||
i2 = (iy - j) + 18;
|
||||
for (ijA = 10 + iy; ijA + 1 <= i2; ijA++) {
|
||||
b_A[ijA] += b_A[ix] * -temp;
|
||||
ix++;
|
||||
}
|
||||
}
|
||||
|
||||
jy += 9;
|
||||
iy += 9;
|
||||
}
|
||||
}
|
||||
|
||||
for (i2 = 0; i2 < 12; i2++) {
|
||||
for (iy = 0; iy < 9; iy++) {
|
||||
Y[iy + 9 * i2] = A[i2 + 12 * iy];
|
||||
}
|
||||
}
|
||||
|
||||
for (jy = 0; jy < 9; jy++) {
|
||||
if (ipiv[jy] != jy + 1) {
|
||||
for (j = 0; j < 12; j++) {
|
||||
temp = Y[jy + 9 * j];
|
||||
Y[jy + 9 * j] = Y[(ipiv[jy] + 9 * j) - 1];
|
||||
Y[(ipiv[jy] + 9 * j) - 1] = temp;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (j = 0; j < 12; j++) {
|
||||
c = 9 * j;
|
||||
for (k = 0; k < 9; k++) {
|
||||
iy = 9 * k;
|
||||
if (Y[k + c] != 0.0F) {
|
||||
for (jy = k + 2; jy < 10; jy++) {
|
||||
Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (j = 0; j < 12; j++) {
|
||||
c = 9 * j;
|
||||
for (k = 8; k > -1; k += -1) {
|
||||
iy = 9 * k;
|
||||
if (Y[k + c] != 0.0F) {
|
||||
Y[k + c] /= b_A[k + iy];
|
||||
for (jy = 0; jy + 1 <= k; jy++) {
|
||||
Y[jy + c] -= Y[k + c] * b_A[jy + iy];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (i2 = 0; i2 < 9; i2++) {
|
||||
for (iy = 0; iy < 12; iy++) {
|
||||
y[iy + 12 * i2] = Y[i2 + 9 * iy];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* End of code generation (mrdivide.c) */
|
||||
+36
@@ -0,0 +1,36 @@
|
||||
/*
|
||||
* mrdivide.h
|
||||
*
|
||||
* Code generation for function 'mrdivide'
|
||||
*
|
||||
* C source code generated on: Sat Jan 19 15:25:29 2013
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __MRDIVIDE_H__
|
||||
#define __MRDIVIDE_H__
|
||||
/* Include files */
|
||||
#include <math.h>
|
||||
#include <stddef.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include "rt_defines.h"
|
||||
#include "rt_nonfinite.h"
|
||||
|
||||
#include "rtwtypes.h"
|
||||
#include "attitudeKalmanfilter_types.h"
|
||||
|
||||
/* Type Definitions */
|
||||
|
||||
/* Named Constants */
|
||||
|
||||
/* Variable Declarations */
|
||||
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
extern void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]);
|
||||
extern void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]);
|
||||
extern void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]);
|
||||
#endif
|
||||
/* End of code generation (mrdivide.h) */
|
||||
+54
@@ -0,0 +1,54 @@
|
||||
/*
|
||||
* norm.c
|
||||
*
|
||||
* Code generation for function 'norm'
|
||||
*
|
||||
* C source code generated on: Sat Jan 19 15:25:29 2013
|
||||
*
|
||||
*/
|
||||
|
||||
/* Include files */
|
||||
#include "rt_nonfinite.h"
|
||||
#include "attitudeKalmanfilter.h"
|
||||
#include "norm.h"
|
||||
|
||||
/* Type Definitions */
|
||||
|
||||
/* Named Constants */
|
||||
|
||||
/* Variable Declarations */
|
||||
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
|
||||
/* Function Definitions */
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
real32_T norm(const real32_T x[3])
|
||||
{
|
||||
real32_T y;
|
||||
real32_T scale;
|
||||
int32_T k;
|
||||
real32_T absxk;
|
||||
real32_T t;
|
||||
y = 0.0F;
|
||||
scale = 1.17549435E-38F;
|
||||
for (k = 0; k < 3; k++) {
|
||||
absxk = (real32_T)fabs(x[k]);
|
||||
if (absxk > scale) {
|
||||
t = scale / absxk;
|
||||
y = 1.0F + y * t * t;
|
||||
scale = absxk;
|
||||
} else {
|
||||
t = absxk / scale;
|
||||
y += t * t;
|
||||
}
|
||||
}
|
||||
|
||||
return scale * (real32_T)sqrt(y);
|
||||
}
|
||||
|
||||
/* End of code generation (norm.c) */
|
||||
+34
@@ -0,0 +1,34 @@
|
||||
/*
|
||||
* norm.h
|
||||
*
|
||||
* Code generation for function 'norm'
|
||||
*
|
||||
* C source code generated on: Sat Jan 19 15:25:29 2013
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __NORM_H__
|
||||
#define __NORM_H__
|
||||
/* Include files */
|
||||
#include <math.h>
|
||||
#include <stddef.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include "rt_defines.h"
|
||||
#include "rt_nonfinite.h"
|
||||
|
||||
#include "rtwtypes.h"
|
||||
#include "attitudeKalmanfilter_types.h"
|
||||
|
||||
/* Type Definitions */
|
||||
|
||||
/* Named Constants */
|
||||
|
||||
/* Variable Declarations */
|
||||
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
extern real32_T norm(const real32_T x[3]);
|
||||
#endif
|
||||
/* End of code generation (norm.h) */
|
||||
+38
@@ -0,0 +1,38 @@
|
||||
/*
|
||||
* rdivide.c
|
||||
*
|
||||
* Code generation for function 'rdivide'
|
||||
*
|
||||
* C source code generated on: Sat Jan 19 15:25:29 2013
|
||||
*
|
||||
*/
|
||||
|
||||
/* Include files */
|
||||
#include "rt_nonfinite.h"
|
||||
#include "attitudeKalmanfilter.h"
|
||||
#include "rdivide.h"
|
||||
|
||||
/* Type Definitions */
|
||||
|
||||
/* Named Constants */
|
||||
|
||||
/* Variable Declarations */
|
||||
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
|
||||
/* Function Definitions */
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void rdivide(const real32_T x[3], real32_T y, real32_T z[3])
|
||||
{
|
||||
int32_T i;
|
||||
for (i = 0; i < 3; i++) {
|
||||
z[i] = x[i] / y;
|
||||
}
|
||||
}
|
||||
|
||||
/* End of code generation (rdivide.c) */
|
||||
+34
@@ -0,0 +1,34 @@
|
||||
/*
|
||||
* rdivide.h
|
||||
*
|
||||
* Code generation for function 'rdivide'
|
||||
*
|
||||
* C source code generated on: Sat Jan 19 15:25:29 2013
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __RDIVIDE_H__
|
||||
#define __RDIVIDE_H__
|
||||
/* Include files */
|
||||
#include <math.h>
|
||||
#include <stddef.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include "rt_defines.h"
|
||||
#include "rt_nonfinite.h"
|
||||
|
||||
#include "rtwtypes.h"
|
||||
#include "attitudeKalmanfilter_types.h"
|
||||
|
||||
/* Type Definitions */
|
||||
|
||||
/* Named Constants */
|
||||
|
||||
/* Variable Declarations */
|
||||
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
extern void rdivide(const real32_T x[3], real32_T y, real32_T z[3]);
|
||||
#endif
|
||||
/* End of code generation (rdivide.h) */
|
||||
+139
@@ -0,0 +1,139 @@
|
||||
/*
|
||||
* rtGetInf.c
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Sat Jan 19 15:25:29 2013
|
||||
*
|
||||
*/
|
||||
|
||||
/*
|
||||
* Abstract:
|
||||
* MATLAB for code generation function to initialize non-finite, Inf and MinusInf
|
||||
*/
|
||||
#include "rtGetInf.h"
|
||||
#define NumBitsPerChar 8U
|
||||
|
||||
/* Function: rtGetInf ==================================================
|
||||
* Abstract:
|
||||
* Initialize rtInf needed by the generated code.
|
||||
* Inf is initialized as non-signaling. Assumes IEEE.
|
||||
*/
|
||||
real_T rtGetInf(void)
|
||||
{
|
||||
size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
|
||||
real_T inf = 0.0;
|
||||
if (bitsPerReal == 32U) {
|
||||
inf = rtGetInfF();
|
||||
} else {
|
||||
uint16_T one = 1U;
|
||||
enum {
|
||||
LittleEndian,
|
||||
BigEndian
|
||||
} machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
|
||||
switch (machByteOrder) {
|
||||
case LittleEndian:
|
||||
{
|
||||
union {
|
||||
LittleEndianIEEEDouble bitVal;
|
||||
real_T fltVal;
|
||||
} tmpVal;
|
||||
|
||||
tmpVal.bitVal.words.wordH = 0x7FF00000U;
|
||||
tmpVal.bitVal.words.wordL = 0x00000000U;
|
||||
inf = tmpVal.fltVal;
|
||||
break;
|
||||
}
|
||||
|
||||
case BigEndian:
|
||||
{
|
||||
union {
|
||||
BigEndianIEEEDouble bitVal;
|
||||
real_T fltVal;
|
||||
} tmpVal;
|
||||
|
||||
tmpVal.bitVal.words.wordH = 0x7FF00000U;
|
||||
tmpVal.bitVal.words.wordL = 0x00000000U;
|
||||
inf = tmpVal.fltVal;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return inf;
|
||||
}
|
||||
|
||||
/* Function: rtGetInfF ==================================================
|
||||
* Abstract:
|
||||
* Initialize rtInfF needed by the generated code.
|
||||
* Inf is initialized as non-signaling. Assumes IEEE.
|
||||
*/
|
||||
real32_T rtGetInfF(void)
|
||||
{
|
||||
IEEESingle infF;
|
||||
infF.wordL.wordLuint = 0x7F800000U;
|
||||
return infF.wordL.wordLreal;
|
||||
}
|
||||
|
||||
/* Function: rtGetMinusInf ==================================================
|
||||
* Abstract:
|
||||
* Initialize rtMinusInf needed by the generated code.
|
||||
* Inf is initialized as non-signaling. Assumes IEEE.
|
||||
*/
|
||||
real_T rtGetMinusInf(void)
|
||||
{
|
||||
size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
|
||||
real_T minf = 0.0;
|
||||
if (bitsPerReal == 32U) {
|
||||
minf = rtGetMinusInfF();
|
||||
} else {
|
||||
uint16_T one = 1U;
|
||||
enum {
|
||||
LittleEndian,
|
||||
BigEndian
|
||||
} machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
|
||||
switch (machByteOrder) {
|
||||
case LittleEndian:
|
||||
{
|
||||
union {
|
||||
LittleEndianIEEEDouble bitVal;
|
||||
real_T fltVal;
|
||||
} tmpVal;
|
||||
|
||||
tmpVal.bitVal.words.wordH = 0xFFF00000U;
|
||||
tmpVal.bitVal.words.wordL = 0x00000000U;
|
||||
minf = tmpVal.fltVal;
|
||||
break;
|
||||
}
|
||||
|
||||
case BigEndian:
|
||||
{
|
||||
union {
|
||||
BigEndianIEEEDouble bitVal;
|
||||
real_T fltVal;
|
||||
} tmpVal;
|
||||
|
||||
tmpVal.bitVal.words.wordH = 0xFFF00000U;
|
||||
tmpVal.bitVal.words.wordL = 0x00000000U;
|
||||
minf = tmpVal.fltVal;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return minf;
|
||||
}
|
||||
|
||||
/* Function: rtGetMinusInfF ==================================================
|
||||
* Abstract:
|
||||
* Initialize rtMinusInfF needed by the generated code.
|
||||
* Inf is initialized as non-signaling. Assumes IEEE.
|
||||
*/
|
||||
real32_T rtGetMinusInfF(void)
|
||||
{
|
||||
IEEESingle minfF;
|
||||
minfF.wordL.wordLuint = 0xFF800000U;
|
||||
return minfF.wordL.wordLreal;
|
||||
}
|
||||
|
||||
/* End of code generation (rtGetInf.c) */
|
||||
+23
@@ -0,0 +1,23 @@
|
||||
/*
|
||||
* rtGetInf.h
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Sat Jan 19 15:25:29 2013
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __RTGETINF_H__
|
||||
#define __RTGETINF_H__
|
||||
|
||||
#include <stddef.h>
|
||||
#include "rtwtypes.h"
|
||||
#include "rt_nonfinite.h"
|
||||
|
||||
extern real_T rtGetInf(void);
|
||||
extern real32_T rtGetInfF(void);
|
||||
extern real_T rtGetMinusInf(void);
|
||||
extern real32_T rtGetMinusInfF(void);
|
||||
|
||||
#endif
|
||||
/* End of code generation (rtGetInf.h) */
|
||||
+96
@@ -0,0 +1,96 @@
|
||||
/*
|
||||
* rtGetNaN.c
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Sat Jan 19 15:25:29 2013
|
||||
*
|
||||
*/
|
||||
|
||||
/*
|
||||
* Abstract:
|
||||
* MATLAB for code generation function to initialize non-finite, NaN
|
||||
*/
|
||||
#include "rtGetNaN.h"
|
||||
#define NumBitsPerChar 8U
|
||||
|
||||
/* Function: rtGetNaN ==================================================
|
||||
* Abstract:
|
||||
* Initialize rtNaN needed by the generated code.
|
||||
* NaN is initialized as non-signaling. Assumes IEEE.
|
||||
*/
|
||||
real_T rtGetNaN(void)
|
||||
{
|
||||
size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
|
||||
real_T nan = 0.0;
|
||||
if (bitsPerReal == 32U) {
|
||||
nan = rtGetNaNF();
|
||||
} else {
|
||||
uint16_T one = 1U;
|
||||
enum {
|
||||
LittleEndian,
|
||||
BigEndian
|
||||
} machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
|
||||
switch (machByteOrder) {
|
||||
case LittleEndian:
|
||||
{
|
||||
union {
|
||||
LittleEndianIEEEDouble bitVal;
|
||||
real_T fltVal;
|
||||
} tmpVal;
|
||||
|
||||
tmpVal.bitVal.words.wordH = 0xFFF80000U;
|
||||
tmpVal.bitVal.words.wordL = 0x00000000U;
|
||||
nan = tmpVal.fltVal;
|
||||
break;
|
||||
}
|
||||
|
||||
case BigEndian:
|
||||
{
|
||||
union {
|
||||
BigEndianIEEEDouble bitVal;
|
||||
real_T fltVal;
|
||||
} tmpVal;
|
||||
|
||||
tmpVal.bitVal.words.wordH = 0x7FFFFFFFU;
|
||||
tmpVal.bitVal.words.wordL = 0xFFFFFFFFU;
|
||||
nan = tmpVal.fltVal;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return nan;
|
||||
}
|
||||
|
||||
/* Function: rtGetNaNF ==================================================
|
||||
* Abstract:
|
||||
* Initialize rtNaNF needed by the generated code.
|
||||
* NaN is initialized as non-signaling. Assumes IEEE.
|
||||
*/
|
||||
real32_T rtGetNaNF(void)
|
||||
{
|
||||
IEEESingle nanF = { { 0 } };
|
||||
uint16_T one = 1U;
|
||||
enum {
|
||||
LittleEndian,
|
||||
BigEndian
|
||||
} machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
|
||||
switch (machByteOrder) {
|
||||
case LittleEndian:
|
||||
{
|
||||
nanF.wordL.wordLuint = 0xFFC00000U;
|
||||
break;
|
||||
}
|
||||
|
||||
case BigEndian:
|
||||
{
|
||||
nanF.wordL.wordLuint = 0x7FFFFFFFU;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return nanF.wordL.wordLreal;
|
||||
}
|
||||
|
||||
/* End of code generation (rtGetNaN.c) */
|
||||
+21
@@ -0,0 +1,21 @@
|
||||
/*
|
||||
* rtGetNaN.h
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Sat Jan 19 15:25:29 2013
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __RTGETNAN_H__
|
||||
#define __RTGETNAN_H__
|
||||
|
||||
#include <stddef.h>
|
||||
#include "rtwtypes.h"
|
||||
#include "rt_nonfinite.h"
|
||||
|
||||
extern real_T rtGetNaN(void);
|
||||
extern real32_T rtGetNaNF(void);
|
||||
|
||||
#endif
|
||||
/* End of code generation (rtGetNaN.h) */
|
||||
+24
@@ -0,0 +1,24 @@
|
||||
/*
|
||||
* rt_defines.h
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Sat Jan 19 15:25:29 2013
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __RT_DEFINES_H__
|
||||
#define __RT_DEFINES_H__
|
||||
|
||||
#include <stdlib.h>
|
||||
|
||||
#define RT_PI 3.14159265358979323846
|
||||
#define RT_PIF 3.1415927F
|
||||
#define RT_LN_10 2.30258509299404568402
|
||||
#define RT_LN_10F 2.3025851F
|
||||
#define RT_LOG10E 0.43429448190325182765
|
||||
#define RT_LOG10EF 0.43429449F
|
||||
#define RT_E 2.7182818284590452354
|
||||
#define RT_EF 2.7182817F
|
||||
#endif
|
||||
/* End of code generation (rt_defines.h) */
|
||||
@@ -0,0 +1,87 @@
|
||||
/*
|
||||
* rt_nonfinite.c
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Sat Jan 19 15:25:29 2013
|
||||
*
|
||||
*/
|
||||
|
||||
/*
|
||||
* Abstract:
|
||||
* MATLAB for code generation function to initialize non-finites,
|
||||
* (Inf, NaN and -Inf).
|
||||
*/
|
||||
#include "rt_nonfinite.h"
|
||||
#include "rtGetNaN.h"
|
||||
#include "rtGetInf.h"
|
||||
|
||||
real_T rtInf;
|
||||
real_T rtMinusInf;
|
||||
real_T rtNaN;
|
||||
real32_T rtInfF;
|
||||
real32_T rtMinusInfF;
|
||||
real32_T rtNaNF;
|
||||
|
||||
/* Function: rt_InitInfAndNaN ==================================================
|
||||
* Abstract:
|
||||
* Initialize the rtInf, rtMinusInf, and rtNaN needed by the
|
||||
* generated code. NaN is initialized as non-signaling. Assumes IEEE.
|
||||
*/
|
||||
void rt_InitInfAndNaN(size_t realSize)
|
||||
{
|
||||
(void) (realSize);
|
||||
rtNaN = rtGetNaN();
|
||||
rtNaNF = rtGetNaNF();
|
||||
rtInf = rtGetInf();
|
||||
rtInfF = rtGetInfF();
|
||||
rtMinusInf = rtGetMinusInf();
|
||||
rtMinusInfF = rtGetMinusInfF();
|
||||
}
|
||||
|
||||
/* Function: rtIsInf ==================================================
|
||||
* Abstract:
|
||||
* Test if value is infinite
|
||||
*/
|
||||
boolean_T rtIsInf(real_T value)
|
||||
{
|
||||
return ((value==rtInf || value==rtMinusInf) ? 1U : 0U);
|
||||
}
|
||||
|
||||
/* Function: rtIsInfF =================================================
|
||||
* Abstract:
|
||||
* Test if single-precision value is infinite
|
||||
*/
|
||||
boolean_T rtIsInfF(real32_T value)
|
||||
{
|
||||
return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U);
|
||||
}
|
||||
|
||||
/* Function: rtIsNaN ==================================================
|
||||
* Abstract:
|
||||
* Test if value is not a number
|
||||
*/
|
||||
boolean_T rtIsNaN(real_T value)
|
||||
{
|
||||
#if defined(_MSC_VER) && (_MSC_VER <= 1200)
|
||||
return _isnan(value)? TRUE:FALSE;
|
||||
#else
|
||||
return (value!=value)? 1U:0U;
|
||||
#endif
|
||||
}
|
||||
|
||||
/* Function: rtIsNaNF =================================================
|
||||
* Abstract:
|
||||
* Test if single-precision value is not a number
|
||||
*/
|
||||
boolean_T rtIsNaNF(real32_T value)
|
||||
{
|
||||
#if defined(_MSC_VER) && (_MSC_VER <= 1200)
|
||||
return _isnan((real_T)value)? true:false;
|
||||
#else
|
||||
return (value!=value)? 1U:0U;
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
/* End of code generation (rt_nonfinite.c) */
|
||||
@@ -0,0 +1,53 @@
|
||||
/*
|
||||
* rt_nonfinite.h
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Sat Jan 19 15:25:29 2013
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __RT_NONFINITE_H__
|
||||
#define __RT_NONFINITE_H__
|
||||
|
||||
#if defined(_MSC_VER) && (_MSC_VER <= 1200)
|
||||
#include <float.h>
|
||||
#endif
|
||||
#include <stddef.h>
|
||||
#include "rtwtypes.h"
|
||||
|
||||
extern real_T rtInf;
|
||||
extern real_T rtMinusInf;
|
||||
extern real_T rtNaN;
|
||||
extern real32_T rtInfF;
|
||||
extern real32_T rtMinusInfF;
|
||||
extern real32_T rtNaNF;
|
||||
extern void rt_InitInfAndNaN(size_t realSize);
|
||||
extern boolean_T rtIsInf(real_T value);
|
||||
extern boolean_T rtIsInfF(real32_T value);
|
||||
extern boolean_T rtIsNaN(real_T value);
|
||||
extern boolean_T rtIsNaNF(real32_T value);
|
||||
|
||||
typedef struct {
|
||||
struct {
|
||||
uint32_T wordH;
|
||||
uint32_T wordL;
|
||||
} words;
|
||||
} BigEndianIEEEDouble;
|
||||
|
||||
typedef struct {
|
||||
struct {
|
||||
uint32_T wordL;
|
||||
uint32_T wordH;
|
||||
} words;
|
||||
} LittleEndianIEEEDouble;
|
||||
|
||||
typedef struct {
|
||||
union {
|
||||
real32_T wordLreal;
|
||||
uint32_T wordLuint;
|
||||
} wordL;
|
||||
} IEEESingle;
|
||||
|
||||
#endif
|
||||
/* End of code generation (rt_nonfinite.h) */
|
||||
+159
@@ -0,0 +1,159 @@
|
||||
/*
|
||||
* rtwtypes.h
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Sat Jan 19 15:25:29 2013
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __RTWTYPES_H__
|
||||
#define __RTWTYPES_H__
|
||||
#ifndef TRUE
|
||||
# define TRUE (1U)
|
||||
#endif
|
||||
#ifndef FALSE
|
||||
# define FALSE (0U)
|
||||
#endif
|
||||
#ifndef __TMWTYPES__
|
||||
#define __TMWTYPES__
|
||||
|
||||
#include <limits.h>
|
||||
|
||||
/*=======================================================================*
|
||||
* Target hardware information
|
||||
* Device type: Generic->MATLAB Host Computer
|
||||
* Number of bits: char: 8 short: 16 int: 32
|
||||
* long: 32 native word size: 32
|
||||
* Byte ordering: LittleEndian
|
||||
* Signed integer division rounds to: Zero
|
||||
* Shift right on a signed integer as arithmetic shift: on
|
||||
*=======================================================================*/
|
||||
|
||||
/*=======================================================================*
|
||||
* Fixed width word size data types: *
|
||||
* int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers *
|
||||
* uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers *
|
||||
* real32_T, real64_T - 32 and 64 bit floating point numbers *
|
||||
*=======================================================================*/
|
||||
|
||||
typedef signed char int8_T;
|
||||
typedef unsigned char uint8_T;
|
||||
typedef short int16_T;
|
||||
typedef unsigned short uint16_T;
|
||||
typedef int int32_T;
|
||||
typedef unsigned int uint32_T;
|
||||
typedef float real32_T;
|
||||
typedef double real64_T;
|
||||
|
||||
/*===========================================================================*
|
||||
* Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T, *
|
||||
* ulong_T, char_T and byte_T. *
|
||||
*===========================================================================*/
|
||||
|
||||
typedef double real_T;
|
||||
typedef double time_T;
|
||||
typedef unsigned char boolean_T;
|
||||
typedef int int_T;
|
||||
typedef unsigned int uint_T;
|
||||
typedef unsigned long ulong_T;
|
||||
typedef char char_T;
|
||||
typedef char_T byte_T;
|
||||
|
||||
/*===========================================================================*
|
||||
* Complex number type definitions *
|
||||
*===========================================================================*/
|
||||
#define CREAL_T
|
||||
typedef struct {
|
||||
real32_T re;
|
||||
real32_T im;
|
||||
} creal32_T;
|
||||
|
||||
typedef struct {
|
||||
real64_T re;
|
||||
real64_T im;
|
||||
} creal64_T;
|
||||
|
||||
typedef struct {
|
||||
real_T re;
|
||||
real_T im;
|
||||
} creal_T;
|
||||
|
||||
typedef struct {
|
||||
int8_T re;
|
||||
int8_T im;
|
||||
} cint8_T;
|
||||
|
||||
typedef struct {
|
||||
uint8_T re;
|
||||
uint8_T im;
|
||||
} cuint8_T;
|
||||
|
||||
typedef struct {
|
||||
int16_T re;
|
||||
int16_T im;
|
||||
} cint16_T;
|
||||
|
||||
typedef struct {
|
||||
uint16_T re;
|
||||
uint16_T im;
|
||||
} cuint16_T;
|
||||
|
||||
typedef struct {
|
||||
int32_T re;
|
||||
int32_T im;
|
||||
} cint32_T;
|
||||
|
||||
typedef struct {
|
||||
uint32_T re;
|
||||
uint32_T im;
|
||||
} cuint32_T;
|
||||
|
||||
|
||||
/*=======================================================================*
|
||||
* Min and Max: *
|
||||
* int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers *
|
||||
* uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers *
|
||||
*=======================================================================*/
|
||||
|
||||
#define MAX_int8_T ((int8_T)(127))
|
||||
#define MIN_int8_T ((int8_T)(-128))
|
||||
#define MAX_uint8_T ((uint8_T)(255))
|
||||
#define MIN_uint8_T ((uint8_T)(0))
|
||||
#define MAX_int16_T ((int16_T)(32767))
|
||||
#define MIN_int16_T ((int16_T)(-32768))
|
||||
#define MAX_uint16_T ((uint16_T)(65535))
|
||||
#define MIN_uint16_T ((uint16_T)(0))
|
||||
#define MAX_int32_T ((int32_T)(2147483647))
|
||||
#define MIN_int32_T ((int32_T)(-2147483647-1))
|
||||
#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU))
|
||||
#define MIN_uint32_T ((uint32_T)(0))
|
||||
|
||||
/* Logical type definitions */
|
||||
#if !defined(__cplusplus) && !defined(__true_false_are_keywords)
|
||||
# ifndef false
|
||||
# define false (0U)
|
||||
# endif
|
||||
# ifndef true
|
||||
# define true (1U)
|
||||
# endif
|
||||
#endif
|
||||
|
||||
/*
|
||||
* MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation
|
||||
* for signed integer values.
|
||||
*/
|
||||
#if ((SCHAR_MIN + 1) != -SCHAR_MAX)
|
||||
#error "This code must be compiled using a 2's complement representation for signed integer values"
|
||||
#endif
|
||||
|
||||
/*
|
||||
* Maximum length of a MATLAB identifier (function/variable)
|
||||
* including the null-termination character. Referenced by
|
||||
* rt_logging.c and rt_matrx.c.
|
||||
*/
|
||||
#define TMW_NAME_LENGTH_MAX 64
|
||||
|
||||
#endif
|
||||
#endif
|
||||
/* End of code generation (rtwtypes.h) */
|
||||
@@ -0,0 +1,16 @@
|
||||
|
||||
MODULE_NAME = attitude_estimator_ekf
|
||||
SRCS = attitude_estimator_ekf_main.cpp \
|
||||
attitude_estimator_ekf_params.c \
|
||||
codegen/attitudeKalmanfilter_initialize.c \
|
||||
codegen/attitudeKalmanfilter_terminate.c \
|
||||
codegen/attitudeKalmanfilter.c \
|
||||
codegen/cross.c \
|
||||
codegen/eye.c \
|
||||
codegen/mrdivide.c \
|
||||
codegen/norm.c \
|
||||
codegen/rdivide.c \
|
||||
codegen/rt_nonfinite.c \
|
||||
codegen/rtGetInf.c \
|
||||
codegen/rtGetNaN.c
|
||||
|
||||
@@ -0,0 +1,219 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file calibration_routines.c
|
||||
* Calibration routines implementations.
|
||||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#include "calibration_routines.h"
|
||||
|
||||
|
||||
int sphere_fit_least_squares(const float x[], const float y[], const float z[],
|
||||
unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius)
|
||||
{
|
||||
|
||||
float x_sumplain = 0.0f;
|
||||
float x_sumsq = 0.0f;
|
||||
float x_sumcube = 0.0f;
|
||||
|
||||
float y_sumplain = 0.0f;
|
||||
float y_sumsq = 0.0f;
|
||||
float y_sumcube = 0.0f;
|
||||
|
||||
float z_sumplain = 0.0f;
|
||||
float z_sumsq = 0.0f;
|
||||
float z_sumcube = 0.0f;
|
||||
|
||||
float xy_sum = 0.0f;
|
||||
float xz_sum = 0.0f;
|
||||
float yz_sum = 0.0f;
|
||||
|
||||
float x2y_sum = 0.0f;
|
||||
float x2z_sum = 0.0f;
|
||||
float y2x_sum = 0.0f;
|
||||
float y2z_sum = 0.0f;
|
||||
float z2x_sum = 0.0f;
|
||||
float z2y_sum = 0.0f;
|
||||
|
||||
for (unsigned int i = 0; i < size; i++) {
|
||||
|
||||
float x2 = x[i] * x[i];
|
||||
float y2 = y[i] * y[i];
|
||||
float z2 = z[i] * z[i];
|
||||
|
||||
x_sumplain += x[i];
|
||||
x_sumsq += x2;
|
||||
x_sumcube += x2 * x[i];
|
||||
|
||||
y_sumplain += y[i];
|
||||
y_sumsq += y2;
|
||||
y_sumcube += y2 * y[i];
|
||||
|
||||
z_sumplain += z[i];
|
||||
z_sumsq += z2;
|
||||
z_sumcube += z2 * z[i];
|
||||
|
||||
xy_sum += x[i] * y[i];
|
||||
xz_sum += x[i] * z[i];
|
||||
yz_sum += y[i] * z[i];
|
||||
|
||||
x2y_sum += x2 * y[i];
|
||||
x2z_sum += x2 * z[i];
|
||||
|
||||
y2x_sum += y2 * x[i];
|
||||
y2z_sum += y2 * z[i];
|
||||
|
||||
z2x_sum += z2 * x[i];
|
||||
z2y_sum += z2 * y[i];
|
||||
}
|
||||
|
||||
//
|
||||
//Least Squares Fit a sphere A,B,C with radius squared Rsq to 3D data
|
||||
//
|
||||
// P is a structure that has been computed with the data earlier.
|
||||
// P.npoints is the number of elements; the length of X,Y,Z are identical.
|
||||
// P's members are logically named.
|
||||
//
|
||||
// X[n] is the x component of point n
|
||||
// Y[n] is the y component of point n
|
||||
// Z[n] is the z component of point n
|
||||
//
|
||||
// A is the x coordiante of the sphere
|
||||
// B is the y coordiante of the sphere
|
||||
// C is the z coordiante of the sphere
|
||||
// Rsq is the radius squared of the sphere.
|
||||
//
|
||||
//This method should converge; maybe 5-100 iterations or more.
|
||||
//
|
||||
float x_sum = x_sumplain / size; //sum( X[n] )
|
||||
float x_sum2 = x_sumsq / size; //sum( X[n]^2 )
|
||||
float x_sum3 = x_sumcube / size; //sum( X[n]^3 )
|
||||
float y_sum = y_sumplain / size; //sum( Y[n] )
|
||||
float y_sum2 = y_sumsq / size; //sum( Y[n]^2 )
|
||||
float y_sum3 = y_sumcube / size; //sum( Y[n]^3 )
|
||||
float z_sum = z_sumplain / size; //sum( Z[n] )
|
||||
float z_sum2 = z_sumsq / size; //sum( Z[n]^2 )
|
||||
float z_sum3 = z_sumcube / size; //sum( Z[n]^3 )
|
||||
|
||||
float XY = xy_sum / size; //sum( X[n] * Y[n] )
|
||||
float XZ = xz_sum / size; //sum( X[n] * Z[n] )
|
||||
float YZ = yz_sum / size; //sum( Y[n] * Z[n] )
|
||||
float X2Y = x2y_sum / size; //sum( X[n]^2 * Y[n] )
|
||||
float X2Z = x2z_sum / size; //sum( X[n]^2 * Z[n] )
|
||||
float Y2X = y2x_sum / size; //sum( Y[n]^2 * X[n] )
|
||||
float Y2Z = y2z_sum / size; //sum( Y[n]^2 * Z[n] )
|
||||
float Z2X = z2x_sum / size; //sum( Z[n]^2 * X[n] )
|
||||
float Z2Y = z2y_sum / size; //sum( Z[n]^2 * Y[n] )
|
||||
|
||||
//Reduction of multiplications
|
||||
float F0 = x_sum2 + y_sum2 + z_sum2;
|
||||
float F1 = 0.5f * F0;
|
||||
float F2 = -8.0f * (x_sum3 + Y2X + Z2X);
|
||||
float F3 = -8.0f * (X2Y + y_sum3 + Z2Y);
|
||||
float F4 = -8.0f * (X2Z + Y2Z + z_sum3);
|
||||
|
||||
//Set initial conditions:
|
||||
float A = x_sum;
|
||||
float B = y_sum;
|
||||
float C = z_sum;
|
||||
|
||||
//First iteration computation:
|
||||
float A2 = A * A;
|
||||
float B2 = B * B;
|
||||
float C2 = C * C;
|
||||
float QS = A2 + B2 + C2;
|
||||
float QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum);
|
||||
|
||||
//Set initial conditions:
|
||||
float Rsq = F0 + QB + QS;
|
||||
|
||||
//First iteration computation:
|
||||
float Q0 = 0.5f * (QS - Rsq);
|
||||
float Q1 = F1 + Q0;
|
||||
float Q2 = 8.0f * (QS - Rsq + QB + F0);
|
||||
float aA, aB, aC, nA, nB, nC, dA, dB, dC;
|
||||
|
||||
//Iterate N times, ignore stop condition.
|
||||
int n = 0;
|
||||
|
||||
while (n < max_iterations) {
|
||||
n++;
|
||||
|
||||
//Compute denominator:
|
||||
aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2);
|
||||
aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2);
|
||||
aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2);
|
||||
aA = (aA == 0.0f) ? 1.0f : aA;
|
||||
aB = (aB == 0.0f) ? 1.0f : aB;
|
||||
aC = (aC == 0.0f) ? 1.0f : aC;
|
||||
|
||||
//Compute next iteration
|
||||
nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA);
|
||||
nB = B - ((F3 + 16.0f * (A * XY + C * YZ + y_sum * (-B2 - Q0) + B * (y_sum2 + Q1 - A * x_sum - C * z_sum))) / aB);
|
||||
nC = C - ((F4 + 16.0f * (A * XZ + B * YZ + z_sum * (-C2 - Q0) + C * (z_sum2 + Q1 - A * x_sum - B * y_sum))) / aC);
|
||||
|
||||
//Check for stop condition
|
||||
dA = (nA - A);
|
||||
dB = (nB - B);
|
||||
dC = (nC - C);
|
||||
|
||||
if ((dA * dA + dB * dB + dC * dC) <= delta) { break; }
|
||||
|
||||
//Compute next iteration's values
|
||||
A = nA;
|
||||
B = nB;
|
||||
C = nC;
|
||||
A2 = A * A;
|
||||
B2 = B * B;
|
||||
C2 = C * C;
|
||||
QS = A2 + B2 + C2;
|
||||
QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum);
|
||||
Rsq = F0 + QB + QS;
|
||||
Q0 = 0.5f * (QS - Rsq);
|
||||
Q1 = F1 + Q0;
|
||||
Q2 = 8.0f * (QS - Rsq + QB + F0);
|
||||
}
|
||||
|
||||
*sphere_x = A;
|
||||
*sphere_y = B;
|
||||
*sphere_z = C;
|
||||
*sphere_radius = sqrtf(Rsq);
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,61 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file calibration_routines.h
|
||||
* Calibration routines definitions.
|
||||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*/
|
||||
|
||||
/**
|
||||
* Least-squares fit of a sphere to a set of points.
|
||||
*
|
||||
* Fits a sphere to a set of points on the sphere surface.
|
||||
*
|
||||
* @param x point coordinates on the X axis
|
||||
* @param y point coordinates on the Y axis
|
||||
* @param z point coordinates on the Z axis
|
||||
* @param size number of points
|
||||
* @param max_iterations abort if maximum number of iterations have been reached. If unsure, set to 100.
|
||||
* @param delta abort if error is below delta. If unsure, set to 0 to run max_iterations times.
|
||||
* @param sphere_x coordinate of the sphere center on the X axis
|
||||
* @param sphere_y coordinate of the sphere center on the Y axis
|
||||
* @param sphere_z coordinate of the sphere center on the Z axis
|
||||
* @param sphere_radius sphere radius
|
||||
*
|
||||
* @return 0 on success, 1 on failure
|
||||
*/
|
||||
int sphere_fit_least_squares(const float x[], const float y[], const float z[],
|
||||
unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius);
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,58 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
|
||||
* Lorenz Meier <lm@inf.ethz.ch>
|
||||
* Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* Julian Oes <joes@student.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file commander.h
|
||||
* Main system state machine definition.
|
||||
*
|
||||
* @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef COMMANDER_H_
|
||||
#define COMMANDER_H_
|
||||
|
||||
#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
|
||||
#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
|
||||
|
||||
void tune_confirm(void);
|
||||
void tune_error(void);
|
||||
|
||||
#endif /* COMMANDER_H_ */
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user