diff --git a/apps/ChangeLog.txt b/apps/ChangeLog.txt index e098099155..d24494f54a 100644 --- a/apps/ChangeLog.txt +++ b/apps/ChangeLog.txt @@ -336,3 +336,12 @@ can now be used to limit the server to a single thread. Option CONFIG_NETUTILS_HTTPD_TIMEOUT can be used to generate HTTP 408 errors. Both from Kate. + * apps/netutils/webserver/httpd.c: Improvements to HTTP parser from + Kate. + * apps/netutils/webserver/httpd.c: Add support for Keep-alive connections + (from Kate). + * apps/NxWidget/Kconfig: This is a kludge. I created this NxWidgets + directory that ONLY contains Kconfig. NxWidgets does not like in + either the nuttx/ or the apps/ source trees. This kludge makes it + possible to configure NxWidgets/NxWM without too much trouble (with + the tradeoff being a kind ugly structure and some maintenance issues). diff --git a/apps/Kconfig b/apps/Kconfig index 1f38c58baf..5543ba72dd 100644 --- a/apps/Kconfig +++ b/apps/Kconfig @@ -27,6 +27,10 @@ menu "NSH Library" source "$APPSDIR/nshlib/Kconfig" endmenu +menu "NxWidgets/NxWM" +source "$APPSDIR/NxWidgets/Kconfig" +endmenu + menu "System NSH Add-Ons" source "$APPSDIR/system/Kconfig" endmenu diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c index 8d77e7502e..8ed6db6642 100644 --- a/apps/ardrone_interface/ardrone_interface.c +++ b/apps/ardrone_interface/ardrone_interface.c @@ -1,7 +1,7 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier + * Author: Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -140,9 +140,7 @@ static int ardrone_open_uart(char *uart_name, struct termios *uart_config_origin int speed = B115200; int uart; - /* open uart */ - printf("[ardrone_interface] UART is %s, baud rate is%d\n",uart_name,speed); uart = open(uart_name, O_RDWR | O_NOCTTY); /* Try to set baud rate */ @@ -329,6 +327,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) */ 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); diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c index 787db18773..c68a26df96 100644 --- a/apps/ardrone_interface/ardrone_motor_control.c +++ b/apps/ardrone_interface/ardrone_motor_control.c @@ -306,7 +306,7 @@ int ar_init_motors(int ardrone_uart, int gpios) 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) @@ -368,10 +368,11 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls 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 = 512.0f; /**< 100% thrust equals a value of 512 */ - + 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 */ @@ -387,13 +388,10 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls 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); } diff --git a/apps/ardrone_interface/ardrone_motor_control.h b/apps/ardrone_interface/ardrone_motor_control.h index 664419707a..78b603b63a 100644 --- a/apps/ardrone_interface/ardrone_motor_control.h +++ b/apps/ardrone_interface/ardrone_motor_control.h @@ -87,4 +87,7 @@ int ar_init_motors(int ardrone_uart, int gpio); */ 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/apps/attitude_estimator_ekf/Makefile b/apps/attitude_estimator_ekf/Makefile index cad20d3753..932b49f5c8 100644 --- a/apps/attitude_estimator_ekf/Makefile +++ b/apps/attitude_estimator_ekf/Makefile @@ -33,7 +33,7 @@ APPNAME = attitude_estimator_ekf PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 20000 +STACKSIZE = 2048 CSRCS = attitude_estimator_ekf_main.c \ codegen/eye.c \ @@ -44,7 +44,10 @@ CSRCS = attitude_estimator_ekf_main.c \ codegen/rt_nonfinite.c \ codegen/rtGetInf.c \ codegen/rtGetNaN.c \ - codegen/norm.c + codegen/norm.c \ + codegen/find.c \ + codegen/diag.c \ + codegen/cross.c # XXX this is *horribly* broken INCLUDES += $(TOPDIR)/../mavlink/include/mavlink diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 4b3733be32..867b484e1d 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2012 PX4 Development Team. All rights reserved. * Author: Tobias Naegeli * Lorenz Meier * @@ -35,6 +35,7 @@ /* * @file attitude_estimator_ekf_main.c + * * Extended Kalman Filter for Attitude Estimation. */ @@ -54,6 +55,7 @@ #include #include #include +#include #include #include #include @@ -69,34 +71,106 @@ __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]); // #define PROJECTION_INITIALIZE_COUNTER_LIMIT 5000 // #define REPROJECTION_COUNTER_LIMIT 125 -static unsigned int loop_interval_alarm = 4500; // loop interval in microseconds +static unsigned int loop_interval_alarm = 6500; // loop interval in microseconds static float dt = 1; /* 0, 0, -9.81, 1, 1, 1, wo (gyro offset), w */ /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ static float z_k[9] = {0}; /**< Measurement vector */ -static float x_aposteriori[12] = {0}; /**< */ -static float P_aposteriori[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, 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 +static float x_aposteriori_k[9] = {0,0,0,0,0,-9.81,1,1,-1}; /**< */ +static float x_aposteriori[9] = {0}; +static float P_aposteriori_k[81] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 100.f, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 100.f, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 100.f, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 100.f, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 100.f, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 100.f, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 100.f, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 100.f, + }; +static float P_aposteriori[81] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 100.f, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 100.f, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 100.f, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 100.f, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 100.f, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 100.f, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 100.f, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 100.f, }; /**< init: diagonal matrix with big values */ -static float knownConst[7] = {1, 1, 1, 1, 1, 1, 1}; /**< knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */ +static float knownConst[15] = {1, 1, 1, 1, 1, 0.04, 4, 0.1, 70, 70, -2000, 9.81, 1, 4, 1}; /**< knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */ static float Rot_matrix[9] = {1.f, 0, 0, 0, 1.f, 0, 0, 0, 1.f }; /**< init: identity matrix */ -// static float x_aposteriori_k[12] = {0}; -// static float P_aposteriori_k[144] = {0}; +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 ]\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_create("attitude_estimator_ekf", SCHED_PRIORITY_DEFAULT, 20000, 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) @@ -110,7 +184,7 @@ static float Rot_matrix[9] = {1.f, 0, 0, * @param argc number of commandline arguments (plus command name) * @param argv strings containing the arguments */ -int attitude_estimator_ekf_main(int argc, char *argv[]) +int attitude_estimator_ekf_thread_main(int argc, char *argv[]) { // print text printf("Extended Kalman Filter Attitude Estimator initialized..\n\n"); @@ -124,14 +198,17 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) /* store start time to guard against too slow update rates */ uint64_t last_run = hrt_absolute_time(); - struct sensor_combined_s raw = {0}; - struct vehicle_attitude_s att = {}; + struct sensor_combined_s raw; + struct vehicle_attitude_s att; 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, 5); + /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); @@ -139,20 +216,26 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) 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 = orb_advertise(ORB_ID(debug_key_value), &dbg); + /* Main loop*/ - while (true) { + while (!thread_should_exit) { struct pollfd fds[1] = { { .fd = sub_raw, .events = POLLIN }, }; int ret = poll(fds, 1, 1000); - /* check for a timeout */ - if (ret == 0) { - /* */ - - /* update successful, copy data on every 2nd run and execute filter */ - } else if (loopcounter & 0x01) { + if (ret < 0) { + /* XXX this is seriously bad - should be an emergency */ + } else if (ret == 0) { + /* XXX this means no sensor data - should be critical or emergency */ + printf("[attitude estimator ekf] WARNING: Not getting sensor data - sensor app running?\n"); + } else { orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); @@ -160,24 +243,19 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) dt = (raw.timestamp - last_measurement) / 1000000.0f; last_measurement = raw.timestamp; - // XXX Read out accel range via SPI on init, assuming 4G range at 14 bit res here - float range_g = 4.0f; - float mag_offset[3] = {0}; - /* scale from 14 bit to m/s2 */ - z_k[3] = ((raw.accelerometer_raw[0] * range_g) / 8192.0f) / 9.81f; // = accel * (1 / 32768.0f / 8.0f * 9.81f); - z_k[4] = ((raw.accelerometer_raw[1] * range_g) / 8192.0f) / 9.81f; // = accel * (1 / 32768.0f / 8.0f * 9.81f); - z_k[5] = ((raw.accelerometer_raw[2] * range_g) / 8192.0f) / 9.81f; // = accel * (1 / 32768.0f / 8.0f * 9.81f); - - // XXX Read out mag range via I2C on init, assuming 0.88 Ga and 12 bit res here - z_k[0] = (raw.magnetometer_raw[0] - mag_offset[0]) * 0.01f; - z_k[1] = (raw.magnetometer_raw[1] - mag_offset[1]) * 0.01f; - z_k[2] = (raw.magnetometer_raw[2] - mag_offset[2]) * 0.01f; - /* Fill in gyro measurements */ - z_k[6] = raw.gyro_raw[0] * 0.00026631611f /* = gyro * (500.0f / 180.0f * pi / 32768.0f ) */; - z_k[7] = raw.gyro_raw[1] * 0.00026631611f /* = gyro * (500.0f / 180.0f * pi / 32768.0f ) */; - z_k[8] = raw.gyro_raw[2] * 0.00026631611f /* = gyro * (500.0f / 180.0f * pi / 32768.0f ) */; + z_k[0] = raw.gyro_rad_s[0]; + z_k[1] = raw.gyro_rad_s[1]; + z_k[2] = raw.gyro_rad_s[2]; + /* scale from 14 bit to m/s2 */ + z_k[3] = raw.accelerometer_m_s2[0]; + z_k[4] = raw.accelerometer_m_s2[1]; + z_k[5] = raw.accelerometer_m_s2[2]; + + 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; @@ -186,60 +264,97 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) if (time_elapsed > loop_interval_alarm) { //TODO: add warning, cpu overload here if (overloadcounter == 20) { - printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR BLACK MAGIC (%lu > %lu)\n", time_elapsed, loop_interval_alarm); + printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm); overloadcounter = 0; } overloadcounter++; } -// now = hrt_absolute_time(); - /* filter values */ - /* - * function [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) - */ + int8_t update_vect[9] = {1, 1, 1, 1, 1, 1, 1, 1, 1}; + float euler[3]; + int32_t z_k_sizes = 9; + float u[4] = {0.0f, 0.0f, 0.0f, 0.0f}; + + static bool const_initialized = false; + + /* initialize with good values once we have a reasonable dt estimate */ + if (!const_initialized /*&& dt < 0.05 && dt > 0.005*/) + { + dt = 0.005f; + knownConst[0] = 0.6f*0.6f*dt; + knownConst[1] = 0.6f*0.6f*dt; + knownConst[2] = 0.2f*0.2f*dt; + knownConst[3] = 0.2f*0.2f*dt; + knownConst[4] = 0.000001f*0.000001f*dt; // -9.81,1,1,-1}; + + 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] = z_k[3]; + x_aposteriori_k[4] = z_k[4]; + x_aposteriori_k[5] = z_k[5]; + x_aposteriori_k[6] = z_k[6]; + x_aposteriori_k[7] = z_k[7]; + x_aposteriori_k[8] = 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(dt, z_k, x_aposteriori, P_aposteriori, knownConst, Rot_matrix, x_aposteriori, P_aposteriori); + attitudeKalmanfilter(dt, update_vect, z_k, &z_k_sizes, u, x_aposteriori_k, P_aposteriori_k, knownConst, euler, + Rot_matrix, x_aposteriori, P_aposteriori); + /* swap values for next iteration */ + memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k)); + memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k)); uint64_t timing_diff = hrt_absolute_time() - timing_start; /* print rotation matrix every 200th time */ - if (printcounter % 200 == 0) { - printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt)); - printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100), - (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100), - (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100)); + if (printcounter % 2 == 0) { + // printf("x apo:\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n", + // x_aposteriori[0], x_aposteriori[1], x_aposteriori[2], + // x_aposteriori[3], x_aposteriori[4], x_aposteriori[5], + // x_aposteriori[6], x_aposteriori[7], x_aposteriori[8]); + + + // } + + // printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt)); + // printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f", (double)euler[0], (double)euler[1], (double)euler[2]); + // printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100), + // (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100), + // (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100)); } + int i = printcounter % 9; + + // for (int i = 0; i < 9; i++) { + char name[10]; + sprintf(name, "xapo #%d", i); + memcpy(dbg.key, name, sizeof(dbg.key)); + dbg.value = x_aposteriori[i]; + orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); + printcounter++; -// time_elapsed = hrt_absolute_time() - now; -// if (blubb == 20) -// { -// printf("Estimator: %lu\n", time_elapsed); -// blubb = 0; -// } -// blubb++; + if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); - if (last_data > 0 && raw.timestamp - last_data > 8000) printf("sensor data missed! (%llu)\n", raw.timestamp - last_data); - -// printf("%llu -> %llu = %llu\n", last_data, raw.timestamp, raw.timestamp - last_data); last_data = raw.timestamp; /* send out */ att.timestamp = raw.timestamp; - // att.roll = euler.x; - // att.pitch = euler.y; - // att.yaw = euler.z + F_M_PI; + att.roll = euler[0]; + att.pitch = euler[1]; + att.yaw = euler[2]; - // if (att.yaw > 2 * F_M_PI) { - // att.yaw -= 2 * F_M_PI; - // } - - // att.rollspeed = rates.x; - // att.pitchspeed = rates.y; - // att.yawspeed = rates.z; - - // memcpy()R + att.rollspeed = x_aposteriori[0]; + att.pitchspeed = x_aposteriori[1]; + att.yawspeed = x_aposteriori[2]; // Broadcast orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); @@ -248,11 +363,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) loopcounter++; } - /* Should never reach here */ + thread_running = false; + return 0; } - - - - - diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c index 4e275e3ee9..459dcc9b9d 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Fri Sep 21 13:56:42 2012 * */ @@ -11,8 +11,11 @@ #include "rt_nonfinite.h" #include "attitudeKalmanfilter.h" #include "norm.h" +#include "cross.h" #include "eye.h" #include "mrdivide.h" +#include "diag.h" +#include "find.h" /* Type Definitions */ @@ -23,613 +26,691 @@ /* 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; + if (rtIsNaNF(u0) || rtIsNaNF(u1)) { + y = ((real32_T)rtNaN); + } else if (rtIsInfF(u0) && rtIsInfF(u1)) { + y = (real32_T)atan2f(u0 > 0.0F ? 1.0F : -1.0F, u1 > 0.0F ? 1.0F : -1.0F); + } 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)atan2f(u0, u1); + } + + return y; +} /* - * function [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) + * function [eulerAngles,Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,updVect,z_k,u,x_aposteriori_k,P_aposteriori_k,knownConst) */ -void attitudeKalmanfilter(real32_T dt, const real32_T z_k[9], const real32_T - x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T - knownConst[7], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T - P_aposteriori[144]) +void attitudeKalmanfilter(real32_T dt, const int8_T updVect[9], const real32_T + z_k_data[9], const int32_T z_k_sizes[1], const real32_T u[4], const real32_T + x_aposteriori_k[9], const real32_T P_aposteriori_k[81], const real32_T + knownConst[20], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T + x_aposteriori[9], real32_T P_aposteriori[81]) { + int32_T udpIndVect_sizes; + real_T udpIndVect_data[9]; real32_T R_temp[9]; real_T dv0[9]; - real_T dv1[9]; - int32_T i; + int32_T ib; int32_T i0; - real32_T A_pred[144]; - real32_T x_apriori[12]; - real32_T b_A_pred[144]; + real32_T fv0[81]; + real32_T b_knownConst[27]; + real32_T fv1[9]; + real32_T c_knownConst[9]; + real_T dv1[9]; + real_T dv2[9]; + real32_T A_lin[81]; + real32_T x_n_b[3]; + real32_T K_k_data[81]; int32_T i1; - real32_T c_A_pred[144]; + real32_T b_A_lin[81]; static const int8_T iv0[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; - 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, 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, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1 }; - - real32_T K_k[108]; - 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, 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, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, + real32_T P_apriori[81]; + int32_T ia; + static const int8_T H_k_full[81] = { 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, 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 fv0[81]; - real32_T fv1[81]; - real32_T fv2[81]; - real32_T B; - real_T dv2[144]; - real32_T b_B; - real32_T earth_z[3]; - real32_T y[3]; - real32_T earth_x[3]; + int8_T H_k_data[81]; + int32_T accUpt; + int32_T magUpt; + real32_T y; + real32_T y_k_data[9]; + int32_T P_apriori_sizes[2]; + int32_T H_k_sizes[2]; + int32_T K_k_sizes[2]; + real32_T b_y[9]; + real_T dv3[81]; + real32_T c_y; + real32_T z_n_b[3]; + real32_T y_n_b[3]; /* 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.... */ + /* Example.... */ /* */ /* $Author: Tobias Naegeli $ $Date: 2012 $ $Revision: 1 $ */ /* %define the matrices */ - /* 'attitudeKalmanfilter:19' acc_ProcessNoise=knownConst(1); */ - /* 'attitudeKalmanfilter:20' mag_ProcessNoise=knownConst(2); */ - /* 'attitudeKalmanfilter:21' ratesOffset_ProcessNoise=knownConst(3); */ - /* 'attitudeKalmanfilter:22' rates_ProcessNoise=knownConst(4); */ - /* 'attitudeKalmanfilter:25' acc_MeasurementNoise=knownConst(5); */ - /* 'attitudeKalmanfilter:26' mag_MeasurementNoise=knownConst(6); */ - /* 'attitudeKalmanfilter:27' rates_MeasurementNoise=knownConst(7); */ + /* tpred=0.005; */ + /* */ + /* A=[ 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, 1, 0, 0, 0, 0, 0, 0; */ + /* 1, 0, -1, 0, 0, 0, 0, 0, 0; */ + /* -1, 1, 0, 0, 0, 0, 0, 0, 0; */ + /* 0, -1, 1, 0, 0, 0, 0, 0, 0; */ + /* 1, 0, -1, 0, 0, 0, 0, 0, 0; */ + /* -1, 1, 0, 0, 0, 0, 0, 0, 0]; */ + /* */ + /* */ + /* b=[70, 0, 0; */ + /* 0, 70, 0; */ + /* 0, 0, 0; */ + /* 0, 0, 0; */ + /* 0, 0, 0; */ + /* 0, 0, 0; */ + /* 0, 0, 0; */ + /* 0, 0, 0; */ + /* 0, 0, 0]; */ + /* */ + /* */ + /* C=[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, 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]; */ + /* D=[]; */ + /* */ + /* */ + /* sys=ss(A,b,C,D); */ + /* */ + /* sysd=c2d(sys,tpred); */ + /* */ + /* */ + /* F=sysd.a; */ + /* */ + /* B=sysd.b; */ + /* */ + /* H=sysd.c; */ + /* 'attitudeKalmanfilter:68' udpIndVect=find(updVect); */ + find(updVect, udpIndVect_data, &udpIndVect_sizes); + + /* 'attitudeKalmanfilter:71' rates_ProcessNoiseMatrix=diag([knownConst(1),knownConst(1),knownConst(2)]); */ + /* 'attitudeKalmanfilter:72' acc_ProcessNoise=knownConst(3); */ + /* 'attitudeKalmanfilter:73' mag_ProcessNoise=knownConst(4); */ + /* 'attitudeKalmanfilter:74' rates_MeasurementNoise=knownConst(6); */ + /* 'attitudeKalmanfilter:75' acc_MeasurementNoise=knownConst(7); */ + /* 'attitudeKalmanfilter:76' mag_MeasurementNoise=knownConst(8); */ /* process noise covariance matrix */ - /* 'attitudeKalmanfilter:30' Q = [ eye(3)*acc_ProcessNoise, zeros(3), zeros(3), zeros(3); */ - /* 'attitudeKalmanfilter:31' zeros(3), eye(3)*mag_ProcessNoise, zeros(3), zeros(3); */ - /* 'attitudeKalmanfilter:32' zeros(3), zeros(3), eye(3)*ratesOffset_ProcessNoise, zeros(3); */ - /* 'attitudeKalmanfilter:33' zeros(3), zeros(3), zeros(3), eye(3)*rates_ProcessNoise]; */ - /* measurement noise covariance matrix */ - /* 'attitudeKalmanfilter:36' R = [ eye(3)*acc_MeasurementNoise, zeros(3), zeros(3); */ - /* 'attitudeKalmanfilter:37' zeros(3), eye(3)*mag_MeasurementNoise, zeros(3); */ - /* 'attitudeKalmanfilter:38' zeros(3), zeros(3), eye(3)*rates_MeasurementNoise]; */ + /* 'attitudeKalmanfilter:81' Q = [ rates_ProcessNoiseMatrix, zeros(3), zeros(3); */ + /* 'attitudeKalmanfilter:82' zeros(3), eye(3)*mag_ProcessNoise, zeros(3); */ + /* 'attitudeKalmanfilter:83' zeros(3), zeros(3), eye(3)*acc_ProcessNoise]; */ /* observation matrix */ - /* 'attitudeKalmanfilter:42' H_k=[ eye(3), zeros(3), zeros(3), zeros(3); */ - /* 'attitudeKalmanfilter:43' zeros(3), eye(3), zeros(3), zeros(3); */ - /* 'attitudeKalmanfilter:44' zeros(3), zeros(3), eye(3), eye(3)]; */ + /* 'attitudeKalmanfilter:89' H_k_full=[ eye(3), zeros(3), zeros(3); */ + /* 'attitudeKalmanfilter:90' zeros(3), eye(3), zeros(3); */ + /* 'attitudeKalmanfilter:91' zeros(3), zeros(3), eye(3)]; */ /* compute A(t,w) */ /* x_aposteriori_k[10,11,12] should be [p,q,r] */ /* R_temp=[1,-r, q */ /* r, 1, -p */ /* -q, p, 1] */ - /* 'attitudeKalmanfilter:53' R_temp=[1,-dt*x_aposteriori_k(12),dt*x_aposteriori_k(11); */ - /* 'attitudeKalmanfilter:54' dt*x_aposteriori_k(12),1,-dt*x_aposteriori_k(10); */ - /* 'attitudeKalmanfilter:55' -dt*x_aposteriori_k(11), dt*x_aposteriori_k(10),1]; */ + /* 'attitudeKalmanfilter:100' R_temp=[1, -dt*x_aposteriori_k(3), dt*x_aposteriori_k(2); */ + /* 'attitudeKalmanfilter:101' dt*x_aposteriori_k(3), 1, -dt*x_aposteriori_k(1); */ + /* 'attitudeKalmanfilter:102' -dt*x_aposteriori_k(2), dt*x_aposteriori_k(1), 1]; */ R_temp[0] = 1.0F; - R_temp[3] = -dt * x_aposteriori_k[11]; - R_temp[6] = dt * x_aposteriori_k[10]; - R_temp[1] = dt * x_aposteriori_k[11]; + R_temp[3] = -dt * x_aposteriori_k[2]; + R_temp[6] = dt * x_aposteriori_k[1]; + R_temp[1] = dt * x_aposteriori_k[2]; R_temp[4] = 1.0F; - R_temp[7] = -dt * x_aposteriori_k[9]; - R_temp[2] = -dt * x_aposteriori_k[10]; - R_temp[5] = dt * x_aposteriori_k[9]; + R_temp[7] = -dt * x_aposteriori_k[0]; + R_temp[2] = -dt * x_aposteriori_k[1]; + R_temp[5] = dt * x_aposteriori_k[0]; R_temp[8] = 1.0F; - /* strange, should not be transposed */ - /* 'attitudeKalmanfilter:58' A_pred=[R_temp', zeros(3), zeros(3), zeros(3); */ - /* 'attitudeKalmanfilter:59' zeros(3), R_temp', zeros(3), zeros(3); */ - /* 'attitudeKalmanfilter:60' zeros(3), zeros(3), eye(3), zeros(3); */ - /* 'attitudeKalmanfilter:61' zeros(3), zeros(3), zeros(3), eye(3)]; */ - eye(dv0); - eye(dv1); - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[i0 + 12 * i] = R_temp[i + 3 * i0]; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[i0 + 12 * (i + 3)] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[i0 + 12 * (i + 6)] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[i0 + 12 * (i + 9)] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * i) + 3] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 3)) + 3] = R_temp[i + 3 * i0]; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 6)) + 3] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 9)) + 3] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * i) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 3)) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 6)) + 6] = (real32_T)dv0[i0 + 3 * i]; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 9)) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * i) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 3)) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 6)) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 9)) + 9] = (real32_T)dv1[i0 + 3 * i]; - } - } - + /* 'attitudeKalmanfilter:106' A_pred=[eye(3), zeros(3), zeros(3); */ + /* 'attitudeKalmanfilter:107' zeros(3), R_temp', zeros(3); */ + /* 'attitudeKalmanfilter:108' zeros(3), zeros(3), R_temp']; */ + /* 'attitudeKalmanfilter:110' B_pred=[knownConst(9)*dt, 0, 0; */ + /* 'attitudeKalmanfilter:111' 0, knownConst(10)*dt, 0; */ + /* 'attitudeKalmanfilter:112' 0, 0, 0; */ + /* 'attitudeKalmanfilter:113' 0, 0, 0; */ + /* 'attitudeKalmanfilter:114' 0, 0, 0; */ + /* 'attitudeKalmanfilter:115' 0, 0, 0; */ + /* 'attitudeKalmanfilter:116' 0, 0, 0; */ + /* 'attitudeKalmanfilter:117' 0, 0, 0; */ + /* 'attitudeKalmanfilter:118' 0, 0, 0]; */ /* %prediction step */ - /* 'attitudeKalmanfilter:64' x_apriori=A_pred*x_aposteriori_k; */ - for (i = 0; i < 12; i++) { - x_apriori[i] = 0.0F; - for (i0 = 0; i0 < 12; i0++) { - x_apriori[i] += A_pred[i + 12 * i0] * x_aposteriori_k[i0]; + /* 'attitudeKalmanfilter:121' x_apriori=A_pred*x_aposteriori_k+B_pred*u(1:3); */ + eye(dv0); + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[i0 + 9 * ib] = (real32_T)dv0[i0 + 3 * ib]; } } + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[i0 + 9 * (ib + 3)] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[i0 + 9 * (ib + 6)] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * ib) + 3] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * (ib + 3)) + 3] = R_temp[ib + 3 * i0]; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * (ib + 6)) + 3] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * ib) + 6] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * (ib + 3)) + 6] = 0.0F; + } + } + + b_knownConst[0] = knownConst[8] * dt; + b_knownConst[9] = 0.0F; + b_knownConst[18] = 0.0F; + b_knownConst[1] = 0.0F; + b_knownConst[10] = knownConst[9] * dt; + b_knownConst[19] = 0.0F; + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * (ib + 6)) + 6] = R_temp[ib + 3 * i0]; + } + + b_knownConst[2 + 9 * ib] = 0.0F; + b_knownConst[3 + 9 * ib] = 0.0F; + b_knownConst[4 + 9 * ib] = 0.0F; + b_knownConst[5 + 9 * ib] = 0.0F; + b_knownConst[6 + 9 * ib] = 0.0F; + b_knownConst[7 + 9 * ib] = 0.0F; + b_knownConst[8 + 9 * ib] = 0.0F; + } + + for (ib = 0; ib < 9; ib++) { + fv1[ib] = 0.0F; + for (i0 = 0; i0 < 9; i0++) { + fv1[ib] += fv0[ib + 9 * i0] * x_aposteriori_k[i0]; + } + + c_knownConst[ib] = 0.0F; + for (i0 = 0; i0 < 3; i0++) { + c_knownConst[ib] += b_knownConst[ib + 9 * i0] * u[i0]; + } + + x_aposteriori[ib] = fv1[ib] + c_knownConst[ib]; + } + /* linearization */ - /* 'attitudeKalmanfilter:67' acc_temp_mat=[0, dt*x_aposteriori_k(3), -dt*x_aposteriori_k(2); */ - /* 'attitudeKalmanfilter:68' -dt*x_aposteriori_k(3), 0, dt*x_aposteriori_k(1); */ - /* 'attitudeKalmanfilter:69' dt*x_aposteriori_k(2), -dt*x_aposteriori_k(1), 0]; */ - /* 'attitudeKalmanfilter:71' mag_temp_mat=[0, dt*x_aposteriori_k(6), -dt*x_aposteriori_k(5); */ - /* 'attitudeKalmanfilter:72' -dt*x_aposteriori_k(6), 0, dt*x_aposteriori_k(4); */ - /* 'attitudeKalmanfilter:73' dt*x_aposteriori_k(5), -dt*x_aposteriori_k(4), 0]; */ - /* 'attitudeKalmanfilter:75' A_lin=[R_temp', zeros(3), zeros(3), acc_temp_mat'; */ - /* 'attitudeKalmanfilter:76' zeros(3), R_temp', zeros(3), mag_temp_mat'; */ - /* 'attitudeKalmanfilter:77' zeros(3), zeros(3), eye(3), zeros(3); */ - /* 'attitudeKalmanfilter:78' zeros(3), zeros(3), zeros(3), eye(3)]; */ + /* 'attitudeKalmanfilter:125' temp_mat=[ 0, -dt, dt; */ + /* 'attitudeKalmanfilter:126' dt, 0, -dt; */ + /* 'attitudeKalmanfilter:127' -dt, dt, 0]; */ + R_temp[0] = 0.0F; + R_temp[3] = -dt; + R_temp[6] = dt; + R_temp[1] = dt; + R_temp[4] = 0.0F; + R_temp[7] = -dt; + R_temp[2] = -dt; + R_temp[5] = dt; + R_temp[8] = 0.0F; + + /* 'attitudeKalmanfilter:129' A_lin=[ eye(3), zeros(3), zeros(3); */ + /* 'attitudeKalmanfilter:130' temp_mat, eye(3) , zeros(3); */ + /* 'attitudeKalmanfilter:131' temp_mat, zeros(3), eye(3)]; */ eye(dv0); eye(dv1); - for (i = 0; i < 3; i++) { + eye(dv2); + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[i0 + 12 * i] = R_temp[i + 3 * i0]; + A_lin[i0 + 9 * ib] = (real32_T)dv0[i0 + 3 * ib]; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[i0 + 12 * (i + 3)] = 0.0F; + A_lin[i0 + 9 * (ib + 3)] = 0.0F; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[i0 + 12 * (i + 6)] = 0.0F; + A_lin[i0 + 9 * (ib + 6)] = 0.0F; } } - A_pred[108] = 0.0F; - A_pred[109] = dt * x_aposteriori_k[2]; - A_pred[110] = -dt * x_aposteriori_k[1]; - A_pred[120] = -dt * x_aposteriori_k[2]; - A_pred[121] = 0.0F; - A_pred[122] = dt * x_aposteriori_k[0]; - A_pred[132] = dt * x_aposteriori_k[1]; - A_pred[133] = -dt * x_aposteriori_k[0]; - A_pred[134] = 0.0F; - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * i) + 3] = 0.0F; + A_lin[(i0 + 9 * ib) + 3] = R_temp[i0 + 3 * ib]; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 3)) + 3] = R_temp[i + 3 * i0]; + A_lin[(i0 + 9 * (ib + 3)) + 3] = (real32_T)dv1[i0 + 3 * ib]; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 6)) + 3] = 0.0F; + A_lin[(i0 + 9 * (ib + 6)) + 3] = 0.0F; } } - A_pred[111] = 0.0F; - A_pred[112] = dt * x_aposteriori_k[5]; - A_pred[113] = -dt * x_aposteriori_k[4]; - A_pred[123] = -dt * x_aposteriori_k[5]; - A_pred[124] = 0.0F; - A_pred[125] = dt * x_aposteriori_k[3]; - A_pred[135] = dt * x_aposteriori_k[4]; - A_pred[136] = -dt * x_aposteriori_k[3]; - A_pred[137] = 0.0F; - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * i) + 6] = 0.0F; + A_lin[(i0 + 9 * ib) + 6] = R_temp[i0 + 3 * ib]; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 3)) + 6] = 0.0F; + A_lin[(i0 + 9 * (ib + 3)) + 6] = 0.0F; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 6)) + 6] = (real32_T)dv0[i0 + 3 * i]; + A_lin[(i0 + 9 * (ib + 6)) + 6] = (real32_T)dv2[i0 + 3 * ib]; } } - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 9)) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * i) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 3)) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 6)) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 9)) + 9] = (real32_T)dv1[i0 + 3 * i]; - } - } - - /* 'attitudeKalmanfilter:81' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */ - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - b_A_pred[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - b_A_pred[i + 12 * i0] += A_pred[i + 12 * i1] * P_aposteriori_k[i1 + 12 * + /* 'attitudeKalmanfilter:134' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */ + x_n_b[0] = knownConst[0]; + x_n_b[1] = knownConst[0]; + x_n_b[2] = knownConst[1]; + diag(x_n_b, R_temp); + for (ib = 0; ib < 9; ib++) { + for (i0 = 0; i0 < 9; i0++) { + K_k_data[ib + 9 * i0] = 0.0F; + for (i1 = 0; i1 < 9; i1++) { + K_k_data[ib + 9 * i0] += A_lin[ib + 9 * i1] * P_aposteriori_k[i1 + 9 * i0]; } } - for (i0 = 0; i0 < 12; i0++) { - c_A_pred[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - c_A_pred[i + 12 * i0] += b_A_pred[i + 12 * i1] * A_pred[i0 + 12 * i1]; + for (i0 = 0; i0 < 9; i0++) { + b_A_lin[ib + 9 * i0] = 0.0F; + for (i1 = 0; i1 < 9; i1++) { + b_A_lin[ib + 9 * i0] += K_k_data[ib + 9 * i1] * A_lin[i0 + 9 * i1]; } } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - b_A_pred[i0 + 12 * i] = (real32_T)iv0[i0 + 3 * i] * knownConst[0]; + K_k_data[i0 + 9 * ib] = R_temp[i0 + 3 * ib]; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - b_A_pred[i0 + 12 * (i + 3)] = 0.0F; + K_k_data[i0 + 9 * (ib + 3)] = 0.0F; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - b_A_pred[i0 + 12 * (i + 6)] = 0.0F; + K_k_data[i0 + 9 * (ib + 6)] = 0.0F; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - b_A_pred[i0 + 12 * (i + 9)] = 0.0F; + K_k_data[(i0 + 9 * ib) + 3] = 0.0F; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * i) + 3] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 3)) + 3] = (real32_T)iv0[i0 + 3 * i] * - knownConst[1]; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 6)) + 3] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 9)) + 3] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * i) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 3)) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 6)) + 6] = (real32_T)iv0[i0 + 3 * i] * - knownConst[2]; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 9)) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * i) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 3)) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 6)) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 9)) + 9] = (real32_T)iv0[i0 + 3 * i] * + K_k_data[(i0 + 9 * (ib + 3)) + 3] = (real32_T)iv0[i0 + 3 * ib] * knownConst[3]; } } - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - P_apriori[i0 + 12 * i] = c_A_pred[i0 + 12 * i] + b_A_pred[i0 + 12 * i]; + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + K_k_data[(i0 + 9 * (ib + 6)) + 3] = 0.0F; } } - /* %update step */ - /* 'attitudeKalmanfilter:86' y_k=z_k-H_k*x_apriori; */ - /* 'attitudeKalmanfilter:87' S_k=H_k*P_apriori*H_k'+R; */ - /* 'attitudeKalmanfilter:88' K_k=(P_apriori*H_k'/(S_k)); */ - for (i = 0; i < 12; i++) { + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + K_k_data[(i0 + 9 * ib) + 6] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + K_k_data[(i0 + 9 * (ib + 3)) + 6] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + K_k_data[(i0 + 9 * (ib + 6)) + 6] = (real32_T)iv0[i0 + 3 * ib] * + knownConst[2]; + } + } + + for (ib = 0; ib < 9; ib++) { 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]; - } + P_apriori[i0 + 9 * ib] = b_A_lin[i0 + 9 * ib] + K_k_data[i0 + 9 * ib]; } } - 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]; + /* 'attitudeKalmanfilter:137' if ~isempty(udpIndVect)==1 */ + if (!(udpIndVect_sizes == 0) == 1) { + /* 'attitudeKalmanfilter:138' H_k= H_k_full(udpIndVect,:); */ + for (ib = 0; ib < 9; ib++) { + ia = udpIndVect_sizes - 1; + for (i0 = 0; i0 <= ia; i0++) { + H_k_data[i0 + udpIndVect_sizes * ib] = H_k_full[((int32_T) + udpIndVect_data[i0] + 9 * ib) - 1]; } } - 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]; - } - } - } + /* %update step */ + /* 'attitudeKalmanfilter:140' accUpt=1; */ + accUpt = 1; - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[i0 + 9 * i] = (real32_T)iv0[i0 + 3 * i] * knownConst[4]; - } - } + /* 'attitudeKalmanfilter:141' magUpt=1; */ + magUpt = 1; - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[i0 + 9 * (i + 3)] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[i0 + 9 * (i + 6)] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[(i0 + 9 * i) + 3] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[(i0 + 9 * (i + 3)) + 3] = (real32_T)iv0[i0 + 3 * i] * knownConst[5]; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[(i0 + 9 * (i + 6)) + 3] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[(i0 + 9 * i) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[(i0 + 9 * (i + 3)) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[(i0 + 9 * (i + 6)) + 6] = (real32_T)iv0[i0 + 3 * i] * knownConst[6]; - } - } - - for (i = 0; i < 9; i++) { - for (i0 = 0; i0 < 9; i0++) { - fv2[i0 + 9 * i] = fv0[i0 + 9 * i] + fv1[i0 + 9 * i]; - } - } - - mrdivide(b_P_apriori, fv2, K_k); - - /* 'attitudeKalmanfilter:91' x_aposteriori=x_apriori+K_k*y_k; */ - for (i = 0; i < 9; i++) { - B = 0.0F; - for (i0 = 0; i0 < 12; i0++) { - B += (real32_T)iv2[i + 9 * i0] * x_apriori[i0]; - } - - R_temp[i] = z_k[i] - B; - } - - for (i = 0; i < 12; i++) { - B = 0.0F; - for (i0 = 0; i0 < 9; i0++) { - B += K_k[i + 12 * i0] * R_temp[i0]; - } - - x_aposteriori[i] = x_apriori[i] + B; - } - - /* 'attitudeKalmanfilter:92' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */ - b_eye(dv2); - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - B = 0.0F; - for (i1 = 0; i1 < 9; i1++) { - B += K_k[i + 12 * i1] * (real32_T)iv2[i1 + 9 * i0]; + /* 'attitudeKalmanfilter:142' y_k=z_k-H_k*x_apriori; */ + ia = udpIndVect_sizes - 1; + for (ib = 0; ib <= ia; ib++) { + y = 0.0F; + for (i0 = 0; i0 < 9; i0++) { + y += (real32_T)H_k_data[ib + udpIndVect_sizes * i0] * x_aposteriori[i0]; } - A_pred[i + 12 * i0] = (real32_T)dv2[i + 12 * i0] - B; + y_k_data[ib] = z_k_data[ib] - y; } - } - 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] += A_pred[i + 12 * i1] * P_apriori[i1 + 12 * - i0]; + /* 'attitudeKalmanfilter:143' if updVect(4)==1 */ + if (updVect[3] == 1) { + /* 'attitudeKalmanfilter:144' if (abs(norm(z_k(4:6))-knownConst(12))>knownConst(14)) */ + for (ib = 0; ib < 3; ib++) { + x_n_b[ib] = z_k_data[ib + 3]; + } + + if ((real32_T)fabsf(norm(x_n_b) - knownConst[11]) > knownConst[13]) { + /* 'attitudeKalmanfilter:145' accUpt=10000; */ + accUpt = 10000; } } + + /* 'attitudeKalmanfilter:149' if updVect(7)==1 */ + if (updVect[6] == 1) { + /* 'attitudeKalmanfilter:150' if (abs(norm(z_k(7:9))-knownConst(13))>knownConst(15)) */ + for (ib = 0; ib < 3; ib++) { + x_n_b[ib] = z_k_data[ib + 6]; + } + + if ((real32_T)fabs(norm(x_n_b) - knownConst[12]) > knownConst[14]) { + /* 'attitudeKalmanfilter:152' magUpt=10000; */ + magUpt = 10000; + } + } + + /* measurement noise covariance matrix */ + /* 'attitudeKalmanfilter:157' R = [ eye(3)*rates_MeasurementNoise, zeros(3), zeros(3); */ + /* 'attitudeKalmanfilter:158' zeros(3), eye(3)*acc_MeasurementNoise*accUpt, zeros(3); */ + /* 'attitudeKalmanfilter:159' zeros(3), zeros(3), eye(3)*mag_MeasurementNoise*magUpt]; */ + /* 'attitudeKalmanfilter:161' S_k=H_k*P_apriori*H_k'+R(udpIndVect,udpIndVect); */ + /* 'attitudeKalmanfilter:162' K_k=(P_apriori*H_k'/(S_k)); */ + P_apriori_sizes[0] = 9; + P_apriori_sizes[1] = udpIndVect_sizes; + for (ib = 0; ib < 9; ib++) { + ia = udpIndVect_sizes - 1; + for (i0 = 0; i0 <= ia; i0++) { + b_A_lin[ib + 9 * i0] = 0.0F; + for (i1 = 0; i1 < 9; i1++) { + b_A_lin[ib + 9 * i0] += P_apriori[ib + 9 * i1] * (real32_T)H_k_data[i0 + + udpIndVect_sizes * i1]; + } + } + } + + ia = udpIndVect_sizes - 1; + for (ib = 0; ib <= ia; ib++) { + for (i0 = 0; i0 < 9; i0++) { + K_k_data[ib + udpIndVect_sizes * i0] = 0.0F; + for (i1 = 0; i1 < 9; i1++) { + K_k_data[ib + udpIndVect_sizes * i0] += (real32_T)H_k_data[ib + + udpIndVect_sizes * i1] * P_apriori[i1 + 9 * i0]; + } + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[i0 + 9 * ib] = (real32_T)iv0[i0 + 3 * ib] * knownConst[5]; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[i0 + 9 * (ib + 3)] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[i0 + 9 * (ib + 6)] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * ib) + 3] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * (ib + 3)) + 3] = (real32_T)iv0[i0 + 3 * ib] * knownConst[6] + * (real32_T)accUpt; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * (ib + 6)) + 3] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * ib) + 6] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * (ib + 3)) + 6] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * (ib + 6)) + 6] = (real32_T)iv0[i0 + 3 * ib] * knownConst[7] + * (real32_T)magUpt; + } + } + + H_k_sizes[0] = udpIndVect_sizes; + H_k_sizes[1] = udpIndVect_sizes; + ia = udpIndVect_sizes - 1; + for (ib = 0; ib <= ia; ib++) { + accUpt = udpIndVect_sizes - 1; + for (i0 = 0; i0 <= accUpt; i0++) { + y = 0.0F; + for (i1 = 0; i1 < 9; i1++) { + y += K_k_data[ib + udpIndVect_sizes * i1] * (real32_T)H_k_data[i0 + + udpIndVect_sizes * i1]; + } + + A_lin[ib + H_k_sizes[0] * i0] = y + fv0[((int32_T)udpIndVect_data[ib] + + 9 * ((int32_T)udpIndVect_data[i0] - 1)) - 1]; + } + } + + mrdivide(b_A_lin, P_apriori_sizes, A_lin, H_k_sizes, K_k_data, K_k_sizes); + + /* 'attitudeKalmanfilter:165' x_aposteriori=x_apriori+K_k*y_k; */ + if ((K_k_sizes[1] == 1) || (udpIndVect_sizes == 1)) { + for (ib = 0; ib < 9; ib++) { + b_y[ib] = 0.0F; + ia = udpIndVect_sizes - 1; + for (i0 = 0; i0 <= ia; i0++) { + b_y[ib] += K_k_data[ib + K_k_sizes[0] * i0] * y_k_data[i0]; + } + } + } else { + for (accUpt = 0; accUpt < 9; accUpt++) { + b_y[accUpt] = 0.0F; + } + + magUpt = -1; + for (ib = 0; ib + 1 <= K_k_sizes[1]; ib++) { + if ((real_T)y_k_data[ib] != 0.0) { + ia = magUpt; + for (accUpt = 0; accUpt < 9; accUpt++) { + ia++; + b_y[accUpt] += y_k_data[ib] * K_k_data[ia]; + } + } + + magUpt += 9; + } + } + + for (ib = 0; ib < 9; ib++) { + x_aposteriori[ib] += b_y[ib]; + } + + /* 'attitudeKalmanfilter:166' P_aposteriori=(eye(9)-K_k*H_k)*P_apriori; */ + b_eye(dv3); + for (ib = 0; ib < 9; ib++) { + for (i0 = 0; i0 < 9; i0++) { + y = 0.0F; + ia = K_k_sizes[1] - 1; + for (i1 = 0; i1 <= ia; i1++) { + y += K_k_data[ib + K_k_sizes[0] * i1] * (real32_T)H_k_data[i1 + + udpIndVect_sizes * i0]; + } + + fv0[ib + 9 * i0] = (real32_T)dv3[ib + 9 * i0] - y; + } + } + + for (ib = 0; ib < 9; ib++) { + for (i0 = 0; i0 < 9; i0++) { + P_aposteriori[ib + 9 * i0] = 0.0F; + for (i1 = 0; i1 < 9; i1++) { + P_aposteriori[ib + 9 * i0] += fv0[ib + 9 * i1] * P_apriori[i1 + 9 * i0]; + } + } + } + } else { + /* 'attitudeKalmanfilter:167' else */ + /* 'attitudeKalmanfilter:168' x_aposteriori=x_apriori; */ + /* 'attitudeKalmanfilter:169' P_aposteriori=P_apriori; */ + memcpy((void *)&P_aposteriori[0], (void *)&P_apriori[0], 81U * sizeof + (real32_T)); } - /* %Rotation matrix generation */ - /* 'attitudeKalmanfilter:97' earth_z=x_aposteriori(1:3)/norm(x_aposteriori(1:3)); */ - B = norm(*(real32_T (*)[3])&x_aposteriori[0]); + /* %% euler anglels extraction */ + /* 'attitudeKalmanfilter:175' z_n_b = -x_aposteriori(4:6)./norm(x_aposteriori(4:6)); */ + y = norm(*(real32_T (*)[3])&x_aposteriori[3]); - /* 'attitudeKalmanfilter:98' earth_x=cross(earth_z,x_aposteriori(4:6)/norm(x_aposteriori(4:6))); */ - b_B = norm(*(real32_T (*)[3])&x_aposteriori[3]); - for (i = 0; i < 3; i++) { - earth_z[i] = x_aposteriori[i] / B; - y[i] = x_aposteriori[i + 3] / b_B; + /* 'attitudeKalmanfilter:176' m_n_b = x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */ + c_y = norm(*(real32_T (*)[3])&x_aposteriori[6]); + + /* 'attitudeKalmanfilter:178' y_n_b=cross(z_n_b,m_n_b); */ + for (accUpt = 0; accUpt < 3; accUpt++) { + z_n_b[accUpt] = -x_aposteriori[accUpt + 3] / y; + x_n_b[accUpt] = x_aposteriori[accUpt + 6] / c_y; } - earth_x[0] = earth_z[1] * y[2] - earth_z[2] * y[1]; - earth_x[1] = earth_z[2] * y[0] - earth_z[0] * y[2]; - earth_x[2] = earth_z[0] * y[1] - earth_z[1] * y[0]; + cross(z_n_b, x_n_b, y_n_b); - /* 'attitudeKalmanfilter:99' earth_y=cross(earth_x,earth_z); */ - /* 'attitudeKalmanfilter:101' Rot_matrix=[earth_x,earth_y,earth_z]; */ - y[0] = earth_x[1] * earth_z[2] - earth_x[2] * earth_z[1]; - y[1] = earth_x[2] * earth_z[0] - earth_x[0] * earth_z[2]; - y[2] = earth_x[0] * earth_z[1] - earth_x[1] * earth_z[0]; - for (i = 0; i < 3; i++) { - Rot_matrix[i] = earth_x[i]; - Rot_matrix[3 + i] = y[i]; - Rot_matrix[6 + i] = earth_z[i]; + /* 'attitudeKalmanfilter:179' y_n_b=y_n_b./norm(y_n_b); */ + y = norm(y_n_b); + for (ib = 0; ib < 3; ib++) { + y_n_b[ib] /= y; } + + /* 'attitudeKalmanfilter:181' x_n_b=(cross(y_n_b,z_n_b)); */ + cross(y_n_b, z_n_b, x_n_b); + + /* 'attitudeKalmanfilter:182' x_n_b=x_n_b./norm(x_n_b); */ + y = norm(x_n_b); + for (ib = 0; ib < 3; ib++) { + /* 'attitudeKalmanfilter:188' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */ + Rot_matrix[ib] = x_n_b[ib] / y; + Rot_matrix[3 + ib] = y_n_b[ib]; + Rot_matrix[6 + ib] = z_n_b[ib]; + } + + /* 'attitudeKalmanfilter:192' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */ + /* 'attitudeKalmanfilter:193' theta=-asin(Rot_matrix(1,3)); */ + /* 'attitudeKalmanfilter:194' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */ + /* 'attitudeKalmanfilter:195' eulerAngles=[phi;theta;psi]; */ + eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]); + eulerAngles[1] = -(real32_T)asinf(Rot_matrix[6]); + eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]); } /* End of code generation (attitudeKalmanfilter.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h index 7aa3d048b7..56f02b4c81 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Fri Sep 21 13:56:43 2012 * */ @@ -14,6 +14,8 @@ #include #include #include +#include "rt_defines.h" +#include "rt_nonfinite.h" #include "rtwtypes.h" #include "attitudeKalmanfilter_types.h" @@ -27,6 +29,6 @@ /* Variable Definitions */ /* Function Declarations */ -extern void attitudeKalmanfilter(real32_T dt, const real32_T z_k[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T knownConst[7], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]); +extern void attitudeKalmanfilter(real32_T dt, const int8_T updVect[9], const real32_T z_k_data[9], const int32_T z_k_sizes[1], const real32_T u[4], const real32_T x_aposteriori_k[9], const real32_T P_aposteriori_k[81], const real32_T knownConst[20], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[9], real32_T P_aposteriori[81]); #endif /* End of code generation (attitudeKalmanfilter.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c index 2800191c32..b72256a09e 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter_initialize' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Fri Sep 21 13:56:44 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h index d2d97bb3b9..efb465b255 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter_initialize' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Fri Sep 21 13:56:44 2012 * */ @@ -14,6 +14,8 @@ #include #include #include +#include "rt_defines.h" +#include "rt_nonfinite.h" #include "rtwtypes.h" #include "attitudeKalmanfilter_types.h" diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_ref.rsp b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_ref.rsp deleted file mode 100755 index e69de29bb2..0000000000 diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.bat b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.bat deleted file mode 100755 index 76fb78ca75..0000000000 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.bat +++ /dev/null @@ -1,11 +0,0 @@ -@echo off -call "%VS100COMNTOOLS%..\..\VC\vcvarsall.bat" AMD64 - -cd . -nmake -f attitudeKalmanfilter_rtw.mk GENERATE_REPORT=1 ADD_MDL_NAME_TO_GLOBALS=0 -@if errorlevel 1 goto error_exit -exit /B 0 - -:error_exit -echo The make command returned an error of %errorlevel% -An_error_occurred_during_the_call_to_make diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.mk b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.mk deleted file mode 100755 index b2123704b1..0000000000 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.mk +++ /dev/null @@ -1,328 +0,0 @@ -# Copyright 1994-2010 The MathWorks, Inc. -# -# File : xrt_vcx64.tmf $Revision: 1.1.6.1 $ -# -# Abstract: -# Code generation template makefile for building a Windows-based, -# stand-alone real-time version of MATLAB-code using generated C/C++ -# code and the -# Microsoft Visual C/C++ compiler version 8, 9, 10 for x64 -# -# Note that this template is automatically customized by the -# code generation build procedure to create ".mk" -# -# The following defines can be used to modify the behavior of the -# build: -# -# OPT_OPTS - Optimization option. See DEFAULT_OPT_OPTS in -# vctools.mak for default. -# OPTS - User specific options. -# CPP_OPTS - C++ compiler options. -# USER_SRCS - Additional user sources, such as files needed by -# S-functions. -# USER_INCLUDES - Additional include paths -# (i.e. USER_INCLUDES="-Iwhere-ever -Iwhere-ever2") -# -# To enable debugging: -# set OPT_OPTS=-Zi (may vary with compiler version, see compiler doc) -# modify tmf LDFLAGS to include /DEBUG -# - -#------------------------ Macros read by make_rtw ----------------------------- -# -# The following macros are read by the code generation build procedure: -# -# MAKECMD - This is the command used to invoke the make utility -# HOST - What platform this template makefile is targeted for -# (i.e. PC or UNIX) -# BUILD - Invoke make from the code generation build procedure -# (yes/no)? -# SYS_TARGET_FILE - Name of system target file. - -MAKECMD = nmake -HOST = PC -BUILD = yes -SYS_TARGET_FILE = ert.tlc -BUILD_SUCCESS = ^#^#^# Created -COMPILER_TOOL_CHAIN = vcx64 - -#---------------------- Tokens expanded by make_rtw --------------------------- -# -# The following tokens, when wrapped with "|>" and "<|" are expanded by the -# code generation build procedure. -# -# MODEL_NAME - Name of the Simulink block diagram -# MODEL_MODULES - Any additional generated source modules -# MAKEFILE_NAME - Name of makefile created from template makefile .mk -# MATLAB_ROOT - Path to where MATLAB is installed. -# MATLAB_BIN - Path to MATLAB executable. -# S_FUNCTIONS - List of S-functions. -# S_FUNCTIONS_LIB - List of S-functions libraries to link. -# SOLVER - Solver source file name -# NUMST - Number of sample times -# TID01EQ - yes (1) or no (0): Are sampling rates of continuous task -# (tid=0) and 1st discrete task equal. -# NCSTATES - Number of continuous states -# BUILDARGS - Options passed in at the command line. -# MULTITASKING - yes (1) or no (0): Is solver mode multitasking -# EXT_MODE - yes (1) or no (0): Build for external mode -# TMW_EXTMODE_TESTING - yes (1) or no (0): Build ext_test.c for external mode -# testing. -# EXTMODE_TRANSPORT - Index of transport mechanism (e.g. tcpip, serial) for extmode -# EXTMODE_STATIC - yes (1) or no (0): Use static instead of dynamic mem alloc. -# EXTMODE_STATIC_SIZE - Size of static memory allocation buffer. - -MODEL = attitudeKalmanfilter -MODULES = attitudeKalmanfilter.c eye.c mrdivide.c norm.c attitudeKalmanfilter_initialize.c attitudeKalmanfilter_terminate.c rt_nonfinite.c rtGetNaN.c rtGetInf.c -MAKEFILE = attitudeKalmanfilter_rtw.mk -MATLAB_ROOT = C:\Program Files\MATLAB\R2011a -ALT_MATLAB_ROOT = C:\PROGRA~1\MATLAB\R2011a -MATLAB_BIN = C:\Program Files\MATLAB\R2011a\bin -ALT_MATLAB_BIN = C:\PROGRA~1\MATLAB\R2011a\bin -S_FUNCTIONS = -S_FUNCTIONS_LIB = -SOLVER = -NUMST = 1 -TID01EQ = 0 -NCSTATES = 0 -BUILDARGS = GENERATE_REPORT=1 ADD_MDL_NAME_TO_GLOBALS=0 -MULTITASKING = 0 -EXT_MODE = 0 -TMW_EXTMODE_TESTING = 0 -EXTMODE_TRANSPORT = 0 -EXTMODE_STATIC = 0 -EXTMODE_STATIC_SIZE = 1000000 - -MODELREFS = -SHARED_SRC = -SHARED_SRC_DIR = -SHARED_BIN_DIR = -SHARED_LIB = -TARGET_LANG_EXT = c -OPTIMIZATION_FLAGS = /O2 /Oy- -ADDITIONAL_LDFLAGS = - -#--------------------------- Model and reference models ----------------------- -MODELLIB = attitudeKalmanfilter.lib -MODELREF_LINK_LIBS = -MODELREF_LINK_RSPFILE = attitudeKalmanfilter_ref.rsp -MODELREF_INC_PATH = -RELATIVE_PATH_TO_ANCHOR = F:\CODEGE~1 -MODELREF_TARGET_TYPE = RTW - -!if "$(MATLAB_ROOT)" != "$(ALT_MATLAB_ROOT)" -MATLAB_ROOT = $(ALT_MATLAB_ROOT) -!endif -!if "$(MATLAB_BIN)" != "$(ALT_MATLAB_BIN)" -MATLAB_BIN = $(ALT_MATLAB_BIN) -!endif - -#--------------------------- Tool Specifications ------------------------------ - -CPU = AMD64 -!include $(MATLAB_ROOT)\rtw\c\tools\vctools.mak -APPVER = 5.02 - -PERL = $(MATLAB_ROOT)\sys\perl\win32\bin\perl -#------------------------------ Include/Lib Path ------------------------------ - -MATLAB_INCLUDES = $(MATLAB_ROOT)\simulink\include -MATLAB_INCLUDES = $(MATLAB_INCLUDES);$(MATLAB_ROOT)\extern\include -MATLAB_INCLUDES = $(MATLAB_INCLUDES);$(MATLAB_ROOT)\rtw\c\src -MATLAB_INCLUDES = $(MATLAB_INCLUDES);$(MATLAB_ROOT)\rtw\c\src\ext_mode\common - -# Additional file include paths - -MATLAB_INCLUDES = $(MATLAB_INCLUDES);F:\codegenerationMatlabAttFilter\codegen\lib\attitudeKalmanfilter -MATLAB_INCLUDES = $(MATLAB_INCLUDES);F:\codegenerationMatlabAttFilter - -INCLUDE = .;$(RELATIVE_PATH_TO_ANCHOR);$(MATLAB_INCLUDES);$(INCLUDE);$(MODELREF_INC_PATH) - -!if "$(SHARED_SRC_DIR)" != "" -INCLUDE = $(INCLUDE);$(SHARED_SRC_DIR) -!endif - -#------------------------ External mode --------------------------------------- -# Uncomment -DVERBOSE to have information printed to stdout -# To add a new transport layer, see the comments in -# /toolbox/simulink/simulink/extmode_transports.m -!if $(EXT_MODE) == 1 -EXT_CC_OPTS = -DEXT_MODE # -DVERBOSE -!if $(EXTMODE_TRANSPORT) == 0 #tcpip -EXT_SRC = updown.c rtiostream_interface.c rtiostream_tcpip.c -EXT_LIB = wsock32.lib -!endif -!if $(EXTMODE_TRANSPORT) == 1 #serial_win32 -EXT_SRC = ext_svr.c updown.c ext_work.c ext_svr_serial_transport.c -EXT_SRC = $(EXT_SRC) ext_serial_pkt.c ext_serial_win32_port.c -EXT_LIB = -!endif -!if $(TMW_EXTMODE_TESTING) == 1 -EXT_SRC = $(EXT_SRC) ext_test.c -EXT_CC_OPTS = $(EXT_CC_OPTS) -DTMW_EXTMODE_TESTING -!endif -!if $(EXTMODE_STATIC) == 1 -EXT_SRC = $(EXT_SRC) mem_mgr.c -EXT_CC_OPTS = $(EXT_CC_OPTS) -DEXTMODE_STATIC -DEXTMODE_STATIC_SIZE=$(EXTMODE_STATIC_SIZE) -!endif -!else -EXT_SRC = -EXT_CC_OPTS = -EXT_LIB = -!endif - -#------------------------ rtModel ---------------------------------------------- - -RTM_CC_OPTS = -DUSE_RTMODEL - -#----------------- Compiler and Linker Options -------------------------------- - -# Optimization Options -# Set OPT_OPTS=-Zi for debugging (may depend on compiler version) -OPT_OPTS = $(DEFAULT_OPT_OPTS) - -# General User Options -OPTS = - -!if "$(OPTIMIZATION_FLAGS)" != "" -CC_OPTS = $(OPTS) $(EXT_CC_OPTS) $(RTM_CC_OPTS) $(OPTIMIZATION_FLAGS) -!else -CC_OPTS = $(OPT_OPTS) $(OPTS) $(EXT_CC_OPTS) $(RTM_CC_OPTS) -!endif - -CPP_REQ_DEFINES = -DMODEL=$(MODEL) -DRT -DNUMST=$(NUMST) \ - -DTID01EQ=$(TID01EQ) -DNCSTATES=$(NCSTATES) \ - -DMT=$(MULTITASKING) -DHAVESTDIO - -# Uncomment this line to move warning level to W4 -# cflags = $(cflags:W3=W4) -CFLAGS = $(CC_OPTS) $(CPP_REQ_DEFINES) $(USER_INCLUDES) \ - $(cflags) $(cvarsmt) /wd4996 -CPPFLAGS = $(CPP_OPTS) $(CC_OPTS) $(CPP_REQ_DEFINES) $(USER_INCLUDES) \ - $(cflags) $(cvarsmt) /wd4996 /EHsc- -LDFLAGS = $(ldebug) $(conflags) $(EXT_LIB) $(conlibs) libcpmt.lib $(ADDITIONAL_LDFLAGS) - -# libcpmt.lib is the multi-threaded, static lib version of the C++ standard lib - -#----------------------------- Source Files ----------------------------------- - - -#Standalone executable -!if "$(MODELREF_TARGET_TYPE)" == "NONE" -PRODUCT = $(RELATIVE_PATH_TO_ANCHOR)\$(MODEL).exe -REQ_SRCS = $(MODULES) $(EXT_SRC) - -#Model Reference Target -!else -PRODUCT = $(MODELLIB) -REQ_SRCS = $(MODULES) $(EXT_SRC) -!endif - -USER_SRCS = - -SRCS = $(REQ_SRCS) $(USER_SRCS) $(S_FUNCTIONS) -OBJS_CPP_UPPER = $(SRCS:.CPP=.obj) -OBJS_CPP_LOWER = $(OBJS_CPP_UPPER:.cpp=.obj) -OBJS_C_UPPER = $(OBJS_CPP_LOWER:.C=.obj) -OBJS = $(OBJS_C_UPPER:.c=.obj) -SHARED_OBJS = $(SHARED_SRC:.c=.obj) -# ------------------------- Additional Libraries ------------------------------ - -LIBS = - - -LIBS = $(LIBS) - -# ---------------------------- Linker Script ---------------------------------- - -CMD_FILE = $(MODEL).lnk -GEN_LNK_SCRIPT = $(MATLAB_ROOT)\rtw\c\tools\mkvc_lnk.pl - -#--------------------------------- Rules -------------------------------------- -all: set_environment_variables $(PRODUCT) - -!if "$(MODELREF_TARGET_TYPE)" == "NONE" -#--- Stand-alone model --- -$(PRODUCT) : $(OBJS) $(SHARED_LIB) $(LIBS) $(MODELREF_LINK_LIBS) - @cmd /C "echo ### Linking ..." - $(PERL) $(GEN_LNK_SCRIPT) $(CMD_FILE) $(OBJS) - $(LD) $(LDFLAGS) $(S_FUNCTIONS_LIB) $(SHARED_LIB) $(LIBS) $(MAT_LIBS) @$(CMD_FILE) @$(MODELREF_LINK_RSPFILE) -out:$@ - @del $(CMD_FILE) - @cmd /C "echo $(BUILD_SUCCESS) executable $(MODEL).exe" -!else -#--- Model reference code generation Target --- -$(PRODUCT) : $(OBJS) $(SHARED_LIB) - @cmd /C "echo ### Linking ..." - $(PERL) $(GEN_LNK_SCRIPT) $(CMD_FILE) $(OBJS) - $(LD) -lib /OUT:$(PRODUCT) @$(CMD_FILE) $(S_FUNCTIONS_LIB) - @cmd /C "echo $(BUILD_SUCCESS) static library $(MODELLIB)" -!endif - -{$(MATLAB_ROOT)\rtw\c\src}.c.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CFLAGS) $< - -{$(MATLAB_ROOT)\rtw\c\src\ext_mode\common}.c.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CFLAGS) $< - -{$(MATLAB_ROOT)\rtw\c\src\rtiostream\rtiostreamtcpip}.c.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CFLAGS) $< - -{$(MATLAB_ROOT)\rtw\c\src\ext_mode\serial}.c.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CFLAGS) $< - -{$(MATLAB_ROOT)\rtw\c\src\ext_mode\custom}.c.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CFLAGS) $< - -# Additional sources - - - - - -# Put these rule last, otherwise nmake will check toolboxes first - -{$(RELATIVE_PATH_TO_ANCHOR)}.c.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CFLAGS) $< - -{$(RELATIVE_PATH_TO_ANCHOR)}.cpp.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CPPFLAGS) $< - -.c.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CFLAGS) $< - -.cpp.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CPPFLAGS) $< - -!if "$(SHARED_LIB)" != "" -$(SHARED_LIB) : $(SHARED_SRC) - @cmd /C "echo ### Creating $@" - @$(CC) $(CFLAGS) -Fo$(SHARED_BIN_DIR)\ @<< -$? -<< - @$(LIBCMD) /nologo /out:$@ $(SHARED_OBJS) - @cmd /C "echo ### $@ Created" -!endif - - -set_environment_variables: - @set INCLUDE=$(INCLUDE) - @set LIB=$(LIB) - -# Libraries: - - - - - -#----------------------------- Dependencies ----------------------------------- - -$(OBJS) : $(MAKEFILE) rtw_proj.tmw diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c index eab8c7d6e0..d0bf625f09 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter_terminate' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Fri Sep 21 13:56:44 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h index fafd866e42..1ad84575f8 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter_terminate' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Fri Sep 21 13:56:44 2012 * */ @@ -14,6 +14,8 @@ #include #include #include +#include "rt_defines.h" +#include "rt_nonfinite.h" #include "rtwtypes.h" #include "attitudeKalmanfilter_types.h" diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h index 05f4664cd9..bd83cc168a 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Fri Sep 21 13:56:42 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/buildInfo.mat b/apps/attitude_estimator_ekf/codegen/buildInfo.mat deleted file mode 100755 index b811d00397..0000000000 Binary files a/apps/attitude_estimator_ekf/codegen/buildInfo.mat and /dev/null differ diff --git a/apps/attitude_estimator_ekf/codegen/cross.c b/apps/attitude_estimator_ekf/codegen/cross.c new file mode 100755 index 0000000000..c888694536 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/cross.c @@ -0,0 +1,37 @@ +/* + * cross.c + * + * Code generation for function 'cross' + * + * C source code generated on: Fri Sep 21 13:56:44 2012 + * + */ + +/* 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/apps/attitude_estimator_ekf/codegen/cross.h b/apps/attitude_estimator_ekf/codegen/cross.h new file mode 100755 index 0000000000..92e3a884d0 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/cross.h @@ -0,0 +1,34 @@ +/* + * cross.h + * + * Code generation for function 'cross' + * + * C source code generated on: Fri Sep 21 13:56:44 2012 + * + */ + +#ifndef __CROSS_H__ +#define __CROSS_H__ +/* Include files */ +#include +#include +#include +#include +#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/apps/attitude_estimator_ekf/codegen/diag.c b/apps/attitude_estimator_ekf/codegen/diag.c new file mode 100755 index 0000000000..522f6e2851 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/diag.c @@ -0,0 +1,42 @@ +/* + * diag.c + * + * Code generation for function 'diag' + * + * C source code generated on: Fri Sep 21 13:56:43 2012 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "attitudeKalmanfilter.h" +#include "diag.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ + +/* + * + */ +void diag(const real32_T v[3], real32_T d[9]) +{ + int32_T j; + for (j = 0; j < 9; j++) { + d[j] = 0.0F; + } + + for (j = 0; j < 3; j++) { + d[j + 3 * j] = v[j]; + } +} + +/* End of code generation (diag.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/diag.h b/apps/attitude_estimator_ekf/codegen/diag.h new file mode 100755 index 0000000000..bb92fb2424 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/diag.h @@ -0,0 +1,34 @@ +/* + * diag.h + * + * Code generation for function 'diag' + * + * C source code generated on: Fri Sep 21 13:56:43 2012 + * + */ + +#ifndef __DIAG_H__ +#define __DIAG_H__ +/* Include files */ +#include +#include +#include +#include +#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 diag(const real32_T v[3], real32_T d[9]); +#endif +/* End of code generation (diag.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/eye.c b/apps/attitude_estimator_ekf/codegen/eye.c index e4655839c4..e67071dcea 100755 --- a/apps/attitude_estimator_ekf/codegen/eye.c +++ b/apps/attitude_estimator_ekf/codegen/eye.c @@ -3,7 +3,7 @@ * * Code generation for function 'eye' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Fri Sep 21 13:56:43 2012 * */ @@ -27,12 +27,12 @@ /* * */ -void b_eye(real_T I[144]) +void b_eye(real_T I[81]) { int32_T i; - memset((void *)&I[0], 0, 144U * sizeof(real_T)); - for (i = 0; i < 12; i++) { - I[i + 12 * i] = 1.0; + memset((void *)&I[0], 0, 81U * sizeof(real_T)); + for (i = 0; i < 9; i++) { + I[i + 9 * i] = 1.0; } } diff --git a/apps/attitude_estimator_ekf/codegen/eye.h b/apps/attitude_estimator_ekf/codegen/eye.h index e8881747f6..c07a1bc970 100755 --- a/apps/attitude_estimator_ekf/codegen/eye.h +++ b/apps/attitude_estimator_ekf/codegen/eye.h @@ -3,7 +3,7 @@ * * Code generation for function 'eye' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Fri Sep 21 13:56:43 2012 * */ @@ -14,6 +14,8 @@ #include #include #include +#include "rt_defines.h" +#include "rt_nonfinite.h" #include "rtwtypes.h" #include "attitudeKalmanfilter_types.h" @@ -27,7 +29,7 @@ /* Variable Definitions */ /* Function Declarations */ -extern void b_eye(real_T I[144]); +extern void b_eye(real_T I[81]); extern void eye(real_T I[9]); #endif /* End of code generation (eye.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/find.c b/apps/attitude_estimator_ekf/codegen/find.c new file mode 100755 index 0000000000..8f05ef146e --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/find.c @@ -0,0 +1,77 @@ +/* + * find.c + * + * Code generation for function 'find' + * + * C source code generated on: Fri Sep 21 13:56:43 2012 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "attitudeKalmanfilter.h" +#include "find.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ + +/* + * + */ +void find(const int8_T x[9], real_T i_data[9], int32_T i_sizes[1]) +{ + int32_T idx; + int32_T ii; + boolean_T exitg1; + boolean_T guard1 = FALSE; + int32_T i2; + int8_T b_i_data[9]; + idx = 0; + i_sizes[0] = 9; + ii = 1; + exitg1 = 0U; + while ((exitg1 == 0U) && (ii <= 9)) { + guard1 = FALSE; + if (x[ii - 1] != 0) { + idx++; + i_data[idx - 1] = (real_T)ii; + if (idx >= 9) { + exitg1 = 1U; + } else { + guard1 = TRUE; + } + } else { + guard1 = TRUE; + } + + if (guard1 == TRUE) { + ii++; + } + } + + if (1 > idx) { + idx = 0; + } + + ii = idx - 1; + for (i2 = 0; i2 <= ii; i2++) { + b_i_data[i2] = (int8_T)i_data[i2]; + } + + i_sizes[0] = idx; + ii = idx - 1; + for (i2 = 0; i2 <= ii; i2++) { + i_data[i2] = (real_T)b_i_data[i2]; + } +} + +/* End of code generation (find.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/find.h b/apps/attitude_estimator_ekf/codegen/find.h new file mode 100755 index 0000000000..ca525c6009 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/find.h @@ -0,0 +1,34 @@ +/* + * find.h + * + * Code generation for function 'find' + * + * C source code generated on: Fri Sep 21 13:56:43 2012 + * + */ + +#ifndef __FIND_H__ +#define __FIND_H__ +/* Include files */ +#include +#include +#include +#include +#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 find(const int8_T x[9], real_T i_data[9], int32_T i_sizes[1]); +#endif +/* End of code generation (find.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.c b/apps/attitude_estimator_ekf/codegen/mrdivide.c index cb81b53282..d098162d54 100755 --- a/apps/attitude_estimator_ekf/codegen/mrdivide.c +++ b/apps/attitude_estimator_ekf/codegen/mrdivide.c @@ -3,7 +3,7 @@ * * Code generation for function 'mrdivide' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Fri Sep 21 13:56:44 2012 * */ @@ -21,136 +21,719 @@ /* Variable Definitions */ /* Function Declarations */ +static real32_T b_eml_matlab_zlarfg(real32_T *alpha1, const real32_T *x); +static void eml_lusolve(const real32_T A_data[81], const int32_T A_sizes[2], + real32_T B_data[81], int32_T B_sizes[2]); +static real32_T eml_matlab_zlarfg(int32_T n, real32_T *alpha1, real32_T x_data + [81], int32_T x_sizes[2], int32_T ix0); +static void eml_qrsolve(const real32_T A_data[81], const int32_T A_sizes[2], + real32_T B_data[81], int32_T B_sizes[2], real32_T Y_data[81], int32_T Y_sizes + [2]); +static real32_T eml_xnrm2(int32_T n, const real32_T x_data[81], const int32_T + x_sizes[2], int32_T ix0); /* Function Definitions */ /* * */ -void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]) +static real32_T b_eml_matlab_zlarfg(real32_T *alpha1, const real32_T *x) { - int32_T jy; + return 0.0F; +} + +/* + * + */ +static void eml_lusolve(const real32_T A_data[81], const int32_T A_sizes[2], + real32_T B_data[81], int32_T B_sizes[2]) +{ + int32_T loop_ub; int32_T iy; - real32_T b_A[81]; - int8_T ipiv[9]; + real32_T b_A_data[81]; + int32_T jA; + int32_T tmp_data[9]; + int32_T ipiv_data[9]; + int32_T ldap1; int32_T j; int32_T mmj; int32_T jj; - int32_T jp1j; - int32_T c; int32_T ix; real32_T temp; int32_T k; real32_T s; - int32_T loop_ub; - real32_T Y[108]; - for (jy = 0; jy < 9; jy++) { - for (iy = 0; iy < 9; iy++) { - b_A[iy + 9 * jy] = B[jy + 9 * iy]; - } - - ipiv[jy] = (int8_T)(1 + jy); + int32_T jrow; + int32_T b_loop_ub; + int32_T c; + loop_ub = A_sizes[0] * A_sizes[1] - 1; + for (iy = 0; iy <= loop_ub; iy++) { + b_A_data[iy] = A_data[iy]; } - for (j = 0; j < 8; j++) { - mmj = -j; - jj = j * 10; - jp1j = jj + 1; - c = mmj + 9; - jy = 0; - ix = jj; - temp = fabsf(b_A[jj]); - for (k = 2; k <= c; k++) { - ix++; - s = fabsf(b_A[ix]); - if (s > temp) { - jy = k - 1; - temp = s; + iy = A_sizes[1]; + jA = A_sizes[1]; + jA = iy <= jA ? iy : jA; + if (jA < 1) { + } else { + loop_ub = jA - 1; + for (iy = 0; iy <= loop_ub; iy++) { + tmp_data[iy] = 1 + iy; + } + + loop_ub = jA - 1; + for (iy = 0; iy <= loop_ub; iy++) { + ipiv_data[iy] = tmp_data[iy]; + } + } + + ldap1 = A_sizes[1] + 1; + iy = A_sizes[1] - 1; + jA = A_sizes[1]; + loop_ub = iy <= jA ? iy : jA; + for (j = 1; j <= loop_ub; j++) { + mmj = (A_sizes[1] - j) + 1; + jj = (j - 1) * ldap1; + if (mmj < 1) { + jA = -1; + } else { + jA = 0; + if (mmj > 1) { + ix = jj; + temp = (real32_T)fabs(b_A_data[jj]); + for (k = 1; k + 1 <= mmj; k++) { + ix++; + s = (real32_T)fabs(b_A_data[ix]); + if (s > temp) { + jA = k; + temp = s; + } + } } } - if ((real_T)b_A[jj + jy] != 0.0) { - if (jy != 0) { - ipiv[j] = (int8_T)((j + jy) + 1); - ix = j; - iy = j + jy; - for (k = 0; k < 9; k++) { - temp = b_A[ix]; - b_A[ix] = b_A[iy]; - b_A[iy] = temp; - ix += 9; - iy += 9; + if ((real_T)b_A_data[jj + jA] != 0.0) { + if (jA != 0) { + ipiv_data[j - 1] = j + jA; + jrow = j - 1; + iy = jrow + jA; + for (k = 1; k <= A_sizes[1]; k++) { + temp = b_A_data[jrow]; + b_A_data[jrow] = b_A_data[iy]; + b_A_data[iy] = temp; + jrow += A_sizes[1]; + iy += A_sizes[1]; } } - loop_ub = (jp1j + mmj) + 8; - for (iy = jp1j; iy + 1 <= loop_ub; iy++) { - b_A[iy] /= b_A[jj]; + b_loop_ub = jj + mmj; + for (jA = jj + 1; jA + 1 <= b_loop_ub; jA++) { + b_A_data[jA] /= b_A_data[jj]; } } - c = 8 - j; - jy = jj + 9; - for (iy = 1; iy <= c; iy++) { - if ((real_T)b_A[jy] != 0.0) { - temp = b_A[jy] * -1.0F; - ix = jp1j; - loop_ub = (mmj + jj) + 18; - for (k = 10 + jj; k + 1 <= loop_ub; k++) { - b_A[k] += b_A[ix] * temp; + c = A_sizes[1] - j; + jA = jj + ldap1; + iy = jj + A_sizes[1]; + for (jrow = 1; jrow <= c; jrow++) { + if ((real_T)b_A_data[iy] != 0.0) { + temp = b_A_data[iy] * -1.0F; + ix = jj; + b_loop_ub = (mmj + jA) - 1; + for (k = jA; k + 1 <= b_loop_ub; k++) { + b_A_data[k] += b_A_data[ix + 1] * temp; ix++; } } - jy += 9; - jj += 9; + iy += A_sizes[1]; + jA += A_sizes[1]; } } - for (jy = 0; jy < 12; jy++) { - for (iy = 0; iy < 9; iy++) { - Y[iy + 9 * jy] = A[jy + 12 * iy]; - } - } - - for (iy = 0; iy < 9; iy++) { - if (ipiv[iy] != iy + 1) { - for (j = 0; j < 12; j++) { - temp = Y[iy + 9 * j]; - Y[iy + 9 * j] = Y[(ipiv[iy] + 9 * j) - 1]; - Y[(ipiv[iy] + 9 * j) - 1] = temp; + for (jA = 0; jA + 1 <= A_sizes[1]; jA++) { + if (ipiv_data[jA] != jA + 1) { + for (j = 0; j < 9; j++) { + temp = B_data[jA + B_sizes[0] * j]; + B_data[jA + B_sizes[0] * j] = B_data[(ipiv_data[jA] + B_sizes[0] * j) - + 1]; + B_data[(ipiv_data[jA] + B_sizes[0] * j) - 1] = temp; } } } - for (j = 0; j < 12; j++) { - c = 9 * j; - for (k = 0; k < 9; k++) { - jy = 9 * k; - if ((real_T)Y[k + c] != 0.0) { - for (iy = k + 2; iy < 10; iy++) { - Y[(iy + c) - 1] -= Y[k + c] * b_A[(iy + jy) - 1]; + if (B_sizes[0] == 0) { + } else { + for (j = 0; j < 9; j++) { + c = A_sizes[1] * j; + for (k = 0; k + 1 <= A_sizes[1]; k++) { + iy = A_sizes[1] * k; + if ((real_T)B_data[k + c] != 0.0) { + for (jA = k + 1; jA + 1 <= A_sizes[1]; jA++) { + B_data[jA + c] -= B_data[k + c] * b_A_data[jA + iy]; + } } } } } - for (j = 0; j < 12; j++) { - c = 9 * j; - for (k = 8; k > -1; k += -1) { - jy = 9 * k; - if ((real_T)Y[k + c] != 0.0) { - Y[k + c] /= b_A[k + jy]; - for (iy = 0; iy + 1 <= k; iy++) { - Y[iy + c] -= Y[k + c] * b_A[iy + jy]; + if (B_sizes[0] == 0) { + } else { + for (j = 0; j < 9; j++) { + c = A_sizes[1] * j; + for (k = A_sizes[1] - 1; k + 1 > 0; k--) { + iy = A_sizes[1] * k; + if ((real_T)B_data[k + c] != 0.0) { + B_data[k + c] /= b_A_data[k + iy]; + for (jA = 0; jA + 1 <= k; jA++) { + B_data[jA + c] -= B_data[k + c] * b_A_data[jA + iy]; + } + } + } + } + } +} + +/* + * + */ +static real32_T eml_matlab_zlarfg(int32_T n, real32_T *alpha1, real32_T x_data + [81], int32_T x_sizes[2], int32_T ix0) +{ + real32_T tau; + int32_T nm1; + real32_T xnorm; + real32_T a; + int32_T knt; + int32_T loop_ub; + int32_T k; + tau = 0.0F; + if (n <= 0) { + } else { + nm1 = n - 2; + xnorm = eml_xnrm2(nm1 + 1, x_data, x_sizes, ix0); + if ((real_T)xnorm != 0.0) { + a = (real32_T)fabs(*alpha1); + xnorm = (real32_T)fabs(xnorm); + if (a < xnorm) { + a /= xnorm; + xnorm *= (real32_T)sqrt(a * a + 1.0F); + } else if (a > xnorm) { + xnorm /= a; + xnorm = (real32_T)sqrt(xnorm * xnorm + 1.0F) * a; + } else if (rtIsNaNF(xnorm)) { + } else { + xnorm = a * 1.41421354F; + } + + if ((real_T)*alpha1 >= 0.0) { + xnorm = -xnorm; + } + + if ((real32_T)fabs(xnorm) < 9.86076132E-32F) { + knt = 0; + do { + knt++; + loop_ub = ix0 + nm1; + for (k = ix0; k <= loop_ub; k++) { + x_data[k - 1] *= 1.01412048E+31F; + } + + xnorm *= 1.01412048E+31F; + *alpha1 *= 1.01412048E+31F; + } while (!((real32_T)fabs(xnorm) >= 9.86076132E-32F)); + + xnorm = eml_xnrm2(nm1 + 1, x_data, x_sizes, ix0); + a = (real32_T)fabs(*alpha1); + xnorm = (real32_T)fabs(xnorm); + if (a < xnorm) { + a /= xnorm; + xnorm *= (real32_T)sqrt(a * a + 1.0F); + } else if (a > xnorm) { + xnorm /= a; + xnorm = (real32_T)sqrt(xnorm * xnorm + 1.0F) * a; + } else if (rtIsNaNF(xnorm)) { + } else { + xnorm = a * 1.41421354F; + } + + if ((real_T)*alpha1 >= 0.0) { + xnorm = -xnorm; + } + + tau = (xnorm - *alpha1) / xnorm; + *alpha1 = 1.0F / (*alpha1 - xnorm); + loop_ub = ix0 + nm1; + for (k = ix0; k <= loop_ub; k++) { + x_data[k - 1] *= *alpha1; + } + + for (k = 1; k <= knt; k++) { + xnorm *= 9.86076132E-32F; + } + + *alpha1 = xnorm; + } else { + tau = (xnorm - *alpha1) / xnorm; + *alpha1 = 1.0F / (*alpha1 - xnorm); + loop_ub = ix0 + nm1; + for (k = ix0; k <= loop_ub; k++) { + x_data[k - 1] *= *alpha1; + } + + *alpha1 = xnorm; + } + } + } + + return tau; +} + +/* + * + */ +static void eml_qrsolve(const real32_T A_data[81], const int32_T A_sizes[2], + real32_T B_data[81], int32_T B_sizes[2], real32_T Y_data[81], int32_T Y_sizes + [2]) +{ + real_T rankR; + real_T u1; + int32_T mn; + int32_T b_A_sizes[2]; + int32_T loop_ub; + int32_T k; + real32_T b_A_data[81]; + int32_T pvt; + int32_T b_mn; + int32_T tmp_data[9]; + int32_T jpvt_data[9]; + int8_T unnamed_idx_0; + real32_T work_data[9]; + int32_T nmi; + real32_T vn1_data[9]; + real32_T vn2_data[9]; + int32_T i; + int32_T i_i; + int32_T mmi; + int32_T ix; + real32_T smax; + real32_T s; + int32_T iy; + real32_T atmp; + real32_T tau_data[9]; + int32_T i_ip1; + int32_T lastv; + int32_T lastc; + boolean_T exitg2; + int32_T exitg1; + boolean_T firstNonZero; + real32_T t; + rankR = (real_T)A_sizes[0]; + u1 = (real_T)A_sizes[1]; + rankR = rankR <= u1 ? rankR : u1; + mn = (int32_T)rankR; + b_A_sizes[0] = A_sizes[0]; + b_A_sizes[1] = A_sizes[1]; + loop_ub = A_sizes[0] * A_sizes[1] - 1; + for (k = 0; k <= loop_ub; k++) { + b_A_data[k] = A_data[k]; + } + + k = A_sizes[0]; + pvt = A_sizes[1]; + b_mn = k <= pvt ? k : pvt; + if (A_sizes[1] < 1) { + } else { + loop_ub = A_sizes[1] - 1; + for (k = 0; k <= loop_ub; k++) { + tmp_data[k] = 1 + k; + } + + loop_ub = A_sizes[1] - 1; + for (k = 0; k <= loop_ub; k++) { + jpvt_data[k] = tmp_data[k]; + } + } + + if ((A_sizes[0] == 0) || (A_sizes[1] == 0)) { + } else { + unnamed_idx_0 = (int8_T)A_sizes[1]; + loop_ub = unnamed_idx_0 - 1; + for (k = 0; k <= loop_ub; k++) { + work_data[k] = 0.0F; + } + + k = 1; + for (nmi = 0; nmi + 1 <= A_sizes[1]; nmi++) { + vn1_data[nmi] = eml_xnrm2(A_sizes[0], A_data, A_sizes, k); + vn2_data[nmi] = vn1_data[nmi]; + k += A_sizes[0]; + } + + for (i = 0; i + 1 <= b_mn; i++) { + i_i = i + i * A_sizes[0]; + nmi = A_sizes[1] - i; + mmi = (A_sizes[0] - i) - 1; + if (nmi < 1) { + pvt = -1; + } else { + pvt = 0; + if (nmi > 1) { + ix = i; + smax = (real32_T)fabs(vn1_data[i]); + for (k = 1; k + 1 <= nmi; k++) { + ix++; + s = (real32_T)fabs(vn1_data[ix]); + if (s > smax) { + pvt = k; + smax = s; + } + } + } + } + + pvt += i; + if (pvt + 1 != i + 1) { + ix = A_sizes[0] * pvt; + iy = A_sizes[0] * i; + for (k = 1; k <= A_sizes[0]; k++) { + s = b_A_data[ix]; + b_A_data[ix] = b_A_data[iy]; + b_A_data[iy] = s; + ix++; + iy++; + } + + k = jpvt_data[pvt]; + jpvt_data[pvt] = jpvt_data[i]; + jpvt_data[i] = k; + vn1_data[pvt] = vn1_data[i]; + vn2_data[pvt] = vn2_data[i]; + } + + if (i + 1 < A_sizes[0]) { + atmp = b_A_data[i_i]; + smax = eml_matlab_zlarfg(mmi + 1, &atmp, b_A_data, b_A_sizes, i_i + 2); + tau_data[i] = smax; + } else { + atmp = b_A_data[i_i]; + smax = b_A_data[i_i]; + s = b_eml_matlab_zlarfg(&atmp, &smax); + b_A_data[i_i] = smax; + tau_data[i] = s; + } + + b_A_data[i_i] = atmp; + if (i + 1 < A_sizes[1]) { + atmp = b_A_data[i_i]; + b_A_data[i_i] = 1.0F; + i_ip1 = (i + (i + 1) * A_sizes[0]) + 1; + if ((real_T)tau_data[i] != 0.0) { + lastv = mmi; + pvt = i_i + mmi; + while ((lastv + 1 > 0) && ((real_T)b_A_data[pvt] == 0.0)) { + lastv--; + pvt--; + } + + lastc = nmi - 1; + exitg2 = 0U; + while ((exitg2 == 0U) && (lastc > 0)) { + k = i_ip1 + (lastc - 1) * A_sizes[0]; + pvt = k + lastv; + do { + exitg1 = 0U; + if (k <= pvt) { + if ((real_T)b_A_data[k - 1] != 0.0) { + exitg1 = 1U; + } else { + k++; + } + } else { + lastc--; + exitg1 = 2U; + } + } while (exitg1 == 0U); + + if (exitg1 == 1U) { + exitg2 = 1U; + } + } + } else { + lastv = -1; + lastc = 0; + } + + if (lastv + 1 > 0) { + if (lastc == 0) { + } else { + for (iy = 1; iy <= lastc; iy++) { + work_data[iy - 1] = 0.0F; + } + + iy = 0; + loop_ub = i_ip1 + A_sizes[0] * (lastc - 1); + pvt = i_ip1; + while ((A_sizes[0] > 0) && (pvt <= loop_ub)) { + ix = i_i; + smax = 0.0F; + k = pvt + lastv; + for (nmi = pvt; nmi <= k; nmi++) { + smax += b_A_data[nmi - 1] * b_A_data[ix]; + ix++; + } + + work_data[iy] += smax; + iy++; + pvt += A_sizes[0]; + } + } + + smax = -tau_data[i]; + if ((real_T)smax == 0.0) { + } else { + pvt = 0; + for (nmi = 1; nmi <= lastc; nmi++) { + if ((real_T)work_data[pvt] != 0.0) { + s = work_data[pvt] * smax; + ix = i_i; + loop_ub = lastv + i_ip1; + for (k = i_ip1 - 1; k + 1 <= loop_ub; k++) { + b_A_data[k] += b_A_data[ix] * s; + ix++; + } + } + + pvt++; + i_ip1 += A_sizes[0]; + } + } + } + + b_A_data[i_i] = atmp; + } + + for (nmi = i + 1; nmi + 1 <= A_sizes[1]; nmi++) { + if ((real_T)vn1_data[nmi] != 0.0) { + smax = (real32_T)fabs(b_A_data[i + b_A_sizes[0] * nmi]) / vn1_data[nmi]; + smax = 1.0F - smax * smax; + if ((real_T)smax < 0.0) { + smax = 0.0F; + } + + s = vn1_data[nmi] / vn2_data[nmi]; + if (smax * (s * s) <= 0.000345266977F) { + if (i + 1 < A_sizes[0]) { + k = (i + A_sizes[0] * nmi) + 1; + smax = 0.0F; + if (mmi < 1) { + } else if (mmi == 1) { + smax = (real32_T)fabs(b_A_data[k]); + } else { + s = 0.0F; + firstNonZero = TRUE; + pvt = k + mmi; + while (k + 1 <= pvt) { + if (b_A_data[k] != 0.0F) { + atmp = (real32_T)fabs(b_A_data[k]); + if (firstNonZero) { + s = atmp; + smax = 1.0F; + firstNonZero = FALSE; + } else if (s < atmp) { + t = s / atmp; + smax = 1.0F + smax * t * t; + s = atmp; + } else { + t = atmp / s; + smax += t * t; + } + } + + k++; + } + + smax = s * (real32_T)sqrt(smax); + } + + vn1_data[nmi] = smax; + vn2_data[nmi] = vn1_data[nmi]; + } else { + vn1_data[nmi] = 0.0F; + vn2_data[nmi] = 0.0F; + } + } else { + vn1_data[nmi] *= (real32_T)sqrt(smax); + } } } } } - for (jy = 0; jy < 9; jy++) { - for (iy = 0; iy < 12; iy++) { - y[iy + 12 * jy] = Y[jy + 9 * iy]; + rankR = (real_T)A_sizes[0]; + u1 = (real_T)A_sizes[1]; + rankR = rankR >= u1 ? rankR : u1; + smax = (real32_T)rankR * (real32_T)fabs(b_A_data[0]) * 1.1920929E-7F; + rankR = 0.0; + k = 0; + while ((k + 1 <= mn) && (!((real32_T)fabs(b_A_data[k + b_A_sizes[0] * k]) <= + smax))) { + rankR++; + k++; + } + + unnamed_idx_0 = (int8_T)A_sizes[1]; + Y_sizes[0] = (int32_T)unnamed_idx_0; + Y_sizes[1] = 9; + loop_ub = unnamed_idx_0 * 9 - 1; + for (k = 0; k <= loop_ub; k++) { + Y_data[k] = 0.0F; + } + + for (nmi = 0; nmi + 1 <= mn; nmi++) { + if ((real_T)tau_data[nmi] != 0.0) { + for (k = 0; k < 9; k++) { + smax = B_data[nmi + B_sizes[0] * k]; + for (i = nmi + 1; i + 1 <= A_sizes[0]; i++) { + smax += b_A_data[i + b_A_sizes[0] * nmi] * B_data[i + B_sizes[0] * k]; + } + + smax *= tau_data[nmi]; + if ((real_T)smax != 0.0) { + B_data[nmi + B_sizes[0] * k] -= smax; + for (i = nmi + 1; i + 1 <= A_sizes[0]; i++) { + B_data[i + B_sizes[0] * k] -= b_A_data[i + b_A_sizes[0] * nmi] * + smax; + } + } + } + } + } + + for (k = 0; k < 9; k++) { + for (i = 0; (real_T)(i + 1) <= rankR; i++) { + Y_data[(jpvt_data[i] + Y_sizes[0] * k) - 1] = B_data[i + B_sizes[0] * k]; + } + + for (nmi = (int32_T)rankR - 1; nmi + 1 >= 1; nmi--) { + Y_data[(jpvt_data[nmi] + Y_sizes[0] * k) - 1] /= b_A_data[nmi + b_A_sizes + [0] * nmi]; + for (i = 0; i + 1 <= nmi; i++) { + Y_data[(jpvt_data[i] + Y_sizes[0] * k) - 1] -= Y_data[(jpvt_data[nmi] + + Y_sizes[0] * k) - 1] * b_A_data[i + b_A_sizes[0] * nmi]; + } + } + } +} + +/* + * + */ +static real32_T eml_xnrm2(int32_T n, const real32_T x_data[81], const int32_T + x_sizes[2], int32_T ix0) +{ + real32_T y; + real32_T scale; + boolean_T firstNonZero; + int32_T kend; + int32_T k; + real32_T absxk; + real32_T t; + y = 0.0F; + if (n < 1) { + } else if (n == 1) { + y = (real32_T)fabs(x_data[ix0 - 1]); + } else { + scale = 0.0F; + firstNonZero = TRUE; + kend = (ix0 + n) - 1; + for (k = ix0 - 1; k + 1 <= kend; k++) { + if (x_data[k] != 0.0F) { + absxk = (real32_T)fabs(x_data[k]); + if (firstNonZero) { + scale = absxk; + y = 1.0F; + firstNonZero = FALSE; + } else if (scale < absxk) { + t = scale / absxk; + y = 1.0F + y * t * t; + scale = absxk; + } else { + t = absxk / scale; + y += t * t; + } + } + } + + y = scale * (real32_T)sqrt(y); + } + + return y; +} + +/* + * + */ +void mrdivide(const real32_T A_data[81], const int32_T A_sizes[2], const + real32_T B_data[81], const int32_T B_sizes[2], real32_T y_data[81], + int32_T y_sizes[2]) +{ + int32_T b_A_sizes[2]; + int32_T loop_ub; + int32_T i3; + int32_T b_loop_ub; + int32_T i4; + real32_T b_A_data[81]; + int32_T b_B_sizes[2]; + real32_T b_B_data[81]; + int8_T unnamed_idx_0; + int32_T c_B_sizes[2]; + real32_T c_B_data[81]; + b_A_sizes[0] = B_sizes[1]; + b_A_sizes[1] = B_sizes[0]; + loop_ub = B_sizes[0] - 1; + for (i3 = 0; i3 <= loop_ub; i3++) { + b_loop_ub = B_sizes[1] - 1; + for (i4 = 0; i4 <= b_loop_ub; i4++) { + b_A_data[i4 + b_A_sizes[0] * i3] = B_data[i3 + B_sizes[0] * i4]; + } + } + + b_B_sizes[0] = A_sizes[1]; + b_B_sizes[1] = 9; + for (i3 = 0; i3 < 9; i3++) { + loop_ub = A_sizes[1] - 1; + for (i4 = 0; i4 <= loop_ub; i4++) { + b_B_data[i4 + b_B_sizes[0] * i3] = A_data[i3 + A_sizes[0] * i4]; + } + } + + if ((b_A_sizes[0] == 0) || (b_A_sizes[1] == 0) || (b_B_sizes[0] == 0)) { + unnamed_idx_0 = (int8_T)b_A_sizes[1]; + b_B_sizes[0] = (int32_T)unnamed_idx_0; + b_B_sizes[1] = 9; + loop_ub = unnamed_idx_0 * 9 - 1; + for (i3 = 0; i3 <= loop_ub; i3++) { + b_B_data[i3] = 0.0F; + } + } else if (b_A_sizes[0] == b_A_sizes[1]) { + eml_lusolve(b_A_data, b_A_sizes, b_B_data, b_B_sizes); + } else { + c_B_sizes[0] = b_B_sizes[0]; + c_B_sizes[1] = 9; + loop_ub = b_B_sizes[0] * 9 - 1; + for (i3 = 0; i3 <= loop_ub; i3++) { + c_B_data[i3] = b_B_data[i3]; + } + + eml_qrsolve(b_A_data, b_A_sizes, c_B_data, c_B_sizes, b_B_data, b_B_sizes); + } + + y_sizes[0] = 9; + y_sizes[1] = b_B_sizes[0]; + loop_ub = b_B_sizes[0] - 1; + for (i3 = 0; i3 <= loop_ub; i3++) { + for (i4 = 0; i4 < 9; i4++) { + y_data[i4 + 9 * i3] = b_B_data[i3 + b_B_sizes[0] * i4]; } } } diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.h b/apps/attitude_estimator_ekf/codegen/mrdivide.h index e81bfffc0a..e807afcc8b 100755 --- a/apps/attitude_estimator_ekf/codegen/mrdivide.h +++ b/apps/attitude_estimator_ekf/codegen/mrdivide.h @@ -3,7 +3,7 @@ * * Code generation for function 'mrdivide' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Fri Sep 21 13:56:44 2012 * */ @@ -14,6 +14,8 @@ #include #include #include +#include "rt_defines.h" +#include "rt_nonfinite.h" #include "rtwtypes.h" #include "attitudeKalmanfilter_types.h" @@ -27,6 +29,6 @@ /* Variable Definitions */ /* Function Declarations */ -extern void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]); +extern void mrdivide(const real32_T A_data[81], const int32_T A_sizes[2], const real32_T B_data[81], const int32_T B_sizes[2], real32_T y_data[81], int32_T y_sizes[2]); #endif /* End of code generation (mrdivide.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/norm.c b/apps/attitude_estimator_ekf/codegen/norm.c index 1fbde052bc..fa07e1a90e 100755 --- a/apps/attitude_estimator_ekf/codegen/norm.c +++ b/apps/attitude_estimator_ekf/codegen/norm.c @@ -3,7 +3,7 @@ * * Code generation for function 'norm' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Fri Sep 21 13:56:43 2012 * */ @@ -40,7 +40,7 @@ real32_T norm(const real32_T x[3]) firstNonZero = TRUE; for (k = 0; k < 3; k++) { if (x[k] != 0.0F) { - absxk = fabsf(x[k]); + absxk = (real32_T)fabsf(x[k]); if (firstNonZero) { scale = absxk; y = 1.0F; @@ -56,7 +56,7 @@ real32_T norm(const real32_T x[3]) } } - return scale * sqrtf(y); + return scale * (real32_T)sqrtf(y); } /* End of code generation (norm.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/norm.h b/apps/attitude_estimator_ekf/codegen/norm.h index b0c7fe430e..63294717f4 100755 --- a/apps/attitude_estimator_ekf/codegen/norm.h +++ b/apps/attitude_estimator_ekf/codegen/norm.h @@ -3,7 +3,7 @@ * * Code generation for function 'norm' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Fri Sep 21 13:56:44 2012 * */ @@ -14,6 +14,8 @@ #include #include #include +#include "rt_defines.h" +#include "rt_nonfinite.h" #include "rtwtypes.h" #include "attitudeKalmanfilter_types.h" diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.c b/apps/attitude_estimator_ekf/codegen/rtGetInf.c index 3cef176848..f1bb71c873 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetInf.c +++ b/apps/attitude_estimator_ekf/codegen/rtGetInf.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Fri Sep 21 13:56:45 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.h b/apps/attitude_estimator_ekf/codegen/rtGetInf.h index ab2d5a70d5..6d32d51b56 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetInf.h +++ b/apps/attitude_estimator_ekf/codegen/rtGetInf.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Fri Sep 21 13:56:45 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c index 17a093461f..50581f34df 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c +++ b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Fri Sep 21 13:56:45 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h index 2c254bbbf6..12d8734f5b 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h +++ b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Fri Sep 21 13:56:44 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_defines.h b/apps/attitude_estimator_ekf/codegen/rt_defines.h new file mode 100755 index 0000000000..419edf01c9 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/rt_defines.h @@ -0,0 +1,24 @@ +/* + * rt_defines.h + * + * Code generation for function 'attitudeKalmanfilter' + * + * C source code generated on: Fri Sep 21 13:56:44 2012 + * + */ + +#ifndef __RT_DEFINES_H__ +#define __RT_DEFINES_H__ + +#include + +#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/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c index 005c4f28de..42ff21d39e 100755 --- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c +++ b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Fri Sep 21 13:56:44 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h index 6481f011f8..60128156ea 100755 --- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h +++ b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Fri Sep 21 13:56:44 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtw_proj.tmw b/apps/attitude_estimator_ekf/codegen/rtw_proj.tmw deleted file mode 100755 index 1fb585abd7..0000000000 --- a/apps/attitude_estimator_ekf/codegen/rtw_proj.tmw +++ /dev/null @@ -1 +0,0 @@ -Code generation project for attitudeKalmanfilter using C:\Program Files\MATLAB\R2011a\toolbox\coder\coder\rtw\c\xrt\xrt_vcx64.tmf. MATLAB root = C:\Program Files\MATLAB\R2011a. diff --git a/apps/attitude_estimator_ekf/codegen/rtwtypes.h b/apps/attitude_estimator_ekf/codegen/rtwtypes.h index fd629ebcd0..2709915e95 100755 --- a/apps/attitude_estimator_ekf/codegen/rtwtypes.h +++ b/apps/attitude_estimator_ekf/codegen/rtwtypes.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Fri Sep 21 13:56:44 2012 * */ diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 7535b90469..960c5d3837 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -65,7 +65,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -83,7 +84,8 @@ #include - +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 */ #include extern struct system_load_s system_load; @@ -94,14 +96,15 @@ extern struct system_load_s system_load; #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 7500 -#define STICK_THRUST_RANGE 20000 +#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_COUNTER_LIMIT 50 +#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; @@ -114,6 +117,8 @@ 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; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ @@ -251,22 +256,6 @@ int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, u return 0; } -static void cal_bsort(float a[], int n) -{ - int i,j,t; - for(i=0;ia[j+1]) { - t=a[j]; - a[j]=a[j+1]; - a[j+1]=t; - } - } - } -} - static const char *parameter_file = "/eeprom/parameters"; static int pm_save_eeprom(bool only_unsaved) @@ -307,30 +296,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) const uint64_t calibration_interval_us = 45 * 1000000; unsigned int calibration_counter = 0; - const int peak_samples = 2000; - /* Get rid of 10% */ - const int outlier_margin = (peak_samples) / 10; - - float *mag_maxima[3]; - mag_maxima[0] = (float*)malloc(peak_samples * sizeof(float)); - mag_maxima[1] = (float*)malloc(peak_samples * sizeof(float)); - mag_maxima[2] = (float*)malloc(peak_samples * sizeof(float)); - - float *mag_minima[3]; - mag_minima[0] = (float*)malloc(peak_samples * sizeof(float)); - mag_minima[1] = (float*)malloc(peak_samples * sizeof(float)); - mag_minima[2] = (float*)malloc(peak_samples * sizeof(float)); - - /* initialize data table */ - for (int i = 0; i < peak_samples; i++) { - mag_maxima[0][i] = FLT_MIN; - mag_maxima[1][i] = FLT_MIN; - mag_maxima[2][i] = FLT_MIN; - - mag_minima[0][i] = FLT_MAX; - mag_minima[1][i] = FLT_MAX; - mag_minima[2][i] = FLT_MAX; - } + 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, 0); struct mag_scale mscale_null = { @@ -357,34 +324,32 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); /* get min/max values */ - /* iterate through full list */ - for (int i = 0; i < peak_samples; i++) { - /* x minimum */ - if (raw.magnetometer_raw[0] < mag_minima[0][i]) - mag_minima[0][i] = raw.magnetometer_ga[0]; - /* y minimum */ - if (raw.magnetometer_raw[1] < mag_minima[1][i]) - mag_minima[1][i] = raw.magnetometer_ga[1]; - /* z minimum */ - if (raw.magnetometer_raw[2] < mag_minima[2][i]) - mag_minima[2][i] = raw.magnetometer_ga[2]; + if (raw.magnetometer_ga[0] < mag_min[0]) { + mag_min[0] = raw.magnetometer_ga[0]; + } + else if (raw.magnetometer_ga[0] > mag_max[0]) { + mag_max[0] = raw.magnetometer_ga[0]; + } - /* x maximum */ - if (raw.magnetometer_raw[0] > mag_maxima[0][i]) - mag_maxima[0][i] = raw.magnetometer_ga[0]; - /* y maximum */ - if (raw.magnetometer_raw[1] > mag_maxima[1][i]) - mag_maxima[1][i] = raw.magnetometer_ga[1]; - /* z maximum */ - if (raw.magnetometer_raw[2] > mag_maxima[2][i]) - mag_maxima[2][i] = raw.magnetometer_ga[2]; + 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 { /* any poll failure for 1s is a reason to abort */ - //mavlink_log_info(mavlink_fd, "[commander] mag calibration aborted, please retry."); - //break; + mavlink_log_info(mavlink_fd, "[commander] mag calibration aborted, please retry."); + break; } } @@ -392,67 +357,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) status->flag_preflight_mag_calibration = false; state_machine_publish(status_pub, status, mavlink_fd); - /* sort values */ - cal_bsort(mag_minima[0], peak_samples); - cal_bsort(mag_minima[1], peak_samples); - cal_bsort(mag_minima[2], peak_samples); - - cal_bsort(mag_maxima[0], peak_samples); - cal_bsort(mag_maxima[1], peak_samples); - cal_bsort(mag_maxima[2], peak_samples); - - float min_avg[3] = { 0.0f, 0.0f, 0.0f }; - float max_avg[3] = { 0.0f, 0.0f, 0.0f }; - - // printf("start:\n"); - - // for (int i = 0; i < 10; i++) { - // printf("mag min: %8.4f\t%8.4f\t%8.4f\tmax: %8.4f\t%8.4f\t%8.4f\n", - // mag_minima[0][i], - // mag_minima[1][i], - // mag_minima[2][i], - // mag_maxima[0][i], - // mag_maxima[1][i], - // mag_maxima[2][i]); - // usleep(10000); - // } - // printf("-----\n"); - - // for (int i = (peak_samples - outlier_margin)-10; i < (peak_samples - outlier_margin); i++) { - // printf("mag min: %8.4f\t%8.4f\t%8.4f\tmax: %8.4f\t%8.4f\t%8.4f\n", - // mag_minima[0][i], - // mag_minima[1][i], - // mag_minima[2][i], - // mag_maxima[0][i], - // mag_maxima[1][i], - // mag_maxima[2][i]); - // usleep(10000); - // } - - // printf("end\n"); - - /* take average of center value group */ - for (int i = 0; i < (peak_samples - outlier_margin); i++) { - min_avg[0] += mag_minima[0][i+outlier_margin]; - min_avg[1] += mag_minima[1][i+outlier_margin]; - min_avg[2] += mag_minima[2][i+outlier_margin]; - - max_avg[0] += mag_maxima[0][i]; - max_avg[1] += mag_maxima[1][i]; - max_avg[2] += mag_maxima[2][i]; - } - - min_avg[0] /= (peak_samples - outlier_margin); - min_avg[1] /= (peak_samples - outlier_margin); - min_avg[2] /= (peak_samples - outlier_margin); - - max_avg[0] /= (peak_samples - outlier_margin); - max_avg[1] /= (peak_samples - outlier_margin); - max_avg[2] /= (peak_samples - outlier_margin); - - // printf("\nFINAL:\nmag min: %8.4f\t%8.4f\t%8.4f\nmax: %8.4f\t%8.4f\t%8.4f\n", (double)min_avg[0], - // (double)min_avg[1], (double)min_avg[2], (double)max_avg[0], (double)max_avg[1], (double)max_avg[2]); - float mag_offset[3]; /** @@ -467,31 +371,24 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) * offset = (max + min) / 2.0f */ - mag_offset[0] = (max_avg[0] + min_avg[0]) / 2.0f; - mag_offset[1] = (max_avg[1] + min_avg[1]) / 2.0f; - mag_offset[2] = (max_avg[2] + min_avg[2]) / 2.0f; + mag_offset[0] = (mag_max[0] + mag_min[0]) / 2.0f; + mag_offset[1] = (mag_max[1] + mag_min[1]) / 2.0f; + mag_offset[2] = (mag_max[2] + mag_min[2]) / 2.0f; - if (!isfinite(mag_offset[1]) || !isfinite(mag_offset[1]) || !isfinite(mag_offset[2])) { + printf("mag off x: %4.4f, y: %4.4f, z: %4.4f\n",(double)mag_offset[0],(double)mag_offset[0],(double)mag_offset[2]); - mavlink_log_critical(mavlink_fd, "[commander] MAG calibration failed (INF/NAN)"); - } else { - /* announce and set new offset */ + /* announce and set new offset */ - // char offset_output[50]; - // sprintf(offset_output, "[commander] mag cal: %8.4f %8.4f %8.4f", (double)mag_offset[0], (double)mag_offset[1], (double)mag_offset[2]); - // mavlink_log_info(mavlink_fd, offset_output); + if (param_set(param_find("SENS_MAG_XOFF"), &(mag_offset[0]))) { + fprintf(stderr, "[commander] Setting X mag offset failed!\n"); + } - if (param_set(param_find("SENS_MAG_XOFF"), &(mag_offset[0]))) { - fprintf(stderr, "[commander] Setting X mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_YOFF"), &(mag_offset[1]))) { - fprintf(stderr, "[commander] Setting Y mag offset failed!\n"); - } + if (param_set(param_find("SENS_MAG_YOFF"), &(mag_offset[1]))) { + fprintf(stderr, "[commander] Setting Y mag offset failed!\n"); + } - if (param_set(param_find("SENS_MAG_ZOFF"), &(mag_offset[2]))) { - fprintf(stderr, "[commander] Setting Z mag offset failed!\n"); - } + if (param_set(param_find("SENS_MAG_ZOFF"), &(mag_offset[2]))) { + fprintf(stderr, "[commander] Setting Z mag offset failed!\n"); } fd = open(MAG_DEVICE_PATH, 0); @@ -507,14 +404,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) warn("WARNING: failed to set scale / offsets for mag"); close(fd); - free(mag_maxima[0]); - free(mag_maxima[1]); - free(mag_maxima[2]); - - free(mag_minima[0]); - free(mag_minima[1]); - free(mag_minima[2]); - /* auto-save to EEPROM */ int save_ret = pm_save_eeprom(false); if(save_ret != 0) { @@ -567,7 +456,7 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) calibration_counter++; } else { /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "[commander] gyro calibration aborted, please retry."); + mavlink_log_info(mavlink_fd, "[commander] gyro calibration aborted, retry"); return; } } @@ -614,9 +503,7 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) mavlink_log_info(mavlink_fd, "[commander] gyro calibration finished"); - // char offset_output[50]; - // sprintf(offset_output, "[commander] gyro 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, offset_output); + printf("[commander] gyro cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); close(sub_sensor_combined); } @@ -624,9 +511,8 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) void do_accel_calibration(int status_pub, struct vehicle_status_s *status) { /* announce change */ - usleep(5000); - mavlink_log_info(mavlink_fd, "[commander] The system should be level and not moved"); + mavlink_log_info(mavlink_fd, "[commander] keep it level and still"); /* set to accel calibration mode */ status->flag_preflight_accel_calibration = true; state_machine_publish(status_pub, status, mavlink_fd); @@ -651,7 +537,6 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) 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 */ @@ -665,11 +550,10 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) calibration_counter++; } else { /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "[commander] gyro calibration aborted, please retry."); + mavlink_log_info(mavlink_fd, "[commander] 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; @@ -724,18 +608,11 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) if(save_ret != 0) { warn("WARNING: auto-save of params to EEPROM failed"); } - /* exit to gyro calibration mode */ status->flag_preflight_accel_calibration = false; state_machine_publish(status_pub, status, mavlink_fd); - mavlink_log_info(mavlink_fd, "[commander] acceleration calibration finished"); - - // char offset_output[50]; - // sprintf(offset_output, "[commander] accel cal: x:%8.4f y:%8.4f z:%8.4f", (double)accel_offset[0], - // (double)accel_offset[1], (double)accel_offset[2]); - // mavlink_log_info(mavlink_fd, offset_output); - + printf("[commander] accel calibration: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0],(double)accel_offset[1], (double)accel_offset[2]); close(sub_sensor_combined); } @@ -794,6 +671,31 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } break; + case PX4_CMD_CONTROLLER_SELECTION: { + bool changed = false; + if ((int)cmd->param1 != (int)current_vehicle_status->flag_control_rates_enabled) { + current_vehicle_status->flag_control_rates_enabled = cmd->param1; + changed = true; + } + if ((int)cmd->param2 != (int)current_vehicle_status->flag_control_attitude_enabled) { + current_vehicle_status->flag_control_attitude_enabled = cmd->param2; + changed = true; + } + if ((int)cmd->param3 != (int)current_vehicle_status->flag_control_velocity_enabled) { + current_vehicle_status->flag_control_velocity_enabled = cmd->param3; + changed = true; + } + if ((int)cmd->param4 != (int)current_vehicle_status->flag_control_position_enabled) { + current_vehicle_status->flag_control_position_enabled = cmd->param4; + changed = true; + } + + if (changed) { + /* publish current state machine */ + state_machine_publish(status_pub, current_vehicle_status, mavlink_fd); + } + } + // /* request to land */ // case MAV_CMD_NAV_LAND: // { @@ -878,7 +780,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* none found */ if (!handled) { //fprintf(stderr, "[commander] refusing unsupported calibration request\n"); - mavlink_log_critical(mavlink_fd, "[commander] CMD refusing unsupported calibration request"); + mavlink_log_critical(mavlink_fd, "[commander] CMD refusing unsup. calib. request"); result = MAV_RESULT_UNSUPPORTED; } } @@ -1075,6 +977,10 @@ int commander_thread_main(int argc, char *argv[]) /* not yet initialized */ commander_initialized = false; + /* set parameters */ + failsafe_lowlevel_timeout_ms = 0; + param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms); + /* welcome user */ printf("[commander] I am in command now!\n"); @@ -1101,6 +1007,8 @@ int commander_thread_main(int argc, char *argv[]) memset(¤t_status, 0, sizeof(current_status)); current_status.state_machine = SYSTEM_STATE_PREFLIGHT; current_status.flag_system_armed = false; + current_status.offboard_control_signal_found_once = false; + current_status.rc_signal_found_once = false; /* advertise to ORB */ stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); @@ -1146,10 +1054,15 @@ int commander_thread_main(int argc, char *argv[]) int gps_quality_good_counter = 0; - /* Subscribe to RC data */ - int rc_sub = orb_subscribe(ORB_ID(rc_channels)); - struct rc_channels_s rc; - memset(&rc, 0, sizeof(rc)); + /* 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 gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); struct vehicle_gps_position_s gps; @@ -1168,11 +1081,23 @@ int commander_thread_main(int argc, char *argv[]) commander_initialized = true; uint64_t start_time = hrt_absolute_time(); + uint64_t failsave_ll_start_time = 0; + + bool state_changed = true; while (!thread_should_exit) { /* Get current values */ - orb_copy(ORB_ID(rc_channels), rc_sub, &rc); + 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_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps); orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); @@ -1283,10 +1208,10 @@ int commander_thread_main(int argc, char *argv[]) /* End battery voltage check */ /* Check if last transition deserved an audio event */ -#warning This code depends on state that is no longer? maintained -#if 0 - trigger_audio_alarm(vehicle_mode_previous, vehicle_state_previous, current_status.mode, current_status.state_machine); -#endif +// #warning This code depends on state that is no longer? maintained +// #if 0 +// trigger_audio_alarm(vehicle_mode_previous, vehicle_state_previous, current_status.mode, current_status.state_machine); +// #endif /* only check gps fix if we are outdoor */ // if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { @@ -1341,89 +1266,176 @@ int commander_thread_main(int argc, char *argv[]) /* end: check gps */ - /* Start RC state check */ - bool prev_lost = current_status.rc_signal_lost; + /* 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 (rc.chan_count > 4 && (hrt_absolute_time() - rc.timestamp) < 100000) { + if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { - /* quadrotor specific logic - check against system type in the future */ + /* check if left stick is in lower left position --> switch to standby state */ + if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && sp_man.throttle < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values + if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { + update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); + stick_on_counter = 0; - int16_t rc_yaw_scale = rc.chan[rc.function[YAW]].scale; - int16_t rc_throttle_scale = rc.chan[rc.function[THROTTLE]].scale; - int16_t mode_switch_rc_value = rc.chan[rc.function[OVERRIDE]].scale; - /* Check the value of the rc channel of the mode switch */ - mode_switch_rc_value = rc.chan[rc.function[OVERRIDE]].scale; + } else { + stick_off_counter++; + stick_on_counter = 0; + } + } - /* check if left stick is in lower left position --> switch to standby state */ - if (rc_yaw_scale < -STICK_ON_OFF_LIMIT && rc_throttle_scale < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values - if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); - 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) { //TODO: remove hardcoded values + 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; + } + } + //printf("RC: y:%i/t:%i s:%i chans: %i\n", rc_yaw_scale, rc_throttle_scale, mode_switch_rc_value, rc.chan_count); + + if (sp_man.override_mode_switch > STICK_ON_OFF_LIMIT) { + current_status.flag_control_attitude_enabled = true; + current_status.flag_control_rates_enabled = false; + update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); + + } else if (sp_man.override_mode_switch < -STICK_ON_OFF_LIMIT) { + current_status.flag_control_attitude_enabled = true; + current_status.flag_control_rates_enabled = false; + update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); } else { - stick_off_counter++; - stick_on_counter = 0; + current_status.flag_control_attitude_enabled = true; + current_status.flag_control_rates_enabled = false; + update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); } - } - - /* check if left stick is in lower right position --> arm */ - if (rc_yaw_scale > STICK_ON_OFF_LIMIT && rc_throttle_scale < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values - if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); - stick_on_counter = 0; + /* 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, "[commander] DETECTED RC SIGNAL FIRST TIME."); } else { - stick_on_counter++; - stick_off_counter = 0; + if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[commander] RECOVERY - RC SIGNAL GAINED!"); } - } - //printf("RC: y:%i/t:%i s:%i chans: %i\n", rc_yaw_scale, rc_throttle_scale, mode_switch_rc_value, rc.chan_count); - if (mode_switch_rc_value > STICK_ON_OFF_LIMIT) { - update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); - - } else if (mode_switch_rc_value < -STICK_ON_OFF_LIMIT) { - update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); + current_status.rc_signal_cutting_off = false; + current_status.rc_signal_lost = false; + current_status.rc_signal_lost_interval = 0; } else { - update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); + 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, "[commander] 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); + // } } - - /* Publish RC signal */ - - - /* handle the case where RC signal was regained */ - if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[commander] RECOVERY - RC SIGNAL GAINED!"); - 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_lost || ((hrt_absolute_time() - last_print_time) > 5000000)) { - mavlink_log_critical(mavlink_fd, "[commander] 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_cutting_off = true; - current_status.rc_signal_lost_interval = hrt_absolute_time() - rc.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; } - /* Check if this is the first loss or first gain*/ - if ((!prev_lost && current_status.rc_signal_lost) || - prev_lost && !current_status.rc_signal_lost) { - /* publish rc lost */ - 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; + + mavlink_log_critical(mavlink_fd, "[commander] DETECTED OFFBOARD CONTROL SIGNAL FIRST TIME."); + } else { + if (current_status.offboard_control_signal_lost) { + mavlink_log_critical(mavlink_fd, "[commander] RECOVERY - OFFBOARD CONTROL SIGNAL GAINED!"); + state_changed = true; + } + } + + 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, "[commander] CRITICAL - NO OFFBOARD CONTROL 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.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(); + current_status.failsave_lowlevel = true; + + /* kill motors after timeout */ + if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms*1000) { + state_changed = true; + } + } + } + } + + current_status.counter++; current_status.timestamp = hrt_absolute_time(); @@ -1438,8 +1450,10 @@ int commander_thread_main(int argc, char *argv[]) } /* publish at least with 1 Hz */ - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + 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 */ @@ -1457,7 +1471,8 @@ int commander_thread_main(int argc, char *argv[]) /* close fds */ led_deinit(); buzzer_deinit(); - close(rc_sub); + close(sp_man_sub); + close(sp_offboard_sub); close(gps_sub); close(sensor_sub); diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index da85d0868e..e1d24d6a6d 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -223,7 +223,8 @@ void publish_armed_status(const struct vehicle_status_s *current_status) { /* lock down actuators if required */ // XXX FIXME Currently any loss of RC will completely disable all actuators // needs proper failsafe - armed.lockdown = (current_status->rc_signal_lost || current_status->flag_hil_enabled) ? true : false; + armed.lockdown = ((current_status->rc_signal_lost && current_status->offboard_control_signal_lost) + || 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); } diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c index 67e5d1b289..bdf7976736 100644 --- a/apps/fixedwing_control/fixedwing_control.c +++ b/apps/fixedwing_control/fixedwing_control.c @@ -44,16 +44,12 @@ #include #include #include -#include #include -#include +#include #include #include #include -#include -#include #include -#include #include #include #include @@ -62,6 +58,8 @@ #include #include #include +#include +#include static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ @@ -82,6 +80,280 @@ int fixedwing_control_thread_main(int argc, char *argv[]); */ static void usage(const char *reason); +/* + * Controller parameters, accessible via MAVLink + * + */ +PARAM_DEFINE_FLOAT(FW_ROLL_POS_P, 0.3f); +PARAM_DEFINE_FLOAT(FW_ROLL_POS_I, 0.002f); +PARAM_DEFINE_FLOAT(FW_ROLL_POS_AWU, 0.2f); +PARAM_DEFINE_FLOAT(FW_ROLL_POS_LIM, 0.6f); + +struct fw_att_control_params { + float roll_pos_p; + float roll_pos_i; + float roll_pos_awu; + float roll_pos_lim; +}; + +struct fw_att_control_param_handles { + param_t roll_pos_p; + param_t roll_pos_i; + param_t roll_pos_awu; + param_t roll_pos_lim; +}; + + +// XXX outsource position control to a separate app some time + +PARAM_DEFINE_FLOAT(FW_HEADING_P, 0.1f); +PARAM_DEFINE_FLOAT(FW_HEADING_LIM, 0.15f); + +struct fw_pos_control_params { + float heading_p; + float heading_lim; +}; + +struct fw_pos_control_param_handles { + param_t heading_p; + param_t heading_lim; +}; + +/** + * Initialize all parameter handles and values + * + */ +static int att_parameters_init(struct fw_att_control_param_handles *h); + +/** + * Update all parameters + * + */ +static int att_parameters_update(const struct fw_att_control_param_handles *h, struct fw_att_control_params *p); + +/** + * Initialize all parameter handles and values + * + */ +static int pos_parameters_init(struct fw_pos_control_param_handles *h); + +/** + * Update all parameters + * + */ +static int pos_parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p); + + +/** + * The fixed wing control main loop. + * + * This loop executes continously and calculates the control + * response. + * + * @param argc number of arguments + * @param argv argument array + * + * @return 0 + * + */ +int fixedwing_control_thread_main(int argc, char *argv[]) +{ + // /* read arguments */ + // bool verbose = false; + + // for (int i = 1; i < argc; i++) { + // if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) { + // verbose = true; + // } + // } + + /* welcome user */ + printf("[fixedwing control] started\n"); + + /* output structs */ + struct actuator_controls_s actuators; + struct vehicle_attitude_setpoint_s att_sp; + memset(&att_sp, 0, sizeof(att_sp)); + + /* publish actuator controls */ + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) + actuators.control[i] = 0.0f; + orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); + orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + + /* Subscribe to global position, attitude and rc */ + /* declare and safely initialize all structs */ + struct vehicle_status_s state; + memset(&state, 0, sizeof(state)); + struct vehicle_attitude_s att; + memset(&att_sp, 0, sizeof(att_sp)); + struct manual_control_setpoint_s manual; + memset(&manual, 0, sizeof(manual)); + + /* subscribe to attitude, motor setpoints and system state */ + struct vehicle_global_position_s global_pos; + int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + struct vehicle_global_position_setpoint_s global_setpoint; + int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); + int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + int state_sub = orb_subscribe(ORB_ID(vehicle_status)); + int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + + /* Mainloop setup */ + unsigned int loopcounter = 0; + + uint64_t last_run = 0; + uint64_t last_run_pos = 0; + + bool global_sp_updated_set_once = false; + + struct fw_att_control_params p; + struct fw_att_control_param_handles h; + + struct fw_pos_control_params ppos; + struct fw_pos_control_param_handles hpos; + + /* initialize the pid controllers */ + att_parameters_init(&h); + att_parameters_update(&h, &p); + + pos_parameters_init(&hpos); + pos_parameters_update(&hpos, &ppos); + + PID_t roll_pos_controller; + pid_init(&roll_pos_controller, p.roll_pos_p, p.roll_pos_i, 0.0f, p.roll_pos_awu, + PID_MODE_DERIVATIV_SET); + + PID_t heading_controller; + pid_init(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f, + PID_MODE_DERIVATIV_SET); + + while(!thread_should_exit) { + + struct pollfd fds[1] = { + { .fd = att_sub, .events = POLLIN }, + }; + int ret = poll(fds, 1, 1000); + + if (ret < 0) { + /* XXX this is seriously bad - should be an emergency */ + } else if (ret == 0) { + /* XXX this means no sensor data - should be critical or emergency */ + printf("[fixedwing control] WARNING: Not getting attitude - estimator running?\n"); + } else { + + // FIXME SUBSCRIBE + if (loopcounter % 20 == 0) { + att_parameters_update(&h, &p); + pos_parameters_update(&hpos, &ppos); + pid_set_parameters(&roll_pos_controller, p.roll_pos_p, p.roll_pos_i, 0.0f, p.roll_pos_awu); + pid_set_parameters(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f); + } + + /* if position updated, run position control loop */ + bool pos_updated; + orb_check(global_pos_sub, &pos_updated); + bool global_sp_updated; + orb_check(global_setpoint_sub, &global_sp_updated); + if (global_sp_updated) { + global_sp_updated_set_once = true; + } + /* checking has to happen before the read, as the read clears the changed flag */ + + /* get a local copy of system state */ + orb_copy(ORB_ID(vehicle_status), state_sub, &state); + /* get a local copy of manual setpoint */ + orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); + /* get a local copy of attitude */ + orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); + /* get a local copy of attitude setpoint */ + orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp); + + if (pos_updated) { + + /* get position */ + orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); + + if (global_sp_updated_set_once) { + orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint); + + + /* calculate delta T */ + const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); + + /* calculate bearing error */ + float target_bearing = get_bearing_to_next_waypoint(global_pos.lat / (double)1e7d, global_pos.lon / (double)1e7d, + global_setpoint.lat / (double)1e7d, global_setpoint.lon / (double)1e7d); + + /* shift error to prevent wrapping issues */ + float bearing_error = att.yaw - target_bearing; + + if (bearing_error < M_PI_F) { + bearing_error += 2.0f * M_PI_F; + } + + if (bearing_error > M_PI_F) { + bearing_error -= 2.0f * M_PI_F; + } + + /* calculate roll setpoint, do this artificially around zero */ + att_sp.roll_body = pid_calculate(&heading_controller, bearing_error, + 0.0f, att.yawspeed, deltaT); + + /* limit roll angle output */ + if (att_sp.roll_body > ppos.heading_lim) { + att_sp.roll_body = ppos.heading_lim; + heading_controller.saturated = 1; + } + + if (att_sp.roll_body < -ppos.heading_lim) { + att_sp.roll_body = -ppos.heading_lim; + heading_controller.saturated = 1; + } + + att_sp.roll_body = att_sp.roll_body; + att_sp.pitch_body = 0.0f; + att_sp.yaw_body = 0.0f; + + } else { + /* no setpoint, maintain level flight */ + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + att_sp.yaw_body = 0.0f; + } + + att_sp.thrust = 0.7f; + } + + /* calculate delta T */ + const float deltaTpos = (hrt_absolute_time() - last_run_pos) / 1000000.0f; + last_run_pos = hrt_absolute_time(); + + actuators.control[0] = pid_calculate(&roll_pos_controller, att_sp.roll_body, + att.roll, att.rollspeed, deltaTpos); + actuators.control[1] = 0; + actuators.control[2] = 0; + actuators.control[3] = att_sp.thrust; + + /* publish attitude setpoint (for MAVLink) */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + + /* publish actuator setpoints (for mixer) */ + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + + loopcounter++; + + } + } + + printf("[fixedwing_control] exiting.\n"); + thread_running = false; + + return 0; +} + static void usage(const char *reason) { @@ -136,783 +408,40 @@ int fixedwing_control_main(int argc, char *argv[]) exit(1); } -#define PID_DT 0.01f -#define PID_SCALER 0.1f -#define PID_DERIVMODE_CALC 0 -#define HIL_MODE 32 -#define AUTO -1000 -#define MANUAL 3000 -#define SERVO_MIN 1000 -#define SERVO_MAX 2000 - -pthread_t control_thread; -pthread_t nav_thread; -pthread_t servo_thread; - -/** - * Servo channels function enumerator used for - * the servo writing part - */ -enum SERVO_CHANNELS_FUNCTION { - - AIL_1 = 0, - AIL_2 = 1, - MOT = 2, - ACT_1 = 3, - ACT_2 = 4, - ACT_3 = 5, - ACT_4 = 6, - ACT_5 = 7 -}; - -/** - * The plane_data structure. - * - * The plane data structure is the local storage of all the flight information of the aircraft - */ -typedef struct { - double lat; - double lon; - float alt; - float vx; - float vy; - float vz; - float yaw; - float hdg; - float pitch; - float roll; - float yawspeed; - float pitchspeed; - float rollspeed; - float rollb; /* body frame angles */ - float pitchb; - float yawb; - float p; - float q; - float r; /* body angular rates */ - - /* PID parameters*/ - - float Kp_att; - float Ki_att; - float Kd_att; - float Kp_pos; - float Ki_pos; - float Kd_pos; - float intmax_att; - float intmax_pos; - - /* Next waypoint*/ - - double wp_x; - double wp_y; - float wp_z; - - /* Setpoints */ - - float airspeed; - float groundspeed; - float roll_setpoint; - float pitch_setpoint; - float throttle_setpoint; - - /* Navigation mode*/ - int mode; - -} plane_data_t; - -/** - * The control_outputs structure. - * - * The control outputs structure contains the control outputs - * of the aircraft - */ -typedef struct { - float roll_ailerons; - float pitch_elevator; - float yaw_rudder; - float throttle; - // set the aux values to 0 per default - float aux1; - float aux2; - float aux3; - float aux4; - uint8_t mode; // HIL_ENABLED: 32 - uint8_t nav_mode; -} control_outputs_t; - -/** - * Generic PID algorithm with PD scaling - */ -static float pid(float error, float error_deriv, uint16_t dt, float scaler, float K_p, float K_i, float K_d, float intmax); - -/* - * Output calculations - */ - -static void calc_body_angular_rates(float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed); -static void calc_rotation_matrix(float roll, float pitch, float yaw, float x, float y, float z); -static void calc_bodyframe_angles(float roll, float pitch, float yaw); -static float calc_bearing(void); -static float calc_roll_ail(void); -static float calc_pitch_elev(void); -static float calc_yaw_rudder(float hdg); -static float calc_throttle(void); -static float calc_gnd_speed(void); -static void get_parameters(plane_data_t *plane_data); -static float calc_roll_setpoint(void); -static float calc_pitch_setpoint(void); -static float calc_throttle_setpoint(void); -static float calc_wp_distance(void); -static void set_plane_mode(void); - -/**************************************************************************** - * Public Data - ****************************************************************************/ - -plane_data_t plane_data; -control_outputs_t control_outputs; -float scaler = 1; //M_PI; - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - -/** - * Calculates the PID control output given an error. - * - * Incorporates PD scaling and low-pass filter for the derivative component. - * - * @param error the input error - * @param error_deriv the derivative of the input error - * @param dt time constant - * @param scaler PD scaler - * @param K_p P gain - * @param K_i I gain - * @param K_d D gain - * @param intmax Integration limit - * - * @return the PID control output - */ - -static float pid(float error, float error_deriv, uint16_t dt, float scale, float K_p, float K_i, float K_d, float intmax) +static int att_parameters_init(struct fw_att_control_param_handles *h) { - float Kp = K_p; - float Ki = K_i; - float Kd = K_d; - float delta_time = dt; - float lerror; - float imax = intmax; - float integrator; - float derivative; - float lderiv; - int fCut = 20; /* anything above 20 Hz is considered noise - low pass filter for the derivative */ - float output = 0; + /* PID parameters */ + h->roll_pos_p = param_find("FW_ROLL_POS_P"); + h->roll_pos_i = param_find("FW_ROLL_POS_I"); + h->roll_pos_lim = param_find("FW_ROLL_POS_LIM"); + h->roll_pos_awu = param_find("FW_ROLL_POS_AWU"); - output += error * Kp; - - if ((fabsf(Kd) > 0) && (dt > 0)) { - - if (PID_DERIVMODE_CALC) { - derivative = (error - lerror) / delta_time; - - /* - * discrete low pass filter, cuts out the - * high frequency noise that can drive the controller crazy - */ - float RC = 1.0f / (2.0f * M_PI_F * fCut); - derivative = lderiv + - (delta_time / (RC + delta_time)) * (derivative - lderiv); - - /* update state */ - lerror = error; - lderiv = derivative; - - } else { - derivative = error_deriv; - } - - /* add in derivative component */ - output += Kd * derivative; - } - - //printf("PID derivative %i\n", (int)(1000*derivative)); - - /* scale the P and D components with the PD scaler */ - output *= scale; - - /* Compute integral component if time has elapsed */ - if ((fabsf(Ki) > 0) && (dt > 0)) { - integrator += (error * Ki) * scaler * delta_time; - - if (integrator < -imax) { - integrator = -imax; - - } else if (integrator > imax) { - integrator = imax; - } - - output += integrator; - } - - //printf("PID Integrator %i\n", (int)(1000*integrator)); - - return output; + return OK; } -PARAM_DEFINE_FLOAT(FW_ATT_P, 2.0f); -PARAM_DEFINE_FLOAT(FW_ATT_I, 0.0f); -PARAM_DEFINE_FLOAT(FW_ATT_D, 0.0f); -PARAM_DEFINE_FLOAT(FW_ATT_AWU, 5.0f); -PARAM_DEFINE_FLOAT(FW_ATT_LIM, 10.0f); - -PARAM_DEFINE_FLOAT(FW_POS_P, 2.0f); -PARAM_DEFINE_FLOAT(FW_POS_I, 0.0f); -PARAM_DEFINE_FLOAT(FW_POS_D, 0.0f); -PARAM_DEFINE_FLOAT(FW_POS_AWU, 5.0f); -PARAM_DEFINE_FLOAT(FW_POS_LIM, 10.0f); - -/** - * Load parameters from global storage. - * - * @param plane_data Fixed wing data structure - * - * Fetches the current parameters from the global parameter storage and writes them - * to the plane_data structure - */ -static void get_parameters(plane_data_t * pdata) +static int att_parameters_update(const struct fw_att_control_param_handles *h, struct fw_att_control_params *p) { - static bool initialized = false; - static param_t att_p; - static param_t att_i; - static param_t att_d; - static param_t att_awu; - static param_t att_lim; + param_get(h->roll_pos_p, &(p->roll_pos_p)); + param_get(h->roll_pos_i, &(p->roll_pos_i)); + param_get(h->roll_pos_lim, &(p->roll_pos_lim)); + param_get(h->roll_pos_awu, &(p->roll_pos_awu)); - static param_t pos_p; - static param_t pos_i; - static param_t pos_d; - static param_t pos_awu; - static param_t pos_lim; - - if (!initialized) { - att_p = param_find("FW_ATT_P"); - att_i = param_find("FW_ATT_I"); - att_d = param_find("FW_ATT_D"); - att_awu = param_find("FW_ATT_AWU"); - att_lim = param_find("FW_ATT_LIM"); - - pos_p = param_find("FW_POS_P"); - pos_i = param_find("FW_POS_I"); - pos_d = param_find("FW_POS_D"); - pos_awu = param_find("FW_POS_AWU"); - pos_lim = param_find("FW_POS_LIM"); - - initialized = true; - } - - param_get(att_p, &(pdata->Kp_att)); - param_get(att_i, &(pdata->Ki_att)); - param_get(att_d, &(pdata->Kd_att)); - param_get(pos_p, &(pdata->Kp_pos)); - param_get(pos_i, &(pdata->Ki_pos)); - param_get(pos_d, &(pdata->Kd_pos)); - param_get(att_awu, &(pdata->intmax_att)); - param_get(pos_awu, &(pdata->intmax_pos)); - pdata->airspeed = 10; - pdata->mode = 1; + return OK; } -/** - * Calculates the body angular rates. - * - * Calculates the rates of the plane using inertia matrix and - * writes them to the plane_data structure - * - * @param roll - * @param pitch - * @param yaw - * @param rollspeed - * @param pitchspeed - * @param yawspeed - * - */ -static void calc_body_angular_rates(float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) +static int pos_parameters_init(struct fw_pos_control_param_handles *h) { - plane_data.p = rollspeed - sinf(pitch) * yawspeed; - plane_data.q = cosf(roll) * pitchspeed + sinf(roll) * cosf(pitch) * yawspeed; - plane_data.r = -sinf(roll) * pitchspeed + cosf(roll) * cosf(pitch) * yawspeed; + /* PID parameters */ + h->heading_p = param_find("FW_HEADING_P"); + h->heading_lim = param_find("FW_HEADING_LIM"); + + return OK; } -/** - * - * Calculates the attitude angles in the body reference frame. - * - * Writes them to the plane data structure - * - * @param roll - * @param pitch - * @param yaw - */ - -static void calc_bodyframe_angles(float roll, float pitch, float yaw) +static int pos_parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p) { - plane_data.rollb = cosf(yaw) * cosf(pitch) * roll + - (cosf(yaw) * sinf(pitch) * sinf(roll) + sinf(yaw) * cosf(roll)) * pitch - + (-cosf(yaw) * sinf(pitch) * cosf(roll) + sinf(yaw) * sinf(roll)) * yaw; - plane_data.pitchb = -sinf(yaw) * cosf(pitch) * roll + - (-sinf(yaw) * sinf(pitch) * sinf(roll) + cosf(yaw) * cosf(roll)) * pitch - + (sinf(yaw) * sinf(pitch) * cosf(roll) + cosf(yaw) * sinf(roll)) * yaw; - plane_data.yawb = sinf(pitch) * roll - cosf(pitch) * sinf(roll) * pitch + cosf(pitch) * cosf(roll) * yaw; -} - -/** - * calc_rotation_matrix - * - * Calculates the rotation matrix - * - * @param roll - * @param pitch - * @param yaw - * @param x - * @param y - * @param z - * - */ - -static void calc_rotation_matrix(float roll, float pitch, float yaw, float x, float y, float z) -{ - plane_data.rollb = cosf(yaw) * cosf(pitch) * x + - (cosf(yaw) * sinf(pitch) * sinf(roll) + sinf(yaw) * cosf(roll)) * y - + (-cosf(yaw) * sinf(pitch) * cosf(roll) + sinf(yaw) * sinf(roll)) * z; - plane_data.pitchb = -sinf(yaw) * cosf(pitch) * x + - (-sinf(yaw) * sinf(pitch) * sinf(roll) + cosf(yaw) * cosf(roll)) * y - + (sinf(yaw) * sinf(pitch) * cosf(roll) + cosf(yaw) * sinf(roll)) * z; - plane_data.yawb = sinf(pitch) * x - cosf(pitch) * sinf(roll) * y + cosf(pitch) * cosf(roll) * z; -} - -/** - * calc_bearing - * - * Calculates the bearing error of the plane compared to the waypoints - * - * @return bearing Bearing error - * - */ -static float calc_bearing() -{ - float bearing = M_PI_F/2.0f + (float)atan2(-(plane_data.wp_y - plane_data.lat), (plane_data.wp_x - plane_data.lon)); - - if (bearing < 0.0f) { - bearing += 2*M_PI_F; - } - - return bearing; -} - -/** - * calc_roll_ail - * - * Calculates the roll ailerons control output - * - * @return Roll ailerons control output (-1,1) - */ - -static float calc_roll_ail() -{ - float ret = pid((plane_data.roll_setpoint - plane_data.roll), plane_data.rollspeed, PID_DT, PID_SCALER, - plane_data.Kp_att, plane_data.Ki_att, plane_data.Kd_att, plane_data.intmax_att); - - if (ret < -1) - return -1; - - if (ret > 1) - return 1; - - return ret; -} - -/** - * calc_pitch_elev - * - * Calculates the pitch elevators control output - * - * @return Pitch elevators control output (-1,1) - */ -static float calc_pitch_elev() -{ - float ret = pid((plane_data.pitch_setpoint - plane_data.pitch), plane_data.pitchspeed, PID_DT, PID_SCALER, - plane_data.Kp_att, plane_data.Ki_att, plane_data.Kd_att, plane_data.intmax_att); - - if (ret < -1) - return -1; - - if (ret > 1) - return 1; - - return ret; -} - -/** - * calc_yaw_rudder - * - * Calculates the yaw rudder control output (only if yaw rudder exists on the model) - * - * @return Yaw rudder control output (-1,1) - * - */ -static float calc_yaw_rudder(float hdg) -{ - float ret = pid((plane_data.yaw - abs(hdg)), plane_data.yawspeed, PID_DT, PID_SCALER, - plane_data.Kp_pos, plane_data.Ki_pos, plane_data.Kd_pos, plane_data.intmax_pos); - - if (ret < -1) - return -1; - - if (ret > 1) - return 1; - - return ret; -} - -/** - * calc_throttle - * - * Calculates the throttle control output - * - * @return Throttle control output (0,1) - */ - -static float calc_throttle() -{ - float ret = pid(plane_data.throttle_setpoint - calc_gnd_speed(), 0, PID_DT, PID_SCALER, - plane_data.Kp_pos, plane_data.Ki_pos, plane_data.Kd_pos, plane_data.intmax_pos); - - if (ret < 0.2f) - return 0.2f; - - if (ret > 1.0f) - return 1.0f; - - return ret; -} - -/** - * calc_gnd_speed - * - * Calculates the ground speed using the x and y components - * - * Input: none (operation on global data) - * - * Output: Ground speed of the plane - * - */ - -static float calc_gnd_speed() -{ - float gnd_speed = sqrtf(plane_data.vx * plane_data.vx + plane_data.vy * plane_data.vy); - return gnd_speed; -} - -/** - * calc_wp_distance - * - * Calculates the distance to the next waypoint - * - * @return the distance to the next waypoint - * - */ - -static float calc_wp_distance() -{ - return sqrtf((plane_data.lat - plane_data.wp_y) * (plane_data.lat - plane_data.wp_y) + - (plane_data.lon - plane_data.wp_x) * (plane_data.lon - plane_data.wp_x)); -} - -/** - * calc_roll_setpoint - * - * Calculates the offset angle for the roll plane, - * saturates at +- 35 deg. - * - * @return setpoint on which attitude control should stabilize while changing heading - * - */ - -static float calc_roll_setpoint() -{ - float setpoint = 0; - - setpoint = calc_bearing() - plane_data.yaw; - - if (setpoint < (-35.0f/180.0f)*M_PI_F) - return (-35.0f/180.0f)*M_PI_F; - - if (setpoint > (35/180.0f)*M_PI_F) - return (-35.0f/180.0f)*M_PI_F; - - return setpoint; -} - -/** - * calc_pitch_setpoint - * - * Calculates the offset angle for the pitch plane - * saturates at +- 35 deg. - * - * @return setpoint on which attitude control should stabilize while changing altitude - * - */ - -static float calc_pitch_setpoint() -{ - float setpoint = 0.0f; - - // if (plane_data.mode == TAKEOFF) { - // setpoint = ((35/180.0f)*M_PI_F); - - // } else { - setpoint = atanf((plane_data.wp_z - plane_data.alt) / calc_wp_distance()); - - if (setpoint < (-35.0f/180.0f)*M_PI_F) - return (-35.0f/180.0f)*M_PI_F; - - if (setpoint > (35/180.0f)*M_PI_F) - return (-35.0f/180.0f)*M_PI_F; - // } - - return setpoint; -} - -/** - * - * Calculates the throttle setpoint for different flight modes - * - * @return throttle output setpoint - * - */ -static float calc_throttle_setpoint() -{ - float setpoint = 0; - - // // if TAKEOFF full throttle - // if (plane_data.mode == TAKEOFF) { - // setpoint = 60.0f; - // } - - // // if CRUISE - parameter value - // if (plane_data.mode == CRUISE) { - setpoint = plane_data.airspeed; - // } - - // // if LAND no throttle - // if (plane_data.mode == LAND) { - // setpoint = 0.0f; - // } - - return setpoint; -} - -/** - * - * @param argc number of arguments - * @param argv argument array - * - * @return 0 - * - */ -int fixedwing_control_thread_main(int argc, char *argv[]) -{ - /* default values for arguments */ - char *fixedwing_uart_name = "/dev/ttyS1"; - char *commandline_usage = "\tusage: %s -d fixedwing-devicename\n"; - - /* read arguments */ - bool verbose = false; - - for (int i = 1; i < argc; i++) { - if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { - if (argc > i + 1) { - fixedwing_uart_name = argv[i + 1]; - - } else { - printf(commandline_usage, argv[0]); - return 0; - } - } - if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) { - verbose = true; - } - } - - /* welcome user */ - printf("[fixedwing control] started\n"); - - /* output structs */ - struct actuator_controls_s actuators; - struct vehicle_attitude_setpoint_s att_sp; - memset(&att_sp, 0, sizeof(att_sp)); - - /* publish actuator controls */ - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) - actuators.control[i] = 0.0f; - orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); - orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - - /* Subscribe to global position, attitude and rc */ - struct vehicle_global_position_s global_pos; - int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - struct vehicle_global_position_setpoint_s global_setpoint; - int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); - /* declare and safely initialize all structs */ - struct vehicle_status_s state; - memset(&state, 0, sizeof(state)); - struct vehicle_attitude_s att; - memset(&att_sp, 0, sizeof(att_sp)); - struct manual_control_setpoint_s manual; - memset(&manual, 0, sizeof(manual)); - - /* subscribe to attitude, motor setpoints and system state */ - int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - int state_sub = orb_subscribe(ORB_ID(vehicle_status)); - int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - - /* Mainloop setup */ - unsigned int loopcounter = 0; - unsigned int failcounter = 0; - - /* Control constants */ - control_outputs.mode = HIL_MODE; - control_outputs.nav_mode = 0; - - /* Servo setup */ - - /* - * Main control, navigation and servo routine - */ - - while(1) { - /* - * DATA Handling - * Fetch current flight data - */ - - /* get position, attitude and rc inputs */ - // XXX add error checking - orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); - orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint); - - /* get a local copy of system state */ - orb_copy(ORB_ID(vehicle_status), state_sub, &state); - /* get a local copy of manual setpoint */ - orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); - /* get a local copy of attitude */ - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - /* get a local copy of attitude setpoint */ - orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp); - - /* scaling factors are defined by the data from the APM Planner - * TODO: ifdef for other parameters (HIL/Real world switch) - */ - - /* position values*/ - plane_data.lat = global_pos.lat / 10000000.0; - plane_data.lon = global_pos.lon / 10000000.0; - plane_data.alt = global_pos.alt / 1000.0f; - plane_data.vx = global_pos.vx / 100.0f; - plane_data.vy = global_pos.vy / 100.0f; - plane_data.vz = global_pos.vz / 100.0f; - - /* attitude values*/ - plane_data.roll = att.roll; - plane_data.pitch = att.pitch; - plane_data.yaw = att.yaw; - plane_data.rollspeed = att.rollspeed; - plane_data.pitchspeed = att.pitchspeed; - plane_data.yawspeed = att.yawspeed; - - plane_data.wp_x = global_setpoint.lat / (double)1e7; - plane_data.wp_y = global_setpoint.lon / (double)1e7; - plane_data.wp_z = global_setpoint.altitude; - - /* parameter values */ - if (loopcounter % 20 == 0) { - get_parameters(&plane_data); - } - - /* Attitude control part */ - - if (verbose && loopcounter % 20 == 0) { - /******************************** DEBUG OUTPUT ************************************************************/ - - printf("Parameter: %i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i\n", (int)plane_data.Kp_att, (int)plane_data.Ki_att, - (int)plane_data.Kd_att, (int)plane_data.intmax_att, (int)plane_data.Kp_pos, (int)plane_data.Ki_pos, - (int)plane_data.Kd_pos, (int)plane_data.intmax_pos, (int)plane_data.airspeed, - (int)plane_data.wp_x, (int)plane_data.wp_y, (int)plane_data.wp_z); - - printf("PITCH SETPOINT: %i\n", (int)(100.0f*plane_data.pitch_setpoint)); - printf("ROLL SETPOINT: %i\n", (int)(100.0f*plane_data.roll_setpoint)); - printf("THROTTLE SETPOINT: %i\n", (int)(100.0f*calc_throttle_setpoint())); - - // printf("\n\nVx: %i\t Vy: %i\t Current speed:%i\n\n", (int)plane_data.vx, (int)plane_data.vy, (int)(calc_gnd_speed())); - - // printf("Current Altitude: %i\n\n", (int)plane_data.alt); - - printf("\nAttitude values: \n R:%i \n P: %i \n Y: %i \n\n RS: %i \n PS: %i \n YS: %i \n ", - (int)(180.0f * plane_data.roll/M_PI_F), (int)(180.0f * plane_data.pitch/M_PI_F), (int)(180.0f * plane_data.yaw/M_PI_F), - (int)(180.0f * plane_data.rollspeed/M_PI_F), (int)(180.0f * plane_data.pitchspeed/M_PI_F), (int)(180.0f * plane_data.yawspeed)/M_PI_F); - - // printf("\nBody Rates: \n P: %i \n Q: %i \n R: %i \n", - // (int)(10000 * plane_data.p), (int)(10000 * plane_data.q), (int)(10000 * plane_data.r)); - - printf("\nCalculated outputs: \n R: %8.4f\n P: %8.4f\n Y: %8.4f\n T: %8.4f \n", - att_sp.roll_body, att_sp.pitch_body, - att_sp.yaw_body, att_sp.thrust); - - /************************************************************************************************************/ - } - - /* Calculate the P,Q,R body rates of the aircraft */ - //calc_body_angular_rates(plane_data.roll, plane_data.pitch, plane_data.yaw, - // plane_data.rollspeed, plane_data.pitchspeed, plane_data.yawspeed); - - /* Calculate the body frame angles of the aircraft */ - //calc_bodyframe_angles(plane_data.roll,plane_data.pitch,plane_data.yaw); - - // if (manual.override_mode_switch < XXX) { - - - /* Navigation part */ - - // Get Waypoint - - // XXX FIXME - - //else if (manual.override_mode_switch > MANUAL) { // AUTO mode - - /* calculate setpoint based on current position and waypoint */ - att_sp.roll_body = calc_roll_setpoint(); - att_sp.pitch_body = calc_pitch_setpoint(); - att_sp.thrust = calc_throttle_setpoint(); - - /* calculate the control outputs based on roll / pitch / yaw setpoints */ - actuators.control[0] = calc_roll_ail(); - actuators.control[1] = calc_pitch_elev(); - actuators.control[2] = calc_yaw_rudder(att.yaw); - actuators.control[3] = calc_throttle(); - - /* publish attitude setpoint (for MAVLink) */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - - /* publish actuator setpoints (for mixer) */ - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - - loopcounter++; - - /* 20Hz loop*/ - usleep(50000); - } - - return 0; + param_get(h->heading_p, &(p->heading_p)); + param_get(h->heading_lim, &(p->heading_lim)); + + return OK; } diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index fb96866b0d..a1458ca03b 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -65,7 +65,7 @@ #include #include #include -#include +#include #include #include #include @@ -126,14 +126,13 @@ static struct vehicle_attitude_s hil_attitude; static struct vehicle_global_position_s hil_global_pos; -static struct ardrone_motors_setpoint_s ardrone_motors; +static struct offboard_control_setpoint_s offboard_control_sp; static struct vehicle_command_s vcmd; static struct actuator_armed_s armed; static orb_advert_t pub_hil_global_pos = -1; -static orb_advert_t ardrone_motors_pub = -1; static orb_advert_t cmd_pub = -1; static orb_advert_t flow_pub = -1; static orb_advert_t global_position_setpoint_pub = -1; @@ -188,6 +187,12 @@ static struct mavlink_subscriptions { .initialized = false }; +static struct mavlink_publications { + orb_advert_t offboard_control_sp_pub; +} mavlink_pubs = { + .offboard_control_sp_pub = -1 +}; + /* 3: Define waypoint helper functions */ void mavlink_wpm_send_message(mavlink_message_t *msg); @@ -198,6 +203,7 @@ int mavlink_missionlib_send_gcs_string(const char *string); uint64_t mavlink_missionlib_get_system_timestamp(void); void handleMessage(mavlink_message_t *msg); +static void mavlink_update_system(); /** * Enable / disable Hardware in the Loop simulation mode. @@ -931,29 +937,6 @@ static void *uorb_receiveloop(void *arg) orb_copy(ORB_ID(vehicle_attitude_setpoint), subs->spa_sub, &buf.att_sp); mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, buf.att_sp.timestamp/1000, buf.att_sp.roll_body, buf.att_sp.pitch_body, buf.att_sp.yaw_body, buf.att_sp.thrust); - - /* Only send in HIL mode */ - if (mavlink_hil_enabled) { - - /* translate the current syste state to mavlink state and mode */ - uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode); - - /* HIL message as per MAVLink spec */ - mavlink_msg_hil_controls_send(chan, - hrt_absolute_time(), - buf.att_sp.roll_body, /* this may be replaced by ctrl0 later */ - buf.att_sp.pitch_body, - buf.att_sp.yaw_body, - buf.att_sp.thrust, - 0, - 0, - 0, - 0, - mavlink_mode, - 0); - } } /* --- ACTUATOR OUTPUTS 0 --- */ @@ -970,6 +953,7 @@ static void *uorb_receiveloop(void *arg) buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]); + // if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) { // mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(), // 1 /* port 1 */, @@ -1072,8 +1056,8 @@ static void *uorb_receiveloop(void *arg) if (fds[ifds++].revents & POLLIN) { /* copy local position data into local buffer */ orb_copy(ORB_ID(manual_control_setpoint), subs->man_control_sp_sub, &buf.man_control); - mavlink_msg_manual_control_send(MAVLINK_COMM_0, mavlink_system.sysid, buf.man_control.roll, - buf.man_control.pitch, buf.man_control.yaw, buf.man_control.throttle, 0); + mavlink_msg_manual_control_send(MAVLINK_COMM_0, mavlink_system.sysid, buf.man_control.roll*1000, + buf.man_control.pitch*1000, buf.man_control.yaw*1000, buf.man_control.throttle*1000, 0); } /* --- ACTUATOR ARMED --- */ @@ -1088,6 +1072,29 @@ static void *uorb_receiveloop(void *arg) mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl1 ", buf.actuators.control[1]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl2 ", buf.actuators.control[2]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl3 ", buf.actuators.control[3]); + + /* Only send in HIL mode */ + if (mavlink_hil_enabled) { + + /* translate the current syste state to mavlink state and mode */ + uint8_t mavlink_state = 0; + uint8_t mavlink_mode = 0; + get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode); + + /* HIL message as per MAVLink spec */ + mavlink_msg_hil_controls_send(chan, + hrt_absolute_time(), + buf.actuators.control[0], + buf.actuators.control[1], + buf.actuators.control[2], + buf.actuators.control[3], + 0, + 0, + 0, + 0, + mavlink_mode, + 0); + } } /* --- DEBUG KEY/VALUE --- */ @@ -1133,6 +1140,8 @@ mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) } } +#define MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED 0x10 + /**************************************************************************** * Public Functions ****************************************************************************/ @@ -1242,30 +1251,105 @@ void handleMessage(mavlink_message_t *msg) // printf("got MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT target_system=%u, sysid = %u\n", quad_motors_setpoint.target_system, mavlink_system.sysid); if (mavlink_system.sysid < 4) { - ardrone_motors.p1 = quad_motors_setpoint.roll[mavlink_system.sysid]; - ardrone_motors.p2 = quad_motors_setpoint.pitch[mavlink_system.sysid]; - ardrone_motors.p3 = quad_motors_setpoint.yaw[mavlink_system.sysid]; - ardrone_motors.p4 = quad_motors_setpoint.thrust[mavlink_system.sysid]; + /* + * rate control mode - defined by MAVLink + */ - ardrone_motors.timestamp = hrt_absolute_time(); + uint8_t ml_mode = 0; + bool ml_armed = false; - /* only send if RC is off */ - if (v_status.rc_signal_lost) { - /* check if input has to be enabled */ - if (!v_status.flag_control_offboard_enabled) { - /* XXX Enable offboard control */ - } - - /* XXX decode mode and set flags */ - // if (mode == abc) xxx flag_control_rates_enabled; - - /* check if topic has to be advertised */ - if (ardrone_motors_pub <= 0) { - ardrone_motors_pub = orb_advertise(ORB_ID(ardrone_motors_setpoint), &ardrone_motors); - } - /* Publish */ - orb_publish(ORB_ID(ardrone_motors_setpoint), ardrone_motors_pub, &ardrone_motors); + if (quad_motors_setpoint.mode & MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED) { + ml_armed = true; } + + switch (quad_motors_setpoint.mode) { + case 0: + break; + case 1: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; + break; + case 2: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; + break; + case 3: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; + break; + case 4: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; + break; + } + + offboard_control_sp.p1 = quad_motors_setpoint.roll[mavlink_system.sysid] / (float)INT16_MAX; + offboard_control_sp.p2 = quad_motors_setpoint.pitch[mavlink_system.sysid] / (float)INT16_MAX; + offboard_control_sp.p3= quad_motors_setpoint.yaw[mavlink_system.sysid] / (float)INT16_MAX; + offboard_control_sp.p4 = quad_motors_setpoint.thrust[mavlink_system.sysid] / (float)UINT16_MAX; + offboard_control_sp.armed = ml_armed; + offboard_control_sp.mode = ml_mode; + + offboard_control_sp.timestamp = hrt_absolute_time(); + + /* check if topic has to be advertised */ + if (mavlink_pubs.offboard_control_sp_pub <= 0) { + mavlink_pubs.offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); + } else { + /* Publish */ + orb_publish(ORB_ID(offboard_control_setpoint), mavlink_pubs.offboard_control_sp_pub, &offboard_control_sp); + } + + // /* change armed status if required */ + // bool cmd_armed = (quad_motors_setpoint.mode & MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED); + + // bool cmd_generated = false; + + // if (v_status.flag_control_offboard_enabled != cmd_armed) { + // vcmd.param1 = cmd_armed; + // vcmd.param2 = 0; + // vcmd.param3 = 0; + // vcmd.param4 = 0; + // vcmd.param5 = 0; + // vcmd.param6 = 0; + // vcmd.param7 = 0; + // vcmd.command = MAV_CMD_COMPONENT_ARM_DISARM; + // vcmd.target_system = mavlink_system.sysid; + // vcmd.target_component = MAV_COMP_ID_ALL; + // vcmd.source_system = msg->sysid; + // vcmd.source_component = msg->compid; + // vcmd.confirmation = 1; + + // cmd_generated = true; + // } + + // /* check if input has to be enabled */ + // if ((v_status.flag_control_rates_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_RATES)) || + // (v_status.flag_control_attitude_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE)) || + // (v_status.flag_control_velocity_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY)) || + // (v_status.flag_control_position_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_POSITION))) { + // vcmd.param1 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_RATES); + // vcmd.param2 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE); + // vcmd.param3 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY); + // vcmd.param4 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_POSITION); + // vcmd.param5 = 0; + // vcmd.param6 = 0; + // vcmd.param7 = 0; + // vcmd.command = PX4_CMD_CONTROLLER_SELECTION; + // vcmd.target_system = mavlink_system.sysid; + // vcmd.target_component = MAV_COMP_ID_ALL; + // vcmd.source_system = msg->sysid; + // vcmd.source_component = msg->compid; + // vcmd.confirmation = 1; + + // cmd_generated = true; + // } + + // if (cmd_generated) { + // /* check if topic is advertised */ + // if (cmd_pub <= 0) { + // cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + // } else { + // /* create command */ + // orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); + // } + // } } } @@ -1332,10 +1416,10 @@ void handleMessage(mavlink_message_t *msg) memset(&rc_hil, 0, sizeof(rc_hil)); static orb_advert_t rc_pub = 0; - rc_hil.chan[0].raw = 1510 + man.x * 500; - rc_hil.chan[1].raw = 1520 + man.y * 500; - rc_hil.chan[2].raw = 1590 + man.r * 500; - rc_hil.chan[3].raw = 1420 + man.z * 500; + rc_hil.chan[0].raw = 1510 + man.x / 2; + rc_hil.chan[1].raw = 1520 + man.y / 2; + rc_hil.chan[2].raw = 1590 + man.r / 2; + rc_hil.chan[3].raw = 1420 + man.z / 2; rc_hil.chan[0].scaled = man.x; rc_hil.chan[1].scaled = man.y; @@ -1345,10 +1429,10 @@ void handleMessage(mavlink_message_t *msg) struct manual_control_setpoint_s mc; static orb_advert_t mc_pub = 0; - mc.roll = man.x; - mc.pitch = man.y; - mc.yaw = man.r; - mc.roll = man.z; + mc.roll = man.x / 1000.0f; + mc.pitch = man.y / 1000.0f; + mc.yaw = man.r / 1000.0f; + mc.roll = man.z / 1000.0f; /* fake RC channels with manual control input from simulator */ @@ -1465,6 +1549,39 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf return uart; } +void mavlink_update_system() +{ + static initialized = false; + param_t param_system_id; + param_t param_component_id; + param_t param_system_type; + + if (!initialized) { + param_system_id = param_find("MAV_SYS_ID"); + param_component_id = param_find("MAV_COMP_ID"); + param_system_type = param_find("MAV_TYPE"); + } + + /* update system and component id */ + int32_t system_id; + param_get(param_system_id, &system_id); + if (system_id > 0 && system_id < 255) { + mavlink_system.sysid = system_id; + } + + int32_t component_id; + param_get(param_component_id, &component_id); + if (component_id > 0 && component_id < 255) { + mavlink_system.compid = component_id; + } + + int32_t system_type; + param_get(param_system_type, &system_type); + if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) { + mavlink_system.type = system_type; + } +} + /** * MAVLink Protocol main function. */ @@ -1479,7 +1596,7 @@ int mavlink_thread_main(int argc, char *argv[]) memset(&rc, 0, sizeof(rc)); memset(&hil_attitude, 0, sizeof(hil_attitude)); memset(&hil_global_pos, 0, sizeof(hil_global_pos)); - memset(&ardrone_motors, 0, sizeof(ardrone_motors)); + memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); memset(&vcmd, 0, sizeof(vcmd)); /* print welcome text */ @@ -1541,9 +1658,7 @@ int mavlink_thread_main(int argc, char *argv[]) fflush(stdout); /* Initialize system properties */ - param_t param_system_id = param_find("MAV_SYS_ID"); - param_t param_component_id = param_find("MAV_COMP_ID"); - param_t param_system_type = param_find("MAV_TYPE"); + mavlink_update_system(); /* topics to subscribe globally */ /* subscribe to ORB for global position */ @@ -1582,7 +1697,7 @@ int mavlink_thread_main(int argc, char *argv[]) set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 5); /* 200 Hz / 5 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 5); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 5); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 3); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 5); /* 5 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 200); @@ -1647,24 +1762,7 @@ int mavlink_thread_main(int argc, char *argv[]) /* 1 Hz */ if (lowspeed_counter == 10) { - /* update system and component id */ - int32_t system_id; - param_get(param_system_id, &system_id); - if (system_id > 0 && system_id < 255) { - mavlink_system.sysid = system_id; - } - - int32_t component_id; - param_get(param_component_id, &component_id); - if (component_id > 0 && component_id < 255) { - mavlink_system.compid = component_id; - } - - int32_t system_type; - param_get(param_system_type, &system_type); - if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) { - mavlink_system.type = system_type; - } + mavlink_update_system(); /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 5300337015..ebd9911a36 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -1,7 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,6 +36,8 @@ * @file multirotor_att_control_main.c * * Implementation of multirotor attitude control main loop. + * + * @author Lorenz Meier */ #include @@ -54,36 +56,92 @@ #include #include #include +#include #include #include #include #include -#include +#include +#include #include #include #include #include "multirotor_attitude_control.h" +#include "multirotor_rate_control.h" __EXPORT int multirotor_att_control_main(int argc, char *argv[]); - -static enum { - CONTROL_MODE_RATES = 0, - CONTROL_MODE_ATTITUDE = 1, -} control_mode; - - static bool thread_should_exit; static int mc_task; static bool motor_test_mode = false; +static orb_advert_t actuator_pub; + +static struct vehicle_status_s state; + +/** + * Perform rate control right after gyro reading + */ +static void *rate_control_thread_main(void *arg) +{ + prctl(PR_SET_NAME, "mc rate control", getpid()); + + struct actuator_controls_s actuators; + + int gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); + int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + + struct pollfd fds = { .fd = gyro_sub, .events = POLLIN }; + + struct gyro_report gyro_report; + struct vehicle_rates_setpoint_s rates_sp; + memset(&rates_sp, 0, sizeof(rates_sp)); + float gyro_lp[3] = {0.0f, 0.0f, 0.0f}; + + while (!thread_should_exit) { + /* rate control at maximum rate */ + /* wait for a sensor update, check for exit condition every 1000 ms */ + int ret = poll(&fds, 1, 1000); + + if (ret < 0) { + /* XXX this is seriously bad - should be an emergency */ + } else if (ret == 0) { + /* XXX this means no sensor data - should be critical or emergency */ + printf("[mc att control] WARNING: Not getting gyro data, no rate control\n"); + } else { + /* get data */ + orb_copy(ORB_ID(sensor_gyro), gyro_sub, &gyro_report); + bool rates_sp_valid = false; + orb_check(rates_sp_sub, &rates_sp_valid); + if (rates_sp_valid) { + orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp); + } + + /* perform local lowpass */ + + /* apply controller */ + if (state.flag_control_rates_enabled) { + /* lowpass gyros */ + // XXX + gyro_lp[0] = gyro_report.x; + gyro_lp[1] = gyro_report.y; + gyro_lp[2] = gyro_report.z; + + multirotor_control_rates(&rates_sp, gyro_lp, &actuators); + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + } + } + } + + return NULL; +} + static int mc_thread_main(int argc, char *argv[]) { /* declare and safely initialize all structs */ - struct vehicle_status_s state; memset(&state, 0, sizeof(state)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); @@ -93,18 +151,20 @@ mc_thread_main(int argc, char *argv[]) memset(&manual, 0, sizeof(manual)); struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); - struct ardrone_motors_setpoint_s setpoint; - memset(&setpoint, 0, sizeof(setpoint)); + struct offboard_control_setpoint_s offboard_sp; + memset(&offboard_sp, 0, sizeof(offboard_sp)); + struct vehicle_rates_setpoint_s rates_sp; + memset(&rates_sp, 0, sizeof(rates_sp)); struct actuator_controls_s actuators; /* subscribe to attitude, motor setpoints and system state */ int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); int state_sub = orb_subscribe(ORB_ID(vehicle_status)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - int setpoint_sub = orb_subscribe(ORB_ID(ardrone_motors_setpoint)); /* * Do not rate-limit the loop to prevent aliasing @@ -121,8 +181,9 @@ mc_thread_main(int argc, char *argv[]) actuators.control[i] = 0.0f; } - orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); + actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); /* register the perf counter */ perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control"); @@ -130,6 +191,13 @@ mc_thread_main(int argc, char *argv[]) /* welcome user */ printf("[multirotor_att_control] starting\n"); + /* ready, spawn pthread */ + pthread_attr_t rate_control_attr; + pthread_attr_init(&rate_control_attr); + pthread_attr_setstacksize(&rate_control_attr, 2048); + pthread_t rate_control_thread; + pthread_create(&rate_control_thread, &rate_control_attr, rate_control_thread_main, NULL); + while (!thread_should_exit) { /* wait for a sensor update, check for exit condition every 500 ms */ @@ -138,13 +206,22 @@ mc_thread_main(int argc, char *argv[]) perf_begin(mc_loop_perf); /* get a local copy of system state */ - orb_copy(ORB_ID(vehicle_status), state_sub, &state); + bool updated; + orb_check(state_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_status), state_sub, &state); + } /* get a local copy of manual setpoint */ orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); /* get a local copy of attitude */ orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); /* get a local copy of attitude setpoint */ orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp); + /* get a local copy of rates setpoint */ + orb_check(setpoint_sub, &updated); + if (updated) { + orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp); + } /* get a local copy of the current sensor values */ orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); @@ -155,41 +232,57 @@ mc_thread_main(int argc, char *argv[]) /* manual inputs, from RC control or joystick */ att_sp.roll_body = manual.roll; att_sp.pitch_body = manual.pitch; - att_sp.yaw_rate_body = manual.yaw; + att_sp.yaw_body = manual.yaw; // XXX Hack, remove, switch to yaw rate controller + /* set yaw rate */ + rates_sp.yaw = manual.yaw; + att_sp.thrust = manual.throttle; att_sp.timestamp = hrt_absolute_time(); + /* STEP 2: publish the result to the vehicle actuators */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); if (motor_test_mode) { att_sp.roll_body = 0.0f; att_sp.pitch_body = 0.0f; att_sp.yaw_body = 0.0f; - att_sp.roll_rate_body = 0.0f; - att_sp.pitch_rate_body = 0.0f; - att_sp.yaw_rate_body = 0.0f; att_sp.thrust = 0.1f; - } else { - att_sp.thrust = manual.throttle; + att_sp.timestamp = hrt_absolute_time(); + /* STEP 2: publish the result to the vehicle actuators */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } + } else if (state.flag_control_offboard_enabled) { /* offboard inputs */ + if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { + rates_sp.roll = offboard_sp.p1; + rates_sp.pitch = offboard_sp.p2; + rates_sp.yaw = offboard_sp.p3; + rates_sp.thrust = offboard_sp.p4; + rates_sp.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); + } else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { + att_sp.roll_body = offboard_sp.p1; + att_sp.pitch_body = offboard_sp.p2; + att_sp.yaw_body = offboard_sp.p3; + att_sp.thrust = offboard_sp.p4; + att_sp.timestamp = hrt_absolute_time(); + /* STEP 2: publish the result to the vehicle actuators */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } /* decide wether we want rate or position input */ } - /** STEP 2: Identify the controller setup to run and set up the inputs correctly */ + /** STEP 3: Identify the controller setup to run and set up the inputs correctly */ /* run attitude controller */ - if (state.flag_control_attitude_enabled) { - multirotor_control_attitude(&att_sp, &att, &actuators); + if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) { + multirotor_control_attitude(&att_sp, &att, NULL, &actuators); + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + } else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) { + multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL); + orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); } - /* XXX could be also run in an independent loop */ - if (state.flag_control_rates_enabled) { - multirotor_control_rates(&att_sp, &raw.gyro_rad_s, &actuators); - } - - /* STEP 3: publish the result to the vehicle actuators */ - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); perf_end(mc_loop_perf); } @@ -211,6 +304,8 @@ mc_thread_main(int argc, char *argv[]) perf_print_counter(mc_loop_perf); perf_free(mc_loop_perf); + pthread_join(rate_control_thread, NULL); + fflush(stdout); exit(0); } @@ -229,22 +324,10 @@ usage(const char *reason) int multirotor_att_control_main(int argc, char *argv[]) { int ch; - - control_mode = CONTROL_MODE_RATES; unsigned int optioncount = 0; while ((ch = getopt(argc, argv, "tm:")) != EOF) { switch (ch) { - case 'm': - if (!strcmp(optarg, "rates")) { - control_mode = CONTROL_MODE_RATES; - } else if (!strcmp(optarg, "attitude")) { - control_mode = CONTROL_MODE_RATES; - } else { - usage("unrecognized -m value"); - } - optioncount += 2; - break; case 't': motor_test_mode = true; optioncount += 1; @@ -255,6 +338,7 @@ int multirotor_att_control_main(int argc, char *argv[]) default: fprintf(stderr, "option: -%c\n", ch); usage("unrecognized option"); + break; } } argc -= optioncount; diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index 997aa0c108..2129915d12 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -188,7 +188,7 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc } void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct actuator_controls_s *actuators) + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators) { static uint64_t last_run = 0; const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; @@ -214,11 +214,11 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s // pid_init(&yaw_pos_controller, p.yaw_p, p.yaw_i, p.yaw_d, p.yaw_awu, // PID_MODE_DERIVATIV_SET, 154); pid_init(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, p.yawrate_awu, - PID_MODE_DERIVATIV_SET, 155); + PID_MODE_DERIVATIV_SET); pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu, - PID_MODE_DERIVATIV_SET, 156); + PID_MODE_DERIVATIV_SET); pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu, - PID_MODE_DERIVATIV_SET, 157); + PID_MODE_DERIVATIV_SET); initialized = true; } @@ -243,7 +243,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s float roll_control = pid_calculate(&roll_controller, att_sp->roll_body + p.att_yoff, att->roll, att->rollspeed, deltaT); /* control yaw rate */ - float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_rate_body, att->yawspeed, 0.0f, deltaT); + float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_body, att->yawspeed, 0.0f, deltaT); /* * compensate the vertical loss of thrust @@ -305,10 +305,20 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s roll_controller.saturated = 1; } - actuators->control[0] = roll_control; - actuators->control[1] = pitch_control; - actuators->control[2] = yaw_rate_control; - actuators->control[3] = motor_thrust; + if (actuators) { + actuators->control[0] = roll_control; + actuators->control[1] = pitch_control; + actuators->control[2] = yaw_rate_control; + actuators->control[3] = motor_thrust; + } + + // XXX change yaw rate to yaw pos controller + if (rates_sp) { + rates_sp->roll = roll_control; + rates_sp->pitch = pitch_control; + rates_sp->yaw = yaw_rate_control; + rates_sp->thrust = motor_thrust; + } motor_skip_counter++; } diff --git a/apps/multirotor_att_control/multirotor_attitude_control.h b/apps/multirotor_att_control/multirotor_attitude_control.h index aa8d27369b..d9d3c04447 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.h +++ b/apps/multirotor_att_control/multirotor_attitude_control.h @@ -48,9 +48,10 @@ #include #include #include +#include #include -void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, - struct actuator_controls_s *actuators); +void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators); #endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */ diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index 8a3ac78cdc..b4eb3469b5 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -132,7 +132,7 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru return OK; } -void multirotor_control_rates(const struct vehicle_attitude_setpoint_s *rate_sp, +void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, const float rates[], struct actuator_controls_s *actuators) { static uint64_t last_run = 0; @@ -157,11 +157,11 @@ void multirotor_control_rates(const struct vehicle_attitude_setpoint_s *rate_sp, parameters_update(&h, &p); pid_init(&yaw_speed_controller, p.yawrate_p, 0, p.yawrate_i, p.yawrate_awu, - PID_MODE_DERIVATIV_SET, 155); + PID_MODE_DERIVATIV_SET); pid_init(&pitch_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu, - PID_MODE_DERIVATIV_SET, 156); + PID_MODE_DERIVATIV_SET); pid_init(&roll_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu, - PID_MODE_DERIVATIV_SET, 157); + PID_MODE_DERIVATIV_SET); initialized = true; } @@ -179,13 +179,13 @@ void multirotor_control_rates(const struct vehicle_attitude_setpoint_s *rate_sp, /* calculate current control outputs */ /* control pitch (forward) output */ - float pitch_control = pid_calculate(&pitch_controller, rate_sp->pitch_rate_body, + float pitch_control = pid_calculate(&pitch_controller, rate_sp->pitch, rates[1], 0.0f, deltaT); /* control roll (left/right) output */ - float roll_control = pid_calculate(&roll_controller, rate_sp->roll_rate_body, + float roll_control = pid_calculate(&roll_controller, rate_sp->roll, rates[0], 0.0f, deltaT); /* control yaw rate */ - float yaw_rate_control = pid_calculate(&yaw_speed_controller, rate_sp->yaw_rate_body, rates[2], 0.0f, deltaT); + float yaw_rate_control = pid_calculate(&yaw_speed_controller, rate_sp->yaw, rates[2], 0.0f, deltaT); /* * compensate the vertical loss of thrust diff --git a/apps/multirotor_att_control/multirotor_rate_control.h b/apps/multirotor_att_control/multirotor_rate_control.h index 6f34d99e7a..3c3c508014 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.h +++ b/apps/multirotor_att_control/multirotor_rate_control.h @@ -47,10 +47,10 @@ #include #include -#include +#include #include -void multirotor_control_rates(const struct vehicle_attitude_setpoint_s *rate_sp, +void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, const float rates[], struct actuator_controls_s *actuators); #endif /* MULTIROTOR_RATE_CONTROL_H_ */ diff --git a/apps/multirotor_pos_control/position_control.c b/apps/multirotor_pos_control/position_control.c index fd731a33de..b98f5bedea 100644 --- a/apps/multirotor_pos_control/position_control.c +++ b/apps/multirotor_pos_control/position_control.c @@ -51,46 +51,6 @@ #include "multirotor_position_control.h" -float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) -{ - double lat_now_rad = lat_now / 180.0 * M_PI; - double lon_now_rad = lon_now / 180.0 * M_PI; - double lat_next_rad = lat_next / 180.0 * M_PI; - double lon_next_rad = lon_next / 180.0 * M_PI; - - - double d_lat = lat_next_rad - lat_now_rad; - double d_lon = lon_next_rad - lon_now_rad; - - double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0) * cos(lat_now_rad) * cos(lat_next_rad); - double c = 2 * atan2(sqrt(a), sqrt(1 - a)); - - const double radius_earth = 6371000.0; - - return radius_earth * c; -} - -float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) -{ - double lat_now_rad = lat_now / 180.0 * M_PI; - double lon_now_rad = lon_now / 180.0 * M_PI; - double lat_next_rad = lat_next / 180.0 * M_PI; - double lon_next_rad = lon_next / 180.0 * M_PI; - - double d_lat = lat_next_rad - lat_now_rad; - double d_lon = lon_next_rad - lon_now_rad; - - /* conscious mix of double and float trig function to maximize speed and efficiency */ - float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); - - // XXX wrapping check is incomplete - if (theta < 0.0f) { - theta = theta + 2.0f * M_PI_F; - } - - return theta; -} - void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual, const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos, const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp) diff --git a/apps/px4/ground_estimator/ground_estimator.c b/apps/px4/ground_estimator/ground_estimator.c index 46f86a348d..ccf9ee3ec1 100644 --- a/apps/px4/ground_estimator/ground_estimator.c +++ b/apps/px4/ground_estimator/ground_estimator.c @@ -75,7 +75,7 @@ int ground_estimator_thread_main(int argc, char *argv[]) { /* subscribe to raw data */ int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); - /* advertise attitude */ + /* advertise debug value */ struct debug_key_value_s dbg = { .key = "posx", .value = 0.0f }; orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg); @@ -97,7 +97,7 @@ int ground_estimator_thread_main(int argc, char *argv[]) { /* XXX this is seriously bad - should be an emergency */ } else if (ret == 0) { /* XXX this means no sensor data - should be critical or emergency */ - printf("[attitude estimator bm] WARNING: Not getting sensor data - sensor app running?\n"); + printf("[ground estimator bm] WARNING: Not getting sensor data - sensor app running?\n"); } else { struct sensor_combined_s s; orb_copy(ORB_ID(sensor_combined), sub_raw, &s); diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index b99c6652c8..f1c1f9a239 100644 --- a/apps/sdlog/sdlog.c +++ b/apps/sdlog/sdlog.c @@ -289,6 +289,8 @@ int sdlog_thread_main(int argc, char *argv[]) { /* subscribe to ORB for sensors raw */ subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); fds[fdsc_count].fd = subs.sensor_sub; + /* rate-limit raw data updates to 200Hz */ + orb_set_interval(subs.sensor_sub, 5); fds[fdsc_count].events = POLLIN; fdsc_count++; @@ -459,7 +461,7 @@ int sdlog_thread_main(int argc, char *argv[]) { sysvector_bytes += write(sysvector_file, (const char*)&sysvector, sizeof(sysvector)); - usleep(4900); + usleep(10000); } unsigned bytes = sensor_combined_bytes + actuator_outputs_bytes + blackbox_file_bytes + actuator_controls_bytes; diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index bd1b7b37da..f81dfa9b8f 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -567,7 +567,7 @@ Sensors::accel_init() _fd_bma180 = open("/dev/bma180", O_RDONLY); if (_fd_bma180 < 0) { warn("/dev/bma180"); - errx(1, "FATAL: no accelerometer found"); + warn("FATAL: no accelerometer found"); } /* discard first (junk) reading */ @@ -600,7 +600,7 @@ Sensors::gyro_init() _fd_gyro_l3gd20 = open("/dev/l3gd20", O_RDONLY); if (_fd_gyro_l3gd20 < 0) { warn("/dev/l3gd20"); - errx(1, "FATAL: no gyro found"); + warn("FATAL: no gyro found"); } /* discard first (junk) reading */ @@ -968,7 +968,7 @@ Sensors::ppm_poll() _rc.chan[i].scaled = (ppm_buffer[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]); } else if (ppm_buffer[i] < (_parameters.trim[i] - _parameters.dz[i])) { /* division by zero impossible for trim == min (as for throttle), as this falls in the above if clause */ - _rc.chan[i].scaled = -1.0f + ((ppm_buffer[i] - _parameters.min[i]) / (float)(_parameters.trim[i] - _parameters.min[i])); + _rc.chan[i].scaled = -((_parameters.trim[i] - ppm_buffer[i]) / (float)(_parameters.trim[i] - _parameters.min[i])); } else { /* in the configured dead zone, output zero */ @@ -988,6 +988,8 @@ Sensors::ppm_poll() _rc.chan_count = ppm_decoded_channels; _rc.timestamp = ppm_last_valid_decode; + manual_control.timestamp = ppm_last_valid_decode; + /* roll input - rolling right is stick-wise and rotation-wise positive */ manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled; if (manual_control.roll < -1.0f) manual_control.roll = -1.0f; @@ -1027,6 +1029,7 @@ Sensors::ppm_poll() orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); + } #endif @@ -1090,7 +1093,7 @@ Sensors::task_main() /* advertise the manual_control topic */ { struct manual_control_setpoint_s manual_control; - manual_control.mode = ROLLPOS_PITCHPOS_YAWRATE_THROTTLE; + manual_control.mode = MANUAL_CONTROL_MODE_ATT_YAW_RATE; manual_control.roll = 0.0f; manual_control.pitch = 0.0f; manual_control.yaw = 0.0f; diff --git a/apps/systemlib/Makefile b/apps/systemlib/Makefile index ec6bb3fb32..b49a3c54a4 100644 --- a/apps/systemlib/Makefile +++ b/apps/systemlib/Makefile @@ -48,7 +48,8 @@ CSRCS = err.c \ # ifeq ($(TARGET),px4fmu) CSRCS += systemlib.c \ - pid/pid.c + pid/pid.c \ + geo/geo.c endif include $(APPDIR)/mk/app.mk diff --git a/apps/systemlib/geo/geo.c b/apps/systemlib/geo/geo.c new file mode 100644 index 0000000000..42d8d9c150 --- /dev/null +++ b/apps/systemlib/geo/geo.c @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * Lorenz Meier + * + * 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 geo.c + * + * Geo / math functions to perform geodesic calculations + * + * @author Thomas Gubler + * @author Julian Oes + * @author Lorenz Meier + */ + +#include +#include + +__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) +{ + double lat_now_rad = lat_now / 180.0 * M_PI; + double lon_now_rad = lon_now / 180.0 * M_PI; + double lat_next_rad = lat_next / 180.0 * M_PI; + double lon_next_rad = lon_next / 180.0 * M_PI; + + + double d_lat = lat_next_rad - lat_now_rad; + double d_lon = lon_next_rad - lon_now_rad; + + double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0) * cos(lat_now_rad) * cos(lat_next_rad); + double c = 2 * atan2(sqrt(a), sqrt(1 - a)); + + const double radius_earth = 6371000.0; + + return radius_earth * c; +} + +__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) +{ + double lat_now_rad = lat_now / 180.0 * M_PI; + double lon_now_rad = lon_now / 180.0 * M_PI; + double lat_next_rad = lat_next / 180.0 * M_PI; + double lon_next_rad = lon_next / 180.0 * M_PI; + + double d_lat = lat_next_rad - lat_now_rad; + double d_lon = lon_next_rad - lon_now_rad; + + /* conscious mix of double and float trig function to maximize speed and efficiency */ + float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); + + // XXX wrapping check is incomplete + if (theta < 0.0f) { + theta = theta + 2.0f * M_PI_F; + } + + return theta; +} \ No newline at end of file diff --git a/apps/systemlib/geo/geo.h b/apps/systemlib/geo/geo.h new file mode 100644 index 0000000000..0e86b3599d --- /dev/null +++ b/apps/systemlib/geo/geo.h @@ -0,0 +1,49 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * Lorenz Meier + * + * 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 geo.h + * + * Definition of geo / math functions to perform geodesic calculations + * + * @author Thomas Gubler + * @author Julian Oes + * @author Lorenz Meier + */ + +__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); + +__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c index a01c170a68..0ab7c0ea30 100644 --- a/apps/systemlib/param/param.c +++ b/apps/systemlib/param/param.c @@ -484,7 +484,7 @@ param_export(int fd, bool only_unsaved) s->unsaved = false; - /* append the appripriate BSON type object */ + /* append the appropriate BSON type object */ switch (param_type(s->param)) { case PARAM_TYPE_INT32: param_get(s->param, &i); @@ -688,4 +688,4 @@ param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_chang func(arg, param); } -} \ No newline at end of file +} diff --git a/apps/systemlib/pid/pid.c b/apps/systemlib/pid/pid.c index 807373c158..61dd5757fb 100644 --- a/apps/systemlib/pid/pid.c +++ b/apps/systemlib/pid/pid.c @@ -40,34 +40,54 @@ */ #include "pid.h" +#include __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, - uint8_t mode, uint8_t plot_i) + uint8_t mode) { pid->kp = kp; pid->ki = ki; pid->kd = kd; pid->intmax = intmax; pid->mode = mode; - pid->plot_i = plot_i; pid->count = 0; pid->saturated = 0; + pid->last_output = 0; pid->sp = 0; pid->error_previous = 0; pid->integral = 0; } -__EXPORT void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax) +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax) { - pid->kp = kp; - pid->ki = ki; - pid->kd = kd; - pid->intmax = intmax; - // pid->mode = mode; + int ret = 0; - // pid->sp = 0; - // pid->error_previous = 0; - // pid->integral = 0; + if (isfinite(kp)) { + pid->kp = kp; + } else { + ret = 1; + } + + if (isfinite(ki)) { + pid->ki = ki; + } else { + ret = 1; + } + + if (isfinite(kd)) { + pid->kd = kd; + } else { + ret = 1; + } + + if (isfinite(intmax)) { + pid->intmax = intmax; + } else { + ret = 1; + } + + // pid->limit = limit; + return ret; } //void pid_set(PID_t *pid, float sp) @@ -95,6 +115,11 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo goto start */ + if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt)) + { + return pid->last_output; + } + float i, d; pid->sp = sp; float error = pid->sp - val; @@ -111,7 +136,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo } // Anti-Windup. Needed if we don't use the saturation above. - if (pid->intmax != 0.0) { + if (pid->intmax != 0.0f) { if (i > pid->intmax) { pid->integral = pid->intmax; @@ -122,14 +147,6 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo } else { pid->integral = i; } - - //Send Controller integrals - // Disabled because of new possibilities with debug_vect. - // Now sent in Main Loop at 5 Hz. 26.06.2010 Laurens - // if (pid->plot_i && (pid->count++ % 16 == 0)&&(global_data.param[PARAM_SEND_SLOT_DEBUG_2] == 1)) - // { - // mavlink_msg_debug_send(MAVLINK_COMM_1, pid->plot_i, pid->integral); - // } } if (pid->mode == PID_MODE_DERIVATIV_CALC) { @@ -139,10 +156,38 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo d = -val_dot; } else { - d = 0; + d = 0.0f; } - pid->error_previous = error; + if (pid->kd == 0.0f) { + d = 0.0f; + } - return (error * pid->kp) + (i * pid->ki) + (d * pid->kd); + if (pid->ki == 0.0f) { + i = 0; + } + + float p; + + if (pid->kp == 0.0f) { + p = 0.0f; + } else { + p = error; + } + + if (isfinite(error)) { + pid->error_previous = error; + } + + float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); + + if (isfinite(output)) { + pid->last_output = output; + } + + if (!isfinite(pid->integral)) { + pid->integral = 0; + } + + return pid->last_output; } diff --git a/apps/systemlib/pid/pid.h b/apps/systemlib/pid/pid.h index 83bf09b59f..098b2bef81 100644 --- a/apps/systemlib/pid/pid.h +++ b/apps/systemlib/pid/pid.h @@ -58,14 +58,15 @@ typedef struct { float sp; float integral; float error_previous; + float last_output; + float limit; uint8_t mode; - uint8_t plot_i; uint8_t count; uint8_t saturated; } PID_t; -__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, uint8_t mode, uint8_t plot_i); -__EXPORT void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax); +__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, uint8_t mode); +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax); //void pid_set(PID_t *pid, float sp); __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp index 1cad57a436..f211648a90 100644 --- a/apps/uORB/objects_common.cpp +++ b/apps/uORB/objects_common.cpp @@ -59,7 +59,6 @@ ORB_DEFINE(output_pwm, struct pwm_output_values); #include ORB_DEFINE(input_rc, struct rc_input_values); -// XXX need to check wether these should be here #include "topics/vehicle_attitude.h" ORB_DEFINE(vehicle_attitude, struct vehicle_attitude_s); @@ -78,8 +77,8 @@ ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s); #include "topics/vehicle_local_position.h" ORB_DEFINE(vehicle_local_position, struct vehicle_local_position_s); -#include "topics/ardrone_motors_setpoint.h" -ORB_DEFINE(ardrone_motors_setpoint, struct ardrone_motors_setpoint_s); +#include "topics/vehicle_rates_setpoint.h" +ORB_DEFINE(vehicle_rates_setpoint, struct vehicle_rates_setpoint_s); #include "topics/rc_channels.h" ORB_DEFINE(rc_channels, struct rc_channels_s); @@ -99,6 +98,9 @@ ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s); #include "topics/manual_control_setpoint.h" ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s); +#include "topics/offboard_control_setpoint.h" +ORB_DEFINE(offboard_control_setpoint, struct offboard_control_setpoint_s); + #include "topics/optical_flow.h" ORB_DEFINE(optical_flow, struct optical_flow_s); diff --git a/apps/uORB/topics/manual_control_setpoint.h b/apps/uORB/topics/manual_control_setpoint.h index b01c7438d9..a7f5be708d 100644 --- a/apps/uORB/topics/manual_control_setpoint.h +++ b/apps/uORB/topics/manual_control_setpoint.h @@ -1,21 +1,21 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier * * 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. + * 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. + * 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. + * 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 @@ -57,31 +57,27 @@ */ enum MANUAL_CONTROL_MODE { - DIRECT = 0, - ROLLPOS_PITCHPOS_YAWRATE_THROTTLE = 1, - ROLLRATE_PITCHRATE_YAWRATE_THROTTLE = 2, - ROLLPOS_PITCHPOS_YAWPOS_THROTTLE = 3 + MANUAL_CONTROL_MODE_DIRECT = 0, + MANUAL_CONTROL_MODE_ATT_YAW_RATE = 1, + MANUAL_CONTROL_MODE_ATT_YAW_POS = 2, + MANUAL_CONTROL_MODE_MULTIROTOR_SIMPLE = 3 /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ }; struct manual_control_setpoint_s { + uint64_t timestamp; - enum MANUAL_CONTROL_MODE mode; /**< The current control inputs mode */ - float roll; /**< roll / roll rate input */ - float pitch; - float yaw; - float throttle; + enum MANUAL_CONTROL_MODE mode; /**< The current control inputs mode */ + float roll; /**< ailerons roll / roll rate input */ + float pitch; /**< elevator / pitch / pitch rate */ + float yaw; /**< rudder / yaw rate / yaw */ + float throttle; /**< throttle / collective thrust / altitude */ - float override_mode_switch; + float override_mode_switch; - float ailerons; - float elevator; - float rudder; - float flaps; - - float aux1_cam_pan; - float aux2_cam_tilt; - float aux3_cam_zoom; - float aux4_cam_roll; + float aux1_cam_pan_flaps; + float aux2_cam_tilt; + float aux3_cam_zoom; + float aux4_cam_roll; }; /**< manual control inputs */ diff --git a/apps/uORB/topics/offboard_control_setpoint.h b/apps/uORB/topics/offboard_control_setpoint.h new file mode 100644 index 0000000000..2e895c59c4 --- /dev/null +++ b/apps/uORB/topics/offboard_control_setpoint.h @@ -0,0 +1,94 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * + * 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 offboard_control_setpoint.h + * Definition of the manual_control_setpoint uORB topic. + */ + +#ifndef TOPIC_OFFBOARD_CONTROL_SETPOINT_H_ +#define TOPIC_OFFBOARD_CONTROL_SETPOINT_H_ + +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * Off-board control inputs. + * + * Typically sent by a ground control station / joystick or by + * some off-board controller via C or SIMULINK. + */ +enum OFFBOARD_CONTROL_MODE +{ + OFFBOARD_CONTROL_MODE_DIRECT = 0, + OFFBOARD_CONTROL_MODE_DIRECT_RATES = 1, + OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE = 2, + OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY = 3, + OFFBOARD_CONTROL_MODE_DIRECT_POSITION = 4, + OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 5, + OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 6, + OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 7, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ +}; + +struct offboard_control_setpoint_s { + uint64_t timestamp; + + enum OFFBOARD_CONTROL_MODE mode; /**< The current control inputs mode */ + bool armed; /**< Armed flag set, yes / no */ + float p1; /**< ailerons roll / roll rate input */ + float p2; /**< elevator / pitch / pitch rate */ + float p3; /**< rudder / yaw rate / yaw */ + float p4; /**< throttle / collective thrust / altitude */ + + float override_mode_switch; + + float aux1_cam_pan_flaps; + float aux2_cam_tilt; + float aux3_cam_zoom; + float aux4_cam_roll; + +}; /**< offboard control inputs */ +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(offboard_control_setpoint); + +#endif diff --git a/apps/uORB/topics/vehicle_attitude_setpoint.h b/apps/uORB/topics/vehicle_attitude_setpoint.h index 0c33ebff8c..62367cf77a 100644 --- a/apps/uORB/topics/vehicle_attitude_setpoint.h +++ b/apps/uORB/topics/vehicle_attitude_setpoint.h @@ -56,21 +56,18 @@ struct vehicle_attitude_setpoint_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - float roll_tait_bryan; /**< Tait-Bryan angle in NED frame */ - float pitch_tait_bryan; /**< Tait-Bryan angle in NED frame */ - float yaw_tait_bryan; /**< Tait-Bryan angle in NED frame */ - float tait_bryan_valid; /**< Set to true if Tait-Bryan angles are valid */ + //float roll_tait_bryan; /**< Tait-Bryan angle in NED frame */ + //float pitch_tait_bryan; /**< Tait-Bryan angle in NED frame */ + //float yaw_tait_bryan; /**< Tait-Bryan angle in NED frame */ + //float tait_bryan_valid; /**< Set to true if Tait-Bryan angles are valid */ float roll_body; /**< body angle in NED frame */ float pitch_body; /**< body angle in NED frame */ float yaw_body; /**< body angle in NED frame */ - float roll_rate_body; /**< body angle in NED frame */ - float pitch_rate_body; /**< body angle in NED frame */ - float yaw_rate_body; /**< body angle in NED frame */ - float body_valid; /**< Set to true if Tait-Bryan angles are valid */ + //float body_valid; /**< Set to true if body angles are valid */ - float R_body[9]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */ - bool R_valid; /**< Set to true if rotation matrix is valid */ + //float R_body[9]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */ + //bool R_valid; /**< Set to true if rotation matrix is valid */ float thrust; /**< Thrust in Newton the power system should generate */ diff --git a/apps/uORB/topics/vehicle_command.h b/apps/uORB/topics/vehicle_command.h index dca928efd2..3e220965d9 100644 --- a/apps/uORB/topics/vehicle_command.h +++ b/apps/uORB/topics/vehicle_command.h @@ -50,6 +50,10 @@ * @{ */ +enum PX4_CMD { + PX4_CMD_CONTROLLER_SELECTION = 1000, +}; + struct vehicle_command_s { float param1; /**< Parameter 1, as defined by MAVLink MAV_CMD enum. */ diff --git a/apps/uORB/topics/ardrone_motors_setpoint.h b/apps/uORB/topics/vehicle_rates_setpoint.h similarity index 70% rename from apps/uORB/topics/ardrone_motors_setpoint.h rename to apps/uORB/topics/vehicle_rates_setpoint.h index d0cb0ad733..46e62c4b71 100644 --- a/apps/uORB/topics/ardrone_motors_setpoint.h +++ b/apps/uORB/topics/vehicle_rates_setpoint.h @@ -33,12 +33,12 @@ ****************************************************************************/ /** - * @file ardrone_motors_setpoint.h - * Definition of the ardrone_motors_setpoint uORB topic. + * @file vehicle_rates_setpoint.h + * Definition of the vehicle rates setpoint topic */ -#ifndef TOPIC_ARDRONE_MOTORS_SETPOINT_H_ -#define TOPIC_ARDRONE_MOTORS_SETPOINT_H_ +#ifndef TOPIC_VEHICLE_RATES_SETPOINT_H_ +#define TOPIC_VEHICLE_RATES_SETPOINT_H_ #include #include "../uORB.h" @@ -47,24 +47,22 @@ * @addtogroup topics * @{ */ - -struct ardrone_motors_setpoint_s +struct vehicle_rates_setpoint_s { - uint64_t timestamp; //in microseconds since system start, is set whenever the writing thread stores new data + uint64_t timestamp; /**< in microseconds since system start */ - uint8_t group; /**< quadrotor group */ - uint8_t mode; /**< requested flight mode XXX define */ - float p1; /**< parameter 1: rate control: roll rate, att control: roll angle (in radians, NED) */ - float p2; /**< parameter 2: rate control: pitch rate, att control: pitch angle (in radians, NED) */ - float p3; /**< parameter 3: rate control: yaw rate, att control: yaw angle (in radians, NED) */ - float p4; /**< parameter 4: thrust, [0..1] */ -}; /**< AR.Drone low level motors */ + float roll; /**< body angular rates in NED frame */ + float pitch; /**< body angular rates in NED frame */ + float yaw; /**< body angular rates in NED frame */ + float thrust; /**< thrust normalized to 0..1 */ + +}; /**< vehicle_rates_setpoint */ /** * @} */ /* register this as object request broker structure */ -ORB_DECLARE(ardrone_motors_setpoint); +ORB_DECLARE(vehicle_rates_setpoint); #endif diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index cfde2ab53c..23172d7cf1 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -110,6 +110,8 @@ struct vehicle_status_s uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ + uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */ + //uint64_t failsave_highlevel_begin; TO BE COMPLETED commander_state_machine_t state_machine; /**< current flight state, main state machine */ enum VEHICLE_FLIGHT_MODE flight_mode; /**< current flight mode, as defined by mode switch */ @@ -127,17 +129,26 @@ struct vehicle_status_s bool flag_control_rates_enabled; /**< true if rates are stabilized */ bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ - bool flag_control_speed_enabled; /**< true if speed (implies direction) is controlled */ + bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ bool flag_preflight_accel_calibration; + bool rc_signal_found_once; bool rc_signal_lost; /**< true if RC reception is terminally lost */ bool rc_signal_cutting_off; /**< true if RC reception is weak / cutting off */ uint64_t rc_signal_lost_interval; /**< interval in microseconds since when no RC signal is available */ + bool offboard_control_signal_found_once; + bool offboard_control_signal_lost; + bool offboard_control_signal_weak; + uint64_t offboard_control_signal_lost_interval; /**< interval in microseconds without an offboard control message */ + + bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ + //bool failsave_highlevel; + /* see SYS_STATUS mavlink message for the following */ uint32_t onboard_control_sensors_present; uint32_t onboard_control_sensors_enabled; diff --git a/nuttx/ChangeLog b/nuttx/ChangeLog index 7ad830d890..adb52afce7 100644 --- a/nuttx/ChangeLog +++ b/nuttx/ChangeLog @@ -3377,3 +3377,46 @@ commands used in the build (Contributed by Richard Cochran). * drivers/net/enc28j60.c: The ENC28J60 Ethernet driver is now functional. + * configs/fire-stm32v2: Add support or the fire-stm32v3 board as + well (untested because I do not have a v3 board). + * lib/stdio/lib_sscanf.c: Add %n psuedo-format (from Kate). + * lib/stdio/lib_sscanf.c: There is an issue of handling input + when (1) no fieldwidth is provided and (2) there is no space + seperating the input values. No solutions is in place for this + case now (either space or a fieldwidth must be provided). But + at least some of the bad logic that attempted to handle this + case has been removed (noted by Kate). + * arch/arm/src/stm32/stm32_eth.c: DMA buffer sizes must be an + even multiple of 4, 8, or 16 bytes. + * arch/arm/src/stm32/stm32_idle.c: Fixes STM32F107 DMA issues: + We cannot go into sleep mode while Ethernet is actively DMAing. + * configs/shenzhou/src/up_ssd1289.c: Add infrastructure to support + SSD1289 LCD. Initial checkin is just a clone of the + STM32F4Discovery's FSMC-based LCD interface. The Shenzhou + will need a completely need bit-banging interface; this + initial check-in is only for the framework. + * configs/shenzhou/src/up_ssd1289.c: Bit-banging driver is + code complete. + * configs/shenzhou/src/up_lcd.c: Oops. Shenzhou LCD does not + have an SSD1289 controller. Its an ILI93xx. Ported the + STM3240G-EVAL ILI93xx driver to work on the Shenzhou board. + * configs/shenzhou/nxwm: Added an NxWM configuratino for the + Shenzhou board. This is untested on initial check-in. It will + be used to verify the Shenzhou LCD driver (and eventually the + touchscreen driver). + * configs/shenzhou/src/up_touchscreen.c: Add ADS7843E touchscreen + support for the Shenzhou board. The initial check-in is untested + and basically a clone of the the touchscreen support fro the SAM-3U. + * tools/cfgparser.c: There are some NxWidget configuration + settings that must be de-quoted. + * arch/arm/src/stm32/Kconfig: There is no SPI4. Some platforms + SPI3 and some do not (still not clear). + * nuttx/configs/shenzhou: Various fixes to build new NxWM + configuration. + * configs/shenzhou: Oops. The Shenzhou LCD is and SSD1289, + not an ILI93xx. + * configs/shenzhou/src/up_ssd1289.c: The LCD is basically functional + on the Shenzhou board. + * graphics/nxmu: Correct some bad parameter checking that caused + failures when DEBUG was enabled. + diff --git a/nuttx/Kconfig b/nuttx/Kconfig index f920e4984d..0fe6eb0f7c 100644 --- a/nuttx/Kconfig +++ b/nuttx/Kconfig @@ -216,7 +216,7 @@ endmenu menu "Debug Options" config DEBUG - bool "Enable debug features" + bool "Enable Debug Features" default n ---help--- Enables built-in debug features. Selecting this option will (1) Enable @@ -227,87 +227,115 @@ config DEBUG if DEBUG config DEBUG_VERBOSE - bool "Enable debug verbose output" + bool "Enable Debug Verbose Output" default n ---help--- - Enables verbose debug output (assuming debug output is enabled) + Enables verbose debug output (assuming debug output is enabled). As a + general rule, when DEBUG is enabled only errors will be reported in the debug + output. But if you also enable DEBUG_VERBOSE, then very chatty (and + often annoying) output will be generated. This means there are two levels + of debug output: errors-only and everything. config DEBUG_ENABLE - bool "Enable debug controls" + bool "Enable Debug Controls" default n ---help--- Support an interface to dynamically enable or disable debug output. +comment "Subsystem Debug Options" + config DEBUG_SCHED - bool "Enable scheduler debug output" + bool "Enable Scheduler Debug Output" default n ---help--- Enable OS debug output (disabled by default) config DEBUG_MM - bool "Enable memory manager debug output" + bool "Enable Memory Manager Debug Output" default n ---help--- Enable memory management debug output (disabled by default) config DEBUG_NET - bool "Enable network debug output" + bool "Enable Network Debug Output" default n + depends on NET ---help--- Enable network debug output (disabled by default) config DEBUG_USB - bool "Enable USB debug output" + bool "Enable USB Debug Output" default n + depends on USBDEV || USBHOST ---help--- Enable usb debug output (disabled by default) config DEBUG_FS - bool "Enable file system debug output" + bool "Enable File System Debug Output" default n ---help--- Enable file system debug output (disabled by default) config DEBUG_LIB - bool "Enable C library debug output" + bool "Enable C Library Debug Output" default n ---help--- Enable C library debug output (disabled by default) config DEBUG_BINFMT - bool "Enable binary loader debug output" + bool "Enable Binary Loader Debug Output" default n ---help--- Enable binary loader debug output (disabled by default) config DEBUG_GRAPHICS - bool "Enable graphics debug output" + bool "Enable Graphics Debug Output" default n ---help--- Enable NX graphics debug output (disabled by default) -config DEBUG_I2C - bool "Enable I2C debug output" +comment "Driver Debug Options" + +config DEBUG_LCD + bool "Enable Low-level LCD Debug Output" default n + depends on LCD + ---help--- + Enable low level debug output from the LCD driver (disabled by default) + +config DEBUG_INPUT + bool "Enable Input Device Debug Output" + default n + depends on INPUT + ---help--- + Enable low level debug output from the input device drivers such as + mice and touchscreens (disabled by default) + +config DEBUG_I2C + bool "Enable I2C Debug Output" + default n + depends on I2C ---help--- Enable I2C driver debug output (disabled by default) config DEBUG_SPI - bool "Enable SPI debug output" + bool "Enable SPI Debug Output" default n + depends on SPI ---help--- Enable I2C driver debug output (disabled by default) config DEBUG_WATCHDOG - bool "Enable watchdog timer debug output" + bool "Enable Watchdog Timer Debug Output" default n + depends on WATCHDOG ---help--- Enable watchdog timer debug output (disabled by default) endif config DEBUG_SYMBOLS - bool "Enable debug symbols" + bool "Enable Debug Symbols" default n ---help--- Build without optimization and with debug symbols (needed @@ -339,6 +367,10 @@ menu "File Systems" source fs/Kconfig endmenu +menu "Graphics Support" +source graphics/Kconfig +endmenu + menu "Memory Management" source mm/Kconfig endmenu diff --git a/nuttx/Makefile b/nuttx/Makefile index 3c6e9c1985..7a058d88e9 100644 --- a/nuttx/Makefile +++ b/nuttx/Makefile @@ -38,7 +38,8 @@ TOPDIR := ${shell pwd | sed -e 's/ /\\ /g'} -include ${TOPDIR}/tools/Config.mk -include ${TOPDIR}/Make.defs -# Control build verbosity. +# Control build verbosity + ifeq ($(V),1) export Q := else diff --git a/nuttx/README.txt b/nuttx/README.txt index 5c330aa683..7477e0a245 100644 --- a/nuttx/README.txt +++ b/nuttx/README.txt @@ -848,6 +848,8 @@ apps | `- README.txt |- nshlib/ | `- README.txt + |- NxWidgets/ + | `- README.txt |- system/ | |- i2c/README.txt | |- free/README.txt diff --git a/nuttx/arch/arm/src/stm32/Kconfig b/nuttx/arch/arm/src/stm32/Kconfig index 8b0cea32e4..8d93fb104c 100644 --- a/nuttx/arch/arm/src/stm32/Kconfig +++ b/nuttx/arch/arm/src/stm32/Kconfig @@ -340,14 +340,7 @@ config STM32_SPI2 config STM32_SPI3 bool "SPI3" default n - depends on STM32_STM32F20XX || STM32_STM32F40XX - select SPI - select STM32_SPI - -config STM32_SPI4 - bool "SPI4" - default n - depends on STM32_STM32F10XX + depends on STM32_CONNECTIVITYLINE || STM32_STM32F20XX || STM32_STM32F40XX select SPI select STM32_SPI @@ -1623,12 +1616,12 @@ config STM32_I2C_DYNTIMEO config STM32_I2C_DYNTIMEO_USECPERBYTE int "Timeout Microseconds per Byte" - default 0 + default 500 depends on STM32_I2C_DYNTIMEO config STM32_I2C_DYNTIMEO_STARTSTOP int "Timeout for Start/Stop (Milliseconds)" - default 5000 + default 1000 depends on STM32_I2C_DYNTIMEO config STM32_I2CTIMEOSEC @@ -1684,6 +1677,7 @@ config SDIO_WIDTH_D1_ONLY endmenu +if STM32_ETHMAC menu "Ethernet MAC configuration" config STM32_PHYADDR @@ -1695,7 +1689,6 @@ config STM32_PHYADDR config STM32_MII bool "Use MII interface" default n - depends on STM32_ETHMAC ---help--- Support Ethernet MII interface. @@ -1732,16 +1725,15 @@ config STM32_MII_EXTCLK endchoice config STM32_AUTONEG - bool "Use autonegtiation" + bool "Use autonegotiation" default y - depends on STM32_ETHMAC ---help--- - Use PHY autonegotion to determine speed and mode + Use PHY autonegotiation to determine speed and mode config STM32_ETHFD bool "Full duplex" default n - depends on STM32_ETHMAC && !STM32_AUTONEG + depends on !STM32_AUTONEG ---help--- If STM32_AUTONEG is not defined, then this may be defined to select full duplex mode. Default: half-duplex @@ -1749,61 +1741,104 @@ config STM32_ETHFD config STM32_ETH100MBPS bool "100 Mbps" default n - depends on STM32_ETHMAC && !STM32_AUTONEG + depends on !STM32_AUTONEG ---help--- If STM32_AUTONEG is not defined, then this may be defined to select 100 MBps speed. Default: 10 Mbps config STM32_PHYSR - hex "PHY status register address" + int "PHY Status Register Address (decimal)" depends on STM32_AUTONEG ---help--- This must be provided if STM32_AUTONEG is defined. The PHY status register address may diff from PHY to PHY. This configuration sets the address of the PHY status register. -config STM32_PHYSR_SPEED - hex "PHY speed mask" +config STM32_PHYSR_ALTCONFIG + bool "PHY Status Alternate Bit Layout" + default n depends on STM32_AUTONEG + ---help--- + Different PHYs present speed and mode information in different ways. Some + will present separate information for speed and mode (this is the default). + Those PHYs, for example, may provide a 10/100 Mbps indication and a separate + full/half duplex indication. This options selects an alternative representation + where speed and mode information are combined. This might mean, for example, + separate bits for 10HD, 100HD, 10FD and 100FD. + +config STM32_PHYSR_SPEED + hex "PHY Speed Mask" + depends on STM32_AUTONEG && !STM32_PHYSR_ALTCONFIG ---help--- This must be provided if STM32_AUTONEG is defined. This provides bit mask - indicating 10 or 100MBps speed. + for isolating the 10 or 100MBps speed indication. config STM32_PHYSR_100MBPS - hex "PHY 100Mbps speed value" - depends on STM32_AUTONEG + hex "PHY 100Mbps Speed Value" + depends on STM32_AUTONEG && !STM32_PHYSR_ALTCONFIG ---help--- This must be provided if STM32_AUTONEG is defined. This provides the value of the speed bit(s) indicating 100MBps speed. config STM32_PHYSR_MODE - hex "PHY mode mask" - depends on STM32_AUTONEG + hex "PHY Mode Mask" + depends on STM32_AUTONEG && !STM32_PHYSR_ALTCONFIG ---help--- This must be provided if STM32_AUTONEG is defined. This provide bit mask - indicating full or half duplex modes. + for isolating the full or half duplex mode bits. config STM32_PHYSR_FULLDUPLEX - hex "PHY full duplex mode value" - depends on STM32_AUTONEG + hex "PHY Full Duplex Mode Value" + depends on STM32_AUTONEG && !STM32_PHYSR_ALTCONFIG ---help--- This must be provided if STM32_AUTONEG is defined. This provides the value of the mode bits indicating full duplex mode. +config STM32_PHYSR_ALTMODE + hex "PHY Mode Mask" + depends on STM32_AUTONEG && STM32_PHYSR_ALTCONFIG + ---help--- + This must be provided if STM32_AUTONEG is defined. This provide bit mask + for isolating the speed and full/half duplex mode bits. + +config STM32_PHYSR_10HD + hex "10MHz/Half Duplex Value" + depends on STM32_AUTONEG && STM32_PHYSR_ALTCONFIG + ---help--- + This must be provided if STM32_AUTONEG is defined. This is the value + under the bit mask that represents the 10Mbps, half duplex setting. + +config STM32_PHYSR_100HD + hex "100MHz/Half Duplex Value" + depends on STM32_AUTONEG && STM32_PHYSR_ALTCONFIG + ---help--- + This must be provided if STM32_AUTONEG is defined. This is the value + under the bit mask that represents the 100Mbps, half duplex setting. + +config STM32_PHYSR_10FD + hex "10MHz/Full Duplex Value" + depends on STM32_AUTONEG && STM32_PHYSR_ALTCONFIG + ---help--- + This must be provided if STM32_AUTONEG is defined. This is the value + under the bit mask that represents the 10Mbps, full duplex setting. + +config STM32_PHYSR_100FD + hex "100MHz/Full Duplex Value" + depends on STM32_AUTONEG && STM32_PHYSR_ALTCONFIG + ---help--- + This must be provided if STM32_AUTONEG is defined. This is the value + under the bit mask that represents the 100Mbps, full duplex setting. + config STM32_ETH_PTP bool "Precision Time Protocol (PTP)" default n - depends on STM32_ETHMAC ---help--- Precision Time Protocol (PTP). Not supported but some hooks are indicated with this condition. -endmenu - config STM32_RMII bool default y if !STM32_MII - depends on STM32_ETHMAC choice prompt "RMII clock configuration" @@ -1837,6 +1872,16 @@ config STM32_RMII_EXTCLK endchoice +config STM32_ETHMAC_REGDEBUG + bool "Register-Level Debug" + default n + depends on DEBUG + ---help--- + Enable very low-level register access debug. Depends on DEBUG. + +endmenu +endif + menu "USB Host Configuration" config STM32_OTGFS_RXFIFO_SIZE diff --git a/nuttx/arch/arm/src/stm32/chip/stm32_eth.h b/nuttx/arch/arm/src/stm32/chip/stm32_eth.h index 431144009e..a4a109d016 100644 --- a/nuttx/arch/arm/src/stm32/chip/stm32_eth.h +++ b/nuttx/arch/arm/src/stm32/chip/stm32_eth.h @@ -62,7 +62,9 @@ #define STM32_ETH_MACVLANTR_OFFSET 0x001c /* Ethernet MAC VLAN tag register */ #define STM32_ETH_MACRWUFFR_OFFSET 0x0028 /* Ethernet MAC remote wakeup frame filter reg */ #define STM32_ETH_MACPMTCSR_OFFSET 0x002c /* Ethernet MAC PMT control and status register */ -#define STM32_ETH_MACDBGR_OFFSET 0x0034 /* Ethernet MAC debug register */ +#if defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) +# define STM32_ETH_MACDBGR_OFFSET 0x0034 /* Ethernet MAC debug register */ +#endif #define STM32_ETH_MACSR_OFFSET 0x0038 /* Ethernet MAC interrupt status register */ #define STM32_ETH_MACIMR_OFFSET 0x003c /* Ethernet MAC interrupt mask register */ #define STM32_ETH_MACA0HR_OFFSET 0x0040 /* Ethernet MAC address 0 high register */ @@ -132,7 +134,9 @@ #define STM32_ETH_MACVLANTR (STM32_ETHERNET_BASE+STM32_ETH_MACVLANTR_OFFSET) #define STM32_ETH_MACRWUFFR (STM32_ETHERNET_BASE+STM32_ETH_MACRWUFFR_OFFSET) #define STM32_ETH_MACPMTCSR (STM32_ETHERNET_BASE+STM32_ETH_MACPMTCSR_OFFSET) -#define STM32_ETH_MACDBGR (STM32_ETHERNET_BASE+STM32_ETH_MACDBGR_OFFSET) +#if defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) +# define STM32_ETH_MACDBGR (STM32_ETHERNET_BASE+STM32_ETH_MACDBGR_OFFSET) +#endif #define STM32_ETH_MACSR (STM32_ETHERNET_BASE+STM32_ETH_MACSR_OFFSET) #define STM32_ETH_MACIMR (STM32_ETHERNET_BASE+STM32_ETH_MACIMR_OFFSET) #define STM32_ETH_MACA0HR (STM32_ETHERNET_BASE+STM32_ETH_MACA0HR_OFFSET) @@ -216,7 +220,9 @@ # define ETH_MACCR_IFG(n) ((12-((n) >> 3)) << ETH_MACCR_IFG_SHIFT) /* n bit times, n=40,48,..96 */ #define ETH_MACCR_JD (1 << 22) /* Bit 22: Jabber disable */ #define ETH_MACCR_WD (1 << 23) /* Bit 23: Watchdog disable */ -#define ETH_MACCR_CSTF (1 << 25) /* Bits 25: CRC stripping for Type frames */ +#if defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) +# define ETH_MACCR_CSTF (1 << 25) /* Bits 25: CRC stripping for Type frames */ +#endif /* Ethernet MAC frame filter register */ @@ -303,6 +309,8 @@ /* Ethernet MAC debug register */ +#if defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) + #define ETH_MACDBGR_MMRPEA (1 << 0) /* Bit 0: MAC MII receive protocol engine active */ #define ETH_MACDBGR_MSFRWCS_SHIFT (1) /* Bits 1-2: MAC small FIFO read / write controllers status */ #define ETH_MACDBGR_MSFRWCS_MASK (3 << ETH_MACDBGR_MSFRWCS_SHIFT) @@ -337,6 +345,8 @@ #define ETH_MACDBGR_TFNE (1 << 24) /* Bit 24: Tx FIFO not empty */ #define ETH_MACDBGR_TFF (1 << 25) /* Bit 25: Tx FIFO full */ +#endif + /* Ethernet MAC interrupt status register */ #define ETH_MACSR_PMTS (1 << 3) /* Bit 3: PMT status */ @@ -419,7 +429,9 @@ #define ETH_MMCCR_ROR (1 << 2) /* Bit 2: Reset on read */ #define ETH_MMCCR_MCF (1 << 3) /* Bit 3: MMC counter freeze */ #define ETH_MMCCR_MCP (1 << 4) /* Bit 4: MMC counter preset */ -#define ETH_MMCCR_MCFHP (1 << 5) /* Bit 5: MMC counter Full-Half preset */ +#if defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) +# define ETH_MMCCR_MCFHP (1 << 5) /* Bit 5: MMC counter Full-Half preset */ +#endif /* Ethernet MMC receive interrupt and interrupt mask registers */ @@ -453,6 +465,8 @@ #define ETH_PTPTSCR_TSSTU (1 << 3) /* Bit 3: Time stamp system time update */ #define ETH_PTPTSCR_TSITE (1 << 4) /* Bit 4: Time stamp interrupt trigger enable */ #define ETH_PTPTSCR_TSARU (1 << 5) /* Bit 5: Time stamp addend register update */ + +#if defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) #define ETH_PTPTSCR_TSSARFE (1 << 8) /* Bit 8: Time stamp snapshot for all received frames enable */ #define ETH_PTPTSCR_TSSSR (1 << 9) /* Bit 9: Time stamp subsecond rollover: digital or binary rollover control */ #define ETH_PTPTSCR_TSPTPPSV2E (1 << 10) /* Bit 10: Time stamp PTP packet snooping for version2 format enable */ @@ -468,6 +482,7 @@ # define ETH_PTPTSCR_TSCNT_E2E (2 << ETH_PTPTSCR_TSCNT_SHIFT) /* 10: End-to-end transparent clock */ # define ETH_PTPTSCR_TSCNT_P2P (3 << ETH_PTPTSCR_TSCNT_SHIFT) /* 11: Peer-to-peer transparent clock */ #define ETH_PTPTSCR_TSPFFMAE (1 << 18) /* Bit 18: Time stamp PTP frame filtering MAC address enable */ +#endif /* Ethernet PTP subsecond increment register */ @@ -543,7 +558,9 @@ #define ETH_DMABMR_USP (1 << 23) /* Bit 23: Use separate PBL */ #define ETH_DMABMR_FPM (1 << 24) /* Bit 24: 4xPBL mode */ #define ETH_DMABMR_AAB (1 << 25) /* Bit 25: Address-aligned beats */ -#define ETH_DMABMR_MB (1 << 26) /* Bit 26: Mixed burst */ +#if defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) +# define ETH_DMABMR_MB (1 << 26) /* Bit 26: Mixed burst */ +#endif /* Ethernet DMA transmit poll demand register (32-bit) */ /* Ethernet DMA receive poll demand register (32-bit) */ @@ -694,7 +711,9 @@ /* RDES0: Receive descriptor Word0 */ #define ETH_RDES0_PCE (1 << 0) /* Bit 0: Payload checksum error */ -#define ETH_RDES0_ESA (1 << 0) /* Bit 0: Extended status available */ +#if defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) +# define ETH_RDES0_ESA (1 << 0) /* Bit 0: Extended status available */ +#endif #define ETH_RDES0_CE (1 << 1) /* Bit 1: CRC error */ #define ETH_RDES0_DBE (1 << 2) /* Bit 2: Dribble bit error */ #define ETH_RDES0_RE (1 << 3) /* Bit 3: Receive error */ @@ -718,8 +737,9 @@ /* RDES1: Receive descriptor Word1 */ -#define ETH_RDES1_RBS1_SHIFT (0) /* Bits 0-12: Receive buffer 1 size */ +#define ETH_RDES1_RBS1_SHIFT (0) /* Bits 0-12: Receive buffer 1 size */ #define ETH_RDES1_RBS1_MASK (0x1fff << ETH_RDES1_RBS1_SHIFT) + /* Bit 13: Reserved */ #define ETH_RDES1_RCH (1 << 14) /* Bit 14: Second address chained */ #define ETH_RDES1_RER (1 << 15) /* Bit 15: Receive end of ring */ #define ETH_RDES1_RBS2_SHIFT (16) /* Bits 16-28: Receive buffer 2 size */ diff --git a/nuttx/arch/arm/src/stm32/chip/stm32f10xxx_gpio.h b/nuttx/arch/arm/src/stm32/chip/stm32f10xxx_gpio.h index feeeda6dd2..a95d13100e 100644 --- a/nuttx/arch/arm/src/stm32/chip/stm32f10xxx_gpio.h +++ b/nuttx/arch/arm/src/stm32/chip/stm32f10xxx_gpio.h @@ -265,48 +265,58 @@ /* AF remap and debug I/O configuration register */ -#define AFIO_MAPR_SWJ_CFG_SHIFT (24) /* Bits 26-24: Serial Wire JTAG configuration*/ +#define AFIO_MAPR_SPI1_REMAP (1 << 0) /* Bit 0: SPI1 remapping */ +#define AFIO_MAPR_I2C1_REMAP (1 << 1) /* Bit 1: I2C1 remapping */ +#define AFIO_MAPR_USART1_REMAP (1 << 2) /* Bit 2: USART1 remapping */ +#define AFIO_MAPR_USART2_REMAP (1 << 3) /* Bit 3: USART2 remapping */ +#define AFIO_MAPR_USART3_REMAP_SHIFT (4) /* Bits 5-4: USART3 remapping */ +#define AFIO_MAPR_USART3_REMAP_MASK (3 << AFIO_MAPR_USART3_REMAP_SHIFT) +# define AFIO_MAPR_USART3_NOREMAP (0 << AFIO_MAPR_USART3_REMAP_SHIFT) /* 00: No remap */ +# define AFIO_MAPR_USART3_PARTREMAP (1 << AFIO_MAPR_USART3_REMAP_SHIFT) /* 01: Partial remap */ +# define AFIO_MAPR_USART3_FULLREMAP (3 << AFIO_MAPR_USART3_REMAP_SHIFT) /* 11: Full remap */ +#define AFIO_MAPR_TIM1_REMAP_SHIFT (6) /* Bits 7-6: TIM1 remapping */ +#define AFIO_MAPR_TIM1_REMAP_MASK (3 << AFIO_MAPR_TIM1_REMAP_SHIFT) +# define AFIO_MAPR_TIM1_NOREMAP (0 << AFIO_MAPR_TIM1_REMAP_SHIFT) /* 00: No remap */ +# define AFIO_MAPR_TIM1_PARTREMAP (1 << AFIO_MAPR_TIM1_REMAP_SHIFT) /* 01: Partial remap */ +# define AFIO_MAPR_TIM1_FULLREMAP (3 << AFIO_MAPR_TIM1_REMAP_SHIFT) /* 11: Full remap */ +#define AFIO_MAPR_TIM2_REMAP_SHIFT (8) /* Bits 9-8: TIM2 remapping */ +#define AFIO_MAPR_TIM2_REMAP_MASK (3 << AFIO_MAPR_TIM2_REMAP_SHIFT) +# define AFIO_MAPR_TIM2_NOREMAP (0 << AFIO_MAPR_TIM2_REMAP_SHIFT) /* 00: No remap */ +# define AFIO_MAPR_TIM2_PARTREMAP1 (1 << AFIO_MAPR_TIM2_REMAP_SHIFT) /* 01: Partial remap */ +# define AFIO_MAPR_TIM2_PARTREMAP2 (2 << AFIO_MAPR_TIM2_REMAP_SHIFT) /* 10: Partial remap */ +# define AFIO_MAPR_TIM2_FULLREMAP (3 << AFIO_MAPR_TIM2_REMAP_SHIFT) /* 11: Full remap */ +#define AFIO_MAPR_TIM3_REMAP_SHIFT (10) /* Bits 11-10: TIM3 remapping */ +#define AFIO_MAPR_TIM3_REMAP_MASK (3 << AFIO_MAPR_TIM3_REMAP_SHIFT) +# define AFIO_MAPR_TIM3_NOREMAP (0 << AFIO_MAPR_TIM3_REMAP_SHIFT) /* 00: No remap */ +# define AFIO_MAPR_TIM3_PARTREMAP (2 << AFIO_MAPR_TIM3_REMAP_SHIFT) /* 10: Partial remap */ +# define AFIO_MAPR_TIM3_FULLREMAP (3 << AFIO_MAPR_TIM3_REMAP_SHIFT) /* 11: Full remap */ +#define AFIO_MAPR_TIM4_REMAP (1 << 12) /* Bit 12: TIM4 remapping */ +#define AFIO_MAPR_CAN1_REMAP_SHIFT (13) /* Bits 14-13: CAN Alternate function remapping */ +#define AFIO_MAPR_CAN1_REMAP_MASK (3 << AFIO_MAPR_CAN1_REMAP_SHIFT) +# define AFIO_MAPR_PA1112 (0 << AFIO_MAPR_CAN1_REMAP_SHIFT) /* 00: CANRX mapped to PA11, CANTX mapped to PA12 */ +# define AFIO_MAPR_PB89 (2 << AFIO_MAPR_CAN1_REMAP_SHIFT) /* 10: CANRX mapped to PB8, CANTX mapped to PB9 */ +# define AFIO_MAPR_PD01 (3 << AFIO_MAPR_CAN1_REMAP_SHIFT) /* 11: CANRX mapped to PD0, CANTX mapped to PD1 */ +#define AFIO_MAPR_PD01_REMAP (1 << 15) /* Bit 15 : Port D0/Port D1 mapping on OSC_IN/OSC_OUT */ +#define AFIO_MAPR_TIM5CH4_IREMAP (1 << 16) /* Bit 16: TIM5 channel4 internal remap */ + /* Bits 17-20: Reserved */ +#ifdef CONFIG_STM32_CONNECTIVITYLINE +# define AFIO_MAPR_ETH_REMAP (1 << 21) /* Bit 21: Ethernet MAC I/O remapping */ +# define AFIO_MAPR_CAN2_REMAP (1 << 22) /* Bit 22: CAN2 I/O remapping */ +# define AFIO_MAPR_MII_RMII_SEL (1 << 23) /* Bit 23: MII or RMII selection */ +#endif +#define AFIO_MAPR_SWJ_CFG_SHIFT (24) /* Bits 26-24: Serial Wire JTAG configuration */ #define AFIO_MAPR_SWJ_CFG_MASK (7 << AFIO_MAPR_SWJ_CFG_SHIFT) # define AFIO_MAPR_SWJRST (0 << AFIO_MAPR_SWJ_CFG_SHIFT) /* 000: Full SWJ (JTAG-DP + SW-DP): Reset State */ # define AFIO_MAPR_SWJ (1 << AFIO_MAPR_SWJ_CFG_SHIFT) /* 001: Full SWJ (JTAG-DP + SW-DP) but without JNTRST */ # define AFIO_MAPR_SWDP (2 << AFIO_MAPR_SWJ_CFG_SHIFT) /* 010: JTAG-DP Disabled and SW-DP Enabled */ # define AFIO_MAPR_DISAB (4 << AFIO_MAPR_SWJ_CFG_SHIFT) /* 100: JTAG-DP Disabled and SW-DP Disabled */ + /* Bit 27: Reserved */ #ifdef CONFIG_STM32_CONNECTIVITYLINE -# define AFIO_MAPR_MII_RMII_SEL (1 << 23) /* MII or RMII selection */ +# define AFIO_MAPR_SPI3_REMAP (1 << 28) /* Bit 28: SPI3 remapping */ +# define AFIO_MAPR_TIM2ITR1_IREMAP (1 << 29) /* Bit 29: TIM2 internal trigger 1 remapping */ +# define AFIO_MAPR_PTP_PPS_REMAP (1 << 30) /* Bit 30: Ethernet PTP PPS remapping */ #endif -#define AFIO_MAPR_PD01_REMAP (1 << 15) /* Bit 15 : Port D0/Port D1 mapping on OSC_IN/OSC_OUT */ -#define AFIO_MAPR_CAN_REMAP_SHIFT (13) /* Bits 14-13: CAN Alternate function remapping */ -#define AFIO_MAPR_CAN_REMAP_MASK (3 << AFIO_MAPR_CAN_REMAP_SHIFT) -# define AFIO_MAPR_PA1112 (0 << AFIO_MAPR_CAN_REMAP_SHIFT) /* 00: CANRX mapped to PA11, CANTX mapped to PA12 */ -# define AFIO_MAPR_PB89 (2 << AFIO_MAPR_CAN_REMAP_SHIFT) /* 10: CANRX mapped to PB8, CANTX mapped to PB9 */ -# define AFIO_MAPR_PD01 (3 << AFIO_MAPR_CAN_REMAP_SHIFT) /* 11: CANRX mapped to PD0, CANTX mapped to PD1 */ -#define AFIO_MAPR_TIM4_REMAP (1 << 12) /* Bit 12: TIM4 remapping */ -#define AFIO_MAPR_TIM3_REMAP_SHIFT (10) /* Bits 11-10: TIM3 remapping */ -#define AFIO_MAPR_TIM3_REMAP_MASK (3 << AFIO_MAPR_TIM3_REMAP_SHIFT) -# define AFIO_MAPR_TIM3_NOREMAP (0 << AFIO_MAPR_TIM3_REMAP_SHIFT) /* 00: No remap (CH1/PA6, CH2/PA7, CH3/PB0, CH4/PB1) */ -# define AFIO_MAPR_TIM3_PARTREMAP (2 << AFIO_MAPR_TIM3_REMAP_SHIFT) /* 10: Partial remap (CH1/PB4, CH2/PB5, CH3/PB0, CH4/PB1) */ -# define AFIO_MAPR_TIM3_FULLREMAP (3 << AFIO_MAPR_TIM3_REMAP_SHIFT) /* 11: Full remap (CH1/PC6, CH2/PC7, CH3/PC8, CH4/PC9) */ -#define AFIO_MAPR_TIM2_REMAP_SHIFT (8) /* Bits 9-8: TIM2 remapping */ -#define AFIO_MAPR_TIM2_REMAP_MASK (3 << AFIO_MAPR_TIM2_REMAP_SHIFT) -# define AFIO_MAPR_TIM2_NOREMAP (0 << AFIO_MAPR_TIM2_REMAP_SHIFT) /* 00: No remap (CH1/ETR/PA0, CH2/PA1, CH3/PA2, CH4/PA3) */ -# define AFIO_MAPR_TIM2_PARTREMAP1 (1 << AFIO_MAPR_TIM2_REMAP_SHIFT) /* 01: Partial remap (CH1/ETR/PA15, CH2/PB3, CH3/PA2, CH4/PA3) */ -# define AFIO_MAPR_TIM2_PARTREMAP2 (2 << AFIO_MAPR_TIM2_REMAP_SHIFT) /* 10: Partial remap (CH1/ETR/PA0, CH2/PA1, CH3/PB10, CH4/PB11) */ -# define AFIO_MAPR_TIM2_FULLREMAP (3 << AFIO_MAPR_TIM2_REMAP_SHIFT) /* 11: Full remap (CH1/ETR/PA15, CH2/PB3, CH3/PB10, CH4/PB11) */ -#define AFIO_MAPR_TIM1_REMAP_SHIFT (6) /* Bits 7-6: TIM1 remapping */ -#define AFIO_MAPR_TIM1_REMAP_MASK (3 << AFIO_MAPR_TIM1_REMAP_SHIFT) -# define AFIO_MAPR_TIM1_NOREMAP (0 << AFIO_MAPR_TIM1_REMAP_SHIFT) /* 00: No remap (ETR/PA12, CH1/PA8, CH2/PA9, CH3/PA10, CH4/PA11, BKIN/PB12, CH1N/PB13, CH2N/PB14, CH3N/PB15) */ -# define AFIO_MAPR_TIM1_PARTREMAP (1 << AFIO_MAPR_TIM1_REMAP_SHIFT) /* 01: Partial remap (ETR/PA12, CH1/PA8, CH2/PA9, CH3/PA10, CH4/PA11, BKIN/PA6, CH1N/PA7, CH2N/PB0, CH3N/PB1) */ -# define AFIO_MAPR_TIM1_FULLREMAP (3 << AFIO_MAPR_TIM1_REMAP_SHIFT) /* 11: Full remap (ETR/PE7, CH1/PE9, CH2/PE11, CH3/PE13, CH4/PE14, BKIN/PE15, CH1N/PE8, CH2N/PE10, CH3N/PE12) */ -#define AFIO_MAPR_USART3_REMAP_SHIFT (6) /* Bits 5-4: USART3 remapping */ -#define AFIO_MAPR_USART3_REMAP_MASK (3 << AFIO_MAPR_USART3_REMAP_SHIFT) -# define AFIO_MAPR_USART3_NOREMAP (0 << AFIO_MAPR_USART3_REMAP_SHIFT) /* 00: No remap (TX/PB10, RX/PB11, CK/PB12, CTS/PB13, RTS/PB14) */ -# define AFIO_MAPR_USART3_PARTREMAP (1 << AFIO_MAPR_USART3_REMAP_SHIFT) /* 01: Partial remap (TX/PC10, RX/PC11, CK/PC12, CTS/PB13, RTS/PB14) */ -# define AFIO_MAPR_USART3_FULLREMAP (3 << AFIO_MAPR_USART3_REMAP_SHIFT) /* 11: Full remap (TX/PD8, RX/PD9, CK/PD10, CTS/PD11, RTS/PD12) */ -#define AFIO_MAPR_USART2_REMAP (1 << 3) /* Bit 3: USART2 remapping */ -#define AFIO_MAPR_USART1_REMAP (1 << 2) /* Bit 2: USART1 remapping */ -#define AFIO_MAPR_I2C1_REMAP (1 << 1) /* Bit 1: I2C1 remapping */ -#define AFIO_MAPR_SPI1_REMAP (1 << 0) /* Bit 0: SPI1 remapping */ - + /* Bit 31: Reserved */ /* External interrupt configuration register 1 */ #define AFIO_EXTICR_PORT_MASK (0x0f) diff --git a/nuttx/arch/arm/src/stm32/stm32_eth.c b/nuttx/arch/arm/src/stm32/stm32_eth.c index 59f3def7f6..2e892c9e54 100644 --- a/nuttx/arch/arm/src/stm32/stm32_eth.c +++ b/nuttx/arch/arm/src/stm32/stm32_eth.c @@ -135,17 +135,35 @@ # ifndef CONFIG_STM32_PHYSR # error "CONFIG_STM32_PHYSR must be defined in the NuttX configuration" # endif -# ifndef CONFIG_STM32_PHYSR_SPEED -# error "CONFIG_STM32_PHYSR_SPEED must be defined in the NuttX configuration" -# endif -# ifndef CONFIG_STM32_PHYSR_100MBPS -# error "CONFIG_STM32_PHYSR_100MBPS must be defined in the NuttX configuration" -# endif -# ifndef CONFIG_STM32_PHYSR_MODE -# error "CONFIG_STM32_PHYSR_MODE must be defined in the NuttX configuration" -# endif -# ifndef CONFIG_STM32_PHYSR_FULLDUPLEX -# error "CONFIG_STM32_PHYSR_FULLDUPLEX must be defined in the NuttX configuration" +# ifdef CONFIG_STM32_PHYSR_ALTCONFIG +# ifndef CONFIG_STM32_PHYSR_ALTMODE +# error "CONFIG_STM32_PHYSR_ALTMODE must be defined in the NuttX configuration" +# endif +# ifndef CONFIG_STM32_PHYSR_10HD +# error "CONFIG_STM32_PHYSR_10HD must be defined in the NuttX configuration" +# endif +# ifndef CONFIG_STM32_PHYSR_100HD +# error "CONFIG_STM32_PHYSR_100HD must be defined in the NuttX configuration" +# endif +# ifndef CONFIG_STM32_PHYSR_10FD +# error "CONFIG_STM32_PHYSR_10FD must be defined in the NuttX configuration" +# endif +# ifndef CONFIG_STM32_PHYSR_100FD +# error "CONFIG_STM32_PHYSR_100FD must be defined in the NuttX configuration" +# endif +# else +# ifndef CONFIG_STM32_PHYSR_SPEED +# error "CONFIG_STM32_PHYSR_SPEED must be defined in the NuttX configuration" +# endif +# ifndef CONFIG_STM32_PHYSR_100MBPS +# error "CONFIG_STM32_PHYSR_100MBPS must be defined in the NuttX configuration" +# endif +# ifndef CONFIG_STM32_PHYSR_MODE +# error "CONFIG_STM32_PHYSR_MODE must be defined in the NuttX configuration" +# endif +# ifndef CONFIG_STM32_PHYSR_FULLDUPLEX +# error "CONFIG_STM32_PHYSR_FULLDUPLEX must be defined in the NuttX configuration" +# endif # endif #endif @@ -168,10 +186,12 @@ #endif /* Add 4 to the configured buffer size to account for the 2 byte checksum - * memory needed at the end of the maximum size packet. + * memory needed at the end of the maximum size packet. Buffer sizes must + * be an even multiple of 4, 8, or 16 bytes (depending on buswidth). We + * will use the 16-byte alignment in all cases. */ -#define OPTIMAL_ETH_BUFSIZE (CONFIG_NET_BUFSIZE+4) +#define OPTIMAL_ETH_BUFSIZE ((CONFIG_NET_BUFSIZE + 4 + 15) & ~15) #ifndef CONFIG_STM32_ETH_BUFSIZE # define CONFIG_STM32_ETH_BUFSIZE OPTIMAL_ETH_BUFSIZE @@ -181,6 +201,10 @@ # error "CONFIG_STM32_ETH_BUFSIZE is too large" #endif +#if (CONFIG_STM32_ETH_BUFSIZE & 15) != 0 +# error "CONFIG_STM32_ETH_BUFSIZE must be aligned" +#endif + #if CONFIG_STM32_ETH_BUFSIZE != OPTIMAL_ETH_BUFSIZE # warning "You using an incomplete/untested configuration" #endif @@ -265,14 +289,22 @@ * ETH_MACCR_IFG Bits 17-19: Interframe gap * ETH_MACCR_JD Bit 22: Jabber disable * ETH_MACCR_WD Bit 23: Watchdog disable - * ETH_MACCR_CSTF Bits 25: CRC stripping for Type frames + * ETH_MACCR_CSTF Bits 25: CRC stripping for Type frames (F2/F4 only) */ +#if defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) #define MACCR_CLEAR_BITS \ - ( ETH_MACCR_RE | ETH_MACCR_TE | ETH_MACCR_DC | ETH_MACCR_BL_MASK | \ + (ETH_MACCR_RE | ETH_MACCR_TE | ETH_MACCR_DC | ETH_MACCR_BL_MASK | \ ETH_MACCR_APCS | ETH_MACCR_RD | ETH_MACCR_IPCO | ETH_MACCR_DM | \ ETH_MACCR_LM | ETH_MACCR_ROD | ETH_MACCR_FES | ETH_MACCR_CSD | \ - ETH_MACCR_IFG_MASK | ETH_MACCR_JD | ETH_MACCR_WD | ETH_MACCR_CSTF ) + ETH_MACCR_IFG_MASK | ETH_MACCR_JD | ETH_MACCR_WD | ETH_MACCR_CSTF) +#else +#define MACCR_CLEAR_BITS \ + (ETH_MACCR_RE | ETH_MACCR_TE | ETH_MACCR_DC | ETH_MACCR_BL_MASK | \ + ETH_MACCR_APCS | ETH_MACCR_RD | ETH_MACCR_IPCO | ETH_MACCR_DM | \ + ETH_MACCR_LM | ETH_MACCR_ROD | ETH_MACCR_FES | ETH_MACCR_CSD | \ + ETH_MACCR_IFG_MASK | ETH_MACCR_JD | ETH_MACCR_WD) +#endif /* The following bits are set or left zero unconditionally in all modes. * @@ -289,7 +321,7 @@ * ETH_MACCR_IFG Interframe gap 0 (96 bits) * ETH_MACCR_JD Jabber disable 0 (enabled) * ETH_MACCR_WD Watchdog disable 0 (enabled) - * ETH_MACCR_CSTF CRC stripping for Type frames 0 (disabled) + * ETH_MACCR_CSTF CRC stripping for Type frames 0 (disabled, F2/F4 only) * * The following are set conditioinally based on mode and speed. * @@ -444,13 +476,20 @@ * ETH_DMABMR_USP Bit 23: Use separate PBL * ETH_DMABMR_FPM Bit 24: 4xPBL mode * ETH_DMABMR_AAB Bit 25: Address-aligned beats - * ETH_DMABMR_MB Bit 26: Mixed burst + * ETH_DMABMR_MB Bit 26: Mixed burst (F2/F4 only) */ +#if defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) #define DMABMR_CLEAR_MASK \ (ETH_DMABMR_SR | ETH_DMABMR_DA | ETH_DMABMR_DSL_MASK | ETH_DMABMR_EDFE | \ ETH_DMABMR_PBL_MASK | ETH_DMABMR_RTPR_MASK | ETH_DMABMR_FB | ETH_DMABMR_RDP_MASK | \ ETH_DMABMR_USP | ETH_DMABMR_FPM | ETH_DMABMR_AAB | ETH_DMABMR_MB) +#else +#define DMABMR_CLEAR_MASK \ + (ETH_DMABMR_SR | ETH_DMABMR_DA | ETH_DMABMR_DSL_MASK | ETH_DMABMR_EDFE | \ + ETH_DMABMR_PBL_MASK | ETH_DMABMR_RTPR_MASK | ETH_DMABMR_FB | ETH_DMABMR_RDP_MASK | \ + ETH_DMABMR_USP | ETH_DMABMR_FPM | ETH_DMABMR_AAB) +#endif /* The following bits are set or left zero unconditionally in all modes. * @@ -466,7 +505,7 @@ * ETH_DMABMR_USP Use separate PBL 1 (enabled) * ETH_DMABMR_FPM 4xPBL mode 0 (disabled) * ETH_DMABMR_AAB Address-aligned beats 1 (enabled) - * ETH_DMABMR_MB Mixed burst 0 (disabled) + * ETH_DMABMR_MB Mixed burst 0 (disabled, F2/F4 only) */ #ifdef CONFIG_STM32_ETH_ENHANCEDDESC @@ -1437,9 +1476,6 @@ static int stm32_recvframe(FAR struct stm32_ethmac_s *priv) { priv->segments++; - nllvdbg("rxhead: %p rxcurr: %p segments: %d\n", - priv->rxhead, priv->rxcurr, priv->segments); - /* Check if the there is only one segment in the frame */ if (priv->segments == 1) @@ -1451,6 +1487,9 @@ static int stm32_recvframe(FAR struct stm32_ethmac_s *priv) rxcurr = priv->rxcurr; } + nllvdbg("rxhead: %p rxcurr: %p segments: %d\n", + priv->rxhead, priv->rxcurr, priv->segments); + /* Check if any errors are reported in the frame */ if ((rxdesc->rdes0 & ETH_RDES0_ES) == 0) @@ -1983,7 +2022,7 @@ static int stm32_ifup(struct uip_driver_s *dev) ndbg("Bringing up: %d.%d.%d.%d\n", dev->d_ipaddr & 0xff, (dev->d_ipaddr >> 8) & 0xff, - (dev->d_ipaddr >> 16) & 0xff, dev->d_ipaddr >> 24 ); + (dev->d_ipaddr >> 16) & 0xff, dev->d_ipaddr >> 24); /* Configure the Ethernet interface for DMA operation. */ @@ -2224,7 +2263,7 @@ static void stm32_txdescinit(FAR struct stm32_ethmac_s *priv) /* Initialize the next descriptor with the Next Descriptor Polling Enable */ - if( i < (CONFIG_STM32_ETH_NTXDESC-1)) + if (i < (CONFIG_STM32_ETH_NTXDESC-1)) { /* Set next descriptor address register with next descriptor base * address @@ -2303,7 +2342,7 @@ static void stm32_rxdescinit(FAR struct stm32_ethmac_s *priv) /* Initialize the next descriptor with the Next Descriptor Polling Enable */ - if( i < (CONFIG_STM32_ETH_NRXDESC-1)) + if (i < (CONFIG_STM32_ETH_NRXDESC-1)) { /* Set next descriptor address register with next descriptor base * address @@ -2506,7 +2545,7 @@ static int stm32_phyinit(FAR struct stm32_ethmac_s *priv) if (timeout >= PHY_RETRY_TIMEOUT) { - ndbg("Timed out waiting for link status\n"); + ndbg("Timed out waiting for link status: %04x\n", phyval); return -ETIMEDOUT; } @@ -2552,6 +2591,46 @@ static int stm32_phyinit(FAR struct stm32_ethmac_s *priv) /* Remember the selected speed and duplex modes */ + nvdbg("PHYSR[%d]: %04x\n", CONFIG_STM32_PHYSR, phyval); + + /* Different PHYs present speed and mode information in different ways. IF + * This CONFIG_STM32_PHYSR_ALTCONFIG is selected, this indicates that the PHY + * represents speed and mode information are combined, for example, with + * separate bits for 10HD, 100HD, 10FD and 100FD. + */ + +#ifdef CONFIG_STM32_PHYSR_ALTCONFIG + switch (phyval & CONFIG_STM32_PHYSR_ALTMODE) + { + default: + case CONFIG_STM32_PHYSR_10HD: + priv->fduplex = 0; + priv->mbps100 = 0; + break; + + case CONFIG_STM32_PHYSR_100HD: + priv->fduplex = 0; + priv->mbps100 = 1; + break; + + case CONFIG_STM32_PHYSR_10FD: + priv->fduplex = 1; + priv->mbps100 = 0; + break; + + case CONFIG_STM32_PHYSR_100FD: + priv->fduplex = 1; + priv->mbps100 = 1; + break; + } + + /* Different PHYs present speed and mode information in different ways. Some + * will present separate information for speed and mode (this is the default). + * Those PHYs, for example, may provide a 10/100 Mbps indication and a separate + * full/half duplex indication. + */ + +#else if ((phyval & CONFIG_STM32_PHYSR_MODE) == CONFIG_STM32_PHYSR_FULLDUPLEX) { priv->fduplex = 1; @@ -2561,6 +2640,7 @@ static int stm32_phyinit(FAR struct stm32_ethmac_s *priv) { priv->mbps100 = 1; } +#endif #else /* Auto-negotion not selected */ @@ -2854,7 +2934,7 @@ static void stm32_ethreset(FAR struct stm32_ethmac_s *priv) * reset all the registers holds their reset values. */ - regval = stm32_getreg(STM32_ETH_DMABMR); + regval = stm32_getreg(STM32_ETH_DMABMR); regval |= ETH_DMABMR_SR; stm32_putreg(regval, STM32_ETH_DMABMR); diff --git a/nuttx/arch/arm/src/stm32/stm32_gpio.c b/nuttx/arch/arm/src/stm32/stm32_gpio.c index 143e48a2c8..4703e82089 100644 --- a/nuttx/arch/arm/src/stm32/stm32_gpio.c +++ b/nuttx/arch/arm/src/stm32/stm32_gpio.c @@ -124,14 +124,27 @@ static inline void stm32_gpioremap(void) uint32_t val = 0; -#ifdef CONFIG_STM32_JTAG_FULL_ENABLE - /* The reset default */ -#elif CONFIG_STM32_JTAG_NOJNTRST_ENABLE - val |= AFIO_MAPR_SWJ; /* enabled but without JNTRST */ -#elif CONFIG_STM32_JTAG_SW_ENABLE - val |= AFIO_MAPR_SWDP; /* set JTAG-DP disabled and SW-DP enabled */ -#else - val |= AFIO_MAPR_DISAB; /* set JTAG-DP and SW-DP Disabled */ +#ifdef CONFIG_STM32_SPI1_REMAP + val |= AFIO_MAPR_SPI1_REMAP; +#endif +#ifdef CONFIG_STM32_SPI3_REMAP +#endif + +#ifdef CONFIG_STM32_I2C1_REMAP + val |= AFIO_MAPR_I2C1_REMAP; +#endif + +#ifdef CONFIG_STM32_USART1_REMAP + val |= AFIO_MAPR_USART1_REMAP; +#endif +#ifdef CONFIG_STM32_USART2_REMAP + val |= AFIO_MAPR_USART2_REMAP; +#endif +#ifdef CONFIG_STM32_USART3_FULL_REMAP + val |= AFIO_MAPR_USART3_FULLREMAP; +#endif +#ifdef CONFIG_STM32_USART3_PARTIAL_REMAP + val |= AFIO_MAPR_USART3_PARTREMAP; #endif #ifdef CONFIG_STM32_TIM1_FULL_REMAP @@ -159,35 +172,29 @@ static inline void stm32_gpioremap(void) val |= AFIO_MAPR_TIM4_REMAP; #endif -#ifdef CONFIG_STM32_USART1_REMAP - val |= AFIO_MAPR_USART1_REMAP; -#endif -#ifdef CONFIG_STM32_USART2_REMAP - val |= AFIO_MAPR_USART2_REMAP; -#endif -#ifdef CONFIG_STM32_USART3_FULL_REMAP - val |= AFIO_MAPR_USART3_FULLREMAP; -#endif -#ifdef CONFIG_STM32_USART3_PARTIAL_REMAP - val |= AFIO_MAPR_USART3_PARTREMAP; -#endif - -#ifdef CONFIG_STM32_SPI1_REMAP - val |= AFIO_MAPR_SPI1_REMAP; -#endif -#ifdef CONFIG_STM32_SPI3_REMAP -#endif - -#ifdef CONFIG_STM32_I2C1_REMAP - val |= AFIO_MAPR_I2C1_REMAP; -#endif - #ifdef CONFIG_STM32_CAN1_REMAP1 val |= AFIO_MAPR_PB89; #endif #ifdef CONFIG_STM32_CAN1_REMAP2 val |= AFIO_MAPR_PD01; #endif +#ifdef CONFIG_STM32_CAN2_REMAP /* Connectivity line only */ + val |= AFIO_MAPR_CAN2_REMAP; +#endif + +#ifdef CONFIG_STM32_ETH_REMAP /* Connectivity line only */ + val |= AFIO_MAPR_ETH_REMAP; +#endif + +#ifdef CONFIG_STM32_JTAG_FULL_ENABLE + /* The reset default */ +#elif CONFIG_STM32_JTAG_NOJNTRST_ENABLE + val |= AFIO_MAPR_SWJ; /* enabled but without JNTRST */ +#elif CONFIG_STM32_JTAG_SW_ENABLE + val |= AFIO_MAPR_SWDP; /* set JTAG-DP disabled and SW-DP enabled */ +#else + val |= AFIO_MAPR_DISAB; /* set JTAG-DP and SW-DP Disabled */ +#endif putreg32(val, STM32_AFIO_MAPR); #endif diff --git a/nuttx/arch/arm/src/stm32/stm32_idle.c b/nuttx/arch/arm/src/stm32/stm32_idle.c index 791a79429c..83a6808c58 100644 --- a/nuttx/arch/arm/src/stm32/stm32_idle.c +++ b/nuttx/arch/arm/src/stm32/stm32_idle.c @@ -45,6 +45,7 @@ #include +#include "chip.h" #include "stm32_pm.h" #include "up_internal.h" @@ -178,11 +179,33 @@ void up_idle(void) up_idlepm(); - /* Sleep until an interrupt occurs to save power */ + /* Sleep until an interrupt occurs to save power. + * + * NOTE: There is an STM32F107 errata that is fixed by the following + * workaround: + * + * "2.17.11 Ethernet DMA not working after WFI/WFE instruction + * Description + * If a WFI/WFE instruction is executed to put the system in sleep mode + * while the Ethernet MAC master clock on the AHB bus matrix is ON and all + * remaining masters clocks are OFF, the Ethernet DMA will be not able to + * perform any AHB master accesses during sleep mode." + * + * Workaround + * Enable DMA1 or DMA2 clocks in the RCC_AHBENR register before + * executing the WFI/WFE instruction." + * + * Here the workaround is just to avoid SLEEP mode for the connectivity + * line parts if Ethernet is enabled. The errate recommends a more + * general solution: Enabling DMA1/2 clocking in stm32f10xx_rcc.c if the + * STM32107 Ethernet peripheral is enabled. + */ +#if !defined(CONFIG_STM32_CONNECTIVITYLINE) || !defined(CONFIG_STM32_ETHMAC) BEGIN_IDLE(); asm("WFI"); END_IDLE(); #endif +#endif } diff --git a/nuttx/arch/arm/src/stm32/stm32f10xxx_rcc.c b/nuttx/arch/arm/src/stm32/stm32f10xxx_rcc.c index 10b8572cf0..47ed5e016b 100644 --- a/nuttx/arch/arm/src/stm32/stm32f10xxx_rcc.c +++ b/nuttx/arch/arm/src/stm32/stm32f10xxx_rcc.c @@ -90,7 +90,7 @@ static inline void rcc_reset(void) regval = getreg32(STM32_RCC_CR); /* Reset HSEBYP bit */ regval &= ~RCC_CR_HSEBYP; putreg32(regval, STM32_RCC_CR); - + regval = getreg32(STM32_RCC_CFGR); /* Reset PLLSRC, PLLXTPRE, PLLMUL and USBPRE bits */ regval &= ~(RCC_CFGR_PLLSRC|RCC_CFGR_PLLXTPRE|RCC_CFGR_PLLMUL_MASK|RCC_CFGR_USBPRE); putreg32(regval, STM32_RCC_CFGR); @@ -235,7 +235,7 @@ static inline void rcc_enableapb1(void) regval |= RCC_APB1ENR_SPI2EN; #endif - + #ifdef CONFIG_STM32_SPI3 /* SPI 3 clock enable */ @@ -411,13 +411,128 @@ static inline void rcc_enableapb2(void) * Name: stm32_stdclockconfig * * Description: - * Called to change to new clock based on settings in board.h - * + * Called to change to new clock based on settings in board.h. This + * version is for the Connectivity Line parts. + * * NOTE: This logic would need to be extended if you need to select low- * power clocking modes! ****************************************************************************/ -#ifndef CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG +#if !defined(CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG) && defined(CONFIG_STM32_CONNECTIVITYLINE) +static void stm32_stdclockconfig(void) +{ + uint32_t regval; + + /* Enable HSE */ + + regval = getreg32(STM32_RCC_CR); + regval &= ~RCC_CR_HSEBYP; /* Disable HSE clock bypass */ + regval |= RCC_CR_HSEON; /* Enable HSE */ + putreg32(regval, STM32_RCC_CR); + + /* Set flash wait states + * Sysclk runs with 72MHz -> 2 waitstates. + * 0WS from 0-24MHz + * 1WS from 24-48MHz + * 2WS from 48-72MHz + */ + + regval = getreg32(STM32_FLASH_ACR); + regval &= ~FLASH_ACR_LATENCY_MASK; + regval |= (FLASH_ACR_LATENCY_2|FLASH_ACR_PRTFBE); + putreg32(regval, STM32_FLASH_ACR); + + /* Set up PLL input scaling (with source = PLL2) */ + + regval = getreg32(STM32_RCC_CFGR2); + regval &= ~(RCC_CFGR2_PREDIV2_MASK | RCC_CFGR2_PLL2MUL_MASK | + RCC_CFGR2_PREDIV1SRC_MASK | RCC_CFGR2_PREDIV1_MASK); + regval |= (STM32_PLL_PREDIV2 | STM32_PLL_PLL2MUL | + RCC_CFGR2_PREDIV1SRC_PLL2 | STM32_PLL_PREDIV1); + putreg32(regval, STM32_RCC_CFGR2); + + /* Set the PCLK2 divider */ + + regval = getreg32(STM32_RCC_CFGR); + regval &= ~(RCC_CFGR_PPRE2_MASK | RCC_CFGR_HPRE_MASK); + regval |= STM32_RCC_CFGR_PPRE2; + regval |= RCC_CFGR_HPRE_SYSCLK; + putreg32(regval, STM32_RCC_CFGR); + + /* Set the PCLK1 divider */ + + regval = getreg32(STM32_RCC_CFGR); + regval &= ~RCC_CFGR_PPRE1_MASK; + regval |= STM32_RCC_CFGR_PPRE1; + putreg32(regval, STM32_RCC_CFGR); + + /* Enable PLL2 */ + + regval = getreg32(STM32_RCC_CR); + regval |= RCC_CR_PLL2ON; + putreg32(regval, STM32_RCC_CR); + + /* Wait for PLL2 ready */ + + while((getreg32(STM32_RCC_CR) & RCC_CR_PLL2RDY) == 0); + + /* Setup PLL3 for MII/RMII clock on MCO */ + +#if defined(CONFIG_STM32_MII_MCO) || defined(CONFIG_STM32_RMII_MCO) + regval = getreg32(STM32_RCC_CFGR2); + regval &= ~(RCC_CFGR2_PLL3MUL_MASK); + regval |= STM32_PLL_PLL3MUL; + putreg32(regval, STM32_RCC_CFGR2); + + /* Switch PLL3 on */ + + regval = getreg32(STM32_RCC_CR); + regval |= RCC_CR_PLL3ON; + putreg32(regval, STM32_RCC_CR); + + while ((getreg32(STM32_RCC_CR) & RCC_CR_PLL3RDY) == 0); +#endif + + /* Set main PLL source and multiplier */ + + regval = getreg32(STM32_RCC_CFGR); + regval &= ~(RCC_CFGR_PLLSRC | RCC_CFGR_PLLMUL_MASK); + regval |= (RCC_CFGR_PLLSRC | STM32_PLL_PLLMUL); + putreg32(regval, STM32_RCC_CFGR); + + /* Switch main PLL on */ + + regval = getreg32(STM32_RCC_CR); + regval |= RCC_CR_PLLON; + putreg32(regval, STM32_RCC_CR); + + while ((getreg32(STM32_RCC_CR) & RCC_CR_PLLRDY) == 0); + + /* Select PLL as system clock source */ + + regval = getreg32(STM32_RCC_CFGR); + regval &= ~RCC_CFGR_SW_MASK; + regval |= RCC_CFGR_SW_PLL; + putreg32(regval, STM32_RCC_CFGR); + + /* Wait until PLL is used as the system clock source */ + + while ((getreg32(STM32_RCC_CFGR) & RCC_CFGR_SWS_PLL) == 0); +} +#endif + +/**************************************************************************** + * Name: stm32_stdclockconfig + * + * Description: + * Called to change to new clock based on settings in board.h. This + * version is for the non-Connectivity Line parts. + * + * NOTE: This logic would need to be extended if you need to select low- + * power clocking modes! + ****************************************************************************/ + +#if !defined(CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG) && !defined(CONFIG_STM32_CONNECTIVITYLINE) static void stm32_stdclockconfig(void) { uint32_t regval; @@ -430,7 +545,7 @@ static void stm32_stdclockconfig(void) volatile int32_t timeout; /* Enable External High-Speed Clock (HSE) */ - + regval = getreg32(STM32_RCC_CR); regval &= ~RCC_CR_HSEBYP; /* Disable HSE clock bypass */ regval |= RCC_CR_HSEON; /* Enable HSE */ diff --git a/nuttx/configs/README.txt b/nuttx/configs/README.txt index b5ada6ee5d..5bbec874f7 100644 --- a/nuttx/configs/README.txt +++ b/nuttx/configs/README.txt @@ -1570,7 +1570,8 @@ configs/ez80f0910200zco configs/fire-stm32v2 A configuration for the M3 Wildfire STM32 board. This board is based on the - STM32F103VET6 chip. See http://firestm32.taobao.com + STM32F103VET6 chip. See http://firestm32.taobao.com . Version 2 and 3 of + the boards are supported but only version 2 has been tested. configs/hymini-stm32v A configuration for the HY-Mini STM32v board. This board is based on the diff --git a/nuttx/drivers/Kconfig b/nuttx/drivers/Kconfig index 32d127aa43..ea218a5925 100644 --- a/nuttx/drivers/Kconfig +++ b/nuttx/drivers/Kconfig @@ -151,7 +151,7 @@ config SPI_EXCHANGE config SPI_CMDDATA bool "SPI CMD/DATA" - default y + default n ---help--- Devices on the SPI bus require out-of-band support to distinguish command transfers from data transfers. Such devices will often support either 9-bit diff --git a/nuttx/drivers/input/Kconfig b/nuttx/drivers/input/Kconfig index 1303cfbd0f..9fde35ff67 100644 --- a/nuttx/drivers/input/Kconfig +++ b/nuttx/drivers/input/Kconfig @@ -6,8 +6,15 @@ config INPUT_TSC2007 bool "TI TSC2007 touchscreen controller" default n select I2C + ---help--- + Enable support for the TI TSC2007 touchscreen controller + config INPUT_ADS7843E - bool "TI ADS7843E touchscreen controller" + bool "TI ADS7843/TSC2046 touchscreen controller" default n select SPI + ---help--- + Enable support for the TI/Burr-Brown ADS7842 touchscreen controller. I believe + that driver should be compatibile with the TI/Burr-Brown TSC2046 and XPT2046 + touchscreen controllers as well. diff --git a/nuttx/drivers/input/ads7843e.c b/nuttx/drivers/input/ads7843e.c index e08a7a7280..07e5e515df 100644 --- a/nuttx/drivers/input/ads7843e.c +++ b/nuttx/drivers/input/ads7843e.c @@ -9,6 +9,12 @@ * "Touch Screen Controller, ADS7843," Burr-Brown Products from Texas * Instruments, SBAS090B, September 2000, Revised May 2002" * + * See also: + * "Low Voltage I/O Touch Screen Controller, TSC2046," Burr-Brown Products + * from Texas Instruments, SBAS265F, October 2002, Revised August 2007. + * + * "XPT2046 Data Sheet," Shenzhen XPTek Technology Co., Ltd, 2007 + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: diff --git a/nuttx/drivers/input/ads7843e.h b/nuttx/drivers/input/ads7843e.h index 030d1cb332..43b79c7b7d 100644 --- a/nuttx/drivers/input/ads7843e.h +++ b/nuttx/drivers/input/ads7843e.h @@ -8,6 +8,12 @@ * "Touch Screen Controller, ADS7843," Burr-Brown Products from Texas * Instruments, SBAS090B, September 2000, Revised May 2002" * + * See also: + * "Low Voltage I/O Touch Screen Controller, TSC2046," Burr-Brown Products + * from Texas Instruments, SBAS265F, October 2002, Revised August 2007." + * + * "XPT2046 Data Sheet," Shenzhen XPTek Technology Co., Ltd, 2007 + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: diff --git a/nuttx/drivers/lcd/Kconfig b/nuttx/drivers/lcd/Kconfig index 081a79c89d..640239e637 100644 --- a/nuttx/drivers/lcd/Kconfig +++ b/nuttx/drivers/lcd/Kconfig @@ -2,6 +2,21 @@ # For a description of the syntax of this configuration file, # see misc/tools/kconfig-language.txt. # + +config LCD_NOGETRUN + bool "Write-only LCD" + default n + ---help--- + Many LCD hardware interfaces provide only minimal graphics capability. In + particulary, many simple LCD interfaces are write only. That is we, can + write graphics data to the LCD device memory, but we cannot read it back. + If the LCD hardware does not support reading the graphics memory, then + this option should be defined so that the NX layer can taking alternative + measures when the LCD is not readable. For example, if the LCD is not + readable, then NX will not attempt to support transparency. + + See also NX_WRITEONLY in the graphics support menu. + config LCD_MAXCONTRAST int "LCD maximum contrast" default 63 if NOKIA6100_S1D15G10 @@ -28,6 +43,7 @@ config LCD_P14201 p14201.c. Driver for RiT P14201 series display with SD1329 IC controller. This OLED is used with older versions of the TI/Luminary LM3S8962 Evaluation Kit. + if LCD_P14201 config P14201_NINTERFACES int "Number of physical P14201 devices" @@ -174,6 +190,33 @@ config LCD_UG9664HSWAG01 Technology Inc. Used with the LPC Xpresso and Embedded Artists base board. +config LCD_SSD1289 + bool "LCD Based on SSD1289 Controller" + default n + ---help--- + Enables generic support for any LCD based on the Solomon Systech, + Ltd, SSD1289 Controller. Use of this driver will usually require so + detailed customization of the LCD initialization code as necessary + for the specific LCD driven by the SSD1289 controller. + +if LCD_SSD1289 + +choice + prompt "SSD1289 Initialization Profile" + default SSD1289_PROFILE1 + +config SSD1289_PROFILE1 + bool "Profile 1" + +config SSD1289_PROFILE2 + bool "Profile 2" + +config SSD1289_PROFILE3 + bool "Profile 3" + +endchoice +endif + choice prompt "LCD Orientation" default LCD_LANDSCAPE diff --git a/nuttx/drivers/lcd/ssd1289.c b/nuttx/drivers/lcd/ssd1289.c index 58c6069682..3d5ba96d31 100644 --- a/nuttx/drivers/lcd/ssd1289.c +++ b/nuttx/drivers/lcd/ssd1289.c @@ -766,7 +766,7 @@ static int ssd1289_getvideoinfo(FAR struct lcd_dev_s *dev, { DEBUGASSERT(dev && vinfo); lcdvdbg("fmt: %d xres: %d yres: %d nplanes: 1\n", - SSD1289_COLORFMT, SSD1289_XRES, SSD1289_XRES); + SSD1289_COLORFMT, SSD1289_XRES, SSD1289_YRES); vinfo->fmt = SSD1289_COLORFMT; /* Color format: RGB16-565: RRRR RGGG GGGB BBBB */ vinfo->xres = SSD1289_XRES; /* Horizontal resolution in pixel columns */ @@ -925,6 +925,7 @@ static inline int ssd1289_hwinitialize(FAR struct ssd1289_dev_s *priv) #ifndef CONFIG_LCD_NOGETRUN uint16_t id; #endif + int ret; /* Select the LCD */ @@ -1168,19 +1169,20 @@ static inline int ssd1289_hwinitialize(FAR struct ssd1289_dev_s *priv) /* One driver has a 50 msec delay here */ /* up_mdelay(50); */ - return OK; + ret = OK; } #ifndef CONFIG_LCD_NOGETRUN else { lcddbg("Unsupported LCD type\n"); - return -ENODEV; + ret = -ENODEV; } #endif /* De-select the LCD */ lcd->deselect(lcd); + return ret; } /************************************************************************************* diff --git a/nuttx/drivers/net/enc28j60.h b/nuttx/drivers/net/enc28j60.h index 6f7f8f4652..6a0553a95d 100644 --- a/nuttx/drivers/net/enc28j60.h +++ b/nuttx/drivers/net/enc28j60.h @@ -78,7 +78,7 @@ * * The last five locations (0x1b to 0x1f) of all banks point to a common set * of registers: EIE, EIR, ESTAT, ECON2 and ECON1. These are key registers - * usedin controlling and monitoring the operation of the device. Their + * used in controlling and monitoring the operation of the device. Their * common mapping allows easy access without switching the bank. * * Control registers for the ENC28J60 are generically grouped as ETH, MAC and diff --git a/nuttx/graphics/Kconfig b/nuttx/graphics/Kconfig index ae2bf31307..18b1e1ab97 100644 --- a/nuttx/graphics/Kconfig +++ b/nuttx/graphics/Kconfig @@ -2,3 +2,436 @@ # For a description of the syntax of this configuration file, # see misc/tools/kconfig-language.txt. # + +config NX + bool "NX Graphics" + default n + ---help--- + Enables overall support for graphics library and NX + +if NX + +config NX_LCDDRIVER + bool "LCD driver" + default y + depends on LCD + ---help--- + By default, the NX graphics system uses the frame buffer driver interface + defined in include/nuttx/fb.h. However, if LCD is support is enabled, + this this option is provide to select, instead, the LCD driver interface + defined in include/nuttx/lcd/lcd.h. + +config NX_NPLANES + int "Number of Color Planes" + default 1 + ---help--- + Some YUV color formats requires support for multiple planes, one for each + color component. Unless you have such special hardware, this value should be + undefined or set to 1. + +config NX_WRITEONLY + bool "Write-only Graphics Device" + default y if NX_LCDDRIVER && LCD_NOGETRUN + default n if !NX_LCDDRIVER || !LCD_NOGETRUN + ---help--- + Define if the underlying graphics device does not support read operations. + Automatically defined if NX_LCDDRIVER and LCD_NOGETRUN are + defined. + +menu "Supported Pixel Depths" + +config NX_DISABLE_1BPP + bool "1 BPP" + default y + ---help--- + NX supports a variety of pixel depths. You can save some memory by disabling + support for unused color depths. The selection disables support for 1BPP + pixel depth. + +config NX_DISABLE_2BPP + bool "2 BPP" + default y + ---help--- + NX supports a variety of pixel depths. You can save some memory by disabling + support for unused color depths. The selection disables support for 2BPP + pixel depth. + +config NX_DISABLE_4BPP + bool "4 BPP" + default y + ---help--- + NX supports a variety of pixel depths. You can save some memory by disabling + support for unused color depths. The selection disables support for 4BPP + pixel depth. + +config NX_DISABLE_8BPP + bool "8 BPP" + default y + ---help--- + NX supports a variety of pixel depths. You can save some memory by disabling + support for unused color depths. The selection disables support for 8BPP + pixel depth. + +config NX_DISABLE_16BPP + bool "16 BPP" + default y + ---help--- + NX supports a variety of pixel depths. You can save some memory by disabling + support for unused color depths. The selection disables support for 16BPP + pixel depth. + +config NX_DISABLE_24BPP + bool "24 BPP" + default y + ---help--- + NX supports a variety of pixel depths. You can save some memory by disabling + support for unused color depths. The selection disables support for 24BPP + pixel depth. + +config NX_DISABLE_32BPP + bool "32 BPP" + default y + ---help--- + NX supports a variety of pixel depths. You can save some memory by disabling + support for unused color depths. The selection disables support for 32BPP + pixel depth. + +endmenu + +config NX_PACKEDMSFIRST + bool "Packed MS First" + default y + depends on NX_DISABLE_1BPP || NX_DISABLE_2BPP || NX_DISABLE_4BPP + ---help--- + If a pixel depth of less than 8-bits is used, then NX needs to know if the + pixels pack from the MS to LS or from LS to MS + +menu "Input Devices" + +config NX_MOUSE + bool "Mouse/Touchscreen Support" + default n + ---help--- + Build in support for mouse or touchscreeninput. + +config NX_KBD + bool "Keyboard Support" + default n + ---help--- + Build in support of keypad/keyboard input. + +endmenu + +menu "Framed Window Borders" + +config NXTK_BORDERWIDTH + int "Border Width" + default 4 + ---help--- + Specifies with with of the border (in pixels) used with framed windows. + The default is 4. + +config NXTK_BORDERCOLOR1 + hex "Border Color" + default 0 + ---help--- + Specify the colors of the border used with framed windows. + NXTL_BODERCOLOR is the "normal" color of the border. + NXTK_BORDERCOLOR2 is the shadow side color and so is normally darker. + NXTK_BORDERCOLOR3 is the shiny side color and so is normally brighter. + +config NXTK_BORDERCOLOR2 + hex "Darker Border Color" + default 0 + ---help--- + Specify the colors of the border used with framed windows. + NXTL_BODERCOLOR is the "normal" color of the border. + NXTK_BORDERCOLOR2 is the shadow side color and so is normally darker. + NXTK_BORDERCOLOR3 is the shiny side color and so is normally brighter. + +config NXTK_BORDERCOLOR3 + hex "Brighter Border Color" + default 0 + ---help--- + Specify the colors of the border used with framed windows. + NXTL_BODERCOLOR is the "normal" color of the border. + NXTK_BORDERCOLOR2 is the shadow side color and so is normally darker. + NXTK_BORDERCOLOR3 is the shiny side color and so is normally brighter. + +endmenu + +config NXTK_AUTORAISE + bool "Autoraise" + default n + ---help--- + If set, a window will be raised to the top if the mouse position is over a + visible portion of the window. Default: A mouse button must be clicked over + a visible portion of the window. + +menu "Font Selections" + +config NXFONTS_CHARBITS + int "Bits in Character Set" + default 7 + range 7 8 + ---help--- + The number of bits in the character set. Current options are only 7 and 8. + The default is 7. + +config NXFONT_SANS17X22 + bool "Sans 17x22" + default n + ---help--- + This option enables support for a tiny, 17x22 san serif font + (font ID FONTID_SANS17X22 == 14). + +config NXFONT_SANS20X26 + bool "Sans 20x26" + default n + ---help--- + This option enables support for a tiny, 20x26 san serif font + (font ID FONTID_SANS20X26 == 15). + +config NXFONT_SANS23X27 + bool "Sans 23x27" + default n + ---help--- + This option enables support for a tiny, 23x27 san serif font + (font ID FONTID_SANS23X27 == 1). + +config NXFONT_SANS22X29 + bool "Sans 22x29" + default n + ---help--- + This option enables support for a small, 22x29 san serif font + (font ID FONTID_SANS22X29 == 2). + +config NXFONT_SANS28X37 + bool "Sans 28x37" + default n + ---help--- + This option enables support for a medium, 28x37 san serif font + (font ID FONTID_SANS28X37 == 3). + +config NXFONT_SANS39X48 + bool "Sans 39x48" + default n + ---help--- + This option enables support for a large, 39x48 san serif font + (font ID FONTID_SANS39X48 == 4). + +config NXFONT_SANS17X23B + bool "Sans 17x23 Bold" + default n + ---help--- + This option enables support for a tiny, 17x23 san serif bold font + (font ID FONTID_SANS17X23B == 16). + +config NXFONT_SANS20X27B + bool "Sans 20x27 Bold" + default n + ---help--- + This option enables support for a tiny, 20x27 san serif bold font + (font ID FONTID_SANS20X27B == 17). + +config NXFONT_SANS22X29B + bool "Sans 22x29 Bold" + default n + ---help--- + This option enables support for a small, 22x29 san serif bold font + (font ID FONTID_SANS22X29B == 5). + +config NXFONT_SANS28X37B + bool "Sans 28x37 Bold" + default n + ---help--- + This option enables support for a medium, 28x37 san serif bold font + (font ID FONTID_SANS28X37B == 6). + +config NXFONT_SANS40X49B + bool "Sans 40x49 Bold" + default n + ---help--- + This option enables support for a large, 40x49 san serif bold font + (font ID FONTID_SANS40X49B == 7). + +config NXFONT_SERIF22X29 + bool "Serif 22x29" + default n + ---help--- + This option enables support for a small, 22x29 font (with serifs) + (font ID FONTID_SERIF22X29 == 8). + +config NXFONT_SERIF29X37 + bool "Serif 29x37" + default n + ---help--- + This option enables support for a medium, 29x37 font (with serifs) + (font ID FONTID_SERIF29X37 == 9). + +config NXFONT_SERIF38X48 + bool "Serif 38x48" + default n + ---help--- + This option enables support for a large, 38x48 font (with serifs) + (font ID FONTID_SERIF38X48 == 10). + +config NXFONT_SERIF22X28B + bool "Serif 22x28 Bold" + default n + ---help--- + This option enables support for a small, 27x38 bold font (with serifs) + (font ID FONTID_SERIF22X28B == 11). + +config NXFONT_SERIF27X38B + bool "Serif 27x38 Bold" + default n + ---help--- + This option enables support for a medium, 27x38 bold font (with serifs) + (font ID FONTID_SERIF27X38B == 12). + +config NXFONT_SERIF38X49B + bool "Serif 38x49 Bold" + default n + ---help--- + This option enables support for a large, 38x49 bold font (with serifs) + (font ID FONTID_SERIF38X49B == 13). + +endmenu + +menuconfig NXCONSOLE + bool "NxConsole" + default n + ---help--- + Enables building of the NxConsole driver. + +if NXCONSOLE + +comment "NxConsole Output Text/Graphics Options" + +config NXCONSOLE_BPP + int "NxConsole BPP" + default 1 if !NX_DISABLE_1BPP + default 2 if !NX_DISABLE_2BPP + default 4 if !NX_DISABLE_4BPP + default 8 if !NX_DISABLE_8BPP + default 16 if !NX_DISABLE_16BPP + default 24 if !NX_DISABLE_24BPP + default 32 if !NX_DISABLE_32BPP + ---help--- + Currently, NxConsole supports only a single pixel depth. This + configuration setting must be provided to support that single pixel depth. + Default: The smallest enabled pixel depth. (see NX_DISABLE_*BPP) + +config NXCONSOLE_CURSORCHAR + int "Character code to use as the cursor" + default 137 + ---help--- + The bitmap code to use as the cursor. Default '_' (137) + +config NXCONSOLE_MXCHARS + int "Max Characters on Display" + default 128 + ---help--- + NxConsole needs to remember every character written to the console so + that it can redraw the window. This setting determines the size of some + internal memory allocations used to hold the character data. Default: 128. + +config NXCONSOLE_CACHESIZE + int "Font Cache Size" + default 16 + ---help--- + NxConsole supports caching of rendered fonts. This font caching is required + for two reasons: (1) First, it improves text performance, but more + importantly (2) it preserves the font memory. Since the NX server runs on + a separate server thread, it requires that the rendered font memory persist + until the server has a chance to render the font. Unfortunately, the font + cache would be quite large if all fonts were saved. The NXCONSOLE_CACHESIZE + setting will control the size of the font cache (in number of glyphs). Only that + number of the most recently used glyphs will be retained. Default: 16. + NOTE: There can still be a race condition between the NxConsole driver and the + NX task. If you every see character corruption (especially when printing + a lot of data or scrolling), then increasing the value of NXCONSOLE_CACHESIZE + is something that you should try. Alternatively, you can reduce the size of + MQ_MAXMSGSIZE which will force NxConsole task to pace the server task. + NXCONSOLE_CACHESIZE should be larger than MQ_MAXMSGSIZE in any event. + +config NXCONSOLE_LINESEPARATION + int "Line Separation" + default 0 + ---help--- + This the space (in rows) between each row of test. Default: 0 + +config NXCONSOLE_NOWRAP + bool "No wrap" + default n + ---help--- + By default, lines will wrap when the test reaches the right hand side + of the window. This setting can be defining to change this behavior so + that the text is simply truncated until a new line is encountered. + +comment "NxConsole Input options" + +config NXCONSOLE_NXKBDIN + bool "NX KBD input" + default n + ---help--- + Take input from the NX keyboard input callback. By default, keyboard + input is taken from stdin (/dev/console). If this option is set, then + the interface nxcon_kdbin() is enabled. That interface may be driven + by window callback functions so that keyboard input *only* goes to the + top window. + +config NXCONSOLE_KBDBUFSIZE + int "Keyboard Input Buffer Size" + default 16 + ---help--- + If NXCONSOLE_NXKBDIN is enabled, then this value may be used to + define the size of the per-window keyboard input buffer. Default: 16 + +config NXCONSOLE_NPOLLWAITERS + int "Number of Poll Waiters" + default 4 + ---help--- + The number of threads that can be waiting for read data available. + Default: 4 + +endif + +comment "NX Multi-user only options" + +menuconfig NX_MULTIUSER + bool "Multi-user NX Server" + default n + ---help--- + Configures NX in multi-user mode + +if NX_MULTIUSER + +config NX_BLOCKING + bool "Blocking" + default n + ---help--- + Open the client message queues in blocking mode. In this case, + nx_eventhandler() will not return until a message is received and processed. + +config NX_MXSERVERMSGS + int "Max Server Messages" + default 32 + ---help--- + Specifies the maximum number of messages that can fit in the message queues. + No additional resources are allocated, but this can be set to prevent + flooding of the client or server with too many messages (PREALLOC_MQ_MSGS + controls how many messages are pre-allocated). + +config NX_MXCLIENTMSGS + int "Max Client Messages" + default 16 + ---help--- + Specifies the maximum number of messages that can fit in the message queues. + No additional resources are allocated, but this can be set to prevent + flooding of the client or server with too many messages (PREALLOC_MQ_MSGS + controls how many messages are pre-allocated). + +endif +endif diff --git a/nuttx/graphics/nxmu/nx_block.c b/nuttx/graphics/nxmu/nx_block.c index 2f069f0962..3a051f9d7d 100644 --- a/nuttx/graphics/nxmu/nx_block.c +++ b/nuttx/graphics/nxmu/nx_block.c @@ -111,8 +111,8 @@ int nx_block(NXWINDOW hwnd, FAR void *arg) #ifdef CONFIG_DEBUG if (!hwnd) { - errno = EINVAL; - return NULL; + set_errno(EINVAL); + return ERROR; } #endif diff --git a/nuttx/graphics/nxmu/nxmu_sendclient.c b/nuttx/graphics/nxmu/nxmu_sendclient.c index 8b7f121042..a59fa23634 100644 --- a/nuttx/graphics/nxmu/nxmu_sendclient.c +++ b/nuttx/graphics/nxmu/nxmu_sendclient.c @@ -93,9 +93,9 @@ int nxmu_sendclient(FAR struct nxfe_conn_s *conn, FAR const void *msg, /* Sanity checking */ #ifdef CONFIG_DEBUG - if (!conn || conn->swrmq) + if (!conn || !conn->swrmq) { - errno = EINVAL; + set_errno(EINVAL); return ERROR; } #endif diff --git a/nuttx/graphics/nxmu/nxmu_sendserver.c b/nuttx/graphics/nxmu/nxmu_sendserver.c index 7007b81da9..03ece4e7f7 100644 --- a/nuttx/graphics/nxmu/nxmu_sendserver.c +++ b/nuttx/graphics/nxmu/nxmu_sendserver.c @@ -93,9 +93,9 @@ int nxmu_sendserver(FAR struct nxfe_conn_s *conn, FAR const void *msg, /* Sanity checking */ #ifdef CONFIG_DEBUG - if (!conn || conn->cwrmq) + if (!conn || !conn->cwrmq) { - errno = EINVAL; + set_errno(EINVAL); return ERROR; } #endif diff --git a/nuttx/lib/stdio/lib_sscanf.c b/nuttx/lib/stdio/lib_sscanf.c index 01c96c21dd..7e1fae276d 100644 --- a/nuttx/lib/stdio/lib_sscanf.c +++ b/nuttx/lib/stdio/lib_sscanf.c @@ -1,7 +1,7 @@ /**************************************************************************** * lib/stdio/lib_sscanf.c * - * Copyright (C) 2007, 2008, 2011 Gregory Nutt. All rights reserved. + * Copyright (C) 2007, 2008, 2011-2012 Gregory Nutt. All rights reserved. * Author: Gregory Nutt * * Redistribution and use in source and binary forms, with or without @@ -38,9 +38,11 @@ ****************************************************************************/ #include + #include #include #include +#include #include #include #include @@ -63,7 +65,7 @@ * Global Function Prototypes ****************************************************************************/ -int vsscanf(char *buf, const char *s, va_list ap); +int vsscanf(char *buf, const char *fmt, va_list ap); /************************************************************************** * Global Constant Data @@ -79,6 +81,64 @@ int vsscanf(char *buf, const char *s, va_list ap); static const char spaces[] = " \t\n\r\f\v"; +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Function: findwidth + * + * Description: + * Try to figure out the width of the input data. + * + ****************************************************************************/ + +static int findwidth(FAR const char *buf, FAR const char *fmt) +{ + FAR const char *next = fmt + 1; + + /* No... is there a space after the format? Or does the format string end + * here? + */ + + if (isspace(*next) || *next == 0) + { + /* Use the input up until the first white space is encountered. */ + + return strcspn(buf, spaces); + } + + /* No.. Another possibility is the the format character is followed by + * some recognizable delimiting value. + */ + + if (*next != '%') + { + /* If so we will say that the string ends there if we can find that + * delimiter in the input string. + */ + + FAR const char *ptr = strchr(buf, *next); + if (ptr) + { + return (int)(ptr - buf); + } + } + + /* No... the format has not delimiter and is back-to-back with the next + * formats (or no is following by a delimiter that does not exist in the + * input string). At this point we just bail and Use the input up until + * the first white space is encountered. + * + * NOTE: This means that values from the following format may be + * concatenated with the first. This is a bug. We have no generic way of + * determining the width of the data if there is no fieldwith, no space + * separating the input, and no usable delimiter character. + */ + + return strcspn(buf, spaces); +} + /**************************************************************************** * Private Variables ****************************************************************************/ @@ -109,67 +169,83 @@ int sscanf(FAR const char *buf, FAR const char *fmt, ...) * ANSI standard vsscanf implementation. * ****************************************************************************/ -int vsscanf(FAR char *buf, FAR const char *s, va_list ap) + +int vsscanf(FAR char *buf, FAR const char *fmt, va_list ap) { - int count; - int noassign; - int width; - int base = 10; - int lflag; + FAR char *bufstart; FAR char *tv; FAR const char *tc; + bool lflag; + bool noassign; + int count; + int width; + int base = 10; char tmp[MAXLN]; - lvdbg("vsscanf: buf=\"%s\" fmt=\"%s\"\n", buf, s); + lvdbg("vsscanf: buf=\"%s\" fmt=\"%s\"\n", buf, fmt); - count = noassign = width = lflag = 0; - while (*s && *buf) + /* Remember the start of the input buffer. We will need this for %n + * calculations. + */ + + bufstart = buf; + + /* Parse the format, extracting values from the input buffer as needed */ + + count = 0; + width = 0; + noassign = false; + lflag = false; + + while (*fmt && *buf) { /* Skip over white space */ - while (isspace(*s)) + while (isspace(*fmt)) { - s++; + fmt++; } /* Check for a conversion specifier */ - if (*s == '%') + if (*fmt == '%') { lvdbg("vsscanf: Specifier found\n"); /* Check for qualifiers on the conversion specifier */ - s++; - for (; *s; s++) + fmt++; + for (; *fmt; fmt++) { - lvdbg("vsscanf: Processing %c\n", *s); + lvdbg("vsscanf: Processing %c\n", *fmt); - if (strchr("dibouxcsefg%", *s)) + if (strchr("dibouxcsefgn%", *fmt)) { break; } - if (*s == '*') + if (*fmt == '*') { - noassign = 1; + noassign = true; } - else if (*s == 'l' || *s == 'L') + else if (*fmt == 'l' || *fmt == 'L') { - lflag = 1; + /* NOTE: Missing check for long long ('ll') */ + + lflag = true; } - else if (*s >= '1' && *s <= '9') + else if (*fmt >= '1' && *fmt <= '9') { - for (tc = s; isdigit(*s); s++); - strncpy(tmp, tc, s - tc); - tmp[s - tc] = '\0'; + for (tc = fmt; isdigit(*fmt); fmt++); + strncpy(tmp, tc, fmt - tc); + tmp[fmt - tc] = '\0'; width = atoi(tmp); - s--; + fmt--; } } /* Process %s: String conversion */ - if (*s == 's') + if (*fmt == 's') { lvdbg("vsscanf: Performing string conversion\n"); @@ -178,9 +254,13 @@ int vsscanf(FAR char *buf, FAR const char *s, va_list ap) buf++; } + /* Was a fieldwidth specified? */ + if (!width) { - width = strcspn(buf, spaces); + /* No... Guess a field width using some heuristics */ + + width = findwidth(buf, fmt); } if (!noassign) @@ -189,17 +269,22 @@ int vsscanf(FAR char *buf, FAR const char *s, va_list ap) strncpy(tv, buf, width); tv[width] = '\0'; } + buf += width; } /* Process %c: Character conversion */ - else if (*s == 'c') + else if (*fmt == 'c') { lvdbg("vsscanf: Performing character conversion\n"); + /* Was a fieldwidth specified? */ + if (!width) { + /* No, then width is this one single character */ + width = 1; } @@ -209,12 +294,13 @@ int vsscanf(FAR char *buf, FAR const char *s, va_list ap) strncpy(tv, buf, width); tv[width] = '\0'; } + buf += width; } /* Process %d, %o, %b, %x, %u: Various integer conversions */ - else if (strchr("dobxu", *s)) + else if (strchr("dobxu", *fmt)) { lvdbg("vsscanf: Performing integer conversion\n"); @@ -229,37 +315,34 @@ int vsscanf(FAR char *buf, FAR const char *s, va_list ap) * conversion specification. */ - if (*s == 'd' || *s == 'u') + if (*fmt == 'd' || *fmt == 'u') { base = 10; } - else if (*s == 'x') + else if (*fmt == 'x') { base = 16; } - else if (*s == 'o') + else if (*fmt == 'o') { base = 8; } - else if (*s == 'b') + else if (*fmt == 'b') { base = 2; } - /* Copy the integer string into a temporary working buffer. */ + /* Was a fieldwidth specified? */ if (!width) { - if (isspace(*(s + 1)) || *(s + 1) == 0) - { - width = strcspn(buf, spaces); - } - else - { - width = strchr(buf, *(s + 1)) - buf; - } + /* No... Guess a field width using some heuristics */ + + width = findwidth(buf, fmt); } + /* Copy the numeric string into a temporary working buffer. */ + strncpy(tmp, buf, width); tmp[width] = '\0'; @@ -270,21 +353,30 @@ int vsscanf(FAR char *buf, FAR const char *s, va_list ap) buf += width; if (!noassign) { - int *pint = va_arg(ap, int*); #ifdef SDCC char *endptr; - int tmpint = strtol(tmp, &endptr, base); + long tmplong = strtol(tmp, &endptr, base); #else - int tmpint = strtol(tmp, NULL, base); + long tmplong = strtol(tmp, NULL, base); #endif - lvdbg("vsscanf: Return %d to 0x%p\n", tmpint, pint); - *pint = tmpint; + if (lflag) + { + long *plong = va_arg(ap, long*); + lvdbg("vsscanf: Return %ld to 0x%p\n", tmplong, plong); + *plong = tmplong; + } + else + { + int *pint = va_arg(ap, int*); + lvdbg("vsscanf: Return %ld to 0x%p\n", tmplong, pint); + *pint = (int)tmplong; + } } } /* Process %f: Floating point conversion */ - else if (*s == 'f') + else if (*fmt == 'f') { #ifndef CONFIG_LIBC_FLOATINGPOINT /* No floating point conversions */ @@ -303,20 +395,17 @@ int vsscanf(FAR char *buf, FAR const char *s, va_list ap) buf++; } - /* Copy the real string into a temporary working buffer. */ + /* Was a fieldwidth specified? */ if (!width) { - if (isspace(*(s + 1)) || *(s + 1) == 0) - { - width = strcspn(buf, spaces); - } - else - { - width = strchr(buf, *(s + 1)) - buf; - } + /* No... Guess a field width using some heuristics */ + + width = findwidth(buf, fmt); } + /* Copy the real string into a temporary working buffer. */ + strncpy(tmp, buf, width); tmp[width] = '\0'; buf += width; @@ -356,13 +445,41 @@ int vsscanf(FAR char *buf, FAR const char *s, va_list ap) #endif } - if (!noassign) + /* Process %n: Character count */ + + else if (*fmt == 'n') + { + lvdbg("vsscanf: Performing character count\n"); + + if (!noassign) + { + size_t nchars = (size_t)(buf - bufstart); + + if (lflag) + { + long *plong = va_arg(ap, long*); + *plong = (long)nchars; + } + else + { + int *pint = va_arg(ap, int*); + *pint = (int)nchars; + } + } + } + + /* Note %n does not count as a conversion */ + + if (!noassign && *fmt != 'n') { count++; } - width = noassign = lflag = 0; - s++; + width = 0; + noassign = false; + lflag = false; + + fmt++; } /* Its is not a conversion specifier */ @@ -374,13 +491,13 @@ int vsscanf(FAR char *buf, FAR const char *s, va_list ap) buf++; } - if (*s != *buf) + if (*fmt != *buf) { break; } else { - s++; + fmt++; buf++; } } diff --git a/nuttx/tools/cfgparser.c b/nuttx/tools/cfgparser.c index e49b29d5e4..b1f189f6fb 100644 --- a/nuttx/tools/cfgparser.c +++ b/nuttx/tools/cfgparser.c @@ -56,13 +56,24 @@ char line[LINESIZE+1]; ****************************************************************************/ /* These are configuration variable name that are quoted by configuration tool - * but which must be unquoated when used in C code. + * but which must be unquoted when used in C code. */ static const char *dequote_list[] = { - "CONFIG_USER_ENTRYPOINT", - NULL + /* NuttX */ + + "CONFIG_USER_ENTRYPOINT", /* Name of entry point function */ + + /* NxWidgets/NxWM */ + + "CONFIG_NXWM_BACKGROUND_IMAGE", /* Name of bitmap image class */ + "CONFIG_NXWM_STARTWINDOW_ICON", /* Name of bitmap image class */ + "CONFIG_NXWM_NXCONSOLE_ICON", /* Name of bitmap image class */ + "CONFIG_NXWM_CALIBRATION_ICON", /* Name of bitmap image class */ + "CONFIG_NXWM_HEXCALCULATOR_ICON", /* Name of bitmap image class */ + + NULL /* Marks the end of the list */ }; /**************************************************************************** @@ -239,7 +250,7 @@ static char *dequote_value(const char *varname, char *varval) /* Handle the case where nothing is left after dequoting */ - if (len < 0) + if (len <= 0) { dqval = NULL; }