diff options
Diffstat (limited to 'src')
56 files changed, 11739 insertions, 0 deletions
diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c new file mode 100644 index 000000000..aeaf830de --- /dev/null +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -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; +} + diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c new file mode 100644 index 000000000..f15c74a22 --- /dev/null +++ b/src/drivers/ardrone_interface/ardrone_motor_control.c @@ -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]); +} diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.h b/src/drivers/ardrone_interface/ardrone_motor_control.h new file mode 100644 index 000000000..78b603b63 --- /dev/null +++ b/src/drivers/ardrone_interface/ardrone_motor_control.h @@ -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); diff --git a/src/drivers/ardrone_interface/module.mk b/src/drivers/ardrone_interface/module.mk new file mode 100644 index 000000000..058bd1397 --- /dev/null +++ b/src/drivers/ardrone_interface/module.mk @@ -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 diff --git a/src/drivers/boards/px4fmu/module.mk b/src/drivers/boards/px4fmu/module.mk new file mode 100644 index 000000000..2cb261d30 --- /dev/null +++ b/src/drivers/boards/px4fmu/module.mk @@ -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 diff --git a/src/drivers/boards/px4fmu/px4fmu_can.c b/src/drivers/boards/px4fmu/px4fmu_can.c new file mode 100644 index 000000000..187689ff9 --- /dev/null +++ b/src/drivers/boards/px4fmu/px4fmu_can.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 diff --git a/src/drivers/boards/px4fmu/px4fmu_init.c b/src/drivers/boards/px4fmu/px4fmu_init.c new file mode 100644 index 000000000..5dd70c3f9 --- /dev/null +++ b/src/drivers/boards/px4fmu/px4fmu_init.c @@ -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; +} diff --git a/src/drivers/boards/px4fmu/px4fmu_internal.h b/src/drivers/boards/px4fmu/px4fmu_internal.h new file mode 100644 index 000000000..671a58f8f --- /dev/null +++ b/src/drivers/boards/px4fmu/px4fmu_internal.h @@ -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 diff --git a/src/drivers/boards/px4fmu/px4fmu_led.c b/src/drivers/boards/px4fmu/px4fmu_led.c new file mode 100644 index 000000000..fd1e159aa --- /dev/null +++ b/src/drivers/boards/px4fmu/px4fmu_led.c @@ -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); + } +} diff --git a/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c new file mode 100644 index 000000000..cb8918306 --- /dev/null +++ b/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c @@ -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, + } +}; diff --git a/src/drivers/boards/px4fmu/px4fmu_spi.c b/src/drivers/boards/px4fmu/px4fmu_spi.c new file mode 100644 index 000000000..b5d00eac0 --- /dev/null +++ b/src/drivers/boards/px4fmu/px4fmu_spi.c @@ -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; +} + diff --git a/src/drivers/boards/px4fmu/px4fmu_usb.c b/src/drivers/boards/px4fmu/px4fmu_usb.c new file mode 100644 index 000000000..b0b669fbe --- /dev/null +++ b/src/drivers/boards/px4fmu/px4fmu_usb.c @@ -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); +} + diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp new file mode 100644 index 000000000..c7f433dd4 --- /dev/null +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -0,0 +1,887 @@ +/**************************************************************************** + * + * 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 Driver for the ST L3GD20 MEMS gyro connected via SPI. + */ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <stddef.h> +#include <stdlib.h> +#include <semaphore.h> +#include <string.h> +#include <fcntl.h> +#include <poll.h> +#include <errno.h> +#include <stdio.h> +#include <math.h> +#include <unistd.h> + +#include <systemlib/perf_counter.h> +#include <systemlib/err.h> + +#include <nuttx/arch.h> +#include <nuttx/clock.h> + +#include <drivers/drv_hrt.h> +#include <arch/board/board.h> + +#include <drivers/device/spi.h> +#include <drivers/drv_gyro.h> + + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +/* SPI protocol address bits */ +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) +#define ADDR_INCREMENT (1<<6) + +/* register addresses */ +#define ADDR_WHO_AM_I 0x0F +#define WHO_I_AM 0xD4 + +#define ADDR_CTRL_REG1 0x20 +#define REG1_RATE_LP_MASK 0xF0 /* Mask to guard partial register update */ +/* keep lowpass low to avoid noise issues */ +#define RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4)) +#define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4)) +#define RATE_380HZ_LP_30HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4)) +#define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4)) + +#define ADDR_CTRL_REG2 0x21 +#define ADDR_CTRL_REG3 0x22 +#define ADDR_CTRL_REG4 0x23 +#define REG4_RANGE_MASK 0x30 /* Mask to guard partial register update */ +#define RANGE_250DPS (0<<4) +#define RANGE_500DPS (1<<4) +#define RANGE_2000DPS (3<<4) + +#define ADDR_CTRL_REG5 0x24 +#define ADDR_REFERENCE 0x25 +#define ADDR_OUT_TEMP 0x26 +#define ADDR_STATUS_REG 0x27 +#define ADDR_OUT_X_L 0x28 +#define ADDR_OUT_X_H 0x29 +#define ADDR_OUT_Y_L 0x2A +#define ADDR_OUT_Y_H 0x2B +#define ADDR_OUT_Z_L 0x2C +#define ADDR_OUT_Z_H 0x2D +#define ADDR_FIFO_CTRL_REG 0x2E +#define ADDR_FIFO_SRC_REG 0x2F +#define ADDR_INT1_CFG 0x30 +#define ADDR_INT1_SRC 0x31 +#define ADDR_INT1_TSH_XH 0x32 +#define ADDR_INT1_TSH_XL 0x33 +#define ADDR_INT1_TSH_YH 0x34 +#define ADDR_INT1_TSH_YL 0x35 +#define ADDR_INT1_TSH_ZH 0x36 +#define ADDR_INT1_TSH_ZL 0x37 +#define ADDR_INT1_DURATION 0x38 + + +/* Internal configuration values */ +#define REG1_POWER_NORMAL (1<<3) +#define REG1_Z_ENABLE (1<<2) +#define REG1_Y_ENABLE (1<<1) +#define REG1_X_ENABLE (1<<0) + +#define REG4_BDU (1<<7) +#define REG4_BLE (1<<6) +//#define REG4_SPI_3WIRE (1<<0) + +#define REG5_FIFO_ENABLE (1<<6) +#define REG5_REBOOT_MEMORY (1<<7) + +#define STATUS_ZYXOR (1<<7) +#define STATUS_ZOR (1<<6) +#define STATUS_YOR (1<<5) +#define STATUS_XOR (1<<4) +#define STATUS_ZYXDA (1<<3) +#define STATUS_ZDA (1<<2) +#define STATUS_YDA (1<<1) +#define STATUS_XDA (1<<0) + +#define FIFO_CTRL_BYPASS_MODE (0<<5) +#define FIFO_CTRL_FIFO_MODE (1<<5) +#define FIFO_CTRL_STREAM_MODE (1<<6) +#define FIFO_CTRL_STREAM_TO_FIFO_MODE (3<<5) +#define FIFO_CTRL_BYPASS_TO_STREAM_MODE (1<<7) + +extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); } + +class L3GD20 : public device::SPI +{ +public: + L3GD20(int bus, const char* path, spi_dev_e device); + virtual ~L3GD20(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + +private: + + struct hrt_call _call; + unsigned _call_interval; + + unsigned _num_reports; + volatile unsigned _next_report; + volatile unsigned _oldest_report; + struct gyro_report *_reports; + + struct gyro_scale _gyro_scale; + float _gyro_range_scale; + float _gyro_range_rad_s; + orb_advert_t _gyro_topic; + + unsigned _current_rate; + unsigned _current_range; + + perf_counter_t _sample_perf; + + /** + * Start automatic measurement. + */ + void start(); + + /** + * Stop automatic measurement. + */ + void stop(); + + /** + * Static trampoline from the hrt_call context; because we don't have a + * generic hrt wrapper yet. + * + * Called by the HRT in interrupt context at the specified rate if + * automatic polling is enabled. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void measure_trampoline(void *arg); + + /** + * Fetch measurements from the sensor and update the report ring. + */ + void measure(); + + /** + * Read a register from the L3GD20 + * + * @param The register to read. + * @return The value that was read. + */ + uint8_t read_reg(unsigned reg); + + /** + * Write a register in the L3GD20 + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_reg(unsigned reg, uint8_t value); + + /** + * Modify a register in the L3GD20 + * + * Bits are cleared before bits are set. + * + * @param reg The register to modify. + * @param clearbits Bits in the register to clear. + * @param setbits Bits in the register to set. + */ + void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + + /** + * Set the L3GD20 measurement range. + * + * @param max_dps The measurement range is set to permit reading at least + * this rate in degrees per second. + * Zero selects the maximum supported range. + * @return OK if the value can be supported, -ERANGE otherwise. + */ + int set_range(unsigned max_dps); + + /** + * Set the L3GD20 internal sampling frequency. + * + * @param frequency The internal sampling frequency is set to not less than + * this value. + * Zero selects the maximum rate supported. + * @return OK if the value can be supported. + */ + int set_samplerate(unsigned frequency); +}; + +/* helper macro for handling report buffer indices */ +#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) + + +L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : + SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 8000000), + _call_interval(0), + _num_reports(0), + _next_report(0), + _oldest_report(0), + _reports(nullptr), + _gyro_range_scale(0.0f), + _gyro_range_rad_s(0.0f), + _gyro_topic(-1), + _current_rate(0), + _current_range(0), + _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")) +{ + // enable debug() calls + _debug_enabled = true; + + // default scale factors + _gyro_scale.x_offset = 0; + _gyro_scale.x_scale = 1.0f; + _gyro_scale.y_offset = 0; + _gyro_scale.y_scale = 1.0f; + _gyro_scale.z_offset = 0; + _gyro_scale.z_scale = 1.0f; +} + +L3GD20::~L3GD20() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) + delete[] _reports; + + /* delete the perf counter */ + perf_free(_sample_perf); +} + +int +L3GD20::init() +{ + int ret = ERROR; + + /* do SPI init (and probe) first */ + if (SPI::init() != OK) + goto out; + + /* allocate basic report buffers */ + _num_reports = 2; + _oldest_report = _next_report = 0; + _reports = new struct gyro_report[_num_reports]; + + if (_reports == nullptr) + goto out; + + /* advertise sensor topic */ + memset(&_reports[0], 0, sizeof(_reports[0])); + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_reports[0]); + + /* set default configuration */ + write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); + write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ + write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ + write_reg(ADDR_CTRL_REG4, REG4_BDU); + write_reg(ADDR_CTRL_REG5, 0); + + write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ + write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_STREAM_MODE); /* Enable FIFO, old data is overwritten */ + + set_range(500); /* default to 500dps */ + set_samplerate(0); /* max sample rate */ + + ret = OK; +out: + return ret; +} + +int +L3GD20::probe() +{ + /* read dummy value to void to clear SPI statemachine on sensor */ + (void)read_reg(ADDR_WHO_AM_I); + + /* verify that the device is attached and functioning */ + if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) + return OK; + + return -EIO; +} + +ssize_t +L3GD20::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct gyro_report); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_call_interval > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the measurement code while we are doing this; + * we are careful to avoid racing with it. + */ + while (count--) { + if (_oldest_report != _next_report) { + memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); + ret += sizeof(_reports[0]); + INCREMENT(_oldest_report, _num_reports); + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement */ + _oldest_report = _next_report = 0; + measure(); + + /* measurement will have generated a report, copy it out */ + memcpy(buffer, _reports, sizeof(*_reports)); + ret = sizeof(*_reports); + + return ret; +} + +int +L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _call_interval = 0; + return OK; + + /* external signalling not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: + /* With internal low pass filters enabled, 250 Hz is sufficient */ + return ioctl(filp, SENSORIOCSPOLLRATE, 250); + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_call_interval == 0); + + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / arg; + + /* check against maximum sane rate */ + if (ticks < 1000) + return -EINVAL; + + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _call.period = _call_interval = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_call_interval == 0) + return SENSOR_POLLRATE_MANUAL; + + return 1000000 / _call_interval; + + case SENSORIOCSQUEUEDEPTH: { + /* account for sentinel in the ring */ + arg++; + + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + struct gyro_report *buf = new struct gyro_report[arg]; + + if (nullptr == buf) + return -ENOMEM; + + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete[] _reports; + _num_reports = arg; + _reports = buf; + start(); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _num_reports - 1; + + case SENSORIOCRESET: + /* XXX implement */ + return -EINVAL; + + case GYROIOCSSAMPLERATE: + return set_samplerate(arg); + + case GYROIOCGSAMPLERATE: + return _current_rate; + + case GYROIOCSLOWPASS: + case GYROIOCGLOWPASS: + /* XXX not implemented due to wacky interaction with samplerate */ + return -EINVAL; + + case GYROIOCSSCALE: + /* copy scale in */ + memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale)); + return OK; + + case GYROIOCGSCALE: + /* copy scale out */ + memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale)); + return OK; + + case GYROIOCSRANGE: + return set_range(arg); + + case GYROIOCGRANGE: + return _current_range; + + default: + /* give it to the superclass */ + return SPI::ioctl(filp, cmd, arg); + } +} + +uint8_t +L3GD20::read_reg(unsigned reg) +{ + uint8_t cmd[2]; + + cmd[0] = reg | DIR_READ; + + transfer(cmd, cmd, sizeof(cmd)); + + return cmd[1]; +} + +void +L3GD20::write_reg(unsigned reg, uint8_t value) +{ + uint8_t cmd[2]; + + cmd[0] = reg | DIR_WRITE; + cmd[1] = value; + + transfer(cmd, nullptr, sizeof(cmd)); +} + +void +L3GD20::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) +{ + uint8_t val; + + val = read_reg(reg); + val &= ~clearbits; + val |= setbits; + write_reg(reg, val); +} + +int +L3GD20::set_range(unsigned max_dps) +{ + uint8_t bits = REG4_BDU; + + if (max_dps == 0) + max_dps = 2000; + + if (max_dps <= 250) { + _current_range = 250; + bits |= RANGE_250DPS; + + } else if (max_dps <= 500) { + _current_range = 500; + bits |= RANGE_500DPS; + + } else if (max_dps <= 2000) { + _current_range = 2000; + bits |= RANGE_2000DPS; + + } else { + return -EINVAL; + } + + _gyro_range_rad_s = _current_range / 180.0f * M_PI_F; + _gyro_range_scale = _gyro_range_rad_s / 32768.0f; + write_reg(ADDR_CTRL_REG4, bits); + + return OK; +} + +int +L3GD20::set_samplerate(unsigned frequency) +{ + uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE; + + if (frequency == 0) + frequency = 760; + + if (frequency <= 95) { + _current_rate = 95; + bits |= RATE_95HZ_LP_25HZ; + + } else if (frequency <= 190) { + _current_rate = 190; + bits |= RATE_190HZ_LP_25HZ; + + } else if (frequency <= 380) { + _current_rate = 380; + bits |= RATE_380HZ_LP_30HZ; + + } else if (frequency <= 760) { + _current_rate = 760; + bits |= RATE_760HZ_LP_30HZ; + + } else { + return -EINVAL; + } + + write_reg(ADDR_CTRL_REG1, bits); + + return OK; +} + +void +L3GD20::start() +{ + /* make sure we are stopped first */ + stop(); + + /* reset the report ring */ + _oldest_report = _next_report = 0; + + /* start polling at the specified rate */ + hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&L3GD20::measure_trampoline, this); +} + +void +L3GD20::stop() +{ + hrt_cancel(&_call); +} + +void +L3GD20::measure_trampoline(void *arg) +{ + L3GD20 *dev = (L3GD20 *)arg; + + /* make another measurement */ + dev->measure(); +} + +void +L3GD20::measure() +{ + /* status register and data as read back from the device */ +#pragma pack(push, 1) + struct { + uint8_t cmd; + uint8_t temp; + uint8_t status; + int16_t x; + int16_t y; + int16_t z; + } raw_report; +#pragma pack(pop) + + gyro_report *report = &_reports[_next_report]; + + /* start the performance counter */ + perf_begin(_sample_perf); + + /* fetch data from the sensor */ + raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT; + transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report)); + + /* + * 1) Scale raw value to SI units using scaling from datasheet. + * 2) Subtract static offset (in SI units) + * 3) Scale the statically calibrated values with a linear + * dynamically obtained factor + * + * Note: the static sensor offset is the number the sensor outputs + * at a nominally 'zero' input. Therefore the offset has to + * be subtracted. + * + * Example: A gyro outputs a value of 74 at zero angular rate + * the offset is 74 from the origin and subtracting + * 74 from all measurements centers them around zero. + */ + report->timestamp = hrt_absolute_time(); + + /* swap x and y and negate y */ + report->x_raw = raw_report.y; + report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); + report->z_raw = raw_report.z; + + report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; + report->y = ((report->y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; + report->z = ((report->z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; + report->scaling = _gyro_range_scale; + report->range_rad_s = _gyro_range_rad_s; + + /* post a report to the ring - note, not locked */ + INCREMENT(_next_report, _num_reports); + + /* if we are running up against the oldest report, fix it */ + if (_next_report == _oldest_report) + INCREMENT(_oldest_report, _num_reports); + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + /* publish for subscribers */ + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report); + + /* stop the perf counter */ + perf_end(_sample_perf); +} + +void +L3GD20::print_info() +{ + perf_print_counter(_sample_perf); + printf("report queue: %u (%u/%u @ %p)\n", + _num_reports, _oldest_report, _next_report, _reports); +} + +/** + * Local functions in support of the shell command. + */ +namespace l3gd20 +{ + +L3GD20 *g_dev; + +void start(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + int fd; + + if (g_dev != nullptr) + errx(1, "already started"); + + /* create the driver */ + g_dev = new L3GD20(1 /* XXX magic number */, GYRO_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO); + + if (g_dev == nullptr) + goto fail; + + if (OK != g_dev->init()) + goto fail; + + /* set the poll rate to default, starts automatic data collection */ + fd = open(GYRO_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + goto fail; + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + goto fail; + + exit(0); +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + int fd_gyro = -1; + struct gyro_report g_report; + ssize_t sz; + + /* get the driver */ + fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY); + + if (fd_gyro < 0) + err(1, "%s open failed", GYRO_DEVICE_PATH); + + /* reset to manual polling */ + if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) + err(1, "reset to manual polling"); + + /* do a simple demand read */ + sz = read(fd_gyro, &g_report, sizeof(g_report)); + + if (sz != sizeof(g_report)) + err(1, "immediate gyro read failed"); + + warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x); + warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y); + warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z); + warnx("gyro x: \t%d\traw", (int)g_report.x_raw); + warnx("gyro y: \t%d\traw", (int)g_report.y_raw); + warnx("gyro z: \t%d\traw", (int)g_report.z_raw); + warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, + (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); + + /* XXX add poll-rate tests here too */ + + reset(); + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(GYRO_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "failed "); + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) + err(1, "driver reset failed"); + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + err(1, "driver poll restart failed"); + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) + errx(1, "driver not running\n"); + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + + +} // namespace + +int +l3gd20_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + + */ + if (!strcmp(argv[1], "start")) + l3gd20::start(); + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + l3gd20::test(); + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) + l3gd20::reset(); + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info")) + l3gd20::info(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} diff --git a/src/drivers/l3gd20/module.mk b/src/drivers/l3gd20/module.mk new file mode 100644 index 000000000..23e77e871 --- /dev/null +++ b/src/drivers/l3gd20/module.mk @@ -0,0 +1,6 @@ +# +# LSM303D accel/mag driver +# + +MODULE_COMMAND = l3gd20 +SRCS = l3gd20.cpp diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp new file mode 100644 index 000000000..e54724536 --- /dev/null +++ b/src/drivers/px4fmu/fmu.cpp @@ -0,0 +1,1084 @@ +/**************************************************************************** + * + * 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 fmu.cpp + * + * Driver/configurator for the PX4 FMU multi-purpose port. + */ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <stdlib.h> +#include <semaphore.h> +#include <string.h> +#include <fcntl.h> +#include <poll.h> +#include <errno.h> +#include <stdio.h> +#include <math.h> +#include <unistd.h> + +#include <nuttx/arch.h> + +#include <drivers/device/device.h> +#include <drivers/drv_pwm_output.h> +#include <drivers/drv_gpio.h> +#include <drivers/boards/px4fmu/px4fmu_internal.h> +#include <drivers/drv_hrt.h> + +#include <systemlib/systemlib.h> +#include <systemlib/err.h> +#include <systemlib/mixer/mixer.h> +#include <drivers/drv_mixer.h> +#include <drivers/drv_rc_input.h> + +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_effective.h> +#include <uORB/topics/actuator_outputs.h> + +#include <systemlib/err.h> +#include <systemlib/ppm_decode.h> + +class PX4FMU : public device::CDev +{ +public: + enum Mode { + MODE_2PWM, + MODE_4PWM, + MODE_NONE + }; + PX4FMU(); + virtual ~PX4FMU(); + + virtual int ioctl(file *filp, int cmd, unsigned long arg); + virtual ssize_t write(file *filp, const char *buffer, size_t len); + + virtual int init(); + + int set_mode(Mode mode); + + int set_pwm_alt_rate(unsigned rate); + int set_pwm_alt_channels(uint32_t channels); + +private: + static const unsigned _max_actuators = 4; + + Mode _mode; + unsigned _pwm_default_rate; + unsigned _pwm_alt_rate; + uint32_t _pwm_alt_rate_channels; + unsigned _current_update_rate; + int _task; + int _t_actuators; + int _t_armed; + orb_advert_t _t_outputs; + orb_advert_t _t_actuators_effective; + unsigned _num_outputs; + bool _primary_pwm_device; + + volatile bool _task_should_exit; + bool _armed; + + MixerGroup *_mixers; + + actuator_controls_s _controls; + + static void task_main_trampoline(int argc, char *argv[]); + void task_main() __attribute__((noreturn)); + + static int control_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input); + + int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate); + int pwm_ioctl(file *filp, int cmd, unsigned long arg); + + struct GPIOConfig { + uint32_t input; + uint32_t output; + uint32_t alt; + }; + + static const GPIOConfig _gpio_tab[]; + static const unsigned _ngpio; + + void gpio_reset(void); + void gpio_set_function(uint32_t gpios, int function); + void gpio_write(uint32_t gpios, int function); + uint32_t gpio_read(void); + int gpio_ioctl(file *filp, int cmd, unsigned long arg); + +}; + +const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { + {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, + {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, + {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1}, + {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, GPIO_USART2_RTS_1}, + {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, GPIO_USART2_TX_1}, + {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1}, + {GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2}, + {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2}, +}; + +const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]); + +namespace +{ + +PX4FMU *g_fmu; + +} // namespace + +PX4FMU::PX4FMU() : + CDev("fmuservo", "/dev/px4fmu"), + _mode(MODE_NONE), + _pwm_default_rate(50), + _pwm_alt_rate(50), + _pwm_alt_rate_channels(0), + _current_update_rate(0), + _task(-1), + _t_actuators(-1), + _t_armed(-1), + _t_outputs(0), + _t_actuators_effective(0), + _num_outputs(0), + _primary_pwm_device(false), + _task_should_exit(false), + _armed(false), + _mixers(nullptr) +{ + _debug_enabled = true; +} + +PX4FMU::~PX4FMU() +{ + if (_task != -1) { + /* tell the task we want it to go away */ + _task_should_exit = true; + + unsigned i = 10; + + do { + /* wait 50ms - it should wake every 100ms or so worst-case */ + usleep(50000); + + /* if we have given up, kill it */ + if (--i == 0) { + task_delete(_task); + break; + } + + } while (_task != -1); + } + + /* clean up the alternate device node */ + if (_primary_pwm_device) + unregister_driver(PWM_OUTPUT_DEVICE_PATH); + + g_fmu = nullptr; +} + +int +PX4FMU::init() +{ + int ret; + + ASSERT(_task == -1); + + /* do regular cdev init */ + ret = CDev::init(); + + if (ret != OK) + return ret; + + /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ + ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); + + if (ret == OK) { + log("default PWM output device"); + _primary_pwm_device = true; + } + + /* reset GPIOs */ + gpio_reset(); + + /* start the IO interface task */ + _task = task_spawn("fmuservo", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + (main_t)&PX4FMU::task_main_trampoline, + nullptr); + + if (_task < 0) { + debug("task start failed: %d", errno); + return -errno; + } + + return OK; +} + +void +PX4FMU::task_main_trampoline(int argc, char *argv[]) +{ + g_fmu->task_main(); +} + +int +PX4FMU::set_mode(Mode mode) +{ + /* + * Configure for PWM output. + * + * Note that regardless of the configured mode, the task is always + * listening and mixing; the mode just selects which of the channels + * are presented on the output pins. + */ + switch (mode) { + case MODE_2PWM: // multi-port with flow control lines as PWM + case MODE_4PWM: // multi-port as 4 PWM outs + debug("MODE_%dPWM", (mode == MODE_2PWM) ? 2 : 4); + + /* default output rates */ + _pwm_default_rate = 50; + _pwm_alt_rate = 50; + _pwm_alt_rate_channels = 0; + + /* XXX magic numbers */ + up_pwm_servo_init((mode == MODE_2PWM) ? 0x3 : 0xf); + set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + + break; + + case MODE_NONE: + debug("MODE_NONE"); + + _pwm_default_rate = 10; /* artificially reduced output rate */ + _pwm_alt_rate = 10; + _pwm_alt_rate_channels = 0; + + /* disable servo outputs - no need to set rates */ + up_pwm_servo_deinit(); + + break; + + default: + return -EINVAL; + } + + _mode = mode; + return OK; +} + +int +PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate) +{ + debug("set_pwm_rate %x %u %u", rate_map, default_rate, alt_rate); + + for (unsigned pass = 0; pass < 2; pass++) { + for (unsigned group = 0; group < _max_actuators; group++) { + + // get the channel mask for this rate group + uint32_t mask = up_pwm_servo_get_rate_group(group); + if (mask == 0) + continue; + + // all channels in the group must be either default or alt-rate + uint32_t alt = rate_map & mask; + + if (pass == 0) { + // preflight + if ((alt != 0) && (alt != mask)) { + warn("rate group %u mask %x bad overlap %x", group, mask, alt); + // not a legal map, bail + return -EINVAL; + } + } else { + // set it - errors here are unexpected + if (alt != 0) { + if (up_pwm_servo_set_rate_group_update(group, _pwm_alt_rate) != OK) { + warn("rate group set alt failed"); + return -EINVAL; + } + } else { + if (up_pwm_servo_set_rate_group_update(group, _pwm_default_rate) != OK) { + warn("rate group set default failed"); + return -EINVAL; + } + } + } + } + } + _pwm_alt_rate_channels = rate_map; + _pwm_default_rate = default_rate; + _pwm_alt_rate = alt_rate; + + return OK; +} + +int +PX4FMU::set_pwm_alt_rate(unsigned rate) +{ + return set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, rate); +} + +int +PX4FMU::set_pwm_alt_channels(uint32_t channels) +{ + return set_pwm_rate(channels, _pwm_default_rate, _pwm_alt_rate); +} + +void +PX4FMU::task_main() +{ + /* + * Subscribe to the appropriate PWM output topic based on whether we are the + * primary PWM output or not. + */ + _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : + ORB_ID(actuator_controls_1)); + /* force a reset of the update rate */ + _current_update_rate = 0; + + _t_armed = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + + /* advertise the mixed control outputs */ + actuator_outputs_s outputs; + memset(&outputs, 0, sizeof(outputs)); + /* advertise the mixed control outputs */ + _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), + &outputs); + + /* advertise the effective control inputs */ + actuator_controls_effective_s controls_effective; + memset(&controls_effective, 0, sizeof(controls_effective)); + /* advertise the effective control inputs */ + _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), + &controls_effective); + + pollfd fds[2]; + fds[0].fd = _t_actuators; + fds[0].events = POLLIN; + fds[1].fd = _t_armed; + fds[1].events = POLLIN; + + unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4; + + // rc input, published to ORB + struct rc_input_values rc_in; + orb_advert_t to_input_rc = 0; + + memset(&rc_in, 0, sizeof(rc_in)); + rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM; + + log("starting"); + + /* loop until killed */ + while (!_task_should_exit) { + + /* + * Adjust actuator topic update rate to keep up with + * the highest servo update rate configured. + * + * We always mix at max rate; some channels may update slower. + */ + unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate; + if (_current_update_rate != max_rate) { + _current_update_rate = max_rate; + int update_rate_in_ms = int(1000 / _current_update_rate); + + /* reject faster than 500 Hz updates */ + if (update_rate_in_ms < 2) { + update_rate_in_ms = 2; + } + /* reject slower than 10 Hz updates */ + if (update_rate_in_ms > 100) { + update_rate_in_ms = 100; + } + + debug("adjusted actuator update interval to %ums", update_rate_in_ms); + orb_set_interval(_t_actuators, update_rate_in_ms); + + // set to current max rate, even if we are actually checking slower/faster + _current_update_rate = max_rate; + } + + /* sleep waiting for data, stopping to check for PPM + * input at 100Hz */ + int ret = ::poll(&fds[0], 2, 10); + + /* this would be bad... */ + if (ret < 0) { + log("poll error %d", errno); + usleep(1000000); + continue; + } + + /* do we have a control update? */ + if (fds[0].revents & POLLIN) { + + /* get controls - must always do this to avoid spinning */ + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); + + /* can we mix? */ + if (_mixers != nullptr) { + + /* do mixing */ + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); + outputs.timestamp = hrt_absolute_time(); + + // XXX output actual limited values + memcpy(&controls_effective, &_controls, sizeof(controls_effective)); + + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective); + + /* iterate actuators */ + for (unsigned i = 0; i < num_outputs; i++) { + + /* last resort: catch NaN, INF and out-of-band errors */ + if (i < outputs.noutputs && + isfinite(outputs.output[i]) && + outputs.output[i] >= -1.0f && + outputs.output[i] <= 1.0f) { + /* scale for PWM output 900 - 2100us */ + outputs.output[i] = 1500 + (600 * outputs.output[i]); + } else { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + outputs.output[i] = 900; + } + + /* output to the servo */ + up_pwm_servo_set(i, outputs.output[i]); + } + + /* and publish for anyone that cares to see */ + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs); + } + } + + /* how about an arming update? */ + if (fds[1].revents & POLLIN) { + actuator_armed_s aa; + + /* get new value */ + orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); + + /* update PWM servo armed status if armed and not locked down */ + up_pwm_servo_arm(aa.armed && !aa.lockdown); + } + + // see if we have new PPM input data + if (ppm_last_valid_decode != rc_in.timestamp) { + // we have a new PPM frame. Publish it. + rc_in.channel_count = ppm_decoded_channels; + if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) { + rc_in.channel_count = RC_INPUT_MAX_CHANNELS; + } + for (uint8_t i=0; i<rc_in.channel_count; i++) { + rc_in.values[i] = ppm_buffer[i]; + } + rc_in.timestamp = ppm_last_valid_decode; + + /* lazily advertise on first publication */ + if (to_input_rc == 0) { + to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_in); + } else { + orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in); + } + } + } + + ::close(_t_actuators); + ::close(_t_actuators_effective); + ::close(_t_armed); + + /* make sure servos are off */ + up_pwm_servo_deinit(); + + log("stopping"); + + /* note - someone else is responsible for restoring the GPIO config */ + + /* tell the dtor that we are exiting */ + _task = -1; + _exit(0); +} + +int +PX4FMU::control_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input) +{ + const actuator_controls_s *controls = (actuator_controls_s *)handle; + + input = controls->control[control_index]; + return 0; +} + +int +PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) +{ + int ret; + + // XXX disabled, confusing users + //debug("ioctl 0x%04x 0x%08x", cmd, arg); + + /* try it as a GPIO ioctl first */ + ret = gpio_ioctl(filp, cmd, arg); + + if (ret != -ENOTTY) + return ret; + + /* if we are in valid PWM mode, try it as a PWM ioctl as well */ + switch (_mode) { + case MODE_2PWM: + case MODE_4PWM: + ret = pwm_ioctl(filp, cmd, arg); + break; + + default: + debug("not in a PWM mode"); + break; + } + + /* if nobody wants it, let CDev have it */ + if (ret == -ENOTTY) + ret = CDev::ioctl(filp, cmd, arg); + + return ret; +} + +int +PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) +{ + int ret = OK; + + lock(); + + switch (cmd) { + case PWM_SERVO_ARM: + up_pwm_servo_arm(true); + break; + + case PWM_SERVO_DISARM: + up_pwm_servo_arm(false); + break; + + case PWM_SERVO_SET_UPDATE_RATE: + ret = set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, arg); + break; + + case PWM_SERVO_SELECT_UPDATE_RATE: + ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate); + break; + + case PWM_SERVO_SET(2): + case PWM_SERVO_SET(3): + if (_mode != MODE_4PWM) { + ret = -EINVAL; + break; + } + + /* FALLTHROUGH */ + case PWM_SERVO_SET(0): + case PWM_SERVO_SET(1): + if (arg < 2100) { + up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg); + } else { + ret = -EINVAL; + } + + break; + + case PWM_SERVO_GET(2): + case PWM_SERVO_GET(3): + if (_mode != MODE_4PWM) { + ret = -EINVAL; + break; + } + + /* FALLTHROUGH */ + case PWM_SERVO_GET(0): + case PWM_SERVO_GET(1): + *(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0)); + break; + + case PWM_SERVO_GET_RATEGROUP(0): + case PWM_SERVO_GET_RATEGROUP(1): + case PWM_SERVO_GET_RATEGROUP(2): + case PWM_SERVO_GET_RATEGROUP(3): + *(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0)); + break; + + case PWM_SERVO_GET_COUNT: + case MIXERIOCGETOUTPUTCOUNT: + if (_mode == MODE_4PWM) { + *(unsigned *)arg = 4; + + } else { + *(unsigned *)arg = 2; + } + + break; + + case MIXERIOCRESET: + if (_mixers != nullptr) { + delete _mixers; + _mixers = nullptr; + } + + break; + + case MIXERIOCADDSIMPLE: { + mixer_simple_s *mixinfo = (mixer_simple_s *)arg; + + SimpleMixer *mixer = new SimpleMixer(control_callback, + (uintptr_t)&_controls, mixinfo); + + if (mixer->check()) { + delete mixer; + ret = -EINVAL; + + } else { + if (_mixers == nullptr) + _mixers = new MixerGroup(control_callback, + (uintptr_t)&_controls); + + _mixers->add_mixer(mixer); + } + + break; + } + + case MIXERIOCLOADBUF: { + const char *buf = (const char *)arg; + unsigned buflen = strnlen(buf, 1024); + + if (_mixers == nullptr) + _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); + + if (_mixers == nullptr) { + ret = -ENOMEM; + + } else { + + ret = _mixers->load_from_buf(buf, buflen); + + if (ret != 0) { + debug("mixer load failed with %d", ret); + delete _mixers; + _mixers = nullptr; + ret = -EINVAL; + } + } + break; + } + + default: + ret = -ENOTTY; + break; + } + + unlock(); + + return ret; +} + +/* + this implements PWM output via a write() method, for compatibility + with px4io + */ +ssize_t +PX4FMU::write(file *filp, const char *buffer, size_t len) +{ + unsigned count = len / 2; + uint16_t values[4]; + + if (count > 4) { + // we only have 4 PWM outputs on the FMU + count = 4; + } + + // allow for misaligned values + memcpy(values, buffer, count*2); + + for (uint8_t i=0; i<count; i++) { + up_pwm_servo_set(i, values[i]); + } + return count * 2; +} + +void +PX4FMU::gpio_reset(void) +{ + /* + * Setup default GPIO config - all pins as GPIOs, GPIO driver chip + * to input mode. + */ + for (unsigned i = 0; i < _ngpio; i++) + stm32_configgpio(_gpio_tab[i].input); + + stm32_gpiowrite(GPIO_GPIO_DIR, 0); + stm32_configgpio(GPIO_GPIO_DIR); +} + +void +PX4FMU::gpio_set_function(uint32_t gpios, int function) +{ + /* + * GPIOs 0 and 1 must have the same direction as they are buffered + * by a shared 2-port driver. Any attempt to set either sets both. + */ + if (gpios & 3) { + gpios |= 3; + + /* flip the buffer to output mode if required */ + if (GPIO_SET_OUTPUT == function) + stm32_gpiowrite(GPIO_GPIO_DIR, 1); + } + + /* configure selected GPIOs as required */ + for (unsigned i = 0; i < _ngpio; i++) { + if (gpios & (1 << i)) { + switch (function) { + case GPIO_SET_INPUT: + stm32_configgpio(_gpio_tab[i].input); + break; + + case GPIO_SET_OUTPUT: + stm32_configgpio(_gpio_tab[i].output); + break; + + case GPIO_SET_ALT_1: + if (_gpio_tab[i].alt != 0) + stm32_configgpio(_gpio_tab[i].alt); + + break; + } + } + } + + /* flip buffer to input mode if required */ + if ((GPIO_SET_INPUT == function) && (gpios & 3)) + stm32_gpiowrite(GPIO_GPIO_DIR, 0); +} + +void +PX4FMU::gpio_write(uint32_t gpios, int function) +{ + int value = (function == GPIO_SET) ? 1 : 0; + + for (unsigned i = 0; i < _ngpio; i++) + if (gpios & (1 << i)) + stm32_gpiowrite(_gpio_tab[i].output, value); +} + +uint32_t +PX4FMU::gpio_read(void) +{ + uint32_t bits = 0; + + for (unsigned i = 0; i < _ngpio; i++) + if (stm32_gpioread(_gpio_tab[i].input)) + bits |= (1 << i); + + return bits; +} + +int +PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret = OK; + + lock(); + + switch (cmd) { + + case GPIO_RESET: + gpio_reset(); + break; + + case GPIO_SET_OUTPUT: + case GPIO_SET_INPUT: + case GPIO_SET_ALT_1: + gpio_set_function(arg, cmd); + break; + + case GPIO_SET_ALT_2: + case GPIO_SET_ALT_3: + case GPIO_SET_ALT_4: + ret = -EINVAL; + break; + + case GPIO_SET: + case GPIO_CLEAR: + gpio_write(arg, cmd); + break; + + case GPIO_GET: + *(uint32_t *)arg = gpio_read(); + break; + + default: + ret = -ENOTTY; + } + + unlock(); + + return ret; +} + +namespace +{ + +enum PortMode { + PORT_MODE_UNSET = 0, + PORT_FULL_GPIO, + PORT_FULL_SERIAL, + PORT_FULL_PWM, + PORT_GPIO_AND_SERIAL, + PORT_PWM_AND_SERIAL, + PORT_PWM_AND_GPIO, +}; + +PortMode g_port_mode; + +int +fmu_new_mode(PortMode new_mode) +{ + uint32_t gpio_bits; + PX4FMU::Mode servo_mode; + + /* reset to all-inputs */ + g_fmu->ioctl(0, GPIO_RESET, 0); + + gpio_bits = 0; + servo_mode = PX4FMU::MODE_NONE; + + switch (new_mode) { + case PORT_FULL_GPIO: + case PORT_MODE_UNSET: + /* nothing more to do here */ + break; + + case PORT_FULL_SERIAL: + /* set all multi-GPIOs to serial mode */ + gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4; + break; + + case PORT_FULL_PWM: + /* select 4-pin PWM mode */ + servo_mode = PX4FMU::MODE_4PWM; + break; + + case PORT_GPIO_AND_SERIAL: + /* set RX/TX multi-GPIOs to serial mode */ + gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; + break; + + case PORT_PWM_AND_SERIAL: + /* select 2-pin PWM mode */ + servo_mode = PX4FMU::MODE_2PWM; + /* set RX/TX multi-GPIOs to serial mode */ + gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; + break; + + case PORT_PWM_AND_GPIO: + /* select 2-pin PWM mode */ + servo_mode = PX4FMU::MODE_2PWM; + break; + } + + /* adjust GPIO config for serial mode(s) */ + if (gpio_bits != 0) + g_fmu->ioctl(0, GPIO_SET_ALT_1, gpio_bits); + + /* (re)set the PWM output mode */ + g_fmu->set_mode(servo_mode); + + return OK; +} + +int +fmu_start(void) +{ + int ret = OK; + + if (g_fmu == nullptr) { + + g_fmu = new PX4FMU; + + if (g_fmu == nullptr) { + ret = -ENOMEM; + + } else { + ret = g_fmu->init(); + + if (ret != OK) { + delete g_fmu; + g_fmu = nullptr; + } + } + } + + return ret; +} + +void +test(void) +{ + int fd; + + fd = open(PWM_OUTPUT_DEVICE_PATH, 0); + + if (fd < 0) + errx(1, "open fail"); + + if (ioctl(fd, PWM_SERVO_ARM, 0) < 0) err(1, "servo arm failed"); + + if (ioctl(fd, PWM_SERVO_SET(0), 1000) < 0) err(1, "servo 1 set failed"); + + if (ioctl(fd, PWM_SERVO_SET(1), 1200) < 0) err(1, "servo 2 set failed"); + + if (ioctl(fd, PWM_SERVO_SET(2), 1400) < 0) err(1, "servo 3 set failed"); + + if (ioctl(fd, PWM_SERVO_SET(3), 1600) < 0) err(1, "servo 4 set failed"); + + close(fd); + + exit(0); +} + +void +fake(int argc, char *argv[]) +{ + if (argc < 5) + errx(1, "fmu fake <roll> <pitch> <yaw> <thrust> (values -100 .. 100)"); + + actuator_controls_s ac; + + ac.control[0] = strtol(argv[1], 0, 0) / 100.0f; + + ac.control[1] = strtol(argv[2], 0, 0) / 100.0f; + + ac.control[2] = strtol(argv[3], 0, 0) / 100.0f; + + ac.control[3] = strtol(argv[4], 0, 0) / 100.0f; + + orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac); + + if (handle < 0) + errx(1, "advertise failed"); + + actuator_armed_s aa; + + aa.armed = true; + aa.lockdown = false; + + handle = orb_advertise(ORB_ID(actuator_armed), &aa); + + if (handle < 0) + errx(1, "advertise failed 2"); + + exit(0); +} + +} // namespace + +extern "C" __EXPORT int fmu_main(int argc, char *argv[]); + +int +fmu_main(int argc, char *argv[]) +{ + PortMode new_mode = PORT_MODE_UNSET; + const char *verb = argv[1]; + + if (fmu_start() != OK) + errx(1, "failed to start the FMU driver"); + + /* + * Mode switches. + */ + if (!strcmp(verb, "mode_gpio")) { + new_mode = PORT_FULL_GPIO; + + } else if (!strcmp(verb, "mode_serial")) { + new_mode = PORT_FULL_SERIAL; + + } else if (!strcmp(verb, "mode_pwm")) { + new_mode = PORT_FULL_PWM; + + } else if (!strcmp(verb, "mode_gpio_serial")) { + new_mode = PORT_GPIO_AND_SERIAL; + + } else if (!strcmp(verb, "mode_pwm_serial")) { + new_mode = PORT_PWM_AND_SERIAL; + + } else if (!strcmp(verb, "mode_pwm_gpio")) { + new_mode = PORT_PWM_AND_GPIO; + } + + /* was a new mode set? */ + if (new_mode != PORT_MODE_UNSET) { + + /* yes but it's the same mode */ + if (new_mode == g_port_mode) + return OK; + + /* switch modes */ + int ret = fmu_new_mode(new_mode); + exit(ret == OK ? 0 : 1); + } + + if (!strcmp(verb, "test")) + test(); + + if (!strcmp(verb, "fake")) + fake(argc - 1, argv + 1); + + fprintf(stderr, "FMU: unrecognised command, try:\n"); + fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n"); + exit(1); +} diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk new file mode 100644 index 000000000..05bc7a5b3 --- /dev/null +++ b/src/drivers/px4fmu/module.mk @@ -0,0 +1,6 @@ +# +# Interface driver for the PX4FMU board +# + +MODULE_COMMAND = fmu +SRCS = fmu.cpp diff --git a/src/include/visibility.h b/src/include/visibility.h new file mode 100644 index 000000000..2c6adc062 --- /dev/null +++ b/src/include/visibility.h @@ -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
\ No newline at end of file diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp new file mode 100755 index 000000000..1a50dde0f --- /dev/null +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -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; +} diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c new file mode 100755 index 000000000..7d3812abd --- /dev/null +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -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; +} diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h new file mode 100755 index 000000000..09817d58e --- /dev/null +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h @@ -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); diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c new file mode 100755 index 000000000..9e97ad11a --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c @@ -0,0 +1,1148 @@ +/*
+ * attitudeKalmanfilter.c
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "rdivide.h"
+#include "norm.h"
+#include "cross.h"
+#include "eye.h"
+#include "mrdivide.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+static real32_T rt_atan2f_snf(real32_T u0, real32_T u1);
+
+/* Function Definitions */
+static real32_T rt_atan2f_snf(real32_T u0, real32_T u1)
+{
+ real32_T y;
+ int32_T b_u0;
+ int32_T b_u1;
+ if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
+ y = ((real32_T)rtNaN);
+ } else if (rtIsInfF(u0) && rtIsInfF(u1)) {
+ if (u0 > 0.0F) {
+ b_u0 = 1;
+ } else {
+ b_u0 = -1;
+ }
+
+ if (u1 > 0.0F) {
+ b_u1 = 1;
+ } else {
+ b_u1 = -1;
+ }
+
+ y = (real32_T)atan2((real32_T)b_u0, (real32_T)b_u1);
+ } else if (u1 == 0.0F) {
+ if (u0 > 0.0F) {
+ y = RT_PIF / 2.0F;
+ } else if (u0 < 0.0F) {
+ y = -(RT_PIF / 2.0F);
+ } else {
+ y = 0.0F;
+ }
+ } else {
+ y = (real32_T)atan2(u0, u1);
+ }
+
+ return y;
+}
+
+/*
+ * function [eulerAngles,Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter_wo(updateVect,dt,z,x_aposteriori_k,P_aposteriori_k,q,r)
+ */
+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])
+{
+ real32_T wak[3];
+ real32_T O[9];
+ real_T dv0[9];
+ real32_T a[9];
+ int32_T i;
+ real32_T b_a[9];
+ real32_T x_n_b[3];
+ real32_T b_x_aposteriori_k[3];
+ real32_T z_n_b[3];
+ real32_T c_a[3];
+ real32_T d_a[3];
+ int32_T i0;
+ real32_T x_apriori[12];
+ real_T dv1[144];
+ real32_T A_lin[144];
+ static const int8_T iv0[36] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0,
+ 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+
+ real32_T b_A_lin[144];
+ real32_T b_q[144];
+ real32_T c_A_lin[144];
+ real32_T d_A_lin[144];
+ real32_T e_A_lin[144];
+ int32_T i1;
+ real32_T P_apriori[144];
+ real32_T b_P_apriori[108];
+ static const int8_T iv1[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 };
+
+ real32_T K_k[108];
+ real32_T fv0[81];
+ static const int8_T iv2[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 };
+
+ real32_T b_r[81];
+ real32_T fv1[81];
+ real32_T f0;
+ real32_T c_P_apriori[36];
+ static const int8_T iv3[36] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+
+ real32_T fv2[36];
+ static const int8_T iv4[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+
+ real32_T c_r[9];
+ real32_T b_K_k[36];
+ real32_T d_P_apriori[72];
+ static const int8_T iv5[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 1, 0, 0, 0 };
+
+ real32_T c_K_k[72];
+ static const int8_T iv6[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
+ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0 };
+
+ real32_T b_z[6];
+ static const int8_T iv7[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 1 };
+
+ static const int8_T iv8[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
+ 0, 0, 0, 1 };
+
+ real32_T fv3[6];
+ real32_T c_z[6];
+
+ /* Extended Attitude Kalmanfilter */
+ /* */
+ /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
+ /* measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]' */
+ /* knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */
+ /* */
+ /* [x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */
+ /* */
+ /* Example.... */
+ /* */
+ /* $Author: Tobias Naegeli $ $Date: 2012 $ $Revision: 1 $ */
+ /* coder.varsize('udpIndVect', [9,1], [1,0]) */
+ /* udpIndVect=find(updVect); */
+ /* process and measurement noise covariance matrix */
+ /* Q = diag(q.^2*dt); */
+ /* observation matrix */
+ /* 'attitudeKalmanfilter:33' wx= x_aposteriori_k(1); */
+ /* 'attitudeKalmanfilter:34' wy= x_aposteriori_k(2); */
+ /* 'attitudeKalmanfilter:35' wz= x_aposteriori_k(3); */
+ /* 'attitudeKalmanfilter:37' wax= x_aposteriori_k(4); */
+ /* 'attitudeKalmanfilter:38' way= x_aposteriori_k(5); */
+ /* 'attitudeKalmanfilter:39' waz= x_aposteriori_k(6); */
+ /* 'attitudeKalmanfilter:41' zex= x_aposteriori_k(7); */
+ /* 'attitudeKalmanfilter:42' zey= x_aposteriori_k(8); */
+ /* 'attitudeKalmanfilter:43' zez= x_aposteriori_k(9); */
+ /* 'attitudeKalmanfilter:45' mux= x_aposteriori_k(10); */
+ /* 'attitudeKalmanfilter:46' muy= x_aposteriori_k(11); */
+ /* 'attitudeKalmanfilter:47' muz= x_aposteriori_k(12); */
+ /* % prediction section */
+ /* body angular accelerations */
+ /* 'attitudeKalmanfilter:51' wak =[wax;way;waz]; */
+ wak[0] = x_aposteriori_k[3];
+ wak[1] = x_aposteriori_k[4];
+ wak[2] = x_aposteriori_k[5];
+
+ /* body angular rates */
+ /* 'attitudeKalmanfilter:54' wk =[wx; wy; wz] + dt*wak; */
+ /* derivative of the prediction rotation matrix */
+ /* 'attitudeKalmanfilter:57' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */
+ O[0] = 0.0F;
+ O[1] = -x_aposteriori_k[2];
+ O[2] = x_aposteriori_k[1];
+ O[3] = x_aposteriori_k[2];
+ O[4] = 0.0F;
+ O[5] = -x_aposteriori_k[0];
+ O[6] = -x_aposteriori_k[1];
+ O[7] = x_aposteriori_k[0];
+ O[8] = 0.0F;
+
+ /* prediction of the earth z vector */
+ /* 'attitudeKalmanfilter:60' zek =(eye(3)+O*dt)*[zex;zey;zez]; */
+ eye(dv0);
+ for (i = 0; i < 9; i++) {
+ a[i] = (real32_T)dv0[i] + O[i] * dt;
+ }
+
+ /* prediction of the magnetic vector */
+ /* 'attitudeKalmanfilter:63' muk =(eye(3)+O*dt)*[mux;muy;muz]; */
+ eye(dv0);
+ for (i = 0; i < 9; i++) {
+ b_a[i] = (real32_T)dv0[i] + O[i] * dt;
+ }
+
+ /* 'attitudeKalmanfilter:65' EZ=[0,zez,-zey; */
+ /* 'attitudeKalmanfilter:66' -zez,0,zex; */
+ /* 'attitudeKalmanfilter:67' zey,-zex,0]'; */
+ /* 'attitudeKalmanfilter:68' MA=[0,muz,-muy; */
+ /* 'attitudeKalmanfilter:69' -muz,0,mux; */
+ /* 'attitudeKalmanfilter:70' zey,-mux,0]'; */
+ /* 'attitudeKalmanfilter:74' E=eye(3); */
+ /* 'attitudeKalmanfilter:76' Z=zeros(3); */
+ /* 'attitudeKalmanfilter:77' x_apriori=[wk;wak;zek;muk]; */
+ x_n_b[0] = x_aposteriori_k[0];
+ x_n_b[1] = x_aposteriori_k[1];
+ x_n_b[2] = x_aposteriori_k[2];
+ b_x_aposteriori_k[0] = x_aposteriori_k[6];
+ b_x_aposteriori_k[1] = x_aposteriori_k[7];
+ b_x_aposteriori_k[2] = x_aposteriori_k[8];
+ z_n_b[0] = x_aposteriori_k[9];
+ z_n_b[1] = x_aposteriori_k[10];
+ z_n_b[2] = x_aposteriori_k[11];
+ for (i = 0; i < 3; i++) {
+ c_a[i] = 0.0F;
+ for (i0 = 0; i0 < 3; i0++) {
+ c_a[i] += a[i + 3 * i0] * b_x_aposteriori_k[i0];
+ }
+
+ d_a[i] = 0.0F;
+ for (i0 = 0; i0 < 3; i0++) {
+ d_a[i] += b_a[i + 3 * i0] * z_n_b[i0];
+ }
+
+ x_apriori[i] = x_n_b[i] + dt * wak[i];
+ }
+
+ for (i = 0; i < 3; i++) {
+ x_apriori[i + 3] = wak[i];
+ }
+
+ for (i = 0; i < 3; i++) {
+ x_apriori[i + 6] = c_a[i];
+ }
+
+ for (i = 0; i < 3; i++) {
+ x_apriori[i + 9] = d_a[i];
+ }
+
+ /* 'attitudeKalmanfilter:81' A_lin=[ Z, E, Z, Z */
+ /* 'attitudeKalmanfilter:82' Z, Z, Z, Z */
+ /* 'attitudeKalmanfilter:83' EZ, Z, O, Z */
+ /* 'attitudeKalmanfilter:84' MA, Z, Z, O]; */
+ /* 'attitudeKalmanfilter:86' A_lin=eye(12)+A_lin*dt; */
+ b_eye(dv1);
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_lin[i0 + 12 * i] = (real32_T)iv0[i0 + 3 * i];
+ }
+
+ for (i0 = 0; i0 < 3; i0++) {
+ A_lin[(i0 + 12 * i) + 3] = 0.0F;
+ }
+ }
+
+ A_lin[6] = 0.0F;
+ A_lin[7] = x_aposteriori_k[8];
+ A_lin[8] = -x_aposteriori_k[7];
+ A_lin[18] = -x_aposteriori_k[8];
+ A_lin[19] = 0.0F;
+ A_lin[20] = x_aposteriori_k[6];
+ A_lin[30] = x_aposteriori_k[7];
+ A_lin[31] = -x_aposteriori_k[6];
+ A_lin[32] = 0.0F;
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_lin[(i0 + 12 * (i + 3)) + 6] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_lin[(i0 + 12 * (i + 6)) + 6] = O[i0 + 3 * i];
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_lin[(i0 + 12 * (i + 9)) + 6] = 0.0F;
+ }
+ }
+
+ A_lin[9] = 0.0F;
+ A_lin[10] = x_aposteriori_k[11];
+ A_lin[11] = -x_aposteriori_k[10];
+ A_lin[21] = -x_aposteriori_k[11];
+ A_lin[22] = 0.0F;
+ A_lin[23] = x_aposteriori_k[9];
+ A_lin[33] = x_aposteriori_k[7];
+ A_lin[34] = -x_aposteriori_k[9];
+ A_lin[35] = 0.0F;
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_lin[(i0 + 12 * (i + 3)) + 9] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_lin[(i0 + 12 * (i + 6)) + 9] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_lin[(i0 + 12 * (i + 9)) + 9] = O[i0 + 3 * i];
+ }
+ }
+
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ b_A_lin[i0 + 12 * i] = (real32_T)dv1[i0 + 12 * i] + A_lin[i0 + 12 * i] *
+ dt;
+ }
+ }
+
+ /* 'attitudeKalmanfilter:88' Qtemp=[ q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */
+ /* 'attitudeKalmanfilter:89' 0, q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */
+ /* 'attitudeKalmanfilter:90' 0, 0, q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0; */
+ /* 'attitudeKalmanfilter:91' 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0, 0, 0; */
+ /* 'attitudeKalmanfilter:92' 0, 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0, 0; */
+ /* 'attitudeKalmanfilter:93' 0, 0, 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0; */
+ /* 'attitudeKalmanfilter:94' 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0, 0, 0; */
+ /* 'attitudeKalmanfilter:95' 0, 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0, 0; */
+ /* 'attitudeKalmanfilter:96' 0, 0, 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0; */
+ /* 'attitudeKalmanfilter:97' 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4), 0, 0; */
+ /* 'attitudeKalmanfilter:98' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4), 0; */
+ /* 'attitudeKalmanfilter:99' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4)]; */
+ /* 'attitudeKalmanfilter:103' Q=A_lin*Qtemp*A_lin'; */
+ /* 'attitudeKalmanfilter:106' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */
+ b_q[0] = q[0];
+ b_q[12] = 0.0F;
+ b_q[24] = 0.0F;
+ b_q[36] = 0.0F;
+ b_q[48] = 0.0F;
+ b_q[60] = 0.0F;
+ b_q[72] = 0.0F;
+ b_q[84] = 0.0F;
+ b_q[96] = 0.0F;
+ b_q[108] = 0.0F;
+ b_q[120] = 0.0F;
+ b_q[132] = 0.0F;
+ b_q[1] = 0.0F;
+ b_q[13] = q[0];
+ b_q[25] = 0.0F;
+ b_q[37] = 0.0F;
+ b_q[49] = 0.0F;
+ b_q[61] = 0.0F;
+ b_q[73] = 0.0F;
+ b_q[85] = 0.0F;
+ b_q[97] = 0.0F;
+ b_q[109] = 0.0F;
+ b_q[121] = 0.0F;
+ b_q[133] = 0.0F;
+ b_q[2] = 0.0F;
+ b_q[14] = 0.0F;
+ b_q[26] = q[0];
+ b_q[38] = 0.0F;
+ b_q[50] = 0.0F;
+ b_q[62] = 0.0F;
+ b_q[74] = 0.0F;
+ b_q[86] = 0.0F;
+ b_q[98] = 0.0F;
+ b_q[110] = 0.0F;
+ b_q[122] = 0.0F;
+ b_q[134] = 0.0F;
+ b_q[3] = 0.0F;
+ b_q[15] = 0.0F;
+ b_q[27] = 0.0F;
+ b_q[39] = q[1];
+ b_q[51] = 0.0F;
+ b_q[63] = 0.0F;
+ b_q[75] = 0.0F;
+ b_q[87] = 0.0F;
+ b_q[99] = 0.0F;
+ b_q[111] = 0.0F;
+ b_q[123] = 0.0F;
+ b_q[135] = 0.0F;
+ b_q[4] = 0.0F;
+ b_q[16] = 0.0F;
+ b_q[28] = 0.0F;
+ b_q[40] = 0.0F;
+ b_q[52] = q[1];
+ b_q[64] = 0.0F;
+ b_q[76] = 0.0F;
+ b_q[88] = 0.0F;
+ b_q[100] = 0.0F;
+ b_q[112] = 0.0F;
+ b_q[124] = 0.0F;
+ b_q[136] = 0.0F;
+ b_q[5] = 0.0F;
+ b_q[17] = 0.0F;
+ b_q[29] = 0.0F;
+ b_q[41] = 0.0F;
+ b_q[53] = 0.0F;
+ b_q[65] = q[1];
+ b_q[77] = 0.0F;
+ b_q[89] = 0.0F;
+ b_q[101] = 0.0F;
+ b_q[113] = 0.0F;
+ b_q[125] = 0.0F;
+ b_q[137] = 0.0F;
+ b_q[6] = 0.0F;
+ b_q[18] = 0.0F;
+ b_q[30] = 0.0F;
+ b_q[42] = 0.0F;
+ b_q[54] = 0.0F;
+ b_q[66] = 0.0F;
+ b_q[78] = q[2];
+ b_q[90] = 0.0F;
+ b_q[102] = 0.0F;
+ b_q[114] = 0.0F;
+ b_q[126] = 0.0F;
+ b_q[138] = 0.0F;
+ b_q[7] = 0.0F;
+ b_q[19] = 0.0F;
+ b_q[31] = 0.0F;
+ b_q[43] = 0.0F;
+ b_q[55] = 0.0F;
+ b_q[67] = 0.0F;
+ b_q[79] = 0.0F;
+ b_q[91] = q[2];
+ b_q[103] = 0.0F;
+ b_q[115] = 0.0F;
+ b_q[127] = 0.0F;
+ b_q[139] = 0.0F;
+ b_q[8] = 0.0F;
+ b_q[20] = 0.0F;
+ b_q[32] = 0.0F;
+ b_q[44] = 0.0F;
+ b_q[56] = 0.0F;
+ b_q[68] = 0.0F;
+ b_q[80] = 0.0F;
+ b_q[92] = 0.0F;
+ b_q[104] = q[2];
+ b_q[116] = 0.0F;
+ b_q[128] = 0.0F;
+ b_q[140] = 0.0F;
+ b_q[9] = 0.0F;
+ b_q[21] = 0.0F;
+ b_q[33] = 0.0F;
+ b_q[45] = 0.0F;
+ b_q[57] = 0.0F;
+ b_q[69] = 0.0F;
+ b_q[81] = 0.0F;
+ b_q[93] = 0.0F;
+ b_q[105] = 0.0F;
+ b_q[117] = q[3];
+ b_q[129] = 0.0F;
+ b_q[141] = 0.0F;
+ b_q[10] = 0.0F;
+ b_q[22] = 0.0F;
+ b_q[34] = 0.0F;
+ b_q[46] = 0.0F;
+ b_q[58] = 0.0F;
+ b_q[70] = 0.0F;
+ b_q[82] = 0.0F;
+ b_q[94] = 0.0F;
+ b_q[106] = 0.0F;
+ b_q[118] = 0.0F;
+ b_q[130] = q[3];
+ b_q[142] = 0.0F;
+ b_q[11] = 0.0F;
+ b_q[23] = 0.0F;
+ b_q[35] = 0.0F;
+ b_q[47] = 0.0F;
+ b_q[59] = 0.0F;
+ b_q[71] = 0.0F;
+ b_q[83] = 0.0F;
+ b_q[95] = 0.0F;
+ b_q[107] = 0.0F;
+ b_q[119] = 0.0F;
+ b_q[131] = 0.0F;
+ b_q[143] = q[3];
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ A_lin[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_aposteriori_k[i1 + 12 *
+ i0];
+ }
+
+ c_A_lin[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ c_A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * b_q[i1 + 12 * i0];
+ }
+ }
+
+ for (i0 = 0; i0 < 12; i0++) {
+ d_A_lin[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ d_A_lin[i + 12 * i0] += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1];
+ }
+
+ e_A_lin[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ e_A_lin[i + 12 * i0] += c_A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1];
+ }
+ }
+ }
+
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ P_apriori[i0 + 12 * i] = d_A_lin[i0 + 12 * i] + e_A_lin[i0 + 12 * i];
+ }
+ }
+
+ /* % update */
+ /* 'attitudeKalmanfilter:110' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */
+ if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 1)) {
+ /* 'attitudeKalmanfilter:111' if z(6)<4 || z(5)>15 */
+ if ((z[5] < 4.0F) || (z[4] > 15.0F)) {
+ /* 'attitudeKalmanfilter:112' r(2)=10000; */
+ r[1] = 10000.0F;
+ }
+
+ /* 'attitudeKalmanfilter:114' R=[r(1),0,0,0,0,0,0,0,0; */
+ /* 'attitudeKalmanfilter:115' 0,r(1),0,0,0,0,0,0,0; */
+ /* 'attitudeKalmanfilter:116' 0,0,r(1),0,0,0,0,0,0; */
+ /* 'attitudeKalmanfilter:117' 0,0,0,r(2),0,0,0,0,0; */
+ /* 'attitudeKalmanfilter:118' 0,0,0,0,r(2),0,0,0,0; */
+ /* 'attitudeKalmanfilter:119' 0,0,0,0,0,r(2),0,0,0; */
+ /* 'attitudeKalmanfilter:120' 0,0,0,0,0,0,r(3),0,0; */
+ /* 'attitudeKalmanfilter:121' 0,0,0,0,0,0,0,r(3),0; */
+ /* 'attitudeKalmanfilter:122' 0,0,0,0,0,0,0,0,r(3)]; */
+ /* observation matrix */
+ /* [zw;ze;zmk]; */
+ /* 'attitudeKalmanfilter:125' H_k=[ E, Z, Z, Z; */
+ /* 'attitudeKalmanfilter:126' Z, Z, E, Z; */
+ /* 'attitudeKalmanfilter:127' Z, Z, Z, E]; */
+ /* 'attitudeKalmanfilter:129' y_k=z(1:9)-H_k*x_apriori; */
+ /* 'attitudeKalmanfilter:132' S_k=H_k*P_apriori*H_k'+R; */
+ /* 'attitudeKalmanfilter:133' K_k=(P_apriori*H_k'/(S_k)); */
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 9; i0++) {
+ b_P_apriori[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ b_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)iv1[i1
+ + 12 * i0];
+ }
+ }
+ }
+
+ for (i = 0; i < 9; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ K_k[i + 9 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ K_k[i + 9 * i0] += (real32_T)iv2[i + 9 * i1] * P_apriori[i1 + 12 * i0];
+ }
+ }
+
+ for (i0 = 0; i0 < 9; i0++) {
+ fv0[i + 9 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ fv0[i + 9 * i0] += K_k[i + 9 * i1] * (real32_T)iv1[i1 + 12 * i0];
+ }
+ }
+ }
+
+ b_r[0] = r[0];
+ b_r[9] = 0.0F;
+ b_r[18] = 0.0F;
+ b_r[27] = 0.0F;
+ b_r[36] = 0.0F;
+ b_r[45] = 0.0F;
+ b_r[54] = 0.0F;
+ b_r[63] = 0.0F;
+ b_r[72] = 0.0F;
+ b_r[1] = 0.0F;
+ b_r[10] = r[0];
+ b_r[19] = 0.0F;
+ b_r[28] = 0.0F;
+ b_r[37] = 0.0F;
+ b_r[46] = 0.0F;
+ b_r[55] = 0.0F;
+ b_r[64] = 0.0F;
+ b_r[73] = 0.0F;
+ b_r[2] = 0.0F;
+ b_r[11] = 0.0F;
+ b_r[20] = r[0];
+ b_r[29] = 0.0F;
+ b_r[38] = 0.0F;
+ b_r[47] = 0.0F;
+ b_r[56] = 0.0F;
+ b_r[65] = 0.0F;
+ b_r[74] = 0.0F;
+ b_r[3] = 0.0F;
+ b_r[12] = 0.0F;
+ b_r[21] = 0.0F;
+ b_r[30] = r[1];
+ b_r[39] = 0.0F;
+ b_r[48] = 0.0F;
+ b_r[57] = 0.0F;
+ b_r[66] = 0.0F;
+ b_r[75] = 0.0F;
+ b_r[4] = 0.0F;
+ b_r[13] = 0.0F;
+ b_r[22] = 0.0F;
+ b_r[31] = 0.0F;
+ b_r[40] = r[1];
+ b_r[49] = 0.0F;
+ b_r[58] = 0.0F;
+ b_r[67] = 0.0F;
+ b_r[76] = 0.0F;
+ b_r[5] = 0.0F;
+ b_r[14] = 0.0F;
+ b_r[23] = 0.0F;
+ b_r[32] = 0.0F;
+ b_r[41] = 0.0F;
+ b_r[50] = r[1];
+ b_r[59] = 0.0F;
+ b_r[68] = 0.0F;
+ b_r[77] = 0.0F;
+ b_r[6] = 0.0F;
+ b_r[15] = 0.0F;
+ b_r[24] = 0.0F;
+ b_r[33] = 0.0F;
+ b_r[42] = 0.0F;
+ b_r[51] = 0.0F;
+ b_r[60] = r[2];
+ b_r[69] = 0.0F;
+ b_r[78] = 0.0F;
+ b_r[7] = 0.0F;
+ b_r[16] = 0.0F;
+ b_r[25] = 0.0F;
+ b_r[34] = 0.0F;
+ b_r[43] = 0.0F;
+ b_r[52] = 0.0F;
+ b_r[61] = 0.0F;
+ b_r[70] = r[2];
+ b_r[79] = 0.0F;
+ b_r[8] = 0.0F;
+ b_r[17] = 0.0F;
+ b_r[26] = 0.0F;
+ b_r[35] = 0.0F;
+ b_r[44] = 0.0F;
+ b_r[53] = 0.0F;
+ b_r[62] = 0.0F;
+ b_r[71] = 0.0F;
+ b_r[80] = r[2];
+ for (i = 0; i < 9; i++) {
+ for (i0 = 0; i0 < 9; i0++) {
+ fv1[i0 + 9 * i] = fv0[i0 + 9 * i] + b_r[i0 + 9 * i];
+ }
+ }
+
+ mrdivide(b_P_apriori, fv1, K_k);
+
+ /* 'attitudeKalmanfilter:136' x_aposteriori=x_apriori+K_k*y_k; */
+ for (i = 0; i < 9; i++) {
+ f0 = 0.0F;
+ for (i0 = 0; i0 < 12; i0++) {
+ f0 += (real32_T)iv2[i + 9 * i0] * x_apriori[i0];
+ }
+
+ O[i] = z[i] - f0;
+ }
+
+ for (i = 0; i < 12; i++) {
+ f0 = 0.0F;
+ for (i0 = 0; i0 < 9; i0++) {
+ f0 += K_k[i + 12 * i0] * O[i0];
+ }
+
+ x_aposteriori[i] = x_apriori[i] + f0;
+ }
+
+ /* 'attitudeKalmanfilter:137' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */
+ b_eye(dv1);
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ f0 = 0.0F;
+ for (i1 = 0; i1 < 9; i1++) {
+ f0 += K_k[i + 12 * i1] * (real32_T)iv2[i1 + 9 * i0];
+ }
+
+ b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
+ }
+ }
+
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ P_aposteriori[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + 12
+ * i0];
+ }
+ }
+ }
+ } else {
+ /* 'attitudeKalmanfilter:138' else */
+ /* 'attitudeKalmanfilter:139' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */
+ if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 0)) {
+ /* 'attitudeKalmanfilter:141' R=[r(1),0,0; */
+ /* 'attitudeKalmanfilter:142' 0,r(1),0; */
+ /* 'attitudeKalmanfilter:143' 0,0,r(1)]; */
+ /* observation matrix */
+ /* 'attitudeKalmanfilter:146' H_k=[ E, Z, Z, Z]; */
+ /* 'attitudeKalmanfilter:148' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */
+ /* 'attitudeKalmanfilter:150' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */
+ /* 'attitudeKalmanfilter:151' K_k=(P_apriori*H_k(1:3,1:12)'/(S_k)); */
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ c_P_apriori[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ c_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)
+ iv3[i1 + 12 * i0];
+ }
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ fv2[i + 3 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ fv2[i + 3 * i0] += (real32_T)iv4[i + 3 * i1] * P_apriori[i1 + 12 *
+ i0];
+ }
+ }
+
+ for (i0 = 0; i0 < 3; i0++) {
+ O[i + 3 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ O[i + 3 * i0] += fv2[i + 3 * i1] * (real32_T)iv3[i1 + 12 * i0];
+ }
+ }
+ }
+
+ c_r[0] = r[0];
+ c_r[3] = 0.0F;
+ c_r[6] = 0.0F;
+ c_r[1] = 0.0F;
+ c_r[4] = r[0];
+ c_r[7] = 0.0F;
+ c_r[2] = 0.0F;
+ c_r[5] = 0.0F;
+ c_r[8] = r[0];
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ a[i0 + 3 * i] = O[i0 + 3 * i] + c_r[i0 + 3 * i];
+ }
+ }
+
+ b_mrdivide(c_P_apriori, a, b_K_k);
+
+ /* 'attitudeKalmanfilter:154' x_aposteriori=x_apriori+K_k*y_k; */
+ for (i = 0; i < 3; i++) {
+ f0 = 0.0F;
+ for (i0 = 0; i0 < 12; i0++) {
+ f0 += (real32_T)iv4[i + 3 * i0] * x_apriori[i0];
+ }
+
+ x_n_b[i] = z[i] - f0;
+ }
+
+ for (i = 0; i < 12; i++) {
+ f0 = 0.0F;
+ for (i0 = 0; i0 < 3; i0++) {
+ f0 += b_K_k[i + 12 * i0] * x_n_b[i0];
+ }
+
+ x_aposteriori[i] = x_apriori[i] + f0;
+ }
+
+ /* 'attitudeKalmanfilter:155' P_aposteriori=(eye(12)-K_k*H_k(1:3,1:12))*P_apriori; */
+ b_eye(dv1);
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ f0 = 0.0F;
+ for (i1 = 0; i1 < 3; i1++) {
+ f0 += b_K_k[i + 12 * i1] * (real32_T)iv4[i1 + 3 * i0];
+ }
+
+ b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
+ }
+ }
+
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ P_aposteriori[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 +
+ 12 * i0];
+ }
+ }
+ }
+ } else {
+ /* 'attitudeKalmanfilter:156' else */
+ /* 'attitudeKalmanfilter:157' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==0 */
+ if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 0))
+ {
+ /* 'attitudeKalmanfilter:158' if z(6)<4 || z(5)>15 */
+ if ((z[5] < 4.0F) || (z[4] > 15.0F)) {
+ /* 'attitudeKalmanfilter:159' r(2)=10000; */
+ r[1] = 10000.0F;
+ }
+
+ /* 'attitudeKalmanfilter:162' R=[r(1),0,0,0,0,0; */
+ /* 'attitudeKalmanfilter:163' 0,r(1),0,0,0,0; */
+ /* 'attitudeKalmanfilter:164' 0,0,r(1),0,0,0; */
+ /* 'attitudeKalmanfilter:165' 0,0,0,r(2),0,0; */
+ /* 'attitudeKalmanfilter:166' 0,0,0,0,r(2),0; */
+ /* 'attitudeKalmanfilter:167' 0,0,0,0,0,r(2)]; */
+ /* observation matrix */
+ /* 'attitudeKalmanfilter:170' H_k=[ E, Z, Z, Z; */
+ /* 'attitudeKalmanfilter:171' Z, Z, E, Z]; */
+ /* 'attitudeKalmanfilter:173' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */
+ /* 'attitudeKalmanfilter:175' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */
+ /* 'attitudeKalmanfilter:176' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 6; i0++) {
+ d_P_apriori[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)
+ iv5[i1 + 12 * i0];
+ }
+ }
+ }
+
+ for (i = 0; i < 6; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ c_K_k[i + 6 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ c_K_k[i + 6 * i0] += (real32_T)iv6[i + 6 * i1] * P_apriori[i1 + 12
+ * i0];
+ }
+ }
+
+ for (i0 = 0; i0 < 6; i0++) {
+ fv2[i + 6 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv5[i1 + 12 * i0];
+ }
+ }
+ }
+
+ b_K_k[0] = r[0];
+ b_K_k[6] = 0.0F;
+ b_K_k[12] = 0.0F;
+ b_K_k[18] = 0.0F;
+ b_K_k[24] = 0.0F;
+ b_K_k[30] = 0.0F;
+ b_K_k[1] = 0.0F;
+ b_K_k[7] = r[0];
+ b_K_k[13] = 0.0F;
+ b_K_k[19] = 0.0F;
+ b_K_k[25] = 0.0F;
+ b_K_k[31] = 0.0F;
+ b_K_k[2] = 0.0F;
+ b_K_k[8] = 0.0F;
+ b_K_k[14] = r[0];
+ b_K_k[20] = 0.0F;
+ b_K_k[26] = 0.0F;
+ b_K_k[32] = 0.0F;
+ b_K_k[3] = 0.0F;
+ b_K_k[9] = 0.0F;
+ b_K_k[15] = 0.0F;
+ b_K_k[21] = r[1];
+ b_K_k[27] = 0.0F;
+ b_K_k[33] = 0.0F;
+ b_K_k[4] = 0.0F;
+ b_K_k[10] = 0.0F;
+ b_K_k[16] = 0.0F;
+ b_K_k[22] = 0.0F;
+ b_K_k[28] = r[1];
+ b_K_k[34] = 0.0F;
+ b_K_k[5] = 0.0F;
+ b_K_k[11] = 0.0F;
+ b_K_k[17] = 0.0F;
+ b_K_k[23] = 0.0F;
+ b_K_k[29] = 0.0F;
+ b_K_k[35] = r[1];
+ for (i = 0; i < 6; i++) {
+ for (i0 = 0; i0 < 6; i0++) {
+ c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i];
+ }
+ }
+
+ c_mrdivide(d_P_apriori, c_P_apriori, c_K_k);
+
+ /* 'attitudeKalmanfilter:179' x_aposteriori=x_apriori+K_k*y_k; */
+ for (i = 0; i < 6; i++) {
+ f0 = 0.0F;
+ for (i0 = 0; i0 < 12; i0++) {
+ f0 += (real32_T)iv6[i + 6 * i0] * x_apriori[i0];
+ }
+
+ b_z[i] = z[i] - f0;
+ }
+
+ for (i = 0; i < 12; i++) {
+ f0 = 0.0F;
+ for (i0 = 0; i0 < 6; i0++) {
+ f0 += c_K_k[i + 12 * i0] * b_z[i0];
+ }
+
+ x_aposteriori[i] = x_apriori[i] + f0;
+ }
+
+ /* 'attitudeKalmanfilter:180' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */
+ b_eye(dv1);
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ f0 = 0.0F;
+ for (i1 = 0; i1 < 6; i1++) {
+ f0 += c_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0];
+ }
+
+ b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
+ }
+ }
+
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ P_aposteriori[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1
+ + 12 * i0];
+ }
+ }
+ }
+ } else {
+ /* 'attitudeKalmanfilter:181' else */
+ /* 'attitudeKalmanfilter:182' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==1 */
+ if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 1))
+ {
+ /* 'attitudeKalmanfilter:183' R=[r(1),0,0,0,0,0; */
+ /* 'attitudeKalmanfilter:184' 0,r(1),0,0,0,0; */
+ /* 'attitudeKalmanfilter:185' 0,0,r(1),0,0,0; */
+ /* 'attitudeKalmanfilter:186' 0,0,0,r(3),0,0; */
+ /* 'attitudeKalmanfilter:187' 0,0,0,0,r(3),0; */
+ /* 'attitudeKalmanfilter:188' 0,0,0,0,0,r(3)]; */
+ /* observation matrix */
+ /* 'attitudeKalmanfilter:191' H_k=[ E, Z, Z, Z; */
+ /* 'attitudeKalmanfilter:192' Z, Z, Z, E]; */
+ /* 'attitudeKalmanfilter:194' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */
+ /* 'attitudeKalmanfilter:196' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */
+ /* 'attitudeKalmanfilter:197' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 6; i0++) {
+ d_P_apriori[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)
+ iv7[i1 + 12 * i0];
+ }
+ }
+ }
+
+ for (i = 0; i < 6; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ c_K_k[i + 6 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ c_K_k[i + 6 * i0] += (real32_T)iv8[i + 6 * i1] * P_apriori[i1 +
+ 12 * i0];
+ }
+ }
+
+ for (i0 = 0; i0 < 6; i0++) {
+ fv2[i + 6 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv7[i1 + 12 *
+ i0];
+ }
+ }
+ }
+
+ b_K_k[0] = r[0];
+ b_K_k[6] = 0.0F;
+ b_K_k[12] = 0.0F;
+ b_K_k[18] = 0.0F;
+ b_K_k[24] = 0.0F;
+ b_K_k[30] = 0.0F;
+ b_K_k[1] = 0.0F;
+ b_K_k[7] = r[0];
+ b_K_k[13] = 0.0F;
+ b_K_k[19] = 0.0F;
+ b_K_k[25] = 0.0F;
+ b_K_k[31] = 0.0F;
+ b_K_k[2] = 0.0F;
+ b_K_k[8] = 0.0F;
+ b_K_k[14] = r[0];
+ b_K_k[20] = 0.0F;
+ b_K_k[26] = 0.0F;
+ b_K_k[32] = 0.0F;
+ b_K_k[3] = 0.0F;
+ b_K_k[9] = 0.0F;
+ b_K_k[15] = 0.0F;
+ b_K_k[21] = r[2];
+ b_K_k[27] = 0.0F;
+ b_K_k[33] = 0.0F;
+ b_K_k[4] = 0.0F;
+ b_K_k[10] = 0.0F;
+ b_K_k[16] = 0.0F;
+ b_K_k[22] = 0.0F;
+ b_K_k[28] = r[2];
+ b_K_k[34] = 0.0F;
+ b_K_k[5] = 0.0F;
+ b_K_k[11] = 0.0F;
+ b_K_k[17] = 0.0F;
+ b_K_k[23] = 0.0F;
+ b_K_k[29] = 0.0F;
+ b_K_k[35] = r[2];
+ for (i = 0; i < 6; i++) {
+ for (i0 = 0; i0 < 6; i0++) {
+ c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i];
+ }
+ }
+
+ c_mrdivide(d_P_apriori, c_P_apriori, c_K_k);
+
+ /* 'attitudeKalmanfilter:200' x_aposteriori=x_apriori+K_k*y_k; */
+ for (i = 0; i < 3; i++) {
+ b_z[i] = z[i];
+ }
+
+ for (i = 0; i < 3; i++) {
+ b_z[i + 3] = z[i + 6];
+ }
+
+ for (i = 0; i < 6; i++) {
+ fv3[i] = 0.0F;
+ for (i0 = 0; i0 < 12; i0++) {
+ fv3[i] += (real32_T)iv8[i + 6 * i0] * x_apriori[i0];
+ }
+
+ c_z[i] = b_z[i] - fv3[i];
+ }
+
+ for (i = 0; i < 12; i++) {
+ f0 = 0.0F;
+ for (i0 = 0; i0 < 6; i0++) {
+ f0 += c_K_k[i + 12 * i0] * c_z[i0];
+ }
+
+ x_aposteriori[i] = x_apriori[i] + f0;
+ }
+
+ /* 'attitudeKalmanfilter:201' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */
+ b_eye(dv1);
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ f0 = 0.0F;
+ for (i1 = 0; i1 < 6; i1++) {
+ f0 += c_K_k[i + 12 * i1] * (real32_T)iv8[i1 + 6 * i0];
+ }
+
+ b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
+ }
+ }
+
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ P_aposteriori[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] *
+ P_apriori[i1 + 12 * i0];
+ }
+ }
+ }
+ } else {
+ /* 'attitudeKalmanfilter:202' else */
+ /* 'attitudeKalmanfilter:203' x_aposteriori=x_apriori; */
+ for (i = 0; i < 12; i++) {
+ x_aposteriori[i] = x_apriori[i];
+ }
+
+ /* 'attitudeKalmanfilter:204' P_aposteriori=P_apriori; */
+ memcpy(&P_aposteriori[0], &P_apriori[0], 144U * sizeof(real32_T));
+ }
+ }
+ }
+ }
+
+ /* % euler anglels extraction */
+ /* 'attitudeKalmanfilter:213' z_n_b = -x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */
+ for (i = 0; i < 3; i++) {
+ x_n_b[i] = -x_aposteriori[i + 6];
+ }
+
+ rdivide(x_n_b, norm(*(real32_T (*)[3])&x_aposteriori[6]), z_n_b);
+
+ /* 'attitudeKalmanfilter:214' m_n_b = x_aposteriori(10:12)./norm(x_aposteriori(10:12)); */
+ rdivide(*(real32_T (*)[3])&x_aposteriori[9], norm(*(real32_T (*)[3])&
+ x_aposteriori[9]), wak);
+
+ /* 'attitudeKalmanfilter:216' y_n_b=cross(z_n_b,m_n_b); */
+ for (i = 0; i < 3; i++) {
+ x_n_b[i] = wak[i];
+ }
+
+ cross(z_n_b, x_n_b, wak);
+
+ /* 'attitudeKalmanfilter:217' y_n_b=y_n_b./norm(y_n_b); */
+ for (i = 0; i < 3; i++) {
+ x_n_b[i] = wak[i];
+ }
+
+ rdivide(x_n_b, norm(wak), wak);
+
+ /* 'attitudeKalmanfilter:219' x_n_b=(cross(y_n_b,z_n_b)); */
+ cross(wak, z_n_b, x_n_b);
+
+ /* 'attitudeKalmanfilter:220' x_n_b=x_n_b./norm(x_n_b); */
+ for (i = 0; i < 3; i++) {
+ b_x_aposteriori_k[i] = x_n_b[i];
+ }
+
+ rdivide(b_x_aposteriori_k, norm(x_n_b), x_n_b);
+
+ /* 'attitudeKalmanfilter:226' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */
+ for (i = 0; i < 3; i++) {
+ Rot_matrix[i] = x_n_b[i];
+ Rot_matrix[3 + i] = wak[i];
+ Rot_matrix[6 + i] = z_n_b[i];
+ }
+
+ /* 'attitudeKalmanfilter:230' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */
+ /* 'attitudeKalmanfilter:231' theta=-asin(Rot_matrix(1,3)); */
+ /* 'attitudeKalmanfilter:232' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */
+ /* 'attitudeKalmanfilter:233' eulerAngles=[phi;theta;psi]; */
+ eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]);
+ eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]);
+ eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]);
+}
+
+/* End of code generation (attitudeKalmanfilter.c) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h new file mode 100755 index 000000000..afa63c1a9 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h @@ -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) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c new file mode 100755 index 000000000..7d620d7fa --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c @@ -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) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h new file mode 100755 index 000000000..8b599be66 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h @@ -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) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c new file mode 100755 index 000000000..7f9727419 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c @@ -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) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h new file mode 100755 index 000000000..da84a5024 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h @@ -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) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h new file mode 100755 index 000000000..30fd1e75e --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.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) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/cross.c b/src/modules/attitude_estimator_ekf/codegen/cross.c new file mode 100755 index 000000000..84ada9002 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/cross.c @@ -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) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/cross.h b/src/modules/attitude_estimator_ekf/codegen/cross.h new file mode 100755 index 000000000..e727f0684 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/cross.h @@ -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) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/eye.c b/src/modules/attitude_estimator_ekf/codegen/eye.c new file mode 100755 index 000000000..b89ab58ef --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/eye.c @@ -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) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/eye.h b/src/modules/attitude_estimator_ekf/codegen/eye.h new file mode 100755 index 000000000..94fbe7671 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/eye.h @@ -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) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/mrdivide.c b/src/modules/attitude_estimator_ekf/codegen/mrdivide.c new file mode 100755 index 000000000..a810f22e4 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/mrdivide.c @@ -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) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/mrdivide.h b/src/modules/attitude_estimator_ekf/codegen/mrdivide.h new file mode 100755 index 000000000..2d3b0d51f --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/mrdivide.h @@ -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) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/norm.c b/src/modules/attitude_estimator_ekf/codegen/norm.c new file mode 100755 index 000000000..0c418cc7b --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/norm.c @@ -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) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/norm.h b/src/modules/attitude_estimator_ekf/codegen/norm.h new file mode 100755 index 000000000..60cf77b57 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/norm.h @@ -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) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/rdivide.c b/src/modules/attitude_estimator_ekf/codegen/rdivide.c new file mode 100755 index 000000000..d035dae5e --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rdivide.c @@ -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) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/rdivide.h b/src/modules/attitude_estimator_ekf/codegen/rdivide.h new file mode 100755 index 000000000..4bbebebe2 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rdivide.h @@ -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) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c new file mode 100755 index 000000000..34164d104 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c @@ -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) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h new file mode 100755 index 000000000..145373cd0 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h @@ -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) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c new file mode 100755 index 000000000..d84ca9573 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c @@ -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) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h new file mode 100755 index 000000000..65fdaa96f --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h @@ -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) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/rt_defines.h b/src/modules/attitude_estimator_ekf/codegen/rt_defines.h new file mode 100755 index 000000000..356498363 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rt_defines.h @@ -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) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c new file mode 100755 index 000000000..303d1d9d2 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c @@ -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) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h new file mode 100755 index 000000000..bd56b30d9 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h @@ -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) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h b/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h new file mode 100755 index 000000000..9a5c96267 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h @@ -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) */
diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk new file mode 100644 index 000000000..5ce545112 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -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 + diff --git a/src/modules/commander/calibration_routines.c b/src/modules/commander/calibration_routines.c new file mode 100644 index 000000000..a26938637 --- /dev/null +++ b/src/modules/commander/calibration_routines.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; +} diff --git a/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h new file mode 100644 index 000000000..e3e7fbafd --- /dev/null +++ b/src/modules/commander/calibration_routines.h @@ -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);
\ No newline at end of file diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c new file mode 100644 index 000000000..7c0a25f86 --- /dev/null +++ b/src/modules/commander/commander.c @@ -0,0 +1,2181 @@ +/**************************************************************************** + * + * 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.c + * Main system state machine implementation. + * + * @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> + * + */ + +#include "commander.h" + +#include <nuttx/config.h> +#include <pthread.h> +#include <stdio.h> +#include <stdlib.h> +#include <stdbool.h> +#include <string.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> +#include <sys/prctl.h> +#include <string.h> +#include <drivers/drv_led.h> +#include <drivers/drv_hrt.h> +#include <drivers/drv_tone_alarm.h> +#include "state_machine_helper.h" +#include "systemlib/systemlib.h" +#include <math.h> +#include <poll.h> +#include <uORB/uORB.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/battery_status.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/offboard_control_setpoint.h> +#include <uORB/topics/home_position.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_local_position.h> +#include <uORB/topics/vehicle_gps_position.h> +#include <uORB/topics/vehicle_command.h> +#include <uORB/topics/subsystem_info.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/parameter_update.h> +#include <uORB/topics/differential_pressure.h> +#include <mavlink/mavlink_log.h> + +#include <systemlib/param/param.h> +#include <systemlib/systemlib.h> +#include <systemlib/err.h> + +/* XXX MOVE CALIBRATION TO SENSORS APP THREAD */ +#include <drivers/drv_accel.h> +#include <drivers/drv_gyro.h> +#include <drivers/drv_mag.h> +#include <drivers/drv_baro.h> + +#include "calibration_routines.h" + + +PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */ +//PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0); /**< Go into high-level failsafe after 0 ms */ +PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); +PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); +PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); + +#include <systemlib/cpuload.h> +extern struct system_load_s system_load; + +/* Decouple update interval and hysteris counters, all depends on intervals */ +#define COMMANDER_MONITORING_INTERVAL 50000 +#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f)) +#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) +#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) + +#define STICK_ON_OFF_LIMIT 0.75f +#define STICK_THRUST_RANGE 1.0f +#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 +#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) + +#define GPS_FIX_TYPE_2D 2 +#define GPS_FIX_TYPE_3D 3 +#define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000 +#define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) + +/* File descriptors */ +static int leds; +static int buzzer; +static int mavlink_fd; +static bool commander_initialized = false; +static struct vehicle_status_s current_status; /**< Main state machine */ +static orb_advert_t stat_pub; + +// static uint16_t nofix_counter = 0; +// static uint16_t gotfix_counter = 0; + +static unsigned int failsafe_lowlevel_timeout_ms; + +static bool thread_should_exit = false; /**< daemon exit flag */ +static bool thread_running = false; /**< daemon status flag */ +static int daemon_task; /**< Handle of daemon task / thread */ + +/* pthread loops */ +static void *orb_receive_loop(void *arg); + +__EXPORT int commander_main(int argc, char *argv[]); + +/** + * Mainloop of commander. + */ +int commander_thread_main(int argc, char *argv[]); + +static int buzzer_init(void); +static void buzzer_deinit(void); +static int led_init(void); +static void led_deinit(void); +static int led_toggle(int led); +static int led_on(int led); +static int led_off(int led); +static void do_gyro_calibration(int status_pub, struct vehicle_status_s *status); +static void do_mag_calibration(int status_pub, struct vehicle_status_s *status); +static void do_rc_calibration(int status_pub, struct vehicle_status_s *status); +static void do_accel_calibration(int status_pub, struct vehicle_status_s *status); +static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd); + +int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); + + + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +/** + * Sort calibration values. + * + * Sorts the calibration values with bubble sort. + * + * @param a The array to sort + * @param n The number of entries in the array + */ +// static void cal_bsort(float a[], int n); + +static int buzzer_init() +{ + buzzer = open("/dev/tone_alarm", O_WRONLY); + + if (buzzer < 0) { + warnx("Buzzer: open fail\n"); + return ERROR; + } + + return 0; +} + +static void buzzer_deinit() +{ + close(buzzer); +} + + +static int led_init() +{ + leds = open(LED_DEVICE_PATH, 0); + + if (leds < 0) { + warnx("LED: open fail\n"); + return ERROR; + } + + if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) { + warnx("LED: ioctl fail\n"); + return ERROR; + } + + return 0; +} + +static void led_deinit() +{ + close(leds); +} + +static int led_toggle(int led) +{ + static int last_blue = LED_ON; + static int last_amber = LED_ON; + + if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON; + + if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON; + + return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led); +} + +static int led_on(int led) +{ + return ioctl(leds, LED_ON, led); +} + +static int led_off(int led) +{ + return ioctl(leds, LED_OFF, led); +} + +enum AUDIO_PATTERN { + AUDIO_PATTERN_ERROR = 2, + AUDIO_PATTERN_NOTIFY_POSITIVE = 3, + AUDIO_PATTERN_NOTIFY_NEUTRAL = 4, + AUDIO_PATTERN_NOTIFY_NEGATIVE = 5, + AUDIO_PATTERN_NOTIFY_CHARGE = 6 +}; + +int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state) +{ + + /* Trigger alarm if going into any error state */ + if (((new_state == SYSTEM_STATE_GROUND_ERROR) && (old_state != SYSTEM_STATE_GROUND_ERROR)) || + ((new_state == SYSTEM_STATE_MISSION_ABORT) && (old_state != SYSTEM_STATE_MISSION_ABORT))) { + ioctl(buzzer, TONE_SET_ALARM, 0); + ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_ERROR); + } + + /* Trigger neutral on arming / disarming */ + if (((new_state == SYSTEM_STATE_GROUND_READY) && (old_state != SYSTEM_STATE_GROUND_READY))) { + ioctl(buzzer, TONE_SET_ALARM, 0); + ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_NOTIFY_NEUTRAL); + } + + /* Trigger Tetris on being bored */ + + return 0; +} + +void tune_confirm(void) +{ + ioctl(buzzer, TONE_SET_ALARM, 3); +} + +void tune_error(void) +{ + ioctl(buzzer, TONE_SET_ALARM, 4); +} + +void do_rc_calibration(int status_pub, struct vehicle_status_s *status) +{ + if (current_status.offboard_control_signal_lost) { + mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); + return; + } + + int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); + struct manual_control_setpoint_s sp; + orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); + + /* set parameters */ + + float p = sp.roll; + param_set(param_find("TRIM_ROLL"), &p); + p = sp.pitch; + param_set(param_find("TRIM_PITCH"), &p); + p = sp.yaw; + param_set(param_find("TRIM_YAW"), &p); + + /* store to permanent storage */ + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); + } + + mavlink_log_info(mavlink_fd, "trim calibration done"); +} + +void do_mag_calibration(int status_pub, struct vehicle_status_s *status) +{ + + /* set to mag calibration mode */ + status->flag_preflight_mag_calibration = true; + state_machine_publish(status_pub, status, mavlink_fd); + + int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); + struct mag_report mag; + + /* 45 seconds */ + uint64_t calibration_interval = 45 * 1000 * 1000; + + /* maximum 2000 values */ + const unsigned int calibration_maxcount = 500; + unsigned int calibration_counter = 0; + + /* limit update rate to get equally spaced measurements over time (in ms) */ + orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); + + // XXX old cal + // * FLT_MIN is not the most negative float number, + // * but the smallest number by magnitude float can + // * represent. Use -FLT_MAX to initialize the most + // * negative number + + // float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; + // float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX}; + + int fd = open(MAG_DEVICE_PATH, O_RDONLY); + + /* erase old calibration */ + struct mag_scale mscale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { + warn("WARNING: failed to set scale / offsets for mag"); + mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag"); + } + + /* calibrate range */ + if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) { + warnx("failed to calibrate scale"); + } + + close(fd); + + /* calibrate offsets */ + + // uint64_t calibration_start = hrt_absolute_time(); + + uint64_t axis_deadline = hrt_absolute_time(); + uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; + + const char axislabels[3] = { 'X', 'Y', 'Z'}; + int axis_index = -1; + + float *x = (float *)malloc(sizeof(float) * calibration_maxcount); + float *y = (float *)malloc(sizeof(float) * calibration_maxcount); + float *z = (float *)malloc(sizeof(float) * calibration_maxcount); + + if (x == NULL || y == NULL || z == NULL) { + warnx("mag cal failed: out of memory"); + mavlink_log_info(mavlink_fd, "mag cal failed: out of memory"); + warnx("x:%p y:%p z:%p\n", x, y, z); + return; + } + + tune_confirm(); + sleep(2); + tune_confirm(); + + while (hrt_absolute_time() < calibration_deadline && + calibration_counter < calibration_maxcount) { + + /* wait blocking for new data */ + struct pollfd fds[1] = { { .fd = sub_mag, .events = POLLIN } }; + + /* user guidance */ + if (hrt_absolute_time() >= axis_deadline && + axis_index < 3) { + + axis_index++; + + char buf[50]; + sprintf(buf, "Please rotate around %c", axislabels[axis_index]); + mavlink_log_info(mavlink_fd, buf); + tune_confirm(); + + axis_deadline += calibration_interval / 3; + } + + if (!(axis_index < 3)) { + break; + } + + // int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time(); + + // if ((axis_left / 1000) == 0 && axis_left > 0) { + // char buf[50]; + // sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]); + // mavlink_log_info(mavlink_fd, buf); + // } + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); + + x[calibration_counter] = mag.x; + y[calibration_counter] = mag.y; + z[calibration_counter] = mag.z; + + /* get min/max values */ + + // if (mag.x < mag_min[0]) { + // mag_min[0] = mag.x; + // } + // else if (mag.x > mag_max[0]) { + // mag_max[0] = mag.x; + // } + + // if (raw.magnetometer_ga[1] < mag_min[1]) { + // mag_min[1] = raw.magnetometer_ga[1]; + // } + // else if (raw.magnetometer_ga[1] > mag_max[1]) { + // mag_max[1] = raw.magnetometer_ga[1]; + // } + + // if (raw.magnetometer_ga[2] < mag_min[2]) { + // mag_min[2] = raw.magnetometer_ga[2]; + // } + // else if (raw.magnetometer_ga[2] > mag_max[2]) { + // mag_max[2] = raw.magnetometer_ga[2]; + // } + + calibration_counter++; + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)"); + break; + } + } + + float sphere_x; + float sphere_y; + float sphere_z; + float sphere_radius; + + sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); + + free(x); + free(y); + free(z); + + if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) { + + fd = open(MAG_DEVICE_PATH, 0); + + struct mag_scale mscale; + + if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale)) + warn("WARNING: failed to get scale / offsets for mag"); + + mscale.x_offset = sphere_x; + mscale.y_offset = sphere_y; + mscale.z_offset = sphere_z; + + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) + warn("WARNING: failed to set scale / offsets for mag"); + + close(fd); + + /* announce and set new offset */ + + if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { + warnx("Setting X mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) { + warnx("Setting Y mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) { + warnx("Setting Z mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) { + warnx("Setting X mag scale failed!\n"); + } + + if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) { + warnx("Setting Y mag scale failed!\n"); + } + + if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) { + warnx("Setting Z mag scale failed!\n"); + } + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + mavlink_log_info(mavlink_fd, "FAILED storing calibration"); + } + + warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n", + (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale, + (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius); + + char buf[52]; + sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset, + (double)mscale.y_offset, (double)mscale.z_offset); + mavlink_log_info(mavlink_fd, buf); + + sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale, + (double)mscale.y_scale, (double)mscale.z_scale); + mavlink_log_info(mavlink_fd, buf); + + mavlink_log_info(mavlink_fd, "mag calibration done"); + + tune_confirm(); + sleep(2); + tune_confirm(); + sleep(2); + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); + } + + /* disable calibration mode */ + status->flag_preflight_mag_calibration = false; + state_machine_publish(status_pub, status, mavlink_fd); + + close(sub_mag); +} + +void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) +{ + /* set to gyro calibration mode */ + status->flag_preflight_gyro_calibration = true; + state_machine_publish(status_pub, status, mavlink_fd); + + const int calibration_count = 5000; + + int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); + struct sensor_combined_s raw; + + int calibration_counter = 0; + float gyro_offset[3] = {0.0f, 0.0f, 0.0f}; + + /* set offsets to zero */ + int fd = open(GYRO_DEVICE_PATH, 0); + struct gyro_scale gscale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null)) + warn("WARNING: failed to set scale / offsets for gyro"); + + close(fd); + + while (calibration_counter < calibration_count) { + + /* wait blocking for new data */ + struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); + gyro_offset[0] += raw.gyro_rad_s[0]; + gyro_offset[1] += raw.gyro_rad_s[1]; + gyro_offset[2] += raw.gyro_rad_s[2]; + calibration_counter++; + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); + return; + } + } + + gyro_offset[0] = gyro_offset[0] / calibration_count; + gyro_offset[1] = gyro_offset[1] / calibration_count; + gyro_offset[2] = gyro_offset[2] / calibration_count; + + /* exit gyro calibration mode */ + status->flag_preflight_gyro_calibration = false; + state_machine_publish(status_pub, status, mavlink_fd); + + if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { + + if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) + || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1])) + || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) { + mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!"); + } + + /* set offsets to actual value */ + fd = open(GYRO_DEVICE_PATH, 0); + struct gyro_scale gscale = { + gyro_offset[0], + 1.0f, + gyro_offset[1], + 1.0f, + gyro_offset[2], + 1.0f, + }; + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) + warn("WARNING: failed to set scale / offsets for gyro"); + + close(fd); + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + } + + // char buf[50]; + // sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); + // mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "gyro calibration done"); + + tune_confirm(); + sleep(2); + tune_confirm(); + sleep(2); + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)"); + } + + close(sub_sensor_combined); +} + +void do_accel_calibration(int status_pub, struct vehicle_status_s *status) +{ + /* announce change */ + + mavlink_log_info(mavlink_fd, "keep it level and still"); + /* set to accel calibration mode */ + status->flag_preflight_accel_calibration = true; + state_machine_publish(status_pub, status, mavlink_fd); + + const int calibration_count = 2500; + + int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); + struct sensor_combined_s raw; + + int calibration_counter = 0; + float accel_offset[3] = {0.0f, 0.0f, 0.0f}; + + int fd = open(ACCEL_DEVICE_PATH, 0); + struct accel_scale ascale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null)) + warn("WARNING: failed to set scale / offsets for accel"); + + close(fd); + + while (calibration_counter < calibration_count) { + + /* wait blocking for new data */ + struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); + accel_offset[0] += raw.accelerometer_m_s2[0]; + accel_offset[1] += raw.accelerometer_m_s2[1]; + accel_offset[2] += raw.accelerometer_m_s2[2]; + calibration_counter++; + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "acceleration calibration aborted"); + return; + } + } + + accel_offset[0] = accel_offset[0] / calibration_count; + accel_offset[1] = accel_offset[1] / calibration_count; + accel_offset[2] = accel_offset[2] / calibration_count; + + if (isfinite(accel_offset[0]) && isfinite(accel_offset[1]) && isfinite(accel_offset[2])) { + + /* add the removed length from x / y to z, since we induce a scaling issue else */ + float total_len = sqrtf(accel_offset[0] * accel_offset[0] + accel_offset[1] * accel_offset[1] + accel_offset[2] * accel_offset[2]); + + /* if length is correct, zero results here */ + accel_offset[2] = accel_offset[2] + total_len; + + float scale = 9.80665f / total_len; + + if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offset[0])) + || param_set(param_find("SENS_ACC_YOFF"), &(accel_offset[1])) + || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offset[2])) + || param_set(param_find("SENS_ACC_XSCALE"), &(scale)) + || param_set(param_find("SENS_ACC_YSCALE"), &(scale)) + || param_set(param_find("SENS_ACC_ZSCALE"), &(scale))) { + mavlink_log_critical(mavlink_fd, "Setting offs or scale failed!"); + } + + fd = open(ACCEL_DEVICE_PATH, 0); + struct accel_scale ascale = { + accel_offset[0], + scale, + accel_offset[1], + scale, + accel_offset[2], + scale, + }; + + if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) + warn("WARNING: failed to set scale / offsets for accel"); + + close(fd); + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + } + + //char buf[50]; + //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); + //mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "accel calibration done"); + + tune_confirm(); + sleep(2); + tune_confirm(); + sleep(2); + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "accel calibration FAILED (NaN)"); + } + + /* exit accel calibration mode */ + status->flag_preflight_accel_calibration = false; + state_machine_publish(status_pub, status, mavlink_fd); + + close(sub_sensor_combined); +} + +void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status) +{ + /* announce change */ + + mavlink_log_info(mavlink_fd, "keep it still"); + /* set to accel calibration mode */ + status->flag_preflight_airspeed_calibration = true; + state_machine_publish(status_pub, status, mavlink_fd); + + const int calibration_count = 2500; + + int sub_differential_pressure = orb_subscribe(ORB_ID(differential_pressure)); + struct differential_pressure_s differential_pressure; + + int calibration_counter = 0; + float airspeed_offset = 0.0f; + + while (calibration_counter < calibration_count) { + + /* wait blocking for new data */ + struct pollfd fds[1] = { { .fd = sub_differential_pressure, .events = POLLIN } }; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(differential_pressure), sub_differential_pressure, &differential_pressure); + airspeed_offset += differential_pressure.voltage; + calibration_counter++; + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); + return; + } + } + + airspeed_offset = airspeed_offset / calibration_count; + + if (isfinite(airspeed_offset)) { + + if (param_set(param_find("SENS_VAIR_OFF"), &(airspeed_offset))) { + mavlink_log_critical(mavlink_fd, "Setting offs failed!"); + } + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + } + + //char buf[50]; + //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); + //mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "airspeed calibration done"); + + tune_confirm(); + sleep(2); + tune_confirm(); + sleep(2); + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); + } + + /* exit airspeed calibration mode */ + status->flag_preflight_airspeed_calibration = false; + state_machine_publish(status_pub, status, mavlink_fd); + + close(sub_differential_pressure); +} + + + +void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd) +{ + /* result of the command */ + uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; + + /* announce command handling */ + tune_confirm(); + + + /* supported command handling start */ + + /* request to set different system mode */ + switch (cmd->command) { + case VEHICLE_CMD_DO_SET_MODE: { + if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, (uint8_t)cmd->param1)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } + break; + + case VEHICLE_CMD_COMPONENT_ARM_DISARM: { + /* request to arm */ + if ((int)cmd->param1 == 1) { + if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + + /* request to disarm */ + + } else if ((int)cmd->param1 == 0) { + if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } + } + break; + + /* request for an autopilot reboot */ + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { + if ((int)cmd->param1 == 1) { + if (OK == do_state_update(status_pub, current_vehicle_status, mavlink_fd, SYSTEM_STATE_REBOOT)) { + /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + /* system may return here */ + result = VEHICLE_CMD_RESULT_DENIED; + } + } + } + break; + +// /* request to land */ +// case VEHICLE_CMD_NAV_LAND: +// { +// //TODO: add check if landing possible +// //TODO: add landing maneuver +// +// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) { +// result = VEHICLE_CMD_RESULT_ACCEPTED; +// } } +// break; +// +// /* request to takeoff */ +// case VEHICLE_CMD_NAV_TAKEOFF: +// { +// //TODO: add check if takeoff possible +// //TODO: add takeoff maneuver +// +// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) { +// result = VEHICLE_CMD_RESULT_ACCEPTED; +// } +// } +// break; +// + /* preflight calibration */ + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { + bool handled = false; + + /* gyro calibration */ + if ((int)(cmd->param1) == 1) { + /* transition to calibration state */ + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + + if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + mavlink_log_info(mavlink_fd, "starting gyro cal"); + tune_confirm(); + do_gyro_calibration(status_pub, ¤t_status); + mavlink_log_info(mavlink_fd, "finished gyro cal"); + tune_confirm(); + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_critical(mavlink_fd, "REJECTING gyro cal"); + result = VEHICLE_CMD_RESULT_DENIED; + } + + handled = true; + } + + /* magnetometer calibration */ + if ((int)(cmd->param2) == 1) { + /* transition to calibration state */ + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + + if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + mavlink_log_info(mavlink_fd, "starting mag cal"); + tune_confirm(); + do_mag_calibration(status_pub, ¤t_status); + mavlink_log_info(mavlink_fd, "finished mag cal"); + tune_confirm(); + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_critical(mavlink_fd, "REJECTING mag cal"); + result = VEHICLE_CMD_RESULT_DENIED; + } + + handled = true; + } + + /* zero-altitude pressure calibration */ + if ((int)(cmd->param3) == 1) { + /* transition to calibration state */ + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + + if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented"); + tune_confirm(); + + } else { + mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration"); + result = VEHICLE_CMD_RESULT_DENIED; + } + + handled = true; + } + + /* trim calibration */ + if ((int)(cmd->param4) == 1) { + /* transition to calibration state */ + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + + if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + mavlink_log_info(mavlink_fd, "starting trim cal"); + tune_confirm(); + do_rc_calibration(status_pub, ¤t_status); + mavlink_log_info(mavlink_fd, "finished trim cal"); + tune_confirm(); + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_critical(mavlink_fd, "REJECTING trim cal"); + result = VEHICLE_CMD_RESULT_DENIED; + } + + handled = true; + } + + /* accel calibration */ + if ((int)(cmd->param5) == 1) { + /* transition to calibration state */ + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + + if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + mavlink_log_info(mavlink_fd, "CMD starting accel cal"); + tune_confirm(); + do_accel_calibration(status_pub, ¤t_status); + tune_confirm(); + mavlink_log_info(mavlink_fd, "CMD finished accel cal"); + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_critical(mavlink_fd, "REJECTING accel cal"); + result = VEHICLE_CMD_RESULT_DENIED; + } + + handled = true; + } + + /* airspeed calibration */ + if ((int)(cmd->param6) == 1) { //xxx: this is not defined by the mavlink protocol + /* transition to calibration state */ + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + + if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + mavlink_log_info(mavlink_fd, "CMD starting airspeed cal"); + tune_confirm(); + do_airspeed_calibration(status_pub, ¤t_status); + tune_confirm(); + mavlink_log_info(mavlink_fd, "CMD finished airspeed cal"); + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_critical(mavlink_fd, "REJECTING airspeed cal"); + result = VEHICLE_CMD_RESULT_DENIED; + } + + handled = true; + } + + /* none found */ + if (!handled) { + //warnx("refusing unsupported calibration request\n"); + mavlink_log_critical(mavlink_fd, "CMD refusing unsup. calib. request"); + result = VEHICLE_CMD_RESULT_UNSUPPORTED; + } + } + break; + + case VEHICLE_CMD_PREFLIGHT_STORAGE: { + if (current_status.flag_system_armed && + ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) { + /* do not perform expensive memory tasks on multirotors in flight */ + // XXX this is over-safe, as soon as cmd is in low prio thread this can be allowed + mavlink_log_info(mavlink_fd, "REJECTING save cmd while multicopter armed"); + + } else { + + // XXX move this to LOW PRIO THREAD of commander app + /* Read all parameters from EEPROM to RAM */ + + if (((int)(cmd->param1)) == 0) { + + /* read all parameters from EEPROM to RAM */ + int read_ret = param_load_default(); + + if (read_ret == OK) { + //warnx("[mavlink pm] Loaded EEPROM params in RAM\n"); + mavlink_log_info(mavlink_fd, "OK loading params from"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else if (read_ret == 1) { + mavlink_log_info(mavlink_fd, "OK no changes in"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + if (read_ret < -1) { + mavlink_log_info(mavlink_fd, "ERR loading params from"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + + } else { + mavlink_log_info(mavlink_fd, "ERR no param file named"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + } + + result = VEHICLE_CMD_RESULT_FAILED; + } + + } else if (((int)(cmd->param1)) == 1) { + + /* write all parameters from RAM to EEPROM */ + int write_ret = param_save_default(); + + if (write_ret == OK) { + mavlink_log_info(mavlink_fd, "OK saved param file"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + if (write_ret < -1) { + mavlink_log_info(mavlink_fd, "ERR params file does not exit:"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + + } else { + mavlink_log_info(mavlink_fd, "ERR writing params to"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + } + + result = VEHICLE_CMD_RESULT_FAILED; + } + + } else { + mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request"); + result = VEHICLE_CMD_RESULT_UNSUPPORTED; + } + } + } + break; + + default: { + mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command"); + result = VEHICLE_CMD_RESULT_UNSUPPORTED; + /* announce command rejection */ + ioctl(buzzer, TONE_SET_ALARM, 4); + } + break; + } + + /* supported command handling stop */ + if (result == VEHICLE_CMD_RESULT_FAILED || + result == VEHICLE_CMD_RESULT_DENIED || + result == VEHICLE_CMD_RESULT_UNSUPPORTED) { + ioctl(buzzer, TONE_SET_ALARM, 5); + + } else if (result == VEHICLE_CMD_RESULT_ACCEPTED) { + tune_confirm(); + } + + /* send any requested ACKs */ + if (cmd->confirmation > 0) { + /* send acknowledge command */ + // XXX TODO + } + +} + +static void *orb_receive_loop(void *arg) //handles status information coming from subsystems (present, enabled, health), these values do not indicate the quality (variance) of the signal +{ + /* Set thread name */ + prctl(PR_SET_NAME, "commander orb rcv", getpid()); + + /* Subscribe to command topic */ + int subsys_sub = orb_subscribe(ORB_ID(subsystem_info)); + struct subsystem_info_s info; + + struct vehicle_status_s *vstatus = (struct vehicle_status_s *)arg; + + while (!thread_should_exit) { + struct pollfd fds[1] = { { .fd = subsys_sub, .events = POLLIN } }; + + if (poll(fds, 1, 5000) == 0) { + /* timeout, but this is no problem, silently ignore */ + } else { + /* got command */ + orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); + + warnx("Subsys changed: %d\n", (int)info.subsystem_type); + + /* mark / unmark as present */ + if (info.present) { + vstatus->onboard_control_sensors_present |= info.subsystem_type; + + } else { + vstatus->onboard_control_sensors_present &= ~info.subsystem_type; + } + + /* mark / unmark as enabled */ + if (info.enabled) { + vstatus->onboard_control_sensors_enabled |= info.subsystem_type; + + } else { + vstatus->onboard_control_sensors_enabled &= ~info.subsystem_type; + } + + /* mark / unmark as ok */ + if (info.ok) { + vstatus->onboard_control_sensors_health |= info.subsystem_type; + + } else { + vstatus->onboard_control_sensors_health &= ~info.subsystem_type; + } + } + } + + close(subsys_sub); + + return NULL; +} + +/* + * Provides a coarse estimate of remaining battery power. + * + * The estimate is very basic and based on decharging voltage curves. + * + * @return the estimated remaining capacity in 0..1 + */ +float battery_remaining_estimate_voltage(float voltage); + +PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f); +PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f); +PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3); + +float battery_remaining_estimate_voltage(float voltage) +{ + float ret = 0; + static param_t bat_volt_empty; + static param_t bat_volt_full; + static param_t bat_n_cells; + static bool initialized = false; + static unsigned int counter = 0; + static float ncells = 3; + // XXX change cells to int (and param to INT32) + + if (!initialized) { + bat_volt_empty = param_find("BAT_V_EMPTY"); + bat_volt_full = param_find("BAT_V_FULL"); + bat_n_cells = param_find("BAT_N_CELLS"); + initialized = true; + } + + static float chemistry_voltage_empty = 3.2f; + static float chemistry_voltage_full = 4.05f; + + if (counter % 100 == 0) { + param_get(bat_volt_empty, &chemistry_voltage_empty); + param_get(bat_volt_full, &chemistry_voltage_full); + param_get(bat_n_cells, &ncells); + } + + counter++; + + ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty)); + + /* limit to sane values */ + ret = (ret < 0) ? 0 : ret; + ret = (ret > 1) ? 1 : ret; + return ret; +} + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + + fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n"); + exit(1); +} + +/** + * The daemon 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 commander_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("commander already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + daemon_task = task_spawn("commander", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 40, + 3000, + commander_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) { + warnx("\tcommander is running\n"); + + } else { + warnx("\tcommander not started\n"); + } + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +int commander_thread_main(int argc, char *argv[]) +{ + /* not yet initialized */ + commander_initialized = false; + bool home_position_set = false; + + /* set parameters */ + failsafe_lowlevel_timeout_ms = 0; + param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms); + + param_t _param_sys_type = param_find("MAV_TYPE"); + param_t _param_system_id = param_find("MAV_SYS_ID"); + param_t _param_component_id = param_find("MAV_COMP_ID"); + + /* welcome user */ + warnx("I am in command now!\n"); + + /* pthreads for command and subsystem info handling */ + // pthread_t command_handling_thread; + pthread_t subsystem_info_thread; + + /* initialize */ + if (led_init() != 0) { + warnx("ERROR: Failed to initialize leds\n"); + } + + if (buzzer_init() != 0) { + warnx("ERROR: Failed to initialize buzzer\n"); + } + + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + if (mavlink_fd < 0) { + warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.\n"); + } + + /* make sure we are in preflight state */ + memset(¤t_status, 0, sizeof(current_status)); + current_status.state_machine = SYSTEM_STATE_PREFLIGHT; + current_status.flag_system_armed = false; + /* neither manual nor offboard control commands have been received */ + current_status.offboard_control_signal_found_once = false; + current_status.rc_signal_found_once = false; + /* mark all signals lost as long as they haven't been found */ + current_status.rc_signal_lost = true; + current_status.offboard_control_signal_lost = true; + /* allow manual override initially */ + current_status.flag_external_manual_override_ok = true; + /* flag position info as bad, do not allow auto mode */ + current_status.flag_vector_flight_mode_ok = false; + /* set battery warning flag */ + current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; + + /* advertise to ORB */ + stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); + /* publish current state machine */ + state_machine_publish(stat_pub, ¤t_status, mavlink_fd); + + /* home position */ + orb_advert_t home_pub = -1; + struct home_position_s home; + memset(&home, 0, sizeof(home)); + + if (stat_pub < 0) { + warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); + warnx("exiting."); + exit(ERROR); + } + + mavlink_log_info(mavlink_fd, "system is running"); + + /* create pthreads */ + pthread_attr_t subsystem_info_attr; + pthread_attr_init(&subsystem_info_attr); + pthread_attr_setstacksize(&subsystem_info_attr, 2048); + pthread_create(&subsystem_info_thread, &subsystem_info_attr, orb_receive_loop, ¤t_status); + + /* Start monitoring loop */ + uint16_t counter = 0; + uint8_t flight_env; + + /* Initialize to 0.0V */ + float battery_voltage = 0.0f; + bool battery_voltage_valid = false; + bool low_battery_voltage_actions_done = false; + bool critical_battery_voltage_actions_done = false; + uint8_t low_voltage_counter = 0; + uint16_t critical_voltage_counter = 0; + int16_t mode_switch_rc_value; + float bat_remain = 1.0f; + + uint16_t stick_off_counter = 0; + uint16_t stick_on_counter = 0; + + /* Subscribe to manual control data */ + int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + struct manual_control_setpoint_s sp_man; + memset(&sp_man, 0, sizeof(sp_man)); + + /* Subscribe to offboard control data */ + int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); + struct offboard_control_setpoint_s sp_offboard; + memset(&sp_offboard, 0, sizeof(sp_offboard)); + + int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + struct vehicle_global_position_s global_position; + memset(&global_position, 0, sizeof(global_position)); + uint64_t last_global_position_time = 0; + + int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + struct vehicle_local_position_s local_position; + memset(&local_position, 0, sizeof(local_position)); + uint64_t last_local_position_time = 0; + + /* + * The home position is set based on GPS only, to prevent a dependency between + * position estimator and commander. RAW GPS is more than good enough for a + * non-flying vehicle. + */ + int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + struct vehicle_gps_position_s gps_position; + memset(&gps_position, 0, sizeof(gps_position)); + + int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + struct sensor_combined_s sensors; + memset(&sensors, 0, sizeof(sensors)); + + int differential_pressure_sub = orb_subscribe(ORB_ID(differential_pressure)); + struct differential_pressure_s differential_pressure; + memset(&differential_pressure, 0, sizeof(differential_pressure)); + uint64_t last_differential_pressure_time = 0; + + /* Subscribe to command topic */ + int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); + struct vehicle_command_s cmd; + memset(&cmd, 0, sizeof(cmd)); + + /* Subscribe to parameters changed topic */ + int param_changed_sub = orb_subscribe(ORB_ID(parameter_update)); + struct parameter_update_s param_changed; + memset(¶m_changed, 0, sizeof(param_changed)); + + /* subscribe to battery topic */ + int battery_sub = orb_subscribe(ORB_ID(battery_status)); + struct battery_status_s battery; + memset(&battery, 0, sizeof(battery)); + battery.voltage_v = 0.0f; + + // uint8_t vehicle_state_previous = current_status.state_machine; + float voltage_previous = 0.0f; + + uint64_t last_idle_time = 0; + + /* now initialized */ + commander_initialized = true; + thread_running = true; + + uint64_t start_time = hrt_absolute_time(); + uint64_t failsave_ll_start_time = 0; + + bool state_changed = true; + bool param_init_forced = true; + + while (!thread_should_exit) { + + /* Get current values */ + bool new_data; + orb_check(sp_man_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); + } + + orb_check(sp_offboard_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); + } + + orb_check(sensor_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); + } + + orb_check(differential_pressure_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(differential_pressure), differential_pressure_sub, &differential_pressure); + last_differential_pressure_time = differential_pressure.timestamp; + } + + orb_check(cmd_sub, &new_data); + + if (new_data) { + /* got command */ + orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); + + /* handle it */ + handle_command(stat_pub, ¤t_status, &cmd); + } + + /* update parameters */ + orb_check(param_changed_sub, &new_data); + + if (new_data || param_init_forced) { + param_init_forced = false; + /* parameters changed */ + orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); + + + /* update parameters */ + if (!current_status.flag_system_armed) { + if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { + warnx("failed setting new system type"); + } + + /* disable manual override for all systems that rely on electronic stabilization */ + if (current_status.system_type == VEHICLE_TYPE_QUADROTOR || + current_status.system_type == VEHICLE_TYPE_HEXAROTOR || + current_status.system_type == VEHICLE_TYPE_OCTOROTOR) { + current_status.flag_external_manual_override_ok = false; + + } else { + current_status.flag_external_manual_override_ok = true; + } + + /* check and update system / component ID */ + param_get(_param_system_id, &(current_status.system_id)); + param_get(_param_component_id, &(current_status.component_id)); + + } + } + + /* update global position estimate */ + orb_check(global_position_sub, &new_data); + + if (new_data) { + /* position changed */ + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); + last_global_position_time = global_position.timestamp; + } + + /* update local position estimate */ + orb_check(local_position_sub, &new_data); + + if (new_data) { + /* position changed */ + orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); + last_local_position_time = local_position.timestamp; + } + + /* update battery status */ + orb_check(battery_sub, &new_data); + if (new_data) { + orb_copy(ORB_ID(battery_status), battery_sub, &battery); + battery_voltage = battery.voltage_v; + battery_voltage_valid = true; + + /* + * Only update battery voltage estimate if system has + * been running for two and a half seconds. + */ + if (hrt_absolute_time() - start_time > 2500000) { + bat_remain = battery_remaining_estimate_voltage(battery_voltage); + } + } + + /* Slow but important 8 Hz checks */ + if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) { + /* toggle activity (blue) led at 1 Hz in standby, 10 Hz in armed mode */ + if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY || + current_status.state_machine == SYSTEM_STATE_AUTO || + current_status.state_machine == SYSTEM_STATE_MANUAL)) { + /* armed */ + led_toggle(LED_BLUE); + + } else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + /* not armed */ + led_toggle(LED_BLUE); + } + + /* toggle error led at 5 Hz in HIL mode */ + if (current_status.flag_hil_enabled) { + /* hil enabled */ + led_toggle(LED_AMBER); + + } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { + /* toggle error (red) at 5 Hz on low battery or error */ + led_toggle(LED_AMBER); + + } else { + // /* Constant error indication in standby mode without GPS */ + // if (!current_status.gps_valid) { + // led_on(LED_AMBER); + + // } else { + // led_off(LED_AMBER); + // } + } + + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + /* compute system load */ + uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; + + if (last_idle_time > 0) + current_status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle + + last_idle_time = system_load.tasks[0].total_runtime; + } + } + + // // XXX Export patterns and threshold to parameters + /* Trigger audio event for low battery */ + if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 0)) { + /* For less than 10%, start be really annoying at 5 Hz */ + ioctl(buzzer, TONE_SET_ALARM, 0); + ioctl(buzzer, TONE_SET_ALARM, 3); + + } else if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 2)) { + ioctl(buzzer, TONE_SET_ALARM, 0); + + } else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 0)) { + /* For less than 20%, start be slightly annoying at 1 Hz */ + ioctl(buzzer, TONE_SET_ALARM, 0); + tune_confirm(); + + } else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 2)) { + ioctl(buzzer, TONE_SET_ALARM, 0); + } + + /* Check battery voltage */ + /* write to sys_status */ + if (battery_voltage_valid) { + current_status.voltage_battery = battery_voltage; + + } else { + current_status.voltage_battery = 0.0f; + } + + /* if battery voltage is getting lower, warn using buzzer, etc. */ + if (battery_voltage_valid && (bat_remain < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS + + if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { + low_battery_voltage_actions_done = true; + mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!"); + current_status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; + } + + low_voltage_counter++; + } + + /* Critical, this is rather an emergency, kill signal to sdlog and change state machine */ + else if (battery_voltage_valid && (bat_remain < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { + if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { + critical_battery_voltage_actions_done = true; + mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); + current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; + state_machine_emergency(stat_pub, ¤t_status, mavlink_fd); + } + + critical_voltage_counter++; + + } else { + low_voltage_counter = 0; + critical_voltage_counter = 0; + } + + /* End battery voltage check */ + + + /* + * Check for valid position information. + * + * If the system has a valid position source from an onboard + * position estimator, it is safe to operate it autonomously. + * The flag_vector_flight_mode_ok flag indicates that a minimum + * set of position measurements is available. + */ + + /* store current state to reason later about a state change */ + bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; + bool global_pos_valid = current_status.flag_global_position_valid; + bool local_pos_valid = current_status.flag_local_position_valid; + bool airspeed_valid = current_status.flag_airspeed_valid; + + /* check for global or local position updates, set a timeout of 2s */ + if (hrt_absolute_time() - last_global_position_time < 2000000) { + current_status.flag_global_position_valid = true; + // XXX check for controller status and home position as well + + } else { + current_status.flag_global_position_valid = false; + } + + if (hrt_absolute_time() - last_local_position_time < 2000000) { + current_status.flag_local_position_valid = true; + // XXX check for controller status and home position as well + + } else { + current_status.flag_local_position_valid = false; + } + + /* Check for valid airspeed/differential pressure measurements */ + if (hrt_absolute_time() - last_differential_pressure_time < 2000000) { + current_status.flag_airspeed_valid = true; + + } else { + current_status.flag_airspeed_valid = false; + } + + /* + * Consolidate global position and local position valid flags + * for vector flight mode. + */ + if (current_status.flag_local_position_valid || + current_status.flag_global_position_valid) { + current_status.flag_vector_flight_mode_ok = true; + + } else { + current_status.flag_vector_flight_mode_ok = false; + } + + /* consolidate state change, flag as changed if required */ + if (vector_flight_mode_ok != current_status.flag_vector_flight_mode_ok || + global_pos_valid != current_status.flag_global_position_valid || + local_pos_valid != current_status.flag_local_position_valid || + airspeed_valid != current_status.flag_airspeed_valid) { + state_changed = true; + } + + /* + * Mark the position of the first position lock as return to launch (RTL) + * position. The MAV will return here on command or emergency. + * + * Conditions: + * + * 1) The system aquired position lock just now + * 2) The system has not aquired position lock before + * 3) The system is not armed (on the ground) + */ + if (!current_status.flag_valid_launch_position && + !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok && + !current_status.flag_system_armed) { + /* first time a valid position, store it and emit it */ + + // XXX implement storage and publication of RTL position + current_status.flag_valid_launch_position = true; + current_status.flag_auto_flight_mode_ok = true; + state_changed = true; + } + + if (orb_check(ORB_ID(vehicle_gps_position), &new_data)) { + + orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); + + /* check for first, long-term and valid GPS lock -> set home position */ + float hdop_m = gps_position.eph_m; + float vdop_m = gps_position.epv_m; + + /* check if gps fix is ok */ + // XXX magic number + float hdop_threshold_m = 4.0f; + float vdop_threshold_m = 8.0f; + + /* + * If horizontal dilution of precision (hdop / eph) + * and vertical diluation of precision (vdop / epv) + * are below a certain threshold (e.g. 4 m), AND + * home position is not yet set AND the last GPS + * GPS measurement is not older than two seconds AND + * the system is currently not armed, set home + * position to the current position. + */ + + if (gps_position.fix_type == GPS_FIX_TYPE_3D + && (hdop_m < hdop_threshold_m) + && (vdop_m < vdop_threshold_m) + && !home_position_set + && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) + && !current_status.flag_system_armed) { + warnx("setting home position"); + + /* copy position data to uORB home message, store it locally as well */ + home.lat = gps_position.lat; + home.lon = gps_position.lon; + home.alt = gps_position.alt; + + home.eph_m = gps_position.eph_m; + home.epv_m = gps_position.epv_m; + + home.s_variance_m_s = gps_position.s_variance_m_s; + home.p_variance_m = gps_position.p_variance_m; + + /* announce new home position */ + if (home_pub > 0) { + orb_publish(ORB_ID(home_position), home_pub, &home); + } else { + home_pub = orb_advertise(ORB_ID(home_position), &home); + } + + /* mark home position as set */ + home_position_set = true; + tune_confirm(); + } + } + + /* ignore RC signals if in offboard control mode */ + if (!current_status.offboard_control_signal_found_once && sp_man.timestamp != 0) { + /* Start RC state check */ + + if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { + + // /* + // * Check if manual control modes have to be switched + // */ + // if (!isfinite(sp_man.manual_mode_switch)) { + // warnx("man mode sw not finite\n"); + + // /* this switch is not properly mapped, set default */ + // if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { + + // /* assuming a rotary wing, fall back to SAS */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; + // current_status.flag_control_attitude_enabled = true; + // current_status.flag_control_rates_enabled = true; + // } else { + + // /* assuming a fixed wing, fall back to direct pass-through */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; + // current_status.flag_control_attitude_enabled = false; + // current_status.flag_control_rates_enabled = false; + // } + + // } else if (sp_man.manual_mode_switch < -STICK_ON_OFF_LIMIT) { + + // /* bottom stick position, set direct mode for vehicles supporting it */ + // if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { + + // /* assuming a rotary wing, fall back to SAS */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; + // current_status.flag_control_attitude_enabled = true; + // current_status.flag_control_rates_enabled = true; + // } else { + + // /* assuming a fixed wing, set to direct pass-through as requested */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; + // current_status.flag_control_attitude_enabled = false; + // current_status.flag_control_rates_enabled = false; + // } + + // } else if (sp_man.manual_mode_switch > STICK_ON_OFF_LIMIT) { + + // /* top stick position, set SAS for all vehicle types */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; + // current_status.flag_control_attitude_enabled = true; + // current_status.flag_control_rates_enabled = true; + + // } else { + // /* center stick position, set rate control */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_RATES; + // current_status.flag_control_attitude_enabled = false; + // current_status.flag_control_rates_enabled = true; + // } + + // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); + + /* + * Check if manual stability control modes have to be switched + */ + if (!isfinite(sp_man.manual_sas_switch)) { + + /* this switch is not properly mapped, set default */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; + + } else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) { + + /* bottom stick position, set altitude hold */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE; + + } else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) { + + /* top stick position */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE; + + } else { + /* center stick position, set default */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; + } + + /* + * Check if left stick is in lower left position --> switch to standby state. + * Do this only for multirotors, not for fixed wing aircraft. + */ + if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) + ) && + ((sp_man.yaw < -STICK_ON_OFF_LIMIT)) && + (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { + if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { + update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); + stick_on_counter = 0; + + } else { + stick_off_counter++; + stick_on_counter = 0; + } + } + + /* check if left stick is in lower right position --> arm */ + if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) { + if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { + update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); + stick_on_counter = 0; + + } else { + stick_on_counter++; + stick_off_counter = 0; + } + } + + /* check manual override switch - switch to manual or auto mode */ + if (sp_man.manual_override_switch > STICK_ON_OFF_LIMIT) { + /* enable manual override */ + update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); + + } else if (sp_man.manual_override_switch < -STICK_ON_OFF_LIMIT) { + // /* check auto mode switch for correct mode */ + // if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) { + // /* enable guided mode */ + // update_state_machine_mode_guided(stat_pub, ¤t_status, mavlink_fd); + + // } else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) { + // XXX hardcode to auto for now + update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); + + // } + + } else { + /* center stick position, set SAS for all vehicle types */ + update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); + } + + /* handle the case where RC signal was regained */ + if (!current_status.rc_signal_found_once) { + current_status.rc_signal_found_once = true; + mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME."); + + } else { + if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); + } + + current_status.rc_signal_cutting_off = false; + current_status.rc_signal_lost = false; + current_status.rc_signal_lost_interval = 0; + + } else { + static uint64_t last_print_time = 0; + + /* print error message for first RC glitch and then every 5 s / 5000 ms) */ + if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) { + /* only complain if the offboard control is NOT active */ + current_status.rc_signal_cutting_off = true; + mavlink_log_critical(mavlink_fd, "CRITICAL - NO REMOTE SIGNAL!"); + last_print_time = hrt_absolute_time(); + } + + /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ + current_status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; + + /* if the RC signal is gone for a full second, consider it lost */ + if (current_status.rc_signal_lost_interval > 1000000) { + current_status.rc_signal_lost = true; + current_status.failsave_lowlevel = true; + state_changed = true; + } + + // if (hrt_absolute_time() - current_status.failsave_ll_start_time > failsafe_lowlevel_timeout_ms*1000) { + // publish_armed_status(¤t_status); + // } + } + } + + + + + /* End mode switch */ + + /* END RC state check */ + + + /* State machine update for offboard control */ + if (!current_status.rc_signal_found_once && sp_offboard.timestamp != 0) { + if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { + + /* decide about attitude control flag, enable in att/pos/vel */ + bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || + sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || + sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); + + /* decide about rate control flag, enable it always XXX (for now) */ + bool rates_ctrl_enabled = true; + + /* set up control mode */ + if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) { + current_status.flag_control_attitude_enabled = attitude_ctrl_enabled; + state_changed = true; + } + + if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) { + current_status.flag_control_rates_enabled = rates_ctrl_enabled; + state_changed = true; + } + + /* handle the case where offboard control signal was regained */ + if (!current_status.offboard_control_signal_found_once) { + current_status.offboard_control_signal_found_once = true; + /* enable offboard control, disable manual input */ + current_status.flag_control_manual_enabled = false; + current_status.flag_control_offboard_enabled = true; + state_changed = true; + tune_confirm(); + + mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST"); + + } else { + if (current_status.offboard_control_signal_lost) { + mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL"); + state_changed = true; + tune_confirm(); + } + } + + current_status.offboard_control_signal_weak = false; + current_status.offboard_control_signal_lost = false; + current_status.offboard_control_signal_lost_interval = 0; + + /* arm / disarm on request */ + if (sp_offboard.armed && !current_status.flag_system_armed) { + update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); + /* switch to stabilized mode = takeoff */ + update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); + + } else if (!sp_offboard.armed && current_status.flag_system_armed) { + update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); + } + + } else { + static uint64_t last_print_time = 0; + + /* print error message for first RC glitch and then every 5 s / 5000 ms) */ + if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) { + current_status.offboard_control_signal_weak = true; + mavlink_log_critical(mavlink_fd, "CRIT:NO OFFBOARD CONTROL!"); + last_print_time = hrt_absolute_time(); + } + + /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ + current_status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp; + + /* if the signal is gone for 0.1 seconds, consider it lost */ + if (current_status.offboard_control_signal_lost_interval > 100000) { + current_status.offboard_control_signal_lost = true; + current_status.failsave_lowlevel_start_time = hrt_absolute_time(); + tune_confirm(); + + /* kill motors after timeout */ + if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { + current_status.failsave_lowlevel = true; + state_changed = true; + } + } + } + } + + + current_status.counter++; + current_status.timestamp = hrt_absolute_time(); + + + /* If full run came back clean, transition to standby */ + if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT && + current_status.flag_preflight_gyro_calibration == false && + current_status.flag_preflight_mag_calibration == false && + current_status.flag_preflight_accel_calibration == false) { + /* All ok, no calibration going on, go to standby */ + do_state_update(stat_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + } + + /* publish at least with 1 Hz */ + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { + publish_armed_status(¤t_status); + orb_publish(ORB_ID(vehicle_status), stat_pub, ¤t_status); + state_changed = false; + } + + /* Store old modes to detect and act on state transitions */ + voltage_previous = current_status.voltage_battery; + + fflush(stdout); + counter++; + usleep(COMMANDER_MONITORING_INTERVAL); + } + + /* wait for threads to complete */ + // pthread_join(command_handling_thread, NULL); + pthread_join(subsystem_info_thread, NULL); + + /* close fds */ + led_deinit(); + buzzer_deinit(); + close(sp_man_sub); + close(sp_offboard_sub); + close(global_position_sub); + close(sensor_sub); + close(cmd_sub); + + warnx("exiting..\n"); + fflush(stdout); + + thread_running = false; + + return 0; +} diff --git a/src/modules/commander/commander.h b/src/modules/commander/commander.h new file mode 100644 index 000000000..04b4e72ab --- /dev/null +++ b/src/modules/commander/commander.h @@ -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_ */ diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk new file mode 100644 index 000000000..b90da8289 --- /dev/null +++ b/src/modules/commander/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Main system state machine +# + +MODULE_COMMAND = commander +SRCS = commander.c \ + state_machine_helper.c \ + calibration_routines.c diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c new file mode 100644 index 000000000..bea388a10 --- /dev/null +++ b/src/modules/commander/state_machine_helper.c @@ -0,0 +1,752 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: 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 state_machine_helper.c + * State machine helper functions implementations + */ + +#include <stdio.h> +#include <unistd.h> + +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/actuator_controls.h> +#include <systemlib/systemlib.h> +#include <drivers/drv_hrt.h> +#include <mavlink/mavlink_log.h> + +#include "state_machine_helper.h" + +static const char *system_state_txt[] = { + "SYSTEM_STATE_PREFLIGHT", + "SYSTEM_STATE_STANDBY", + "SYSTEM_STATE_GROUND_READY", + "SYSTEM_STATE_MANUAL", + "SYSTEM_STATE_STABILIZED", + "SYSTEM_STATE_AUTO", + "SYSTEM_STATE_MISSION_ABORT", + "SYSTEM_STATE_EMCY_LANDING", + "SYSTEM_STATE_EMCY_CUTOFF", + "SYSTEM_STATE_GROUND_ERROR", + "SYSTEM_STATE_REBOOT", + +}; + +/** + * Transition from one state to another + */ +int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state) +{ + int invalid_state = false; + int ret = ERROR; + + commander_state_machine_t old_state = current_status->state_machine; + + switch (new_state) { + case SYSTEM_STATE_MISSION_ABORT: { + /* Indoor or outdoor */ + // if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { + ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING); + + // } else { + // ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF); + // } + } + break; + + case SYSTEM_STATE_EMCY_LANDING: + /* Tell the controller to land */ + + /* set system flags according to state */ + current_status->flag_system_armed = true; + + warnx("EMERGENCY LANDING!\n"); + mavlink_log_critical(mavlink_fd, "EMERGENCY LANDING!"); + break; + + case SYSTEM_STATE_EMCY_CUTOFF: + /* Tell the controller to cutoff the motors (thrust = 0) */ + + /* set system flags according to state */ + current_status->flag_system_armed = false; + + warnx("EMERGENCY MOTOR CUTOFF!\n"); + mavlink_log_critical(mavlink_fd, "EMERGENCY MOTOR CUTOFF!"); + break; + + case SYSTEM_STATE_GROUND_ERROR: + + /* set system flags according to state */ + + /* prevent actuators from arming */ + current_status->flag_system_armed = false; + + warnx("GROUND ERROR, locking down propulsion system\n"); + mavlink_log_critical(mavlink_fd, "GROUND ERROR, locking down system"); + break; + + case SYSTEM_STATE_PREFLIGHT: + if (current_status->state_machine == SYSTEM_STATE_STANDBY + || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { + /* set system flags according to state */ + current_status->flag_system_armed = false; + mavlink_log_critical(mavlink_fd, "Switched to PREFLIGHT state"); + + } else { + invalid_state = true; + mavlink_log_critical(mavlink_fd, "REFUSED to switch to PREFLIGHT state"); + } + + break; + + case SYSTEM_STATE_REBOOT: + if (current_status->state_machine == SYSTEM_STATE_STANDBY + || current_status->state_machine == SYSTEM_STATE_PREFLIGHT + || current_status->flag_hil_enabled) { + invalid_state = false; + /* set system flags according to state */ + current_status->flag_system_armed = false; + mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); + usleep(500000); + up_systemreset(); + /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ + + } else { + invalid_state = true; + mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT"); + } + + break; + + case SYSTEM_STATE_STANDBY: + /* set system flags according to state */ + + /* standby enforces disarmed */ + current_status->flag_system_armed = false; + + mavlink_log_critical(mavlink_fd, "Switched to STANDBY state"); + break; + + case SYSTEM_STATE_GROUND_READY: + + /* set system flags according to state */ + + /* ground ready has motors / actuators armed */ + current_status->flag_system_armed = true; + + mavlink_log_critical(mavlink_fd, "Switched to GROUND READY state"); + break; + + case SYSTEM_STATE_AUTO: + + /* set system flags according to state */ + + /* auto is airborne and in auto mode, motors armed */ + current_status->flag_system_armed = true; + + mavlink_log_critical(mavlink_fd, "Switched to FLYING / AUTO mode"); + break; + + case SYSTEM_STATE_STABILIZED: + + /* set system flags according to state */ + current_status->flag_system_armed = true; + + mavlink_log_critical(mavlink_fd, "Switched to FLYING / STABILIZED mode"); + break; + + case SYSTEM_STATE_MANUAL: + + /* set system flags according to state */ + current_status->flag_system_armed = true; + + mavlink_log_critical(mavlink_fd, "Switched to FLYING / MANUAL mode"); + break; + + default: + invalid_state = true; + break; + } + + if (invalid_state == false || old_state != new_state) { + current_status->state_machine = new_state; + state_machine_publish(status_pub, current_status, mavlink_fd); + publish_armed_status(current_status); + ret = OK; + } + + if (invalid_state) { + mavlink_log_critical(mavlink_fd, "REJECTING invalid state transition"); + ret = ERROR; + } + + return ret; +} + +void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + /* publish the new state */ + current_status->counter++; + current_status->timestamp = hrt_absolute_time(); + + /* assemble state vector based on flag values */ + if (current_status->flag_control_rates_enabled) { + current_status->onboard_control_sensors_present |= 0x400; + + } else { + current_status->onboard_control_sensors_present &= ~0x400; + } + + current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; + current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; + current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; + current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; + + current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0; + current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; + current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; + current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; + current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; + + orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + printf("[cmd] new state: %s\n", system_state_txt[current_status->state_machine]); +} + +void publish_armed_status(const struct vehicle_status_s *current_status) +{ + struct actuator_armed_s armed; + armed.armed = current_status->flag_system_armed; + /* lock down actuators if required, only in HIL */ + armed.lockdown = (current_status->flag_hil_enabled) ? true : false; + orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); + orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); +} + + +/* + * Private functions, update the state machine + */ +void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + warnx("EMERGENCY HANDLER\n"); + /* Depending on the current state go to one of the error states */ + + if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) { + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR); + + } else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) { + + // DO NOT abort mission + //do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT); + + } else { + warnx("Unknown system state: #%d\n", current_status->state_machine); + } +} + +void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors +{ + if (current_status->state_machine != SYSTEM_STATE_MANUAL) { //if we are in manual: user can react to errors themself + state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd); + + } else { + //global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL); + } + +} + + + +// /* +// * Wrapper functions (to be used in the commander), all functions assume lock on current_status +// */ + +// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR +// * +// * START SUBSYSTEM/EMERGENCY FUNCTIONS +// * */ + +// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_present |= 1 << *subsystem_type; +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); +// } + +// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type); +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + +// /* if a subsystem was removed something went completely wrong */ + +// switch (*subsystem_type) { +// case SUBSYSTEM_TYPE_GYRO: +// //global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_ACC: +// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_MAG: +// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_GPS: +// { +// uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]; + +// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { +// //global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency(status_pub, current_status); +// } +// } +// break; + +// default: +// break; +// } + +// } + +// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type; +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); +// } + +// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type); +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + +// /* if a subsystem was disabled something went completely wrong */ + +// switch (*subsystem_type) { +// case SUBSYSTEM_TYPE_GYRO: +// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_ACC: +// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_MAG: +// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_GPS: +// { +// uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]); + +// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { +// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency(status_pub, current_status); +// } +// } +// break; + +// default: +// break; +// } + +// } + + +// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_health |= 1 << *subsystem_type; +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + +// switch (*subsystem_type) { +// case SUBSYSTEM_TYPE_GYRO: +// //TODO state machine change (recovering) +// break; + +// case SUBSYSTEM_TYPE_ACC: +// //TODO state machine change +// break; + +// case SUBSYSTEM_TYPE_MAG: +// //TODO state machine change +// break; + +// case SUBSYSTEM_TYPE_GPS: +// //TODO state machine change +// break; + +// default: +// break; +// } + + +// } + + +// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// bool previosly_healthy = (bool)(current_status->onboard_control_sensors_health & 1 << *subsystem_type); +// current_status->onboard_control_sensors_health &= ~(1 << *subsystem_type); +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + +// /* if we received unhealthy message more than *_HEALTH_COUNTER_LIMIT, switch to error state */ + +// switch (*subsystem_type) { +// case SUBSYSTEM_TYPE_GYRO: +// //global_data_send_mavlink_statustext_message_out("Commander: gyro unhealthy", MAV_SEVERITY_CRITICAL); + +// if (previosly_healthy) //only throw emergency if previously healthy +// state_machine_emergency_always_critical(status_pub, current_status); + +// break; + +// case SUBSYSTEM_TYPE_ACC: +// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer unhealthy", MAV_SEVERITY_CRITICAL); + +// if (previosly_healthy) //only throw emergency if previously healthy +// state_machine_emergency_always_critical(status_pub, current_status); + +// break; + +// case SUBSYSTEM_TYPE_MAG: +// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer unhealthy", MAV_SEVERITY_CRITICAL); + +// if (previosly_healthy) //only throw emergency if previously healthy +// state_machine_emergency_always_critical(status_pub, current_status); + +// break; + +// case SUBSYSTEM_TYPE_GPS: +// // //TODO: remove this block +// // break; +// // /////////////////// +// //global_data_send_mavlink_statustext_message_out("Commander: GPS unhealthy", MAV_SEVERITY_CRITICAL); + +// // printf("previosly_healthy = %u\n", previosly_healthy); +// if (previosly_healthy) //only throw emergency if previously healthy +// state_machine_emergency(status_pub, current_status); + +// break; + +// default: +// break; +// } + +// } + + +/* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ + + +void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + /* Depending on the current state switch state */ + if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); + } +} + +void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + /* Depending on the current state switch state */ + if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_AUTO) { + state_machine_emergency(status_pub, current_status, mavlink_fd); + } +} + +void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + if (current_status->state_machine == SYSTEM_STATE_STANDBY) { + printf("[cmd] arming\n"); + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); + } +} + +void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { + printf("[cmd] going standby\n"); + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); + + } else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { + printf("[cmd] MISSION ABORT!\n"); + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); + } +} + +void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + int old_mode = current_status->flight_mode; + current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; + + current_status->flag_control_manual_enabled = true; + + /* set behaviour based on airframe */ + if ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)) { + + /* assuming a rotary wing, set to SAS */ + current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; + current_status->flag_control_attitude_enabled = true; + current_status->flag_control_rates_enabled = true; + + } else { + + /* assuming a fixed wing, set to direct pass-through */ + current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; + current_status->flag_control_attitude_enabled = false; + current_status->flag_control_rates_enabled = false; + } + + if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); + + if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { + printf("[cmd] manual mode\n"); + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); + } +} + +void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { + int old_mode = current_status->flight_mode; + int old_manual_control_mode = current_status->manual_control_mode; + current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; + current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; + current_status->flag_control_attitude_enabled = true; + current_status->flag_control_rates_enabled = true; + current_status->flag_control_manual_enabled = true; + + if (old_mode != current_status->flight_mode || + old_manual_control_mode != current_status->manual_control_mode) { + printf("[cmd] att stabilized mode\n"); + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); + state_machine_publish(status_pub, current_status, mavlink_fd); + } + + } +} + +void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + if (!current_status->flag_vector_flight_mode_ok) { + mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. GUIDED MODE"); + tune_error(); + return; + } + + if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { + printf("[cmd] position guided mode\n"); + int old_mode = current_status->flight_mode; + current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB; + current_status->flag_control_manual_enabled = false; + current_status->flag_control_attitude_enabled = true; + current_status->flag_control_rates_enabled = true; + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED); + + if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); + + } +} + +void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + if (!current_status->flag_vector_flight_mode_ok) { + mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. AUTO MODE"); + return; + } + + if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) { + printf("[cmd] auto mode\n"); + int old_mode = current_status->flight_mode; + current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO; + current_status->flag_control_manual_enabled = false; + current_status->flag_control_attitude_enabled = true; + current_status->flag_control_rates_enabled = true; + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO); + + if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); + } +} + + +uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) +{ + uint8_t ret = 1; + + /* Switch on HIL if in standby and not already in HIL mode */ + if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) + && !current_status->flag_hil_enabled) { + if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { + /* Enable HIL on request */ + current_status->flag_hil_enabled = true; + ret = OK; + state_machine_publish(status_pub, current_status, mavlink_fd); + publish_armed_status(current_status); + printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); + + } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && + current_status->flag_system_armed) { + + mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") + + } else { + + mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.") + } + } + + /* switch manual / auto */ + if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { + update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); + + } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { + update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); + + } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { + update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); + + } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { + update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); + } + + /* vehicle is disarmed, mode requests arming */ + if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { + /* only arm in standby state */ + // XXX REMOVE + if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); + ret = OK; + printf("[cmd] arming due to command request\n"); + } + } + + /* vehicle is armed, mode requests disarming */ + if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { + /* only disarm in ground ready */ + if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); + ret = OK; + printf("[cmd] disarming due to command request\n"); + } + } + + /* NEVER actually switch off HIL without reboot */ + if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { + warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); + mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL"); + ret = ERROR; + } + + return ret; +} + +uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode) //TODO: add more checks to avoid state switching in critical situations +{ + commander_state_machine_t current_system_state = current_status->state_machine; + + uint8_t ret = 1; + + switch (custom_mode) { + case SYSTEM_STATE_GROUND_READY: + break; + + case SYSTEM_STATE_STANDBY: + break; + + case SYSTEM_STATE_REBOOT: + printf("try to reboot\n"); + + if (current_system_state == SYSTEM_STATE_STANDBY + || current_system_state == SYSTEM_STATE_PREFLIGHT + || current_status->flag_hil_enabled) { + printf("system will reboot\n"); + mavlink_log_critical(mavlink_fd, "Rebooting.."); + usleep(200000); + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT); + ret = 0; + } + + break; + + case SYSTEM_STATE_AUTO: + printf("try to switch to auto/takeoff\n"); + + if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_MANUAL) { + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO); + printf("state: auto\n"); + ret = 0; + } + + break; + + case SYSTEM_STATE_MANUAL: + printf("try to switch to manual\n"); + + if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_AUTO) { + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); + printf("state: manual\n"); + ret = 0; + } + + break; + + default: + break; + } + + return ret; +} + diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h new file mode 100644 index 000000000..2f2ccc729 --- /dev/null +++ b/src/modules/commander/state_machine_helper.h @@ -0,0 +1,209 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: 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 state_machine_helper.h + * State machine helper functions definitions + */ + +#ifndef STATE_MACHINE_HELPER_H_ +#define STATE_MACHINE_HELPER_H_ + +#define GPS_NOFIX_COUNTER_LIMIT 4 //need GPS_NOFIX_COUNTER_LIMIT gps packets with a bad fix to call an error (if outdoor) +#define GPS_GOTFIX_COUNTER_REQUIRED 4 //need GPS_GOTFIX_COUNTER_REQUIRED gps packets with a good fix to obtain position lock + +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_status.h> + +/** + * Switch to new state with no checking. + * + * do_state_update: this is the functions that all other functions have to call in order to update the state. + * the function does not question the state change, this must be done before + * The function performs actions that are connected with the new state (buzzer, reboot, ...) + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + * + * @return 0 (macro OK) or 1 on error (macro ERROR) + */ +int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state); + +/* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR */ +// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); +// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); + +// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); +// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); + +// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); +// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); + + +/** + * Handle state machine if got position fix + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + +/** + * Handle state machine if position fix lost + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + +/** + * Handle state machine if user wants to arm + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + +/** + * Handle state machine if user wants to disarm + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + +/** + * Handle state machine if mode switch is manual + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + +/** + * Handle state machine if mode switch is stabilized + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + +/** + * Handle state machine if mode switch is guided + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + +/** + * Handle state machine if mode switch is auto + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + +/** + * Publish current state information + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + + +/* + * Functions that handle incoming requests to change the state machine or a parameter (probably from the mavlink app). + * If the request is obeyed the functions return 0 + * + */ + +/** + * Handles *incoming request* to switch to a specific state, if state change is successful returns 0 + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode); + +/** + * Handles *incoming request* to switch to a specific custom state, if state change is successful returns 0 + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode); + +/** + * Always switches to critical mode under any circumstances. + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + +/** + * Switches to emergency if required. + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + +/** + * Publish the armed state depending on the current system state + * + * @param current_status the current system status + */ +void publish_armed_status(const struct vehicle_status_s *current_status); + + + +#endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/systemcmds/eeprom/24xxxx_mtd.c b/src/systemcmds/eeprom/24xxxx_mtd.c new file mode 100644 index 000000000..e34be44e3 --- /dev/null +++ b/src/systemcmds/eeprom/24xxxx_mtd.c @@ -0,0 +1,571 @@ +/************************************************************************************ + * Driver for 24xxxx-style I2C EEPROMs. + * + * Adapted from: + * + * drivers/mtd/at24xx.c + * Driver for I2C-based at24cxx EEPROM(at24c32,at24c64,at24c128,at24c256) + * + * Copyright (C) 2011 Li Zhuoyi. All rights reserved. + * Author: Li Zhuoyi <lzyy.cn@gmail.com> + * History: 0.1 2011-08-20 initial version + * + * 2011-11-1 Added support for larger MTD block sizes: Hal Glenn <hglenn@2g-eng.com> + * + * Derived from drivers/mtd/m25px.c + * + * Copyright (C) 2009-2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ************************************************************************************/ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <stdlib.h> +#include <unistd.h> +#include <string.h> +#include <errno.h> +#include <debug.h> + +#include <nuttx/kmalloc.h> +#include <nuttx/fs/ioctl.h> +#include <nuttx/i2c.h> +#include <nuttx/mtd.h> + +#include "systemlib/perf_counter.h" + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ + +/* + * Configuration is as for the AT24 driver, but the CONFIG_MTD_AT24XX define should be + * omitted in order to avoid building the AT24XX driver as well. + */ + +/* As a minimum, the size of the AT24 part and its 7-bit I2C address are required. */ + +#ifndef CONFIG_AT24XX_SIZE +# warning "Assuming AT24 size 64" +# define CONFIG_AT24XX_SIZE 64 +#endif +#ifndef CONFIG_AT24XX_ADDR +# warning "Assuming AT24 address of 0x50" +# define CONFIG_AT24XX_ADDR 0x50 +#endif + +/* Get the part configuration based on the size configuration */ + +#if CONFIG_AT24XX_SIZE == 32 +# define AT24XX_NPAGES 128 +# define AT24XX_PAGESIZE 32 +#elif CONFIG_AT24XX_SIZE == 48 +# define AT24XX_NPAGES 192 +# define AT24XX_PAGESIZE 32 +#elif CONFIG_AT24XX_SIZE == 64 +# define AT24XX_NPAGES 256 +# define AT24XX_PAGESIZE 32 +#elif CONFIG_AT24XX_SIZE == 128 +# define AT24XX_NPAGES 256 +# define AT24XX_PAGESIZE 64 +#elif CONFIG_AT24XX_SIZE == 256 +# define AT24XX_NPAGES 512 +# define AT24XX_PAGESIZE 64 +#endif + +/* For applications where a file system is used on the AT24, the tiny page sizes + * will result in very inefficient FLASH usage. In such cases, it is better if + * blocks are comprised of "clusters" of pages so that the file system block + * size is, say, 256 or 512 bytes. In any event, the block size *must* be an + * even multiple of the pages. + */ + +#ifndef CONFIG_AT24XX_MTD_BLOCKSIZE +# warning "Assuming driver block size is the same as the FLASH page size" +# define CONFIG_AT24XX_MTD_BLOCKSIZE AT24XX_PAGESIZE +#endif + +/* The AT24 does not respond on the bus during write cycles, so we depend on a long + * timeout to detect problems. The max program time is typically ~5ms. + */ +#ifndef CONFIG_AT24XX_WRITE_TIMEOUT_MS +# define CONFIG_AT24XX_WRITE_TIMEOUT_MS 20 +#endif + +/************************************************************************************ + * Private Types + ************************************************************************************/ + +/* This type represents the state of the MTD device. The struct mtd_dev_s + * must appear at the beginning of the definition so that you can freely + * cast between pointers to struct mtd_dev_s and struct at24c_dev_s. + */ + +struct at24c_dev_s { + struct mtd_dev_s mtd; /* MTD interface */ + FAR struct i2c_dev_s *dev; /* Saved I2C interface instance */ + uint8_t addr; /* I2C address */ + uint16_t pagesize; /* 32, 63 */ + uint16_t npages; /* 128, 256, 512, 1024 */ + + perf_counter_t perf_reads; + perf_counter_t perf_writes; + perf_counter_t perf_resets; + perf_counter_t perf_read_retries; + perf_counter_t perf_read_errors; + perf_counter_t perf_write_errors; +}; + +/************************************************************************************ + * Private Function Prototypes + ************************************************************************************/ + +/* MTD driver methods */ + +static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks); +static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, + size_t nblocks, FAR uint8_t *buf); +static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, + size_t nblocks, FAR const uint8_t *buf); +static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg); + +void at24c_test(void); + +/************************************************************************************ + * Private Data + ************************************************************************************/ + +/* At present, only a single AT24 part is supported. In this case, a statically + * allocated state structure may be used. + */ + +static struct at24c_dev_s g_at24c; + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +static int at24c_eraseall(FAR struct at24c_dev_s *priv) +{ + int startblock = 0; + uint8_t buf[AT24XX_PAGESIZE + 2]; + + struct i2c_msg_s msgv[1] = { + { + .addr = priv->addr, + .flags = 0, + .buffer = &buf[0], + .length = sizeof(buf), + } + }; + + memset(&buf[2], 0xff, priv->pagesize); + + for (startblock = 0; startblock < priv->npages; startblock++) { + uint16_t offset = startblock * priv->pagesize; + buf[1] = offset & 0xff; + buf[0] = (offset >> 8) & 0xff; + + while (I2C_TRANSFER(priv->dev, &msgv[0], 1) < 0) { + fvdbg("erase stall\n"); + usleep(10000); + } + } + + return OK; +} + +/************************************************************************************ + * Name: at24c_erase + ************************************************************************************/ + +static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks) +{ + /* EEprom need not erase */ + + return (int)nblocks; +} + +/************************************************************************************ + * Name: at24c_test + ************************************************************************************/ + +void at24c_test(void) +{ + uint8_t buf[CONFIG_AT24XX_MTD_BLOCKSIZE]; + unsigned count = 0; + unsigned errors = 0; + + for (count = 0; count < 10000; count++) { + ssize_t result = at24c_bread(&g_at24c.mtd, 0, 1, buf); + if (result == ERROR) { + if (errors++ > 2) { + vdbg("too many errors\n"); + return; + } + } else if (result != 1) { + vdbg("unexpected %u\n", result); + } + if ((count % 100) == 0) + vdbg("test %u errors %u\n", count, errors); + } +} + +/************************************************************************************ + * Name: at24c_bread + ************************************************************************************/ + +static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, + size_t nblocks, FAR uint8_t *buffer) +{ + FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; + size_t blocksleft; + uint8_t addr[2]; + int ret; + + struct i2c_msg_s msgv[2] = { + { + .addr = priv->addr, + .flags = 0, + .buffer = &addr[0], + .length = sizeof(addr), + }, + { + .addr = priv->addr, + .flags = I2C_M_READ, + .buffer = 0, + .length = priv->pagesize, + } + }; + +#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE + startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); + nblocks *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); +#endif + blocksleft = nblocks; + + fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); + + if (startblock >= priv->npages) { + return 0; + } + + if (startblock + nblocks > priv->npages) { + nblocks = priv->npages - startblock; + } + + while (blocksleft-- > 0) { + uint16_t offset = startblock * priv->pagesize; + unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS; + + addr[1] = offset & 0xff; + addr[0] = (offset >> 8) & 0xff; + msgv[1].buffer = buffer; + + for (;;) { + + perf_begin(priv->perf_reads); + ret = I2C_TRANSFER(priv->dev, &msgv[0], 2); + perf_end(priv->perf_reads); + + if (ret >= 0) + break; + + fvdbg("read stall"); + usleep(1000); + + /* We should normally only be here on the first read after + * a write. + * + * XXX maybe do special first-read handling with optional + * bus reset as well? + */ + perf_count(priv->perf_read_retries); + + if (--tries == 0) { + perf_count(priv->perf_read_errors); + return ERROR; + } + } + + startblock++; + buffer += priv->pagesize; + } + +#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE + return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); +#else + return nblocks; +#endif +} + +/************************************************************************************ + * Name: at24c_bwrite + * + * Operates on MTD block's and translates to FLASH pages + * + ************************************************************************************/ + +static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks, + FAR const uint8_t *buffer) +{ + FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; + size_t blocksleft; + uint8_t buf[AT24XX_PAGESIZE + 2]; + int ret; + + struct i2c_msg_s msgv[1] = { + { + .addr = priv->addr, + .flags = 0, + .buffer = &buf[0], + .length = sizeof(buf), + } + }; + +#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE + startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); + nblocks *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); +#endif + blocksleft = nblocks; + + if (startblock >= priv->npages) { + return 0; + } + + if (startblock + nblocks > priv->npages) { + nblocks = priv->npages - startblock; + } + + fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); + + while (blocksleft-- > 0) { + uint16_t offset = startblock * priv->pagesize; + unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS; + + buf[1] = offset & 0xff; + buf[0] = (offset >> 8) & 0xff; + memcpy(&buf[2], buffer, priv->pagesize); + + for (;;) { + + perf_begin(priv->perf_writes); + ret = I2C_TRANSFER(priv->dev, &msgv[0], 1); + perf_end(priv->perf_writes); + + if (ret >= 0) + break; + + fvdbg("write stall"); + usleep(1000); + + /* We expect to see a number of retries per write cycle as we + * poll for write completion. + */ + if (--tries == 0) { + perf_count(priv->perf_write_errors); + return ERROR; + } + } + + startblock++; + buffer += priv->pagesize; + } + +#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE + return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); +#else + return nblocks; +#endif +} + +/************************************************************************************ + * Name: at24c_ioctl + ************************************************************************************/ + +static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg) +{ + FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; + int ret = -EINVAL; /* Assume good command with bad parameters */ + + fvdbg("cmd: %d \n", cmd); + + switch (cmd) { + case MTDIOC_GEOMETRY: { + FAR struct mtd_geometry_s *geo = (FAR struct mtd_geometry_s *)((uintptr_t)arg); + + if (geo) { + /* Populate the geometry structure with information need to know + * the capacity and how to access the device. + * + * NOTE: that the device is treated as though it where just an array + * of fixed size blocks. That is most likely not true, but the client + * will expect the device logic to do whatever is necessary to make it + * appear so. + * + * blocksize: + * May be user defined. The block size for the at24XX devices may be + * larger than the page size in order to better support file systems. + * The read and write functions translate BLOCKS to pages for the + * small flash devices + * erasesize: + * It has to be at least as big as the blocksize, bigger serves no + * purpose. + * neraseblocks + * Note that the device size is in kilobits and must be scaled by + * 1024 / 8 + */ + +#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE + geo->blocksize = CONFIG_AT24XX_MTD_BLOCKSIZE; + geo->erasesize = CONFIG_AT24XX_MTD_BLOCKSIZE; + geo->neraseblocks = (CONFIG_AT24XX_SIZE * 1024 / 8) / CONFIG_AT24XX_MTD_BLOCKSIZE; +#else + geo->blocksize = priv->pagesize; + geo->erasesize = priv->pagesize; + geo->neraseblocks = priv->npages; +#endif + ret = OK; + + fvdbg("blocksize: %d erasesize: %d neraseblocks: %d\n", + geo->blocksize, geo->erasesize, geo->neraseblocks); + } + } + break; + + case MTDIOC_BULKERASE: + ret = at24c_eraseall(priv); + break; + + case MTDIOC_XIPBASE: + default: + ret = -ENOTTY; /* Bad command */ + break; + } + + return ret; +} + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: at24c_initialize + * + * Description: + * Create an initialize MTD device instance. MTD devices are not registered + * in the file system, but are created as instances that can be bound to + * other functions (such as a block or character driver front end). + * + ************************************************************************************/ + +FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) { + FAR struct at24c_dev_s *priv; + + fvdbg("dev: %p\n", dev); + + /* Allocate a state structure (we allocate the structure instead of using + * a fixed, static allocation so that we can handle multiple FLASH devices. + * The current implementation would handle only one FLASH part per I2C + * device (only because of the SPIDEV_FLASH definition) and so would have + * to be extended to handle multiple FLASH parts on the same I2C bus. + */ + + priv = &g_at24c; + + if (priv) { + /* Initialize the allocated structure */ + + priv->addr = CONFIG_AT24XX_ADDR; + priv->pagesize = AT24XX_PAGESIZE; + priv->npages = AT24XX_NPAGES; + + priv->mtd.erase = at24c_erase; + priv->mtd.bread = at24c_bread; + priv->mtd.bwrite = at24c_bwrite; + priv->mtd.ioctl = at24c_ioctl; + priv->dev = dev; + + priv->perf_reads = perf_alloc(PC_ELAPSED, "EEPROM read"); + priv->perf_writes = perf_alloc(PC_ELAPSED, "EEPROM write"); + priv->perf_resets = perf_alloc(PC_COUNT, "EEPROM reset"); + priv->perf_read_retries = perf_alloc(PC_COUNT, "EEPROM read retries"); + priv->perf_read_errors = perf_alloc(PC_COUNT, "EEPROM read errors"); + priv->perf_write_errors = perf_alloc(PC_COUNT, "EEPROM write errors"); + } + + /* attempt to read to validate device is present */ + unsigned char buf[5]; + uint8_t addrbuf[2] = {0, 0}; + + struct i2c_msg_s msgv[2] = { + { + .addr = priv->addr, + .flags = 0, + .buffer = &addrbuf[0], + .length = sizeof(addrbuf), + }, + { + .addr = priv->addr, + .flags = I2C_M_READ, + .buffer = &buf[0], + .length = sizeof(buf), + } + }; + + perf_begin(priv->perf_reads); + int ret = I2C_TRANSFER(priv->dev, &msgv[0], 2); + perf_end(priv->perf_reads); + + if (ret < 0) { + return NULL; + } + + /* Return the implementation-specific state structure as the MTD device */ + + fvdbg("Return %p\n", priv); + return (FAR struct mtd_dev_s *)priv; +} + +/* + * XXX: debug hackery + */ +int at24c_nuke(void) +{ + return at24c_eraseall(&g_at24c); +} diff --git a/src/systemcmds/eeprom/eeprom.c b/src/systemcmds/eeprom/eeprom.c new file mode 100644 index 000000000..49da51358 --- /dev/null +++ b/src/systemcmds/eeprom/eeprom.c @@ -0,0 +1,265 @@ +/**************************************************************************** + * + * 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 eeprom.c + * + * EEPROM service and utility app. + */ + +#include <nuttx/config.h> + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <stdbool.h> +#include <unistd.h> +#include <fcntl.h> +#include <sys/mount.h> +#include <sys/ioctl.h> +#include <sys/stat.h> + +#include <nuttx/i2c.h> +#include <nuttx/mtd.h> +#include <nuttx/fs/nxffs.h> +#include <nuttx/fs/ioctl.h> + +#include <arch/board/board.h> + +#include "systemlib/systemlib.h" +#include "systemlib/param/param.h" +#include "systemlib/err.h" + +#ifndef PX4_I2C_BUS_ONBOARD +# error PX4_I2C_BUS_ONBOARD not defined, cannot locate onboard EEPROM +#endif + +__EXPORT int eeprom_main(int argc, char *argv[]); + +static void eeprom_attach(void); +static void eeprom_start(void); +static void eeprom_erase(void); +static void eeprom_ioctl(unsigned operation); +static void eeprom_save(const char *name); +static void eeprom_load(const char *name); +static void eeprom_test(void); + +static bool attached = false; +static bool started = false; +static struct mtd_dev_s *eeprom_mtd; + +int eeprom_main(int argc, char *argv[]) +{ + if (argc >= 2) { + if (!strcmp(argv[1], "start")) + eeprom_start(); + + if (!strcmp(argv[1], "save_param")) + eeprom_save(argv[2]); + + if (!strcmp(argv[1], "load_param")) + eeprom_load(argv[2]); + + if (!strcmp(argv[1], "erase")) + eeprom_erase(); + + if (!strcmp(argv[1], "test")) + eeprom_test(); + + if (0) { /* these actually require a file on the filesystem... */ + + if (!strcmp(argv[1], "reformat")) + eeprom_ioctl(FIOC_REFORMAT); + + if (!strcmp(argv[1], "repack")) + eeprom_ioctl(FIOC_OPTIMIZE); + } + } + + errx(1, "expected a command, try 'start'\n\t'save_param /eeprom/parameters'\n\t'load_param /eeprom/parameters'\n\t'erase'\n"); +} + + +static void +eeprom_attach(void) +{ + /* find the right I2C */ + struct i2c_dev_s *i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD); + /* this resets the I2C bus, set correct bus speed again */ + I2C_SETFREQUENCY(i2c, 400000); + + if (i2c == NULL) + errx(1, "failed to locate I2C bus"); + + /* start the MTD driver, attempt 5 times */ + for (int i = 0; i < 5; i++) { + eeprom_mtd = at24c_initialize(i2c); + if (eeprom_mtd) { + /* abort on first valid result */ + if (i > 0) { + warnx("warning: EEPROM needed %d attempts to attach", i+1); + } + break; + } + } + + /* if last attempt is still unsuccessful, abort */ + if (eeprom_mtd == NULL) + errx(1, "failed to initialize EEPROM driver"); + + attached = true; +} + +static void +eeprom_start(void) +{ + int ret; + + if (started) + errx(1, "EEPROM already mounted"); + + if (!attached) + eeprom_attach(); + + /* start NXFFS */ + ret = nxffs_initialize(eeprom_mtd); + + if (ret < 0) + errx(1, "failed to initialize NXFFS - erase EEPROM to reformat"); + + /* mount the EEPROM */ + ret = mount(NULL, "/eeprom", "nxffs", 0, NULL); + + if (ret < 0) + errx(1, "failed to mount /eeprom - erase EEPROM to reformat"); + + started = true; + warnx("mounted EEPROM at /eeprom"); + exit(0); +} + +extern int at24c_nuke(void); + +static void +eeprom_erase(void) +{ + if (!attached) + eeprom_attach(); + + if (at24c_nuke()) + errx(1, "erase failed"); + + errx(0, "erase done, reboot now"); +} + +static void +eeprom_ioctl(unsigned operation) +{ + int fd; + + fd = open("/eeprom/.", 0); + + if (fd < 0) + err(1, "open /eeprom"); + + if (ioctl(fd, operation, 0) < 0) + err(1, "ioctl"); + + exit(0); +} + +static void +eeprom_save(const char *name) +{ + if (!started) + errx(1, "must be started first"); + + if (!name) + err(1, "missing argument for device name, try '/eeprom/parameters'"); + + warnx("WARNING: 'eeprom save_param' deprecated - use 'param save' instead"); + + /* delete the file in case it exists */ + unlink(name); + + /* create the file */ + int fd = open(name, O_WRONLY | O_CREAT | O_EXCL); + + if (fd < 0) + err(1, "opening '%s' failed", name); + + int result = param_export(fd, false); + close(fd); + + if (result < 0) { + unlink(name); + errx(1, "error exporting to '%s'", name); + } + + exit(0); +} + +static void +eeprom_load(const char *name) +{ + if (!started) + errx(1, "must be started first"); + + if (!name) + err(1, "missing argument for device name, try '/eeprom/parameters'"); + + warnx("WARNING: 'eeprom load_param' deprecated - use 'param load' instead"); + + int fd = open(name, O_RDONLY); + + if (fd < 0) + err(1, "open '%s'", name); + + int result = param_load(fd); + close(fd); + + if (result < 0) + errx(1, "error importing from '%s'", name); + + exit(0); +} + +extern void at24c_test(void); + +static void +eeprom_test(void) +{ + at24c_test(); + exit(0); +} diff --git a/src/systemcmds/eeprom/module.mk b/src/systemcmds/eeprom/module.mk new file mode 100644 index 000000000..3b4fc0479 --- /dev/null +++ b/src/systemcmds/eeprom/module.mk @@ -0,0 +1,6 @@ +# +# EEPROM file system driver +# + +MODULE_COMMAND = eeprom +SRCS = 24xxxx_mtd.c eeprom.c |