diff options
Diffstat (limited to 'src/modules')
23 files changed, 789 insertions, 488 deletions
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp index f01ee0355..799fc2fb9 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp @@ -166,19 +166,19 @@ protected: double lat, lon; /**< lat, lon radians */ float alt; /**< altitude, meters */ // parameters - control::BlockParam<float> _vGyro; /**< gyro process noise */ - control::BlockParam<float> _vAccel; /**< accelerometer process noise */ - control::BlockParam<float> _rMag; /**< magnetometer measurement noise */ - control::BlockParam<float> _rGpsVel; /**< gps velocity measurement noise */ - control::BlockParam<float> _rGpsPos; /**< gps position measurement noise */ - control::BlockParam<float> _rGpsAlt; /**< gps altitude measurement noise */ - control::BlockParam<float> _rPressAlt; /**< press altitude measurement noise */ - control::BlockParam<float> _rAccel; /**< accelerometer measurement noise */ - control::BlockParam<float> _magDip; /**< magnetic inclination with level */ - control::BlockParam<float> _magDec; /**< magnetic declination, clockwise rotation */ - control::BlockParam<float> _g; /**< gravitational constant */ - control::BlockParam<float> _faultPos; /**< fault detection threshold for position */ - control::BlockParam<float> _faultAtt; /**< fault detection threshold for attitude */ + control::BlockParamFloat _vGyro; /**< gyro process noise */ + control::BlockParamFloat _vAccel; /**< accelerometer process noise */ + control::BlockParamFloat _rMag; /**< magnetometer measurement noise */ + control::BlockParamFloat _rGpsVel; /**< gps velocity measurement noise */ + control::BlockParamFloat _rGpsPos; /**< gps position measurement noise */ + control::BlockParamFloat _rGpsAlt; /**< gps altitude measurement noise */ + control::BlockParamFloat _rPressAlt; /**< press altitude measurement noise */ + control::BlockParamFloat _rAccel; /**< accelerometer measurement noise */ + control::BlockParamFloat _magDip; /**< magnetic inclination with level */ + control::BlockParamFloat _magDec; /**< magnetic declination, clockwise rotation */ + control::BlockParamFloat _g; /**< gravitational constant */ + control::BlockParamFloat _faultPos; /**< fault detection threshold for position */ + control::BlockParamFloat _faultAtt; /**< fault detection threshold for attitude */ // status bool _attitudeInitialized; bool _positionInitialized; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0c3546b95..2ef509980 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1193,31 +1193,19 @@ int commander_thread_main(int argc, char *argv[]) bool main_state_changed = check_main_state_changed(); bool navigation_state_changed = check_navigation_state_changed(); - if (arming_state_changed || main_state_changed || navigation_state_changed) { - mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); - status_changed = true; - } - hrt_abstime t1 = hrt_absolute_time(); - /* publish arming state */ - if (arming_state_changed) { - armed.timestamp = t1; - orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); - } - - /* publish control mode */ if (navigation_state_changed || arming_state_changed) { - /* publish new navigation state */ control_mode.flag_armed = armed.armed; // copy armed state to vehicle_control_mode topic - control_mode.counter++; - control_mode.timestamp = t1; - orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); + } + + if (arming_state_changed || main_state_changed || navigation_state_changed) { + mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); + status_changed = true; } /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { - status.counter++; status.timestamp = t1; orb_publish(ORB_ID(vehicle_status), status_pub, &status); control_mode.timestamp = t1; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index efe12be57..490fc8fc6 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -504,7 +504,6 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s if (valid_transition) { current_status->hil_state = new_state; - current_status->counter++; current_status->timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_status), status_pub, current_status); diff --git a/src/modules/controllib/block/BlockParam.hpp b/src/modules/controllib/block/BlockParam.hpp index 58a9bfc0d..36bc8c24b 100644 --- a/src/modules/controllib/block/BlockParam.hpp +++ b/src/modules/controllib/block/BlockParam.hpp @@ -69,22 +69,39 @@ protected: /** * Parameters that are tied to blocks for updating and nameing. */ -template<class T> -class __EXPORT BlockParam : public BlockParamBase + +class __EXPORT BlockParamFloat : public BlockParamBase +{ +public: + BlockParamFloat(Block *block, const char *name, bool parent_prefix=true) : + BlockParamBase(block, name, parent_prefix), + _val() { + update(); + } + float get() { return _val; } + void set(float val) { _val = val; } + void update() { + if (_handle != PARAM_INVALID) param_get(_handle, &_val); + } +protected: + float _val; +}; + +class __EXPORT BlockParamInt : public BlockParamBase { public: - BlockParam(Block *block, const char *name, bool parent_prefix=true) : + BlockParamInt(Block *block, const char *name, bool parent_prefix=true) : BlockParamBase(block, name, parent_prefix), _val() { update(); } - T get() { return _val; } - void set(T val) { _val = val; } + int get() { return _val; } + void set(int val) { _val = val; } void update() { if (_handle != PARAM_INVALID) param_get(_handle, &_val); } protected: - T _val; + int _val; }; } // namespace control diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index fefe99702..66e929038 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -74,8 +74,8 @@ public: float getMax() { return _max.get(); } protected: // attributes - BlockParam<float> _min; - BlockParam<float> _max; + control::BlockParamFloat _min; + control::BlockParamFloat _max; }; int __EXPORT blockLimitTest(); @@ -99,7 +99,7 @@ public: float getMax() { return _max.get(); } protected: // attributes - BlockParam<float> _max; + control::BlockParamFloat _max; }; int __EXPORT blockLimitSymTest(); @@ -126,7 +126,7 @@ public: protected: // attributes float _state; - BlockParam<float> _fCut; + control::BlockParamFloat _fCut; }; int __EXPORT blockLowPassTest(); @@ -157,7 +157,7 @@ protected: // attributes float _u; /**< previous input */ float _y; /**< previous output */ - BlockParam<float> _fCut; /**< cut-off frequency, Hz */ + control::BlockParamFloat _fCut; /**< cut-off frequency, Hz */ }; int __EXPORT blockHighPassTest(); @@ -273,7 +273,7 @@ public: // accessors float getKP() { return _kP.get(); } protected: - BlockParam<float> _kP; + control::BlockParamFloat _kP; }; int __EXPORT blockPTest(); @@ -303,8 +303,8 @@ public: BlockIntegral &getIntegral() { return _integral; } private: BlockIntegral _integral; - BlockParam<float> _kP; - BlockParam<float> _kI; + control::BlockParamFloat _kP; + control::BlockParamFloat _kI; }; int __EXPORT blockPITest(); @@ -334,8 +334,8 @@ public: BlockDerivative &getDerivative() { return _derivative; } private: BlockDerivative _derivative; - BlockParam<float> _kP; - BlockParam<float> _kD; + control::BlockParamFloat _kP; + control::BlockParamFloat _kD; }; int __EXPORT blockPDTest(); @@ -372,9 +372,9 @@ private: // attributes BlockIntegral _integral; BlockDerivative _derivative; - BlockParam<float> _kP; - BlockParam<float> _kI; - BlockParam<float> _kD; + control::BlockParamFloat _kP; + control::BlockParamFloat _kI; + control::BlockParamFloat _kD; }; int __EXPORT blockPIDTest(); @@ -404,7 +404,7 @@ public: float get() { return _val; } private: // attributes - BlockParam<float> _trim; + control::BlockParamFloat _trim; BlockLimit _limit; float _val; }; @@ -439,8 +439,8 @@ public: float getMax() { return _max.get(); } private: // attributes - BlockParam<float> _min; - BlockParam<float> _max; + control::BlockParamFloat _min; + control::BlockParamFloat _max; }; int __EXPORT blockRandUniformTest(); @@ -486,8 +486,8 @@ public: float getStdDev() { return _stdDev.get(); } private: // attributes - BlockParam<float> _mean; - BlockParam<float> _stdDev; + control::BlockParamFloat _mean; + control::BlockParamFloat _stdDev; }; int __EXPORT blockRandGaussTest(); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7dd9e321f..8243748dc 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -167,6 +167,7 @@ handle_message(mavlink_message_t *msg) /* check if topic is advertised */ if (cmd_pub <= 0) { cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + } else { /* publish */ orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); @@ -437,9 +438,11 @@ handle_message(mavlink_message_t *msg) if (pub_hil_airspeed < 0) { pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed); + } else { orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); } + //warnx("SENSOR: IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s); /* individual sensor publications */ @@ -455,49 +458,72 @@ handle_message(mavlink_message_t *msg) if (pub_hil_gyro < 0) { pub_hil_gyro = orb_advertise(ORB_ID(sensor_gyro), &gyro); + } else { orb_publish(ORB_ID(sensor_gyro), pub_hil_gyro, &gyro); } struct accel_report accel; + accel.x_raw = imu.xacc / mg2ms2; + accel.y_raw = imu.yacc / mg2ms2; + accel.z_raw = imu.zacc / mg2ms2; + accel.x = imu.xacc; + accel.y = imu.yacc; + accel.z = imu.zacc; + accel.temperature = imu.temperature; + accel.timestamp = hrt_absolute_time(); if (pub_hil_accel < 0) { pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel); + } else { orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel); } struct mag_report mag; + mag.x_raw = imu.xmag / mga2ga; + mag.y_raw = imu.ymag / mga2ga; + mag.z_raw = imu.zmag / mga2ga; + mag.x = imu.xmag; + mag.y = imu.ymag; + mag.z = imu.zmag; + mag.timestamp = hrt_absolute_time(); if (pub_hil_mag < 0) { pub_hil_mag = orb_advertise(ORB_ID(sensor_mag), &mag); + } else { orb_publish(ORB_ID(sensor_mag), pub_hil_mag, &mag); } struct baro_report baro; + baro.pressure = imu.abs_pressure; + baro.altitude = imu.pressure_alt; + baro.temperature = imu.temperature; + baro.timestamp = hrt_absolute_time(); if (pub_hil_baro < 0) { pub_hil_baro = orb_advertise(ORB_ID(sensor_baro), &baro); + } else { orb_publish(ORB_ID(sensor_baro), pub_hil_baro, &baro); } @@ -505,6 +531,7 @@ handle_message(mavlink_message_t *msg) /* publish combined sensor topic */ if (pub_hil_sensors > 0) { orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); + } else { pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); } @@ -517,6 +544,7 @@ handle_message(mavlink_message_t *msg) /* lazily publish the battery voltage */ if (pub_hil_battery > 0) { orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status); + } else { pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status); } @@ -527,7 +555,7 @@ handle_message(mavlink_message_t *msg) // output if ((timestamp - old_timestamp) > 10000000) { - printf("receiving hil sensor at %d hz\n", hil_frames/10); + printf("receiving hil sensor at %d hz\n", hil_frames / 10); old_timestamp = timestamp; hil_frames = 0; } @@ -552,9 +580,11 @@ handle_message(mavlink_message_t *msg) /* gps.cog is in degrees 0..360 * 100, heading is -PI..+PI */ float heading_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f; + /* go back to -PI..PI */ if (heading_rad > M_PI_F) heading_rad -= 2.0f * M_PI_F; + hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m hil_gps.vel_d_m_s = gps.vd * 1e-2f; // from cm to m @@ -567,6 +597,7 @@ handle_message(mavlink_message_t *msg) /* publish GPS measurement data */ if (pub_hil_gps > 0) { orb_publish(ORB_ID(vehicle_gps_position), pub_hil_gps, &hil_gps); + } else { pub_hil_gps = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); } @@ -585,6 +616,7 @@ handle_message(mavlink_message_t *msg) if (pub_hil_airspeed < 0) { pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed); + } else { orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); } @@ -602,6 +634,7 @@ handle_message(mavlink_message_t *msg) if (pub_hil_global_pos > 0) { orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); + } else { pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); } @@ -613,8 +646,8 @@ handle_message(mavlink_message_t *msg) /* set rotation matrix */ for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - hil_attitude.R[i][j] = C_nb(i, j); - + hil_attitude.R[i][j] = C_nb(i, j); + hil_attitude.R_valid = true; /* set quaternion */ @@ -636,22 +669,32 @@ handle_message(mavlink_message_t *msg) if (pub_hil_attitude > 0) { orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude); + } else { pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); } struct accel_report accel; + accel.x_raw = hil_state.xacc / 9.81f * 1e3f; + accel.y_raw = hil_state.yacc / 9.81f * 1e3f; + accel.z_raw = hil_state.zacc / 9.81f * 1e3f; + accel.x = hil_state.xacc; + accel.y = hil_state.yacc; + accel.z = hil_state.zacc; + accel.temperature = 25.0f; + accel.timestamp = hrt_absolute_time(); if (pub_hil_accel < 0) { pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel); + } else { orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel); } @@ -664,6 +707,7 @@ handle_message(mavlink_message_t *msg) /* lazily publish the battery voltage */ if (pub_hil_battery > 0) { orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status); + } else { pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status); } @@ -733,17 +777,23 @@ receive_thread(void *arg) mavlink_message_t msg; - prctl(PR_SET_NAME, "mavlink uart rcv", getpid()); + prctl(PR_SET_NAME, "mavlink_uart_rcv", getpid()); - while (!thread_should_exit) { + struct pollfd fds[1]; + fds[0].fd = uart_fd; + fds[0].events = POLLIN; - struct pollfd fds[1]; - fds[0].fd = uart_fd; - fds[0].events = POLLIN; + ssize_t nread = 0; + while (!thread_should_exit) { if (poll(fds, 1, timeout) > 0) { + if (nread < sizeof(buf)) { + /* to avoid reading very small chunks wait for data before reading */ + usleep(1000); + } + /* non-blocking read. read may return negative values */ - ssize_t nread = read(uart_fd, buf, sizeof(buf)); + nread = read(uart_fd, buf, sizeof(buf)); /* if read failed, this loop won't execute */ for (ssize_t i = 0; i < nread; i++) { @@ -751,10 +801,10 @@ receive_thread(void *arg) /* handle generic messages and commands */ handle_message(&msg); - /* Handle packet with waypoint component */ + /* handle packet with waypoint component */ mavlink_wpm_message_handler(&msg, &global_pos, &local_pos); - /* Handle packet with parameter component */ + /* handle packet with parameter component */ mavlink_pm_message_handler(MAVLINK_COMM_0, &msg); } } diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 0e0ce2723..f6860930c 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -711,7 +711,7 @@ static void * uorb_receive_thread(void *arg) { /* Set thread name */ - prctl(PR_SET_NAME, "mavlink orb rcv", getpid()); + prctl(PR_SET_NAME, "mavlink_orb_rcv", getpid()); /* * set up poll to block for new data, diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c index e71344982..0edb53a3e 100644 --- a/src/modules/mavlink_onboard/mavlink.c +++ b/src/modules/mavlink_onboard/mavlink.c @@ -167,12 +167,12 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf case 921600: speed = B921600; break; default: - fprintf(stderr, "[mavlink] ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud); + warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600", baud); return -EINVAL; } /* open uart */ - printf("[mavlink] UART is %s, baudrate is %d\n", uart_name, baud); + warnx("UART is %s, baudrate is %d", uart_name, baud); uart = open(uart_name, O_RDWR | O_NOCTTY); /* Try to set baud rate */ @@ -183,7 +183,7 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf if (strcmp(uart_name, "/dev/ttyACM0") != OK) { /* Back up the original uart configuration to restore it after exit */ if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { - fprintf(stderr, "[mavlink] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state); + warnx("ERROR getting baudrate / termios config for %s: %d", uart_name, termios_state); close(uart); return -1; } @@ -196,14 +196,14 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf /* Set baud rate */ if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); + warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)", uart_name, termios_state); close(uart); return -1; } if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { - fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); + warnx("ERROR setting baudrate / termios config for %s (tcsetattr)", uart_name); close(uart); return -1; } @@ -481,9 +481,9 @@ int mavlink_thread_main(int argc, char *argv[]) static void usage() { - fprintf(stderr, "usage: mavlink start [-d <devicename>] [-b <baud rate>]\n" - " mavlink stop\n" - " mavlink status\n"); + fprintf(stderr, "usage: mavlink_onboard start [-d <devicename>] [-b <baud rate>]\n" + " mavlink_onboard stop\n" + " mavlink_onboard status\n"); exit(1); } @@ -499,7 +499,7 @@ int mavlink_onboard_main(int argc, char *argv[]) /* this is not an error */ if (thread_running) - errx(0, "mavlink already running\n"); + errx(0, "already running"); thread_should_exit = false; mavlink_task = task_spawn_cmd("mavlink_onboard", @@ -516,7 +516,7 @@ int mavlink_onboard_main(int argc, char *argv[]) while (thread_running) { usleep(200000); } - warnx("terminated."); + warnx("terminated"); exit(0); } diff --git a/src/modules/mavlink_onboard/mavlink_receiver.c b/src/modules/mavlink_onboard/mavlink_receiver.c index 0236e6126..4658bcc1d 100644 --- a/src/modules/mavlink_onboard/mavlink_receiver.c +++ b/src/modules/mavlink_onboard/mavlink_receiver.c @@ -100,11 +100,11 @@ handle_message(mavlink_message_t *msg) mavlink_msg_command_long_decode(msg, &cmd_mavlink); if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) - || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { + || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { //check for MAVLINK terminate command if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { /* This is the link shutdown command, terminate mavlink */ - printf("[mavlink] Terminating .. \n"); + warnx("terminating..."); fflush(stdout); usleep(50000); @@ -132,6 +132,7 @@ handle_message(mavlink_message_t *msg) if (cmd_pub <= 0) { cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); } + /* publish */ orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); } @@ -156,6 +157,7 @@ handle_message(mavlink_message_t *msg) /* check if topic is advertised */ if (flow_pub <= 0) { flow_pub = orb_advertise(ORB_ID(optical_flow), &f); + } else { /* publish */ orb_publish(ORB_ID(optical_flow), flow_pub, &f); @@ -186,6 +188,7 @@ handle_message(mavlink_message_t *msg) /* 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); @@ -203,6 +206,7 @@ handle_message(mavlink_message_t *msg) if (vicon_position_pub <= 0) { vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); + } else { orb_publish(ORB_ID(vehicle_vicon_position), vicon_position_pub, &vicon_position); } @@ -219,7 +223,7 @@ handle_message(mavlink_message_t *msg) /* switch to a receiving link mode */ gcs_link = false; - /* + /* * rate control mode - defined by MAVLink */ @@ -227,33 +231,37 @@ handle_message(mavlink_message_t *msg) bool ml_armed = false; switch (quad_motors_setpoint.mode) { - case 0: - ml_armed = false; - break; - case 1: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; - ml_armed = true; - - break; - case 2: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; - ml_armed = true; - - break; - case 3: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; - break; - case 4: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; - break; + case 0: + ml_armed = false; + break; + + case 1: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; + ml_armed = true; + + break; + + case 2: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; + ml_armed = true; + + 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 = (float)quad_motors_setpoint.roll[mavlink_system.sysid-1] / (float)INT16_MAX; - offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid-1] / (float)INT16_MAX; - offboard_control_sp.p3= (float)quad_motors_setpoint.yaw[mavlink_system.sysid-1] / (float)INT16_MAX; - offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid-1]/(float)UINT16_MAX; + offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX; - if (quad_motors_setpoint.thrust[mavlink_system.sysid-1] == 0) { + if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) { ml_armed = false; } @@ -265,6 +273,7 @@ handle_message(mavlink_message_t *msg) /* check if topic has to be advertised */ if (offboard_control_sp_pub <= 0) { offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); + } else { /* Publish */ orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp); @@ -281,31 +290,36 @@ handle_message(mavlink_message_t *msg) static void * receive_thread(void *arg) { - int uart_fd = *((int*)arg); + int uart_fd = *((int *)arg); const int timeout = 1000; - uint8_t ch; + uint8_t buf[32]; mavlink_message_t msg; - prctl(PR_SET_NAME, "mavlink offb rcv", getpid()); + prctl(PR_SET_NAME, "mavlink_onboard_rcv", getpid()); - while (!thread_should_exit) { + struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } }; - struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } }; + ssize_t nread = 0; + while (!thread_should_exit) { if (poll(fds, 1, timeout) > 0) { - /* non-blocking read until buffer is empty */ - int nread = 0; + if (nread < sizeof(buf)) { + /* to avoid reading very small chunks wait for data before reading */ + usleep(1000); + } - do { - nread = read(uart_fd, &ch, 1); + /* non-blocking read. read may return negative values */ + nread = read(uart_fd, buf, sizeof(buf)); - if (mavlink_parse_char(chan, ch, &msg, &status)) { //parse the char + /* if read failed, this loop won't execute */ + for (ssize_t i = 0; i < nread; i++) { + if (mavlink_parse_char(chan, buf[i], &msg, &status)) { /* handle generic messages and commands */ handle_message(&msg); } - } while (nread > 0); + } } } @@ -319,8 +333,8 @@ receive_start(int uart) pthread_attr_init(&receiveloop_attr); struct sched_param param; - param.sched_priority = SCHED_PRIORITY_MAX - 40; - (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); + param.sched_priority = SCHED_PRIORITY_MAX - 40; + (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); pthread_attr_setstacksize(&receiveloop_attr, 2048); diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 04582f2a4..44f8f664b 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -89,18 +89,18 @@ static int mc_thread_main(int argc, char *argv[]) { /* declare and safely initialize all structs */ - struct vehicle_control_mode_s control_mode; - memset(&control_mode, 0, sizeof(control_mode)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_attitude_setpoint_s att_sp; memset(&att_sp, 0, sizeof(att_sp)); - struct manual_control_setpoint_s manual; - memset(&manual, 0, sizeof(manual)); - struct sensor_combined_s raw; - memset(&raw, 0, sizeof(raw)); struct offboard_control_setpoint_s offboard_sp; memset(&offboard_sp, 0, sizeof(offboard_sp)); + struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); + struct manual_control_setpoint_s manual; + memset(&manual, 0, sizeof(manual)); + struct sensor_combined_s sensor; + memset(&sensor, 0, sizeof(sensor)); struct vehicle_rates_setpoint_s rates_sp; memset(&rates_sp, 0, sizeof(rates_sp)); struct vehicle_status_s status; @@ -108,29 +108,16 @@ mc_thread_main(int argc, char *argv[]) struct actuator_controls_s actuators; memset(&actuators, 0, sizeof(actuators)); - /* subscribe to attitude, motor setpoints and system state */ - int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int param_sub = orb_subscribe(ORB_ID(parameter_update)); - int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - int status_sub = orb_subscribe(ORB_ID(vehicle_status)); - - /* - * Do not rate-limit the loop to prevent aliasing - * if rate-limiting would be desired later, the line below would - * enable it. - * - * rate-limit the attitude subscription to 200Hz to pace our loop - * orb_set_interval(att_sub, 5); - */ - struct pollfd fds[2] = { - { .fd = att_sub, .events = POLLIN }, - { .fd = param_sub, .events = POLLIN } - }; + /* subscribe */ + int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); + int vehicle_attitude_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + int offboard_control_setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); + int vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); + int vehicle_rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); /* publish actuator controls */ for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { @@ -146,233 +133,246 @@ mc_thread_main(int argc, char *argv[]) perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "multirotor_att_control_interval"); perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "multirotor_att_control_err"); - /* welcome user */ warnx("starting"); /* store last control mode to detect mode switches */ bool control_yaw_position = true; bool reset_yaw_sp = true; + struct pollfd fds[1] = { + { .fd = vehicle_attitude_sub, .events = POLLIN }, + }; + while (!thread_should_exit) { /* wait for a sensor update, check for exit condition every 500 ms */ - int ret = poll(fds, 2, 500); + int ret = poll(fds, 1, 500); if (ret < 0) { /* poll error, count it in perf */ perf_count(mc_err_perf); - } else if (ret == 0) { - /* no return value, ignore */ - } else { + } else if (ret > 0) { + /* only run controller if attitude changed */ + perf_begin(mc_loop_perf); - /* only update parameters if they changed */ - if (fds[1].revents & POLLIN) { - /* read from param to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), param_sub, &update); + /* attitude */ + orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); + + bool updated; + + /* parameters */ + orb_check(parameter_update_sub, &updated); + if (updated) { + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); /* update parameters */ } - /* only run controller if attitude changed */ - if (fds[0].revents & POLLIN) { + /* control mode */ + orb_check(vehicle_control_mode_sub, &updated); - perf_begin(mc_loop_perf); + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub, &control_mode); + } - /* get a local copy of system state */ - bool updated; - orb_check(control_mode_sub, &updated); + /* manual control setpoint */ + orb_check(manual_control_setpoint_sub, &updated); - if (updated) { - orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); - } + if (updated) { + orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual); + } - /* 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); - } + /* attitude setpoint */ + orb_check(vehicle_attitude_setpoint_sub, &updated); - /* get a local copy of status */ - orb_check(status_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp); + } - if (updated) { - orb_copy(ORB_ID(vehicle_status), status_sub, &status); - } + /* offboard control setpoint */ + orb_check(offboard_control_setpoint_sub, &updated); - /* get a local copy of the current sensor values */ - orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); + if (updated) { + orb_copy(ORB_ID(offboard_control_setpoint), offboard_control_setpoint_sub, &offboard_sp); + } - /* set flag to safe value */ - control_yaw_position = true; + /* vehicle status */ + orb_check(vehicle_status_sub, &updated); - /* reset yaw setpoint if not armed */ - if (!control_mode.flag_armed) { - reset_yaw_sp = true; - } + if (updated) { + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &status); + } - /* define which input is the dominating control input */ - if (control_mode.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); + /* sensors */ + orb_check(sensor_combined_sub, &updated); - } 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(); - /* publish the result to the vehicle actuators */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - } + if (updated) { + orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); + } - /* reset yaw setpoint after offboard control */ - reset_yaw_sp = true; + /* set flag to safe value */ + control_yaw_position = true; - } else if (control_mode.flag_control_manual_enabled) { - /* manual input */ - if (control_mode.flag_control_attitude_enabled) { - /* control attitude, update attitude setpoint depending on mode */ - if (att_sp.thrust < 0.1f) { - /* no thrust, don't try to control yaw */ - rates_sp.yaw = 0.0f; - control_yaw_position = false; + /* reset yaw setpoint if not armed */ + if (!control_mode.flag_armed) { + reset_yaw_sp = true; + } - if (status.condition_landed) { - /* reset yaw setpoint if on ground */ - reset_yaw_sp = true; - } + /* define which input is the dominating control input */ + if (control_mode.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 { - /* only move yaw setpoint if manual input is != 0 */ - if (manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) { - /* control yaw rate */ - control_yaw_position = false; - rates_sp.yaw = manual.yaw; - reset_yaw_sp = true; // has no effect on control, just for beautiful log - - } else { - control_yaw_position = true; - } - } + } 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(); + /* publish the result to the vehicle actuators */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } - if (!control_mode.flag_control_velocity_enabled) { - /* update attitude setpoint if not in position control mode */ - att_sp.roll_body = manual.roll; - att_sp.pitch_body = manual.pitch; + /* reset yaw setpoint after offboard control */ + reset_yaw_sp = true; - if (!control_mode.flag_control_climb_rate_enabled) { - /* pass throttle directly if not in altitude control mode */ - att_sp.thrust = manual.throttle; - } + } else if (control_mode.flag_control_manual_enabled) { + /* manual input */ + if (control_mode.flag_control_attitude_enabled) { + /* control attitude, update attitude setpoint depending on mode */ + if (att_sp.thrust < 0.1f) { + /* no thrust, don't try to control yaw */ + rates_sp.yaw = 0.0f; + control_yaw_position = false; + + if (status.condition_landed) { + /* reset yaw setpoint if on ground */ + reset_yaw_sp = true; } - /* reset yaw setpint to current position if needed */ - if (reset_yaw_sp) { - att_sp.yaw_body = att.yaw; - reset_yaw_sp = false; - } + } else { + /* only move yaw setpoint if manual input is != 0 */ + if (manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) { + /* control yaw rate */ + control_yaw_position = false; + rates_sp.yaw = manual.yaw; + reset_yaw_sp = true; // has no effect on control, just for beautiful log - if (motor_test_mode) { - printf("testmode"); - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - att_sp.yaw_body = 0.0f; - att_sp.thrust = 0.1f; + } else { + control_yaw_position = true; } + } - att_sp.timestamp = hrt_absolute_time(); - - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + if (!control_mode.flag_control_velocity_enabled) { + /* update attitude setpoint if not in position control mode */ + att_sp.roll_body = manual.roll; + att_sp.pitch_body = manual.pitch; - } else { - /* manual rate inputs (ACRO), from RC control or joystick */ - if (control_mode.flag_control_rates_enabled) { - rates_sp.roll = manual.roll; - rates_sp.pitch = manual.pitch; - rates_sp.yaw = manual.yaw; - rates_sp.thrust = manual.throttle; - rates_sp.timestamp = hrt_absolute_time(); + if (!control_mode.flag_control_climb_rate_enabled) { + /* pass throttle directly if not in altitude control mode */ + att_sp.thrust = manual.throttle; } + } + + /* reset yaw setpint to current position if needed */ + if (reset_yaw_sp) { + att_sp.yaw_body = att.yaw; + reset_yaw_sp = false; + } - /* reset yaw setpoint after ACRO */ - reset_yaw_sp = true; + if (motor_test_mode) { + printf("testmode"); + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + att_sp.yaw_body = 0.0f; + att_sp.thrust = 0.1f; } + att_sp.timestamp = hrt_absolute_time(); + + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } else { - if (!control_mode.flag_control_auto_enabled) { - /* no control, try to stay on place */ - if (!control_mode.flag_control_velocity_enabled) { - /* no velocity control, reset attitude setpoint */ - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - att_sp.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - } + /* manual rate inputs (ACRO), from RC control or joystick */ + if (control_mode.flag_control_rates_enabled) { + rates_sp.roll = manual.roll; + rates_sp.pitch = manual.pitch; + rates_sp.yaw = manual.yaw; + rates_sp.thrust = manual.throttle; + rates_sp.timestamp = hrt_absolute_time(); } - /* reset yaw setpoint after non-manual control */ + /* reset yaw setpoint after ACRO */ reset_yaw_sp = true; } - /* check if we should we reset integrals */ - bool reset_integral = !control_mode.flag_armed || att_sp.thrust < 0.1f; // TODO use landed status instead of throttle - - /* run attitude controller if needed */ - if (control_mode.flag_control_attitude_enabled) { - multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, reset_integral); - orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); + } else { + if (!control_mode.flag_control_auto_enabled) { + /* no control, try to stay on place */ + if (!control_mode.flag_control_velocity_enabled) { + /* no velocity control, reset attitude setpoint */ + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + att_sp.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } } - /* measure in what intervals the controller runs */ - perf_count(mc_interval_perf); + /* reset yaw setpoint after non-manual control */ + reset_yaw_sp = true; + } - /* run rates controller if needed */ - if (control_mode.flag_control_rates_enabled) { - /* get current rate setpoint */ - bool rates_sp_updated = false; - orb_check(rates_sp_sub, &rates_sp_updated); + /* check if we should we reset integrals */ + bool reset_integral = !control_mode.flag_armed || att_sp.thrust < 0.1f; // TODO use landed status instead of throttle - if (rates_sp_updated) { - orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp); - } + /* run attitude controller if needed */ + if (control_mode.flag_control_attitude_enabled) { + multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, reset_integral); + orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); + } - /* apply controller */ - float rates[3]; - rates[0] = att.rollspeed; - rates[1] = att.pitchspeed; - rates[2] = att.yawspeed; - multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral); + /* measure in what intervals the controller runs */ + perf_count(mc_interval_perf); - } else { - /* rates controller disabled, set actuators to zero for safety */ - actuators.control[0] = 0.0f; - actuators.control[1] = 0.0f; - actuators.control[2] = 0.0f; - actuators.control[3] = 0.0f; + /* run rates controller if needed */ + if (control_mode.flag_control_rates_enabled) { + /* get current rate setpoint */ + bool rates_sp_updated = false; + orb_check(vehicle_rates_setpoint_sub, &rates_sp_updated); + + if (rates_sp_updated) { + orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rates_setpoint_sub, &rates_sp); } - actuators.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + /* apply controller */ + float rates[3]; + rates[0] = att.rollspeed; + rates[1] = att.pitchspeed; + rates[2] = att.yawspeed; + multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral); + + } else { + /* rates controller disabled, set actuators to zero for safety */ + actuators.control[0] = 0.0f; + actuators.control[1] = 0.0f; + actuators.control[2] = 0.0f; + actuators.control[3] = 0.0f; + } + + actuators.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - perf_end(mc_loop_perf); - } /* end of poll call for attitude updates */ - } /* end of poll return value check */ + perf_end(mc_loop_perf); + } } warnx("stopping, disarming motors"); @@ -383,10 +383,9 @@ mc_thread_main(int argc, char *argv[]) orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - - close(att_sub); - close(control_mode_sub); - close(manual_sub); + close(vehicle_attitude_sub); + close(vehicle_control_mode_sub); + close(manual_control_setpoint_sub); close(actuator_pub); close(att_sp_pub); diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index 433976656..fd3b72015 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -285,10 +285,10 @@ dsm_bind(uint16_t cmd, int pulses) /*Pulse RX pin a number of times*/ for (int i = 0; i < pulses; i++) { + up_udelay(25); stm32_gpiowrite(usart1RxAsOutp, false); up_udelay(25); stm32_gpiowrite(usart1RxAsOutp, true); - up_udelay(25); } break; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index ec8033202..080aa550c 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -583,6 +583,15 @@ int sdlog2_thread_main(int argc, char *argv[]) errx(1, "unable to create logging folder, exiting."); } + const char *converter_in = "/etc/logging/conv.zip"; + char* converter_out = malloc(120); + sprintf(converter_out, "%s/conv.zip", folder_path); + + if (file_copy(converter_in, converter_out)) { + errx(1, "unable to copy conversion scripts, exiting."); + } + free(converter_out); + /* only print logging path, important to find log file later */ warnx("logging to directory: %s", folder_path); @@ -1251,7 +1260,7 @@ int file_copy(const char *file_old, const char *file_new) fclose(source); fclose(target); - return ret; + return OK; } void handle_command(struct vehicle_command_s *cmd) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 4d719e0e1..8875f65fc 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -44,8 +44,34 @@ #include <systemlib/param/param.h> +/** + * Gyro X offset FIXME + * + * This is an X-axis offset for the gyro. + * Adjust it according to the calibration data. + * + * @min -10.0 + * @max 10.0 + * @group Gyro Config + */ PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f); + +/** + * Gyro Y offset FIXME with dot. + * + * @min -10.0 + * @max 10.0 + * @group Gyro Config + */ PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f); + +/** + * Gyro Z offset FIXME + * + * @min -5.0 + * @max 5.0 + * @group Gyro Config + */ PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 9ef0d3c83..3fcacaf8f 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -68,7 +68,6 @@ #include <systemlib/err.h> #include <systemlib/perf_counter.h> -#include <systemlib/ppm_decode.h> #include <systemlib/airspeed.h> #include <uORB/uORB.h> @@ -105,22 +104,22 @@ * IN13 - aux1 * IN14 - aux2 * IN15 - pressure sensor - * + * * IO: * IN4 - servo supply rail * IN5 - analog RSSI */ #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 - #define ADC_BATTERY_VOLTAGE_CHANNEL 10 - #define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 +#define ADC_BATTERY_VOLTAGE_CHANNEL 10 +#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 #endif #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 - #define ADC_BATTERY_VOLTAGE_CHANNEL 2 - #define ADC_BATTERY_CURRENT_CHANNEL 3 - #define ADC_5V_RAIL_SENSE 4 - #define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 +#define ADC_BATTERY_VOLTAGE_CHANNEL 2 +#define ADC_BATTERY_CURRENT_CHANNEL 3 +#define ADC_5V_RAIL_SENSE 4 +#define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 #endif #define BAT_VOL_INITIAL 0.f @@ -134,8 +133,6 @@ */ #define PCB_TEMP_ESTIMATE_DEG 5.0f -#define PPM_INPUT_TIMEOUT_INTERVAL 50000 /**< 50 ms timeout / 20 Hz */ - #define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg) /** @@ -143,70 +140,68 @@ * This enum maps from board attitude to airframe attitude. */ enum Rotation { - ROTATION_NONE = 0, - ROTATION_YAW_45 = 1, - ROTATION_YAW_90 = 2, - ROTATION_YAW_135 = 3, - ROTATION_YAW_180 = 4, - ROTATION_YAW_225 = 5, - ROTATION_YAW_270 = 6, - ROTATION_YAW_315 = 7, - ROTATION_ROLL_180 = 8, - ROTATION_ROLL_180_YAW_45 = 9, - ROTATION_ROLL_180_YAW_90 = 10, - ROTATION_ROLL_180_YAW_135 = 11, - ROTATION_PITCH_180 = 12, - ROTATION_ROLL_180_YAW_225 = 13, - ROTATION_ROLL_180_YAW_270 = 14, - ROTATION_ROLL_180_YAW_315 = 15, - ROTATION_ROLL_90 = 16, - ROTATION_ROLL_90_YAW_45 = 17, - ROTATION_ROLL_90_YAW_90 = 18, - ROTATION_ROLL_90_YAW_135 = 19, - ROTATION_ROLL_270 = 20, - ROTATION_ROLL_270_YAW_45 = 21, - ROTATION_ROLL_270_YAW_90 = 22, - ROTATION_ROLL_270_YAW_135 = 23, - ROTATION_PITCH_90 = 24, - ROTATION_PITCH_270 = 25, - ROTATION_MAX + ROTATION_NONE = 0, + ROTATION_YAW_45 = 1, + ROTATION_YAW_90 = 2, + ROTATION_YAW_135 = 3, + ROTATION_YAW_180 = 4, + ROTATION_YAW_225 = 5, + ROTATION_YAW_270 = 6, + ROTATION_YAW_315 = 7, + ROTATION_ROLL_180 = 8, + ROTATION_ROLL_180_YAW_45 = 9, + ROTATION_ROLL_180_YAW_90 = 10, + ROTATION_ROLL_180_YAW_135 = 11, + ROTATION_PITCH_180 = 12, + ROTATION_ROLL_180_YAW_225 = 13, + ROTATION_ROLL_180_YAW_270 = 14, + ROTATION_ROLL_180_YAW_315 = 15, + ROTATION_ROLL_90 = 16, + ROTATION_ROLL_90_YAW_45 = 17, + ROTATION_ROLL_90_YAW_90 = 18, + ROTATION_ROLL_90_YAW_135 = 19, + ROTATION_ROLL_270 = 20, + ROTATION_ROLL_270_YAW_45 = 21, + ROTATION_ROLL_270_YAW_90 = 22, + ROTATION_ROLL_270_YAW_135 = 23, + ROTATION_PITCH_90 = 24, + ROTATION_PITCH_270 = 25, + ROTATION_MAX }; -typedef struct -{ - uint16_t roll; - uint16_t pitch; - uint16_t yaw; +typedef struct { + uint16_t roll; + uint16_t pitch; + uint16_t yaw; } rot_lookup_t; -const rot_lookup_t rot_lookup[] = -{ - { 0, 0, 0 }, - { 0, 0, 45 }, - { 0, 0, 90 }, - { 0, 0, 135 }, - { 0, 0, 180 }, - { 0, 0, 225 }, - { 0, 0, 270 }, - { 0, 0, 315 }, - {180, 0, 0 }, - {180, 0, 45 }, - {180, 0, 90 }, - {180, 0, 135 }, - { 0, 180, 0 }, - {180, 0, 225 }, - {180, 0, 270 }, - {180, 0, 315 }, - { 90, 0, 0 }, - { 90, 0, 45 }, - { 90, 0, 90 }, - { 90, 0, 135 }, - {270, 0, 0 }, - {270, 0, 45 }, - {270, 0, 90 }, - {270, 0, 135 }, - { 0, 90, 0 }, - { 0, 270, 0 } +const rot_lookup_t rot_lookup[] = { + { 0, 0, 0 }, + { 0, 0, 45 }, + { 0, 0, 90 }, + { 0, 0, 135 }, + { 0, 0, 180 }, + { 0, 0, 225 }, + { 0, 0, 270 }, + { 0, 0, 315 }, + {180, 0, 0 }, + {180, 0, 45 }, + {180, 0, 90 }, + {180, 0, 135 }, + { 0, 180, 0 }, + {180, 0, 225 }, + {180, 0, 270 }, + {180, 0, 315 }, + { 90, 0, 0 }, + { 90, 0, 45 }, + { 90, 0, 90 }, + { 90, 0, 135 }, + {270, 0, 0 }, + {270, 0, 45 }, + {270, 0, 90 }, + {270, 0, 135 }, + { 0, 90, 0 }, + { 0, 270, 0 } }; /** @@ -239,12 +234,12 @@ public: private: static const unsigned _rc_max_chan_count = RC_CHANNELS_MAX; /**< maximum number of r/c channels we handle */ - hrt_abstime _ppm_last_valid; /**< last time we got a valid ppm signal */ + hrt_abstime _rc_last_valid; /**< last time we got a valid RC signal */ /** - * Gather and publish PPM input data. + * Gather and publish RC input data. */ - void ppm_poll(); + void rc_poll(); /* XXX should not be here - should be own driver */ int _fd_adc; /**< ADC driver handle */ @@ -501,7 +496,7 @@ Sensors *g_sensors = nullptr; } Sensors::Sensors() : - _ppm_last_valid(0), + _rc_last_valid(0), _fd_adc(-1), _last_adc(0), @@ -532,8 +527,8 @@ Sensors::Sensors() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), - _board_rotation(3,3), - _external_mag_rotation(3,3), + _board_rotation(3, 3), + _external_mag_rotation(3, 3), _mag_is_external(false) { @@ -660,9 +655,9 @@ int Sensors::parameters_update() { bool rc_valid = true; - float tmpScaleFactor = 0.0f; - float tmpRevFactor = 0.0f; - + float tmpScaleFactor = 0.0f; + float tmpRevFactor = 0.0f; + /* rc values */ for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) { @@ -674,19 +669,19 @@ Sensors::parameters_update() tmpScaleFactor = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]); tmpRevFactor = tmpScaleFactor * _parameters.rev[i]; - + /* handle blowup in the scaling factor calculation */ if (!isfinite(tmpScaleFactor) || (tmpRevFactor < 0.000001f) || - (tmpRevFactor > 0.2f) ) { + (tmpRevFactor > 0.2f)) { warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, tmpScaleFactor, (int)(_parameters.rev[i])); /* scaling factors do not make sense, lock them down */ _parameters.scaling_factor[i] = 0.0f; rc_valid = false; + + } else { + _parameters.scaling_factor[i] = tmpScaleFactor; } - else { - _parameters.scaling_factor[i] = tmpScaleFactor; - } } /* handle wrong values */ @@ -812,7 +807,7 @@ void Sensors::get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix) { /* first set to zero */ - rot_matrix->Matrix::zero(3,3); + rot_matrix->Matrix::zero(3, 3); float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll; float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch; @@ -823,7 +818,7 @@ Sensors::get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix) math::Dcm R(euler); for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - (*rot_matrix)(i,j) = R(i, j); + (*rot_matrix)(i, j) = R(i, j); } void @@ -841,7 +836,7 @@ Sensors::accel_init() // XXX do the check more elegantly - #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 /* set the accel internal sampling rate up to at leat 1000Hz */ ioctl(fd, ACCELIOCSSAMPLERATE, 1000); @@ -849,7 +844,7 @@ Sensors::accel_init() /* set the driver to poll at 1000Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 1000); - #elif CONFIG_ARCH_BOARD_PX4FMU_V2 +#elif CONFIG_ARCH_BOARD_PX4FMU_V2 /* set the accel internal sampling rate up to at leat 800Hz */ ioctl(fd, ACCELIOCSSAMPLERATE, 800); @@ -857,10 +852,10 @@ Sensors::accel_init() /* set the driver to poll at 800Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 800); - #else - #error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2 +#else +#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2 - #endif +#endif warnx("using system accel"); close(fd); @@ -882,7 +877,7 @@ Sensors::gyro_init() // XXX do the check more elegantly - #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 /* set the gyro internal sampling rate up to at least 1000Hz */ if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK) @@ -892,7 +887,7 @@ Sensors::gyro_init() if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK) ioctl(fd, SENSORIOCSPOLLRATE, 800); - #else +#else /* set the gyro internal sampling rate up to at least 760Hz */ ioctl(fd, GYROIOCSSAMPLERATE, 760); @@ -900,7 +895,7 @@ Sensors::gyro_init() /* set the driver to poll at 760Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 760); - #endif +#endif warnx("using system gyro"); close(fd); @@ -924,23 +919,28 @@ Sensors::mag_init() ret = ioctl(fd, MAGIOCSSAMPLERATE, 150); + if (ret == OK) { /* set the pollrate accordingly */ ioctl(fd, SENSORIOCSPOLLRATE, 150); + } else { ret = ioctl(fd, MAGIOCSSAMPLERATE, 100); + /* if the slower sampling rate still fails, something is wrong */ if (ret == OK) { /* set the driver to poll also at the slower rate */ ioctl(fd, SENSORIOCSPOLLRATE, 100); + } else { errx(1, "FATAL: mag sampling rate could not be set"); } } - + ret = ioctl(fd, MAGIOCGEXTERNAL, 0); + if (ret < 0) errx(1, "FATAL: unknown if magnetometer is external or onboard"); else if (ret == 1) @@ -993,7 +993,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z}; - vect = _board_rotation*vect; + vect = _board_rotation * vect; raw.accelerometer_m_s2[0] = vect(0); raw.accelerometer_m_s2[1] = vect(1); @@ -1019,7 +1019,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z}; - vect = _board_rotation*vect; + vect = _board_rotation * vect; raw.gyro_rad_s[0] = vect(0); raw.gyro_rad_s[1] = vect(1); @@ -1047,9 +1047,9 @@ Sensors::mag_poll(struct sensor_combined_s &raw) math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z}; if (_mag_is_external) - vect = _external_mag_rotation*vect; + vect = _external_mag_rotation * vect; else - vect = _board_rotation*vect; + vect = _board_rotation * vect; raw.magnetometer_ga[0] = vect(0); raw.magnetometer_ga[1] = vect(1); @@ -1094,8 +1094,8 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) raw.differential_pressure_counter++; _airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_pa); - _airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar*1e2f, - raw.baro_pres_mbar*1e2f, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); + _airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar * 1e2f, + raw.baro_pres_mbar * 1e2f, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); /* announce the airspeed if needed, just publish else */ if (_airspeed_pub > 0) { @@ -1236,12 +1236,12 @@ Sensors::adc_poll(struct sensor_combined_s &raw) int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc)); for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) { - + if (ret >= (int)sizeof(buf_adc[0])) { /* Save raw voltage values */ if (i < (sizeof(raw.adc_voltage_v)) / sizeof(raw.adc_voltage_v[0])) { - raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f); + raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f); } /* look for specific channels and process the raw voltage to measurement data */ @@ -1269,12 +1269,12 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } else { _battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status); } - } + } } else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { /* calculate airspeed, raw is the difference from */ - float voltage = (float)(buf_adc[i].am_data ) * 3.3f / 4096.0f * 2.0f; //V_ref/4096 * (voltage divider factor) + float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f * 2.0f; //V_ref/4096 * (voltage divider factor) /** * The voltage divider pulls the signal down, only act on @@ -1306,17 +1306,13 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } void -Sensors::ppm_poll() +Sensors::rc_poll() { + bool rc_updated; + orb_check(_rc_sub, &rc_updated); - /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ - struct pollfd fds[1]; - fds[0].fd = _rc_sub; - fds[0].events = POLLIN; - /* check non-blocking for new data */ - int poll_ret = poll(fds, 1, 0); - - if (poll_ret > 0) { + if (rc_updated) { + /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ struct rc_input_values rc_input; orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); @@ -1352,7 +1348,7 @@ Sensors::ppm_poll() channel_limit = _rc_max_chan_count; /* we are accepting this message */ - _ppm_last_valid = rc_input.timestamp; + _rc_last_valid = rc_input.timestamp; /* Read out values from raw message */ for (unsigned int i = 0; i < channel_limit; i++) { @@ -1362,6 +1358,7 @@ Sensors::ppm_poll() */ if (rc_input.values[i] < _parameters.min[i]) rc_input.values[i] = _parameters.min[i]; + if (rc_input.values[i] > _parameters.max[i]) rc_input.values[i] = _parameters.max[i]; @@ -1622,7 +1619,7 @@ Sensors::task_main() orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw); /* Look for new r/c input data */ - ppm_poll(); + rc_poll(); perf_end(_loop_perf); } @@ -1640,11 +1637,11 @@ Sensors::start() /* start the task */ _sensors_task = task_spawn_cmd("sensors_task", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2048, - (main_t)&Sensors::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&Sensors::task_main_trampoline, + nullptr); if (_sensors_task < 0) { warn("task start failed"); diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp index b1bb1a66d..cce46bf5f 100644 --- a/src/modules/systemlib/mixer/mixer.cpp +++ b/src/modules/systemlib/mixer/mixer.cpp @@ -50,6 +50,8 @@ #include <stdio.h> #include <math.h> #include <unistd.h> +#include <ctype.h> +#include <systemlib/err.h> #include "mixer.h" @@ -114,6 +116,33 @@ Mixer::scale_check(struct mixer_scaler_s &scaler) return 0; } +const char * +Mixer::findtag(const char *buf, unsigned &buflen, char tag) +{ + while (buflen >= 2) { + if ((buf[0] == tag) && (buf[1] == ':')) + return buf; + buf++; + buflen--; + } + return nullptr; +} + +const char * +Mixer::skipline(const char *buf, unsigned &buflen) +{ + const char *p; + + /* if we can find a CR or NL in the buffer, skip up to it */ + if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen))) { + /* skip up to it AND one beyond - could be on the NUL symbol now */ + buflen -= (p - buf) + 1; + return p + 1; + } + + return nullptr; +} + /****************************************************************************/ NullMixer::NullMixer() : diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index bbfa130a9..723bf9f3b 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -128,7 +128,9 @@ #ifndef _SYSTEMLIB_MIXER_MIXER_H #define _SYSTEMLIB_MIXER_MIXER_H value +#include <nuttx/config.h> #include "drivers/drv_mixer.h" +#include "mixer_load.h" /** * Abstract class defining a mixer mixing zero or more inputs to @@ -210,6 +212,24 @@ protected: */ static int scale_check(struct mixer_scaler_s &scaler); + /** + * Find a tag + * + * @param buf The buffer to operate on. + * @param buflen length of the buffer. + * @param tag character to search for. + */ + static const char * findtag(const char *buf, unsigned &buflen, char tag); + + /** + * Skip a line + * + * @param buf The buffer to operate on. + * @param buflen length of the buffer. + * @return 0 / OK if a line could be skipped, 1 else + */ + static const char * skipline(const char *buf, unsigned &buflen); + private: }; @@ -239,6 +259,11 @@ public: void reset(); /** + * Count the mixers in the group. + */ + unsigned count(); + + /** * Adds mixers to the group based on a text description in a buffer. * * Mixer definitions begin with a single capital letter and a colon. diff --git a/src/modules/systemlib/mixer/mixer_group.cpp b/src/modules/systemlib/mixer/mixer_group.cpp index 1dbc512cd..3ed99fba0 100644 --- a/src/modules/systemlib/mixer/mixer_group.cpp +++ b/src/modules/systemlib/mixer/mixer_group.cpp @@ -111,6 +111,20 @@ MixerGroup::mix(float *outputs, unsigned space) return index; } +unsigned +MixerGroup::count() +{ + Mixer *mixer = _first; + unsigned index = 0; + + while ((mixer != nullptr)) { + mixer = mixer->_next; + index++; + } + + return index; +} + void MixerGroup::groups_required(uint32_t &groups) { @@ -170,6 +184,7 @@ MixerGroup::load_from_buf(const char *buf, unsigned &buflen) /* only adjust buflen if parsing was successful */ buflen = resid; + debug("SUCCESS - buflen: %d", buflen); } else { /* diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c new file mode 100644 index 000000000..a55ddf8a3 --- /dev/null +++ b/src/modules/systemlib/mixer/mixer_load.c @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mixer_load.c + * + * Programmable multi-channel mixer library. + */ + +#include <nuttx/config.h> +#include <string.h> +#include <stdio.h> +#include <ctype.h> + +#include "mixer_load.h" + +int load_mixer_file(const char *fname, char *buf, unsigned maxlen) +{ + FILE *fp; + char line[120]; + + /* open the mixer definition file */ + fp = fopen(fname, "r"); + if (fp == NULL) { + return 1; + } + + /* read valid lines from the file into a buffer */ + buf[0] = '\0'; + for (;;) { + + /* get a line, bail on error/EOF */ + line[0] = '\0'; + if (fgets(line, sizeof(line), fp) == NULL) + break; + + /* if the line doesn't look like a mixer definition line, skip it */ + if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) + continue; + + /* compact whitespace in the buffer */ + char *t, *f; + for (f = line; *f != '\0'; f++) { + /* scan for space characters */ + if (*f == ' ') { + /* look for additional spaces */ + t = f + 1; + while (*t == ' ') + t++; + if (*t == '\0') { + /* strip trailing whitespace */ + *f = '\0'; + } else if (t > (f + 1)) { + memmove(f + 1, t, strlen(t) + 1); + } + } + } + + /* if the line is too long to fit in the buffer, bail */ + if ((strlen(line) + strlen(buf) + 1) >= maxlen) { + return 1; + } + + /* add the line to the buffer */ + strcat(buf, line); + } + + return 0; +} + diff --git a/src/modules/systemlib/mixer/mixer_load.h b/src/modules/systemlib/mixer/mixer_load.h new file mode 100644 index 000000000..4b7091d5b --- /dev/null +++ b/src/modules/systemlib/mixer/mixer_load.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mixer_load.h + * + */ + + +#ifndef _SYSTEMLIB_MIXER_LOAD_H +#define _SYSTEMLIB_MIXER_LOAD_H value + +#include <nuttx/config.h> + +__BEGIN_DECLS + +__EXPORT int load_mixer_file(const char *fname, char *buf, unsigned maxlen); + +__END_DECLS + +#endif diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 2446cc3fb..b89f341b6 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -130,7 +130,7 @@ const MultirotorMixer::Rotor _config_octa_plus[] = { { 1.000000, 0.000000, -1.00 }, { -1.000000, 0.000000, -1.00 }, }; -const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOMETRY] = { +const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = { &_config_quad_x[0], &_config_quad_plus[0], &_config_quad_v[0], @@ -140,7 +140,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOME &_config_octa_x[0], &_config_octa_plus[0], }; -const unsigned _config_rotor_count[MultirotorMixer::Geometry::MAX_GEOMETRY] = { +const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = { 4, /* quad_x */ 4, /* quad_plus */ 4, /* quad_v */ @@ -205,11 +205,17 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl } if (used > (int)buflen) { - debug("multirotor spec used %d of %u", used, buflen); + debug("OVERFLOW: multirotor spec used %d of %u", used, buflen); return nullptr; } - buflen -= used; + buf = skipline(buf, buflen); + if (buf == nullptr) { + debug("no line ending, line is incomplete"); + return nullptr; + } + + debug("remaining in buf: %d, first char: %c", buflen, buf[0]); if (!strcmp(geomname, "4+")) { geometry = MultirotorMixer::QUAD_PLUS; diff --git a/src/modules/systemlib/mixer/mixer_simple.cpp b/src/modules/systemlib/mixer/mixer_simple.cpp index c8434f991..c3985b5de 100644 --- a/src/modules/systemlib/mixer/mixer_simple.cpp +++ b/src/modules/systemlib/mixer/mixer_simple.cpp @@ -55,7 +55,7 @@ #include "mixer.h" #define debug(fmt, args...) do { } while(0) -// #define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) +//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) SimpleMixer::SimpleMixer(ControlCallback control_cb, uintptr_t cb_handle, @@ -71,28 +71,6 @@ SimpleMixer::~SimpleMixer() free(_info); } -static const char * -findtag(const char *buf, unsigned &buflen, char tag) -{ - while (buflen >= 2) { - if ((buf[0] == tag) && (buf[1] == ':')) - return buf; - buf++; - buflen--; - } - return nullptr; -} - -static void -skipline(const char *buf, unsigned &buflen) -{ - const char *p; - - /* if we can find a CR or NL in the buffer, skip up to it */ - if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen))) - buflen -= (p - buf); -} - int SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler) { @@ -111,7 +89,12 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler debug("out scaler parse failed on '%s' (got %d, consumed %d)", buf, ret, n); return -1; } - skipline(buf, buflen); + + buf = skipline(buf, buflen); + if (buf == nullptr) { + debug("no line ending, line is incomplete"); + return -1; + } scaler.negative_scale = s[0] / 10000.0f; scaler.positive_scale = s[1] / 10000.0f; @@ -130,7 +113,7 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale buf = findtag(buf, buflen, 'S'); if ((buf == nullptr) || (buflen < 16)) { - debug("contorl parser failed finding tag, ret: '%s'", buf); + debug("control parser failed finding tag, ret: '%s'", buf); return -1; } @@ -139,7 +122,12 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale debug("control parse failed on '%s'", buf); return -1; } - skipline(buf, buflen); + + buf = skipline(buf, buflen); + if (buf == nullptr) { + debug("no line ending, line is incomplete"); + return -1; + } control_group = u[0]; control_index = u[1]; @@ -161,29 +149,17 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c int used; const char *end = buf + buflen; - /* enforce that the mixer ends with space or a new line */ - for (int i = buflen - 1; i >= 0; i--) { - if (buf[i] == '\0') - continue; - - /* require a space or newline at the end of the buffer, fail on printable chars */ - if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') { - /* found a line ending or space, so no split symbols / numbers. good. */ - break; - } else { - debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen-1, buf[i]); - goto out; - } - - } - /* get the base info for the mixer */ if (sscanf(buf, "M: %u%n", &inputs, &used) != 1) { debug("simple parse failed on '%s'", buf); goto out; } - buflen -= used; + buf = skipline(buf, buflen); + if (buf == nullptr) { + debug("no line ending, line is incomplete"); + goto out; + } mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs)); diff --git a/src/modules/systemlib/mixer/module.mk b/src/modules/systemlib/mixer/module.mk index 4d45e1c50..fc7485e20 100644 --- a/src/modules/systemlib/mixer/module.mk +++ b/src/modules/systemlib/mixer/module.mk @@ -39,4 +39,5 @@ LIBNAME = mixerlib SRCS = mixer.cpp \ mixer_group.cpp \ mixer_multirotor.cpp \ - mixer_simple.cpp + mixer_simple.cpp \ + mixer_load.c diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 093c6775d..38fb74c9b 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -61,7 +61,6 @@ */ struct vehicle_control_mode_s { - uint16_t counter; /**< incremented by the writing thread every time new data is stored */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ bool flag_armed; |