From 2fd8c6b958bb00c37c8a3fb885b6b657d6c14755 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Oct 2013 13:54:31 +0200 Subject: [PATCH 01/27] multirotor_att_control: cleanup, some refactoring --- .../multirotor_att_control_main.c | 433 +++++++++--------- 1 file changed, 216 insertions(+), 217 deletions(-) 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 04582f2a40..44f8f664bc 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 */ + /* 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), param_sub, &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); + } + + /* attitude setpoint */ + orb_check(vehicle_attitude_setpoint_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp); + } + + /* offboard control setpoint */ + orb_check(offboard_control_setpoint_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(offboard_control_setpoint), offboard_control_setpoint_sub, &offboard_sp); + } + + /* vehicle status */ + orb_check(vehicle_status_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &status); + } + + /* sensors */ + orb_check(sensor_combined_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); + } + + /* set flag to safe value */ + control_yaw_position = true; + + /* reset yaw setpoint if not armed */ + if (!control_mode.flag_armed) { + 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 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); } - /* 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); + /* reset yaw setpoint after offboard control */ + reset_yaw_sp = true; - if (updated) { - orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp); - } + } 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; - /* get a local copy of status */ - orb_check(status_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_status), status_sub, &status); - } - - /* get a local copy of the current sensor values */ - orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); - - /* set flag to safe value */ - control_yaw_position = true; - - /* reset yaw setpoint if not armed */ - if (!control_mode.flag_armed) { - 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 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); - } - - /* reset yaw setpoint after offboard control */ - reset_yaw_sp = 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; - - if (status.condition_landed) { - /* reset yaw setpoint if on ground */ - reset_yaw_sp = true; - } - - } 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; - } + if (status.condition_landed) { + /* reset yaw setpoint if on ground */ + reset_yaw_sp = true; } - 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; - - 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; - } - - 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 { - /* 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; + /* 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; - rates_sp.thrust = manual.throttle; - rates_sp.timestamp = hrt_absolute_time(); - } + reset_yaw_sp = true; // has no effect on control, just for beautiful log - /* reset yaw setpoint after ACRO */ - reset_yaw_sp = true; + } else { + control_yaw_position = true; + } } + 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; + + 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; + } + + 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); - } - - /* measure in what intervals the controller runs */ - perf_count(mc_interval_perf); - - /* 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); - - if (rates_sp_updated) { - orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &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); } - - /* 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); + /* reset yaw setpoint after non-manual control */ + reset_yaw_sp = true; + } - perf_end(mc_loop_perf); - } /* end of poll call for attitude updates */ - } /* end of poll return value check */ + /* 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); + } + + /* measure in what intervals the controller runs */ + perf_count(mc_interval_perf); + + /* 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); + } + + /* 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); + } } 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); From 6e7300fb927535f7727ff6eeb2a47cad9353a346 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Oct 2013 22:02:46 +0200 Subject: [PATCH 02/27] px4io: major optimization and cleanup --- src/drivers/px4io/px4io.cpp | 102 +++++++++++++++++------------------- 1 file changed, 47 insertions(+), 55 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index cba193208d..b6392b5b12 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -94,7 +94,9 @@ extern device::Device *PX4IO_serial_interface() weak_function; #define PX4IO_SET_DEBUG _IOC(0xff00, 0) #define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1) -#define UPDATE_INTERVAL_MIN 2 +#define UPDATE_INTERVAL_MIN 2 // 2 ms -> 500 Hz +#define ORB_CHECK_INTERVAL 200000 // 200 ms -> 5 Hz +#define IO_POLL_INTERVAL 20000 // 20 ms -> 50 Hz /** * The PX4IO class. @@ -734,7 +736,8 @@ PX4IO::task_main_trampoline(int argc, char *argv[]) void PX4IO::task_main() { - hrt_abstime last_poll_time = 0; + hrt_abstime poll_last = 0; + hrt_abstime orb_check_last = 0; log("starting"); @@ -747,18 +750,10 @@ PX4IO::task_main() _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1)); orb_set_interval(_t_actuators, 20); /* default to 50Hz */ - _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */ - _t_vehicle_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); - orb_set_interval(_t_vehicle_control_mode, 200); /* 5Hz update rate max. */ - _t_param = orb_subscribe(ORB_ID(parameter_update)); - orb_set_interval(_t_param, 500); /* 2Hz update rate max. */ - _t_vehicle_command = orb_subscribe(ORB_ID(vehicle_command)); - orb_set_interval(_t_param, 1000); /* 1Hz update rate max. */ if ((_t_actuators < 0) || (_t_actuator_armed < 0) || @@ -770,17 +765,9 @@ PX4IO::task_main() } /* poll descriptor */ - pollfd fds[5]; + pollfd fds[1]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_actuator_armed; - fds[1].events = POLLIN; - fds[2].fd = _t_vehicle_control_mode; - fds[2].events = POLLIN; - fds[3].fd = _t_param; - fds[3].events = POLLIN; - fds[4].fd = _t_vehicle_command; - fds[4].events = POLLIN; log("ready"); @@ -802,7 +789,7 @@ PX4IO::task_main() /* sleep waiting for topic updates, but no more than 20ms */ unlock(); - int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 20); + int ret = ::poll(fds, 1, 20); lock(); /* this would be bad... */ @@ -815,58 +802,66 @@ PX4IO::task_main() hrt_abstime now = hrt_absolute_time(); /* if we have new control data from the ORB, handle it */ - if (fds[0].revents & POLLIN) + if (fds[0].revents & POLLIN) { io_set_control_state(); - - /* if we have an arming state update, handle it */ - if ((fds[1].revents & POLLIN) || (fds[2].revents & POLLIN)) - io_set_arming_state(); - - /* if we have a vehicle command, handle it */ - if (fds[4].revents & POLLIN) { - struct vehicle_command_s cmd; - orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd); - // Check for a DSM pairing command - if ((cmd.command == VEHICLE_CMD_START_RX_PAIR) && (cmd.param1== 0.0f)) { - dsm_bind_ioctl((int)cmd.param2); - } } - /* - * If it's time for another tick of the polling status machine, - * try it now. - */ - if ((now - last_poll_time) >= 20000) { + if (now >= poll_last + IO_POLL_INTERVAL) { + /* run at 50Hz */ + poll_last = now; - /* - * Pull status and alarms from IO. - */ + /* pull status and alarms from IO */ io_get_status(); - /* - * Get raw R/C input from IO. - */ + /* get raw R/C input from IO */ io_publish_raw_rc(); - /* - * Fetch mixed servo controls and PWM outputs from IO. - * - * XXX We could do this at a reduced rate in many/most cases. - */ + /* fetch mixed servo controls and PWM outputs from IO */ io_publish_mixed_controls(); io_publish_pwm_outputs(); + } + + if (now >= orb_check_last + ORB_CHECK_INTERVAL) { + /* run at 5Hz */ + orb_check_last = now; + + /* check updates on uORB topics and handle it */ + bool updated = false; + + /* arming state */ + orb_check(_t_actuator_armed, &updated); + if (!updated) { + orb_check(_t_vehicle_control_mode, &updated); + } + if (updated) { + io_set_arming_state(); + } + + /* vehicle command */ + orb_check(_t_vehicle_command, &updated); + if (updated) { + struct vehicle_command_s cmd; + orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd); + // Check for a DSM pairing command + if ((cmd.command == VEHICLE_CMD_START_RX_PAIR) && (cmd.param1== 0.0f)) { + dsm_bind_ioctl((int)cmd.param2); + } + } /* * If parameters have changed, re-send RC mappings to IO * * XXX this may be a bit spammy */ - if (fds[3].revents & POLLIN) { + orb_check(_t_param, &updated); + if (updated) { parameter_update_s pupdate; + orb_copy(ORB_ID(parameter_update), _t_param, &pupdate); + int32_t dsm_bind_val; param_t dsm_bind_param; - // See if bind parameter has been set, and reset it to -1 + /* see if bind parameter has been set, and reset it to -1 */ param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val); if (dsm_bind_val > -1) { dsm_bind_ioctl(dsm_bind_val); @@ -874,9 +869,6 @@ PX4IO::task_main() param_set(dsm_bind_param, &dsm_bind_val); } - /* copy to reset the notification */ - orb_copy(ORB_ID(parameter_update), _t_param, &pupdate); - /* re-upload RC input config as it may have changed */ io_set_rc_config(); } From 87e1ffe0ba293c62b882a8ae9729878e36a95c4c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Oct 2013 22:04:06 +0200 Subject: [PATCH 03/27] px4io: code style fixed --- src/drivers/px4io/px4io.cpp | 554 +++++++++++++++++++++++------------- 1 file changed, 349 insertions(+), 205 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index b6392b5b12..f3fa3ed29f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -108,35 +108,35 @@ class PX4IO : public device::CDev public: /** * Constructor. - * + * * Initialize all class variables. */ PX4IO(device::Device *interface); /** * Destructor. - * + * * Wait for worker thread to terminate. */ virtual ~PX4IO(); /** * Initialize the PX4IO class. - * + * * Retrieve relevant initial system parameters. Initialize PX4IO registers. */ virtual int init(); /** * Detect if a PX4IO is connected. - * + * * Only validate if there is a PX4IO to talk to. */ virtual int detect(); /** * IO Control handler. - * + * * Handle all IOCTL calls to the PX4IO file descriptor. * * @param[in] filp file handle (not used). This function is always called directly through object reference @@ -147,7 +147,7 @@ public: /** * write handler. - * + * * Handle writes to the PX4IO file descriptor. * * @param[in] filp file handle (not used). This function is always called directly through object reference @@ -214,8 +214,7 @@ public: * * @param[in] enable true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled */ - inline void set_dsm_vcc_ctl(bool enable) - { + inline void set_dsm_vcc_ctl(bool enable) { _dsm_vcc_ctl = enable; }; @@ -224,8 +223,7 @@ public: * * @return true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled */ - inline bool get_dsm_vcc_ctl() - { + inline bool get_dsm_vcc_ctl() { return _dsm_vcc_ctl; }; #endif @@ -401,7 +399,7 @@ private: /** * Send mixer definition text to IO */ - int mixer_send(const char *buf, unsigned buflen, unsigned retries=3); + int mixer_send(const char *buf, unsigned buflen, unsigned retries = 3); /** * Handle a status update from IO. @@ -467,12 +465,12 @@ PX4IO::PX4IO(device::Device *interface) : _to_battery(0), _to_safety(0), _primary_pwm_device(false), - _battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor + _battery_amp_per_volt(90.0f / 5.0f), // this matches the 3DR current sensor _battery_amp_bias(0), _battery_mamphour_total(0), _battery_last_timestamp(0) #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 - ,_dsm_vcc_ctl(false) + , _dsm_vcc_ctl(false) #endif { @@ -515,19 +513,22 @@ PX4IO::detect() /* do regular cdev init */ ret = CDev::init(); + if (ret != OK) return ret; /* get some parameters */ unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); + if (protocol != PX4IO_PROTOCOL_VERSION) { if (protocol == _io_reg_get_error) { log("IO not installed"); + } else { log("IO version error"); mavlink_log_emergency(_mavlink_fd, "IO VERSION MISMATCH, PLEASE UPGRADE SOFTWARE!"); } - + return -1; } } @@ -546,22 +547,26 @@ PX4IO::init() /* do regular cdev init */ ret = CDev::init(); + if (ret != OK) return ret; /* get some parameters */ unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); + if (protocol != PX4IO_PROTOCOL_VERSION) { log("protocol/firmware mismatch"); mavlink_log_emergency(_mavlink_fd, "[IO] protocol/firmware mismatch, abort."); return -1; } + _hardware = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION); _max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT); _max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT); _max_relays = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT); _max_transfer = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER) - 2; _max_rc_input = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT); + if ((_max_actuators < 1) || (_max_actuators > 255) || (_max_relays > 32) || (_max_transfer < 16) || (_max_transfer > 255) || @@ -571,6 +576,7 @@ PX4IO::init() mavlink_log_emergency(_mavlink_fd, "[IO] config read fail, abort."); return -1; } + if (_max_rc_input > RC_INPUT_MAX_CHANNELS) _max_rc_input = RC_INPUT_MAX_CHANNELS; @@ -585,6 +591,7 @@ PX4IO::init() /* get IO's last seen FMU state */ ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, ®, sizeof(reg)); + if (ret != OK) return ret; @@ -598,8 +605,8 @@ PX4IO::init() /* get a status update from IO */ io_get_status(); - mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART"); - log("INAIR RESTART RECOVERY (needs commander app running)"); + mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART"); + log("INAIR RESTART RECOVERY (needs commander app running)"); /* WARNING: COMMANDER app/vehicle status must be initialized. * If this fails (or the app is not started), worst-case IO @@ -611,7 +618,7 @@ PX4IO::init() struct actuator_armed_s safety; uint64_t try_start_time = hrt_absolute_time(); bool updated = false; - + /* keep checking for an update, ensure we got a arming information, not something that was published a long time ago. */ do { @@ -627,7 +634,7 @@ PX4IO::init() usleep(10000); /* abort after 5s */ - if ((hrt_absolute_time() - try_start_time)/1000 > 3000) { + if ((hrt_absolute_time() - try_start_time) / 1000 > 3000) { log("failed to recover from in-air restart (1), aborting IO driver init."); return 1; } @@ -667,7 +674,7 @@ PX4IO::init() usleep(50000); /* abort after 5s */ - if ((hrt_absolute_time() - try_start_time)/1000 > 2000) { + if ((hrt_absolute_time() - try_start_time) / 1000 > 2000) { log("failed to recover from in-air restart (2), aborting IO driver init."); return 1; } @@ -678,25 +685,28 @@ PX4IO::init() log("re-sending arm cmd"); } - /* keep waiting for state change for 2 s */ + /* keep waiting for state change for 2 s */ } while (!safety.armed); - /* regular boot, no in-air restart, init IO */ + /* regular boot, no in-air restart, init IO */ + } else { /* dis-arm IO before touching anything */ - io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, - PX4IO_P_SETUP_ARMING_FMU_ARMED | - PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | - PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | - PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE, 0); + io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, + PX4IO_P_SETUP_ARMING_FMU_ARMED | + PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | + PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | + PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE, 0); if (_rc_handling_disabled) { ret = io_disable_rc_handling(); + } else { /* publish RC config to IO */ ret = io_set_rc_config(); + if (ret != OK) { log("failed to update RC input config"); mavlink_log_info(_mavlink_fd, "[IO] RC config upload fail"); @@ -756,10 +766,10 @@ PX4IO::task_main() _t_vehicle_command = orb_subscribe(ORB_ID(vehicle_command)); if ((_t_actuators < 0) || - (_t_actuator_armed < 0) || - (_t_vehicle_control_mode < 0) || - (_t_param < 0) || - (_t_vehicle_command < 0)) { + (_t_actuator_armed < 0) || + (_t_vehicle_control_mode < 0) || + (_t_param < 0) || + (_t_vehicle_command < 0)) { log("subscription(s) failed"); goto out; } @@ -781,8 +791,10 @@ PX4IO::task_main() if (_update_interval != 0) { if (_update_interval < UPDATE_INTERVAL_MIN) _update_interval = UPDATE_INTERVAL_MIN; + if (_update_interval > 100) _update_interval = 100; + orb_set_interval(_t_actuators, _update_interval); _update_interval = 0; } @@ -830,20 +842,24 @@ PX4IO::task_main() /* arming state */ orb_check(_t_actuator_armed, &updated); + if (!updated) { orb_check(_t_vehicle_control_mode, &updated); } + if (updated) { io_set_arming_state(); } /* vehicle command */ orb_check(_t_vehicle_command, &updated); + if (updated) { struct vehicle_command_s cmd; orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd); + // Check for a DSM pairing command - if ((cmd.command == VEHICLE_CMD_START_RX_PAIR) && (cmd.param1== 0.0f)) { + if ((cmd.command == VEHICLE_CMD_START_RX_PAIR) && (cmd.param1 == 0.0f)) { dsm_bind_ioctl((int)cmd.param2); } } @@ -854,6 +870,7 @@ PX4IO::task_main() * XXX this may be a bit spammy */ orb_check(_t_param, &updated); + if (updated) { parameter_update_s pupdate; orb_copy(ORB_ID(parameter_update), _t_param, &pupdate); @@ -863,6 +880,7 @@ PX4IO::task_main() /* see if bind parameter has been set, and reset it to -1 */ param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val); + if (dsm_bind_val > -1) { dsm_bind_ioctl(dsm_bind_val); dsm_bind_val = -1; @@ -899,7 +917,7 @@ PX4IO::io_set_control_state() /* get controls */ orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - ORB_ID(actuator_controls_1), _t_actuators, &controls); + ORB_ID(actuator_controls_1), _t_actuators, &controls); for (unsigned i = 0; i < _max_controls; i++) regs[i] = FLOAT_TO_REG(controls.control[i]); @@ -972,17 +990,21 @@ PX4IO::io_set_arming_state() if (armed.armed && !armed.lockdown) { set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; + } else { clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } + if (armed.ready_to_arm) { set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; + } else { clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; } if (control_mode.flag_external_manual_override_ok) { set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; + } else { clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; } @@ -1027,22 +1049,27 @@ PX4IO::io_set_rc_config() * autopilots / GCS'. */ param_get(param_find("RC_MAP_ROLL"), &ichan); + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) input_map[ichan - 1] = 0; param_get(param_find("RC_MAP_PITCH"), &ichan); + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) input_map[ichan - 1] = 1; param_get(param_find("RC_MAP_YAW"), &ichan); + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) input_map[ichan - 1] = 2; param_get(param_find("RC_MAP_THROTTLE"), &ichan); + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) input_map[ichan - 1] = 3; ichan = 4; + for (unsigned i = 0; i < _max_rc_input; i++) if (input_map[i] == -1) input_map[i] = ichan++; @@ -1097,6 +1124,7 @@ PX4IO::io_set_rc_config() /* send channel config to IO */ ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE); + if (ret != OK) { log("rc config upload failed"); break; @@ -1124,13 +1152,14 @@ PX4IO::io_handle_status(uint16_t status) /* check for IO reset - force it back to armed if necessary */ if (_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF && !(status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) - && !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { + && !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { /* set the arming flag */ ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_SAFETY_OFF | PX4IO_P_STATUS_FLAGS_ARM_SYNC); /* set new status */ _status = status; _status &= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; + } else if (!(_status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { /* set the sync flag */ @@ -1154,6 +1183,7 @@ PX4IO::io_handle_status(uint16_t status) if (status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { safety.safety_off = true; safety.safety_switch_available = true; + } else { safety.safety_off = false; safety.safety_switch_available = true; @@ -1162,6 +1192,7 @@ PX4IO::io_handle_status(uint16_t status) /* lazily publish the safety status */ if (_to_safety > 0) { orb_publish(ORB_ID(safety), _to_safety, &safety); + } else { _to_safety = orb_advertise(ORB_ID(safety), &safety); } @@ -1177,11 +1208,13 @@ PX4IO::dsm_bind_ioctl(int dsmMode) if ((dsmMode == 0) || (dsmMode == 1)) { mavlink_log_info(_thread_mavlink_fd, "[IO] binding dsm%c rx", (dsmMode == 0) ? '2' : 'x'); ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : DSMX_BIND_PULSES); + } else { mavlink_log_info(_thread_mavlink_fd, "[IO] invalid dsm bind mode, bind request rejected"); } + } else { - mavlink_log_info(_thread_mavlink_fd, "[IO] system armed, bind request rejected"); + mavlink_log_info(_thread_mavlink_fd, "[IO] system armed, bind request rejected"); } } @@ -1207,12 +1240,13 @@ PX4IO::io_get_status() /* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */ ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, ®s[0], sizeof(regs) / sizeof(regs[0])); + if (ret != OK) return ret; io_handle_status(regs[0]); io_handle_alarms(regs[1]); - + /* only publish if battery has a valid minimum voltage */ if (regs[2] > 3300) { battery_status_s battery_status; @@ -1226,22 +1260,24 @@ PX4IO::io_get_status() regs[3] contains the raw ADC count, as 12 bit ADC value, with full range being 3.3v */ - battery_status.current_a = regs[3] * (3.3f/4096.0f) * _battery_amp_per_volt; + battery_status.current_a = regs[3] * (3.3f / 4096.0f) * _battery_amp_per_volt; battery_status.current_a += _battery_amp_bias; /* integrate battery over time to get total mAh used */ if (_battery_last_timestamp != 0) { - _battery_mamphour_total += battery_status.current_a * - (battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600; + _battery_mamphour_total += battery_status.current_a * + (battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600; } + battery_status.discharged_mah = _battery_mamphour_total; _battery_last_timestamp = battery_status.timestamp; /* lazily publish the battery voltage */ if (_to_battery > 0) { orb_publish(ORB_ID(battery_status), _to_battery, &battery_status); + } else { _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); } @@ -1258,7 +1294,7 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) /* we don't have the status bits, so input_source has to be set elsewhere */ input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN; - + static const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT); uint16_t regs[RC_INPUT_MAX_CHANNELS + prolog]; @@ -1269,6 +1305,7 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) */ input_rc.timestamp = hrt_absolute_time(); ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, ®s[0], prolog + 9); + if (ret != OK) return ret; @@ -1277,8 +1314,10 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) * channel count once. */ channel_count = regs[0]; + if (channel_count > 9) { ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, ®s[prolog + 9], channel_count - 9); + if (ret != OK) return ret; } @@ -1301,16 +1340,20 @@ PX4IO::io_publish_raw_rc() rc_val.timestamp = hrt_absolute_time(); int ret = io_get_raw_rc_input(rc_val); + if (ret != OK) return ret; /* sort out the source of the values */ if (_status & PX4IO_P_STATUS_FLAGS_RC_PPM) { rc_val.input_source = RC_INPUT_SOURCE_PX4IO_PPM; + } else if (_status & PX4IO_P_STATUS_FLAGS_RC_DSM) { rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM; + } else if (_status & PX4IO_P_STATUS_FLAGS_RC_SBUS) { rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SBUS; + } else { rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; } @@ -1318,7 +1361,8 @@ PX4IO::io_publish_raw_rc() /* lazily advertise on first publication */ if (_to_input_rc == 0) { _to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_val); - } else { + + } else { orb_publish(ORB_ID(input_rc), _to_input_rc, &rc_val); } @@ -1343,6 +1387,7 @@ PX4IO::io_publish_mixed_controls() /* get actuator controls from IO */ uint16_t act[_max_actuators]; int ret = io_reg_get(PX4IO_PAGE_ACTUATORS, 0, act, _max_actuators); + if (ret != OK) return ret; @@ -1352,16 +1397,17 @@ PX4IO::io_publish_mixed_controls() /* laxily advertise on first publication */ if (_to_actuators_effective == 0) { - _to_actuators_effective = - orb_advertise((_primary_pwm_device ? - ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : - ORB_ID(actuator_controls_effective_1)), - &controls_effective); + _to_actuators_effective = + orb_advertise((_primary_pwm_device ? + ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : + ORB_ID(actuator_controls_effective_1)), + &controls_effective); + } else { - orb_publish((_primary_pwm_device ? - ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : - ORB_ID(actuator_controls_effective_1)), - _to_actuators_effective, &controls_effective); + orb_publish((_primary_pwm_device ? + ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : + ORB_ID(actuator_controls_effective_1)), + _to_actuators_effective, &controls_effective); } return OK; @@ -1381,26 +1427,29 @@ PX4IO::io_publish_pwm_outputs() /* get servo values from IO */ uint16_t ctl[_max_actuators]; int ret = io_reg_get(PX4IO_PAGE_SERVOS, 0, ctl, _max_actuators); + if (ret != OK) return ret; /* convert from register format to float */ for (unsigned i = 0; i < _max_actuators; i++) outputs.output[i] = ctl[i]; + outputs.noutputs = _max_actuators; /* lazily advertise on first publication */ if (_to_outputs == 0) { _to_outputs = orb_advertise((_primary_pwm_device ? - ORB_ID_VEHICLE_CONTROLS : - ORB_ID(actuator_outputs_1)), - &outputs); + ORB_ID_VEHICLE_CONTROLS : + ORB_ID(actuator_outputs_1)), + &outputs); + } else { orb_publish((_primary_pwm_device ? - ORB_ID_VEHICLE_CONTROLS : - ORB_ID(actuator_outputs_1)), - _to_outputs, - &outputs); + ORB_ID_VEHICLE_CONTROLS : + ORB_ID(actuator_outputs_1)), + _to_outputs, + &outputs); } return OK; @@ -1416,10 +1465,12 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned } int ret = _interface->write((page << 8) | offset, (void *)values, num_values); + if (ret != (int)num_values) { debug("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, ret); return -1; } + return OK; } @@ -1439,10 +1490,12 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_v } int ret = _interface->read((page << 8) | offset, reinterpret_cast(values), num_values); + if (ret != (int)num_values) { debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, ret); return -1; } + return OK; } @@ -1464,8 +1517,10 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t uint16_t value; ret = io_reg_get(page, offset, &value, 1); + if (ret != OK) return ret; + value &= ~clearbits; value |= setbits; @@ -1506,6 +1561,7 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) * even. */ unsigned total_len = sizeof(px4io_mixdata) + count; + if (total_len % 2) { msg->text[count] = '\0'; total_len++; @@ -1546,48 +1602,48 @@ PX4IO::print_status() { /* basic configuration */ printf("protocol %u hardware %u bootloader %u buffer %uB\n", - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER)); + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER)); printf("%u controls %u actuators %u R/C inputs %u analog inputs %u relays\n", - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT)); + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT)); /* status */ printf("%u bytes free\n", - io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM)); + io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM)); uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s\n", - flags, - ((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""), - ((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"), - ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""), - ((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"), - ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""), - (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_STATUS_FLAGS_RC_DSM11))) ? " DSM10" : ""), - (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_STATUS_FLAGS_RC_DSM11)) ? " DSM11" : ""), - ((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? " SBUS" : ""), - ((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"), - ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PWM_PASSTHROUGH" : ""), - ((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"), - ((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"), - ((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL"), - ((flags & PX4IO_P_STATUS_FLAGS_FAILSAFE) ? " FAILSAFE" : "")); + flags, + ((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"), + ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"), + ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""), + (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_STATUS_FLAGS_RC_DSM11))) ? " DSM10" : ""), + (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_STATUS_FLAGS_RC_DSM11)) ? " DSM11" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? " SBUS" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"), + ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PWM_PASSTHROUGH" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"), + ((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"), + ((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL"), + ((flags & PX4IO_P_STATUS_FLAGS_FAILSAFE) ? " FAILSAFE" : "")); uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS); printf("alarms 0x%04x%s%s%s%s%s%s%s%s\n", - alarms, - ((alarms & PX4IO_P_STATUS_ALARMS_VBATT_LOW) ? " VBATT_LOW" : ""), - ((alarms & PX4IO_P_STATUS_ALARMS_TEMPERATURE) ? " TEMPERATURE" : ""), - ((alarms & PX4IO_P_STATUS_ALARMS_SERVO_CURRENT) ? " SERVO_CURRENT" : ""), - ((alarms & PX4IO_P_STATUS_ALARMS_ACC_CURRENT) ? " ACC_CURRENT" : ""), - ((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST) ? " FMU_LOST" : ""), - ((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST) ? " RC_LOST" : ""), - ((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) ? " PWM_ERROR" : ""), - ((alarms & PX4IO_P_STATUS_ALARMS_VSERVO_FAULT) ? " VSERVO_FAULT" : "")); + alarms, + ((alarms & PX4IO_P_STATUS_ALARMS_VBATT_LOW) ? " VBATT_LOW" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_TEMPERATURE) ? " TEMPERATURE" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_SERVO_CURRENT) ? " SERVO_CURRENT" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_ACC_CURRENT) ? " ACC_CURRENT" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST) ? " FMU_LOST" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST) ? " RC_LOST" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) ? " PWM_ERROR" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_VSERVO_FAULT) ? " VSERVO_FAULT" : "")); /* now clear alarms */ io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, 0xFFFF); @@ -1600,87 +1656,107 @@ PX4IO::print_status() (double)_battery_amp_per_volt, (double)_battery_amp_bias, (double)_battery_mamphour_total); + } else if (_hardware == 2) { printf("vservo %u mV vservo scale %u\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VSERVO), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VSERVO_SCALE)); printf("vrssi %u\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VRSSI)); } + printf("actuators"); + for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i)); + printf("\n"); printf("servos"); + for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_SERVOS, i)); + printf("\n"); uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); printf("%d raw R/C inputs", raw_inputs); + for (unsigned i = 0; i < raw_inputs; i++) printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i)); + printf("\n"); uint16_t mapped_inputs = io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_VALID); printf("mapped R/C inputs 0x%04x", mapped_inputs); + for (unsigned i = 0; i < _max_rc_input; i++) { if (mapped_inputs & (1 << i)) printf(" %u:%d", i, REG_TO_SIGNED(io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_BASE + i))); } + printf("\n"); uint16_t adc_inputs = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT); printf("ADC inputs"); + for (unsigned i = 0; i < adc_inputs; i++) printf(" %u", io_reg_get(PX4IO_PAGE_RAW_ADC_INPUT, i)); + printf("\n"); /* setup and state */ printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES)); uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); printf("arming 0x%04x%s%s%s%s%s%s\n", - arming, - ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : " FMU_DISARMED"), - ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : " IO_ARM_DENIED"), - ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""), - ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE_CUSTOM" : ""), - ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""), - ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : "")); + arming, + ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : " FMU_DISARMED"), + ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : " IO_ARM_DENIED"), + ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE_CUSTOM" : ""), + ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : "")); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 printf("rates 0x%04x default %u alt %u relays 0x%04x\n", - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE), - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE), - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS)); + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS)); #endif #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 printf("rates 0x%04x default %u alt %u\n", - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE), - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE)); + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE)); #endif printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG)); printf("controls"); + for (unsigned i = 0; i < _max_controls; i++) printf(" %u", io_reg_get(PX4IO_PAGE_CONTROLS, i)); + printf("\n"); + for (unsigned i = 0; i < _max_rc_input; i++) { unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS); printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04x%s%s\n", - i, - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT), - options, - ((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""), - ((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : "")); + i, + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT), + options, + ((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""), + ((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : "")); } + printf("failsafe"); + for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); + printf("\nidle values"); + for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_IDLE_PWM, i)); + printf("\n"); } @@ -1719,27 +1795,29 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) case PWM_SERVO_SELECT_UPDATE_RATE: { - /* blindly clear the PWM update alarm - might be set for some other reason */ - io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR); - - /* attempt to set the rate map */ - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES, arg); - - /* check that the changes took */ - uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS); - if (alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) { - ret = -EINVAL; + /* blindly clear the PWM update alarm - might be set for some other reason */ io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR); + + /* attempt to set the rate map */ + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES, arg); + + /* check that the changes took */ + uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS); + + if (alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) { + ret = -EINVAL; + io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR); + } + + break; } - break; - } case PWM_SERVO_GET_COUNT: *(unsigned *)arg = _max_actuators; break; case DSM_BIND_START: - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); usleep(500000); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up); @@ -1755,68 +1833,80 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS - 1): { - /* TODO: we could go lower for e.g. TurboPWM */ - unsigned channel = cmd - PWM_SERVO_SET(0); - if ((channel >= _max_actuators) || (arg < 900) || (arg > 2100)) { - ret = -EINVAL; - } else { - /* send a direct PWM value */ - ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg); - } + /* TODO: we could go lower for e.g. TurboPWM */ + unsigned channel = cmd - PWM_SERVO_SET(0); - break; - } + if ((channel >= _max_actuators) || (arg < 900) || (arg > 2100)) { + ret = -EINVAL; + + } else { + /* send a direct PWM value */ + ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg); + } + + break; + } case PWM_SERVO_GET(0) ... PWM_SERVO_GET(PWM_OUTPUT_MAX_CHANNELS - 1): { - unsigned channel = cmd - PWM_SERVO_GET(0); + unsigned channel = cmd - PWM_SERVO_GET(0); + + if (channel >= _max_actuators) { + ret = -EINVAL; - if (channel >= _max_actuators) { - ret = -EINVAL; - } else { - /* fetch a current PWM value */ - uint32_t value = io_reg_get(PX4IO_PAGE_SERVOS, channel); - if (value == _io_reg_get_error) { - ret = -EIO; } else { - *(servo_position_t *)arg = value; + /* fetch a current PWM value */ + uint32_t value = io_reg_get(PX4IO_PAGE_SERVOS, channel); + + if (value == _io_reg_get_error) { + ret = -EIO; + + } else { + *(servo_position_t *)arg = value; + } } + + break; } - break; - } case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): { - unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0); + unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0); - *(uint32_t *)arg = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel); - if (*(uint32_t *)arg == _io_reg_get_error) - ret = -EIO; - break; - } + *(uint32_t *)arg = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel); + + if (*(uint32_t *)arg == _io_reg_get_error) + ret = -EIO; + + break; + } case GPIO_RESET: { #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 - uint32_t bits = (1 << _max_relays) - 1; - /* don't touch relay1 if it's controlling RX vcc */ - if (_dsm_vcc_ctl) - bits &= ~PX4IO_P_SETUP_RELAYS_POWER1; - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0); + uint32_t bits = (1 << _max_relays) - 1; + + /* don't touch relay1 if it's controlling RX vcc */ + if (_dsm_vcc_ctl) + bits &= ~PX4IO_P_SETUP_RELAYS_POWER1; + + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0); #endif #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 - ret = -EINVAL; + ret = -EINVAL; #endif - break; - } + break; + } case GPIO_SET: #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 arg &= ((1 << _max_relays) - 1); + /* don't touch relay1 if it's controlling RX vcc */ if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) { ret = -EINVAL; break; } + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg); #endif #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 @@ -1827,12 +1917,14 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) case GPIO_CLEAR: #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 arg &= ((1 << _max_relays) - 1); + /* don't touch relay1 if it's controlling RX vcc */ if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) { ret = -EINVAL; break; } - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0); + + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0); #endif #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 ret = -EINVAL; @@ -1842,8 +1934,10 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) case GPIO_GET: #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 *(uint32_t *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS); + if (*(uint32_t *)arg == _io_reg_get_error) ret = -EIO; + #endif #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 ret = -EINVAL; @@ -1859,53 +1953,60 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) break; case MIXERIOCLOADBUF: { - const char *buf = (const char *)arg; - ret = mixer_send(buf, strnlen(buf, 2048)); - break; - } + const char *buf = (const char *)arg; + ret = mixer_send(buf, strnlen(buf, 2048)); + break; + } case RC_INPUT_GET: { - uint16_t status; - rc_input_values *rc_val = (rc_input_values *)arg; + uint16_t status; + rc_input_values *rc_val = (rc_input_values *)arg; - ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &status, 1); - if (ret != OK) - break; + ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &status, 1); - /* if no R/C input, don't try to fetch anything */ - if (!(status & PX4IO_P_STATUS_FLAGS_RC_OK)) { - ret = -ENOTCONN; + if (ret != OK) + break; + + /* if no R/C input, don't try to fetch anything */ + if (!(status & PX4IO_P_STATUS_FLAGS_RC_OK)) { + ret = -ENOTCONN; + break; + } + + /* sort out the source of the values */ + if (status & PX4IO_P_STATUS_FLAGS_RC_PPM) { + rc_val->input_source = RC_INPUT_SOURCE_PX4IO_PPM; + + } else if (status & PX4IO_P_STATUS_FLAGS_RC_DSM) { + rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM; + + } else if (status & PX4IO_P_STATUS_FLAGS_RC_SBUS) { + rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SBUS; + + } else { + rc_val->input_source = RC_INPUT_SOURCE_UNKNOWN; + } + + /* read raw R/C input values */ + ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, &(rc_val->values[0]), _max_rc_input); break; } - /* sort out the source of the values */ - if (status & PX4IO_P_STATUS_FLAGS_RC_PPM) { - rc_val->input_source = RC_INPUT_SOURCE_PX4IO_PPM; - } else if (status & PX4IO_P_STATUS_FLAGS_RC_DSM) { - rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM; - } else if (status & PX4IO_P_STATUS_FLAGS_RC_SBUS) { - rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SBUS; - } else { - rc_val->input_source = RC_INPUT_SOURCE_UNKNOWN; - } - - /* read raw R/C input values */ - ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, &(rc_val->values[0]), _max_rc_input); - break; - } - case PX4IO_SET_DEBUG: /* set the debug level */ ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG, arg); break; case PX4IO_INAIR_RESTART_ENABLE: + /* set/clear the 'in-air restart' bit */ if (arg) { ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK); + } else { ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK, 0); } + break; default: @@ -1924,11 +2025,14 @@ PX4IO::write(file * /*filp*/, const char *buffer, size_t len) if (count > _max_actuators) count = _max_actuators; + if (count > 0) { int ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count); + if (ret != OK) return ret; } + return count * 2; } @@ -1936,6 +2040,7 @@ int PX4IO::set_update_rate(int rate) { int interval_ms = 1000 / rate; + if (interval_ms < UPDATE_INTERVAL_MIN) { interval_ms = UPDATE_INTERVAL_MIN; warnx("update rate too high, limiting interval to %d ms (%d Hz).", interval_ms, 1000 / interval_ms); @@ -1968,22 +2073,27 @@ get_interface() device::Device *interface = nullptr; #ifndef CONFIG_ARCH_BOARD_PX4FMU_V1 + /* try for a serial interface */ if (PX4IO_serial_interface != nullptr) interface = PX4IO_serial_interface(); + if (interface != nullptr) goto got; + #endif /* try for an I2C interface if we haven't got a serial one */ if (PX4IO_i2c_interface != nullptr) interface = PX4IO_i2c_interface(); + if (interface != nullptr) goto got; errx(1, "cannot alloc interface"); got: + if (interface->init() != OK) { delete interface; errx(1, "interface init failed"); @@ -2017,7 +2127,7 @@ start(int argc, char *argv[]) if (argc > 1) { if (!strcmp(argv[1], "norc")) { - if(g_dev->disable_rc_handling()) + if (g_dev->disable_rc_handling()) warnx("Failed disabling RC handling"); } else { @@ -2034,6 +2144,7 @@ start(int argc, char *argv[]) g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0); } } + #endif exit(0); } @@ -2061,6 +2172,7 @@ detect(int argc, char *argv[]) if (ret) { /* nonzero, error */ exit(1); + } else { exit(0); } @@ -2075,8 +2187,10 @@ bind(int argc, char *argv[]) errx(1, "px4io must be started first"); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + if (!g_dev->get_dsm_vcc_ctl()) errx(1, "DSM bind feature not enabled"); + #endif if (argc < 3) @@ -2086,9 +2200,10 @@ bind(int argc, char *argv[]) pulses = DSM2_BIND_PULSES; else if (!strcmp(argv[2], "dsmx")) pulses = DSMX_BIND_PULSES; - else + else errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]); - if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + + if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) errx(1, "system must not be armed"); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 @@ -2119,6 +2234,7 @@ test(void) /* tell IO that its ok to disable its safety with the switch */ ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); + if (ret != OK) err(1, "PWM_SERVO_SET_ARM_OK"); @@ -2135,22 +2251,27 @@ test(void) /* sweep all servos between 1000..2000 */ servo_position_t servos[servo_count]; + for (unsigned i = 0; i < servo_count; i++) servos[i] = pwm_value; ret = write(fd, servos, sizeof(servos)); + if (ret != (int)sizeof(servos)) err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret); if (direction > 0) { if (pwm_value < 2000) { pwm_value++; + } else { direction = -1; } + } else { if (pwm_value > 1000) { pwm_value--; + } else { direction = 1; } @@ -2162,6 +2283,7 @@ test(void) if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value)) err(1, "error reading PWM servo %d", i); + if (value != servos[i]) errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]); } @@ -2169,9 +2291,11 @@ test(void) /* Check if user wants to quit */ char c; ret = poll(&fds, 1, 0); + if (ret > 0) { read(0, &c, 1); + if (c == 0x03 || c == 0x63 || c == 'q') { warnx("User abort\n"); exit(0); @@ -2310,20 +2434,24 @@ px4io_main(int argc, char *argv[]) if ((argc > 2)) { g_dev->set_update_rate(atoi(argv[2])); + } else { errx(1, "missing argument (50 - 500 Hz)"); return 1; } + exit(0); } if (!strcmp(argv[1], "current")) { if ((argc > 3)) { g_dev->set_battery_current_scaling(atof(argv[2]), atof(argv[3])); + } else { errx(1, "missing argument (apm_per_volt, amp_offset)"); return 1; } + exit(0); } @@ -2340,10 +2468,12 @@ px4io_main(int argc, char *argv[]) /* set channel to commandline argument or to 900 for non-provided channels */ if (argc > i + 2) { - failsafe[i] = atoi(argv[i+2]); + failsafe[i] = atoi(argv[i + 2]); + if (failsafe[i] < 800 || failsafe[i] > 2200) { errx(1, "value out of range of 800 < value < 2200. Aborting."); } + } else { /* a zero value will result in stopping to output any pulse */ failsafe[i] = 0; @@ -2354,6 +2484,7 @@ px4io_main(int argc, char *argv[]) if (ret != OK) errx(ret, "failed setting failsafe values"); + exit(0); } @@ -2368,14 +2499,15 @@ px4io_main(int argc, char *argv[]) /* set values for first 8 channels, fill unassigned channels with 900. */ uint16_t min[8]; - for (unsigned i = 0; i < sizeof(min) / sizeof(min[0]); i++) - { + for (unsigned i = 0; i < sizeof(min) / sizeof(min[0]); i++) { /* set channel to commanline argument or to 900 for non-provided channels */ if (argc > i + 2) { - min[i] = atoi(argv[i+2]); + min[i] = atoi(argv[i + 2]); + if (min[i] < 900 || min[i] > 1200) { errx(1, "value out of range of 900 < value < 1200. Aborting."); } + } else { /* a zero value will the default */ min[i] = 0; @@ -2386,9 +2518,11 @@ px4io_main(int argc, char *argv[]) if (ret != OK) errx(ret, "failed setting min values"); + } else { errx(1, "not loaded"); } + exit(0); } @@ -2403,14 +2537,15 @@ px4io_main(int argc, char *argv[]) /* set values for first 8 channels, fill unassigned channels with 2100. */ uint16_t max[8]; - for (unsigned i = 0; i < sizeof(max) / sizeof(max[0]); i++) - { + for (unsigned i = 0; i < sizeof(max) / sizeof(max[0]); i++) { /* set channel to commanline argument or to 2100 for non-provided channels */ if (argc > i + 2) { - max[i] = atoi(argv[i+2]); + max[i] = atoi(argv[i + 2]); + if (max[i] < 1800 || max[i] > 2100) { errx(1, "value out of range of 1800 < value < 2100. Aborting."); } + } else { /* a zero value will the default */ max[i] = 0; @@ -2421,9 +2556,11 @@ px4io_main(int argc, char *argv[]) if (ret != OK) errx(ret, "failed setting max values"); + } else { errx(1, "not loaded"); } + exit(0); } @@ -2438,14 +2575,15 @@ px4io_main(int argc, char *argv[]) /* set values for first 8 channels, fill unassigned channels with 0. */ uint16_t idle[8]; - for (unsigned i = 0; i < sizeof(idle) / sizeof(idle[0]); i++) - { + for (unsigned i = 0; i < sizeof(idle) / sizeof(idle[0]); i++) { /* set channel to commanline argument or to 0 for non-provided channels */ if (argc > i + 2) { - idle[i] = atoi(argv[i+2]); + idle[i] = atoi(argv[i + 2]); + if (idle[i] < 900 || idle[i] > 2100) { errx(1, "value out of range of 900 < value < 2100. Aborting."); } + } else { /* a zero value will the default */ idle[i] = 0; @@ -2456,9 +2594,11 @@ px4io_main(int argc, char *argv[]) if (ret != OK) errx(ret, "failed setting idle values"); + } else { errx(1, "not loaded"); } + exit(0); } @@ -2467,7 +2607,7 @@ px4io_main(int argc, char *argv[]) /* * Enable in-air restart support. * We can cheat and call the driver directly, as it - * doesn't reference filp in ioctl() + * doesn't reference filp in ioctl() */ g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1); exit(0); @@ -2494,19 +2634,23 @@ px4io_main(int argc, char *argv[]) printf("usage: px4io debug LEVEL\n"); exit(1); } + if (g_dev == nullptr) { printf("px4io is not started\n"); exit(1); } + uint8_t level = atoi(argv[2]); /* we can cheat and call the driver directly, as it * doesn't reference filp in ioctl() */ int ret = g_dev->ioctl(nullptr, PX4IO_SET_DEBUG, level); + if (ret != 0) { printf("SET_DEBUG failed - %d\n", ret); exit(1); } + printf("SET_DEBUG %u OK\n", (unsigned)level); exit(0); } @@ -2527,6 +2671,6 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "bind")) bind(argc, argv); - out: +out: errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'failsafe', 'min, 'max',\n 'idle', 'bind' or 'update'"); } From 3fd20481888fd9e63806f988153abe4bf82e7a04 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Oct 2013 22:16:57 +0200 Subject: [PATCH 04/27] commander: remove duplicate publishing of vehicle_control_mode and actuator_armed topics, remove unused "counter" field from topics --- src/modules/commander/commander.cpp | 24 +++++-------------- .../commander/state_machine_helper.cpp | 1 - .../uORB/topics/vehicle_control_mode.h | 1 - 3 files changed, 6 insertions(+), 20 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 01b7b84d01..562790a7d8 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1181,31 +1181,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(); + hrt_abstime t1 = hrt_absolute_time(); + + if (navigation_state_changed || arming_state_changed) { + control_mode.flag_armed = armed.armed; // copy armed state to vehicle_control_mode topic + } + 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); - } - /* 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 8ce31550f9..e4d235a6bc 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -497,7 +497,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/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 093c6775d1..38fb74c9b9 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; From 5e3bdd77890c25b62e46f2f4f1238ac932801b12 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 8 Oct 2013 09:38:04 +0200 Subject: [PATCH 05/27] mavlink_onboard: major optimization, cleanup and minor fixes, WIP --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- src/modules/mavlink/orb_listener.c | 2 +- src/modules/mavlink_onboard/mavlink.c | 20 ++++++------- .../mavlink_onboard/mavlink_receiver.c | 29 +++++++++++-------- 4 files changed, 29 insertions(+), 24 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7dd9e321fb..b315f3efe2 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -733,7 +733,7 @@ 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) { diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index cec2fdc431..83c930a69f 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -695,7 +695,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 e713449823..0edb53a3e0 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 ] [-b ]\n" - " mavlink stop\n" - " mavlink status\n"); + fprintf(stderr, "usage: mavlink_onboard start [-d ] [-b ]\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 0236e61264..4f62b5fcc2 100644 --- a/src/modules/mavlink_onboard/mavlink_receiver.c +++ b/src/modules/mavlink_onboard/mavlink_receiver.c @@ -104,7 +104,7 @@ handle_message(mavlink_message_t *msg) //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); @@ -284,28 +284,33 @@ receive_thread(void *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()); + + struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } }; + + ssize_t nread = 0; while (!thread_should_exit) { - - struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } }; - 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 */ + ssize_t 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); + } } } From d70d8ae68e4a2971e714aa766cf92874d144a5f4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 8 Oct 2013 11:26:27 +0200 Subject: [PATCH 06/27] mavlink, mavlink_onboard: bugfixes, code style fixed --- src/modules/mavlink/mavlink_receiver.cpp | 72 ++++++++++++++++--- .../mavlink_onboard/mavlink_receiver.c | 65 +++++++++-------- 2 files changed, 98 insertions(+), 39 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index b315f3efe2..8243748dc8 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); } @@ -735,15 +779,21 @@ receive_thread(void *arg) prctl(PR_SET_NAME, "mavlink_uart_rcv", getpid()); + struct pollfd fds[1]; + fds[0].fd = uart_fd; + fds[0].events = POLLIN; + + ssize_t nread = 0; + while (!thread_should_exit) { - - struct pollfd fds[1]; - fds[0].fd = uart_fd; - fds[0].events = POLLIN; - 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_onboard/mavlink_receiver.c b/src/modules/mavlink_onboard/mavlink_receiver.c index 4f62b5fcc2..4658bcc1d4 100644 --- a/src/modules/mavlink_onboard/mavlink_receiver.c +++ b/src/modules/mavlink_onboard/mavlink_receiver.c @@ -100,7 +100,7 @@ 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 */ @@ -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; + case 0: + ml_armed = false; + break; - break; - case 2: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; - ml_armed = true; + case 1: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; + ml_armed = true; - break; - case 3: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; - break; - case 4: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; - break; + 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,7 +290,7 @@ 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 buf[32]; @@ -302,7 +311,7 @@ receive_thread(void *arg) } /* 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++) { @@ -324,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); From b2555d70e37d33d09459b0c73637f7efd6cd9a95 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 8 Oct 2013 13:39:08 +0200 Subject: [PATCH 07/27] sensors: minor optimization, cleanup and refactoring --- src/modules/sensors/sensors.cpp | 239 ++++++++++++++++---------------- 1 file changed, 118 insertions(+), 121 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 085da905c6..be599762fa 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -68,7 +68,6 @@ #include #include -#include #include #include @@ -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) { @@ -1233,12 +1233,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 */ @@ -1266,12 +1266,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 @@ -1303,17 +1303,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); @@ -1349,7 +1345,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++) { @@ -1359,6 +1355,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]; @@ -1619,7 +1616,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); } @@ -1637,11 +1634,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"); From 44deeb85c09451865c7d869f495a31f1b4348fec Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Oct 2013 21:15:03 +0200 Subject: [PATCH 08/27] We compile with GCC 4.7.4 --- .../att_pos_estimator_ekf/KalmanNav.hpp | 26 +++++++------- src/modules/controllib/block/BlockParam.hpp | 29 +++++++++++---- src/modules/controllib/blocks.hpp | 36 +++++++++---------- 3 files changed, 54 insertions(+), 37 deletions(-) diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp index f01ee03553..799fc2fb9e 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 _vGyro; /**< gyro process noise */ - control::BlockParam _vAccel; /**< accelerometer process noise */ - control::BlockParam _rMag; /**< magnetometer measurement noise */ - control::BlockParam _rGpsVel; /**< gps velocity measurement noise */ - control::BlockParam _rGpsPos; /**< gps position measurement noise */ - control::BlockParam _rGpsAlt; /**< gps altitude measurement noise */ - control::BlockParam _rPressAlt; /**< press altitude measurement noise */ - control::BlockParam _rAccel; /**< accelerometer measurement noise */ - control::BlockParam _magDip; /**< magnetic inclination with level */ - control::BlockParam _magDec; /**< magnetic declination, clockwise rotation */ - control::BlockParam _g; /**< gravitational constant */ - control::BlockParam _faultPos; /**< fault detection threshold for position */ - control::BlockParam _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/controllib/block/BlockParam.hpp b/src/modules/controllib/block/BlockParam.hpp index 58a9bfc0d9..36bc8c24ba 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 __EXPORT BlockParam : public BlockParamBase + +class __EXPORT BlockParamFloat : public BlockParamBase { public: - BlockParam(Block *block, const char *name, bool parent_prefix=true) : + BlockParamFloat(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; } + float get() { return _val; } + void set(float val) { _val = val; } void update() { if (_handle != PARAM_INVALID) param_get(_handle, &_val); } protected: - T _val; + float _val; +}; + +class __EXPORT BlockParamInt : public BlockParamBase +{ +public: + BlockParamInt(Block *block, const char *name, bool parent_prefix=true) : + BlockParamBase(block, name, parent_prefix), + _val() { + update(); + } + int get() { return _val; } + void set(int val) { _val = val; } + void update() { + if (_handle != PARAM_INVALID) param_get(_handle, &_val); + } +protected: + int _val; }; } // namespace control diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index fefe99702d..66e9290381 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 _min; - BlockParam _max; + control::BlockParamFloat _min; + control::BlockParamFloat _max; }; int __EXPORT blockLimitTest(); @@ -99,7 +99,7 @@ public: float getMax() { return _max.get(); } protected: // attributes - BlockParam _max; + control::BlockParamFloat _max; }; int __EXPORT blockLimitSymTest(); @@ -126,7 +126,7 @@ public: protected: // attributes float _state; - BlockParam _fCut; + control::BlockParamFloat _fCut; }; int __EXPORT blockLowPassTest(); @@ -157,7 +157,7 @@ protected: // attributes float _u; /**< previous input */ float _y; /**< previous output */ - BlockParam _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 _kP; + control::BlockParamFloat _kP; }; int __EXPORT blockPTest(); @@ -303,8 +303,8 @@ public: BlockIntegral &getIntegral() { return _integral; } private: BlockIntegral _integral; - BlockParam _kP; - BlockParam _kI; + control::BlockParamFloat _kP; + control::BlockParamFloat _kI; }; int __EXPORT blockPITest(); @@ -334,8 +334,8 @@ public: BlockDerivative &getDerivative() { return _derivative; } private: BlockDerivative _derivative; - BlockParam _kP; - BlockParam _kD; + control::BlockParamFloat _kP; + control::BlockParamFloat _kD; }; int __EXPORT blockPDTest(); @@ -372,9 +372,9 @@ private: // attributes BlockIntegral _integral; BlockDerivative _derivative; - BlockParam _kP; - BlockParam _kI; - BlockParam _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 _trim; + control::BlockParamFloat _trim; BlockLimit _limit; float _val; }; @@ -439,8 +439,8 @@ public: float getMax() { return _max.get(); } private: // attributes - BlockParam _min; - BlockParam _max; + control::BlockParamFloat _min; + control::BlockParamFloat _max; }; int __EXPORT blockRandUniformTest(); @@ -486,8 +486,8 @@ public: float getStdDev() { return _stdDev.get(); } private: // attributes - BlockParam _mean; - BlockParam _stdDev; + control::BlockParamFloat _mean; + control::BlockParamFloat _stdDev; }; int __EXPORT blockRandGaussTest(); From 42c412f6ddf3963c8a9bbef5ba37896a24e1f0fa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Oct 2013 21:15:44 +0200 Subject: [PATCH 09/27] Makefile cleanup --- makefiles/config_px4fmu-v1_default.mk | 5 ++++- makefiles/config_px4fmu-v2_default.mk | 6 +++++- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 1dd9490763..46640f3c51 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -134,7 +134,10 @@ MODULES += lib/geo # Tutorial code from # https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control -MODULES += examples/fixedwing_control +#MODULES += examples/fixedwing_control + +# Hardware test +#MODULES += examples/hwtest # # Transitional support - add commands from the NuttX export archive. diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 66216f8fe2..bb589cb9f3 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -128,8 +128,12 @@ MODULES += lib/geo # https://pixhawk.ethz.ch/px4/dev/debug_values #MODULES += examples/px4_mavlink_debug +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control +#MODULES += examples/fixedwing_control + # Hardware test -MODULES += examples/hwtest +#MODULES += examples/hwtest # # Transitional support - add commands from the NuttX export archive. From f1c399a60b3bf5a81740eab67016732714573dfa Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Sat, 12 Oct 2013 01:14:03 -0400 Subject: [PATCH 10/27] Add support for 8 channel DSMX sattelite pairing --- src/drivers/drv_pwm_output.h | 1 + src/drivers/px4io/px4io.cpp | 15 ++++++++++----- src/modules/px4iofirmware/dsm.c | 2 +- 3 files changed, 12 insertions(+), 6 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 94e923d71a..fc96bd8484 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -120,6 +120,7 @@ ORB_DECLARE(output_pwm); #define DSM2_BIND_PULSES 3 /* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */ #define DSMX_BIND_PULSES 7 /* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */ +#define DSMX8_BIND_PULSES 10 /* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */ /** Power up DSM receiver */ #define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 8) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index f6d4f1ed40..d9869bf943 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -82,6 +82,7 @@ #include #include #include + #include #include @@ -709,7 +710,6 @@ PX4IO::init() /* regular boot, no in-air restart, init IO */ } else { - /* dis-arm IO before touching anything */ io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED | @@ -1210,13 +1210,13 @@ PX4IO::dsm_bind_ioctl(int dsmMode) if (!(_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { /* 0: dsm2, 1:dsmx */ if ((dsmMode == 0) || (dsmMode == 1)) { - mavlink_log_info(_thread_mavlink_fd, "[IO] binding dsm%c rx", (dsmMode == 0) ? '2' : 'x'); - ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : DSMX_BIND_PULSES); + mavlink_log_info(_thread_mavlink_fd, "[IO] binding dsm%s rx", (dsmMode == 0) ? "2" : ((dsmMode == 1) ? "x" : "x8")); + ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : ((dsmMode == 1) ? DSMX_BIND_PULSES : DSMX8_BIND_PULSES)); } else { mavlink_log_info(_thread_mavlink_fd, "[IO] invalid dsm bind mode, bind request rejected"); } } else { - mavlink_log_info(_thread_mavlink_fd, "[IO] system armed, bind request rejected"); + mavlink_log_info(_thread_mavlink_fd, "[IO] system armed, bind request rejected"); } } @@ -1870,7 +1870,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) usleep(500000); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up); - usleep(50000); + usleep(72000); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4)); usleep(50000); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart); @@ -2213,8 +2213,13 @@ bind(int argc, char *argv[]) pulses = DSM2_BIND_PULSES; else if (!strcmp(argv[2], "dsmx")) pulses = DSMX_BIND_PULSES; + else if (!strcmp(argv[2], "dsmx8")) + pulses = DSMX8_BIND_PULSES; else errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]); + // Test for custom pulse parameter + if (argc > 3) + pulses = atoi(argv[3]); if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) errx(1, "system must not be armed"); diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index 4339766568..fd3b720150 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; From 4984ab4418e800db1af1f1fb63ff5b6d77aa8e89 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Oct 2013 20:14:47 +0200 Subject: [PATCH 11/27] Comment fix --- src/drivers/px4io/px4io_uploader.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index fe8561a0b8..d01dedb0dd 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -237,7 +237,7 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout) fds[0].fd = _io_fd; fds[0].events = POLLIN; - /* wait 100 ms for a character */ + /* wait ms for a character */ int ret = ::poll(&fds[0], 1, timeout); if (ret < 1) { From c5b890e87d545328734a815d90ce16d396630048 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Oct 2013 20:17:59 +0200 Subject: [PATCH 12/27] Moved mixer file load / compression into mixer library. --- src/modules/systemlib/mixer/mixer.cpp | 2 + src/modules/systemlib/mixer/mixer.h | 2 + src/modules/systemlib/mixer/mixer_load.c | 99 +++++++++++++++++++ src/modules/systemlib/mixer/mixer_load.h | 51 ++++++++++ .../systemlib/mixer/mixer_multirotor.cpp | 4 +- src/modules/systemlib/mixer/module.mk | 3 +- src/systemcmds/mixer/{mixer.c => mixer.cpp} | 55 ++--------- src/systemcmds/mixer/module.mk | 2 +- 8 files changed, 167 insertions(+), 51 deletions(-) create mode 100644 src/modules/systemlib/mixer/mixer_load.c create mode 100644 src/modules/systemlib/mixer/mixer_load.h rename src/systemcmds/mixer/{mixer.c => mixer.cpp} (73%) diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp index b1bb1a66d7..b4b82c539f 100644 --- a/src/modules/systemlib/mixer/mixer.cpp +++ b/src/modules/systemlib/mixer/mixer.cpp @@ -50,6 +50,8 @@ #include #include #include +#include +#include #include "mixer.h" diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index bbfa130a9d..6c322ff92e 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 #include "drivers/drv_mixer.h" +#include "mixer_load.h" /** * Abstract class defining a mixer mixing zero or more inputs to diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c new file mode 100644 index 0000000000..18c4e474a6 --- /dev/null +++ b/src/modules/systemlib/mixer/mixer_load.c @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include + +#include "mixer_load.h" + +int load_mixer_file(const char *fname, char *buf) +{ + FILE *fp; + char line[120]; + + /* open the mixer definition file */ + fp = fopen(fname, "r"); + if (fp == NULL) + err(1, "can't open %s", fname); + + /* 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 = buf; *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) >= sizeof(buf)) + break; + + /* 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 0000000000..b2327a4f72 --- /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 + +__BEGIN_DECLS + +__EXPORT int load_mixer_file(const char *fname, char *buf); + +__END_DECLS + +#endif diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 2446cc3fbe..89afc272c0 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 */ diff --git a/src/modules/systemlib/mixer/module.mk b/src/modules/systemlib/mixer/module.mk index 4d45e1c506..fc7485e202 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/systemcmds/mixer/mixer.c b/src/systemcmds/mixer/mixer.cpp similarity index 73% rename from src/systemcmds/mixer/mixer.c rename to src/systemcmds/mixer/mixer.cpp index e642ed0676..b1ebebbb44 100644 --- a/src/systemcmds/mixer/mixer.c +++ b/src/systemcmds/mixer/mixer.cpp @@ -47,10 +47,15 @@ #include #include -#include +#include #include -__EXPORT int mixer_main(int argc, char *argv[]); +/** + * Mixer utility for loading mixer files to devices + * + * @ingroup apps + */ +extern "C" __EXPORT int mixer_main(int argc, char *argv[]); static void usage(const char *reason) noreturn_function; static void load(const char *devname, const char *fname) noreturn_function; @@ -87,8 +92,6 @@ static void load(const char *devname, const char *fname) { int dev; - FILE *fp; - char line[120]; char buf[2048]; /* open the device */ @@ -99,49 +102,7 @@ load(const char *devname, const char *fname) if (ioctl(dev, MIXERIOCRESET, 0)) err(1, "can't reset mixers on %s", devname); - /* open the mixer definition file */ - fp = fopen(fname, "r"); - if (fp == NULL) - err(1, "can't open %s", fname); - - /* 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 = buf; *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) >= sizeof(buf)) - break; - - /* add the line to the buffer */ - strcat(buf, line); - } + load_mixer_file(fname, &buf[0]); /* XXX pass the buffer to the device */ int ret = ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf); diff --git a/src/systemcmds/mixer/module.mk b/src/systemcmds/mixer/module.mk index d5a3f9f77b..cdbff75f04 100644 --- a/src/systemcmds/mixer/module.mk +++ b/src/systemcmds/mixer/module.mk @@ -36,7 +36,7 @@ # MODULE_COMMAND = mixer -SRCS = mixer.c +SRCS = mixer.cpp MODULE_STACKSIZE = 4096 From 7d6a4ece6b1875bab62c4dbcb95fcc9fa5661674 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Oct 2013 20:19:09 +0200 Subject: [PATCH 13/27] Added mixer test --- src/systemcmds/tests/module.mk | 1 + src/systemcmds/tests/test_mixer.cpp | 74 +++++++++++++++++++++++++++++ src/systemcmds/tests/tests.h | 5 ++ src/systemcmds/tests/tests_main.c | 1 + 4 files changed, 81 insertions(+) create mode 100644 src/systemcmds/tests/test_mixer.cpp diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 754d3a0da9..6729ce4ae3 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -23,6 +23,7 @@ SRCS = test_adc.c \ test_uart_console.c \ test_uart_loopback.c \ test_uart_send.c \ + test_mixer.cpp \ tests_file.c \ tests_main.c \ tests_param.c diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp new file mode 100644 index 0000000000..acb7bd88f6 --- /dev/null +++ b/src/systemcmds/tests/test_mixer.cpp @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file test_mixer.hpp + * + * Mixer load test + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "tests.h" + +int test_mixer(int argc, char *argv[]) +{ + warnx("testing mixer"); + + char *filename = "/etc/mixers/IO_pass.mix"; + + if (argc > 1) + filename = argv[1]; + + warnx("loading: %s", filename); + + char buf[2048]; + + load_mixer_file(filename, &buf[0]); + + warnx("loaded: %s", &buf[0]); + + return 0; +} diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index c02ea6808f..c483108cf0 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -76,6 +76,8 @@ * Public Function Prototypes ****************************************************************************/ +__BEGIN_DECLS + extern int test_sensors(int argc, char *argv[]); extern int test_gpio(int argc, char *argv[]); extern int test_hrt(int argc, char *argv[]); @@ -98,5 +100,8 @@ extern int test_jig_voltages(int argc, char *argv[]); extern int test_param(int argc, char *argv[]); extern int test_bson(int argc, char *argv[]); extern int test_file(int argc, char *argv[]); +extern int test_mixer(int argc, char *argv[]); + +__END_DECLS #endif /* __APPS_PX4_TESTS_H */ diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 9f8c5c9eae..9eb9c632ec 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -110,6 +110,7 @@ const struct { {"param", test_param, 0}, {"bson", test_bson, 0}, {"file", test_file, 0}, + {"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} }; From 42b75ae8963b2f711a72ac1cb6cfd1b44bd826b2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Oct 2013 20:19:34 +0200 Subject: [PATCH 14/27] Added host-building mixer test --- Tools/tests-host/.gitignore | 2 ++ Tools/tests-host/Makefile | 36 +++++++++++++++++++++++++++++ Tools/tests-host/arch/board/board.h | 0 Tools/tests-host/mixer_test.cpp | 12 ++++++++++ Tools/tests-host/nuttx/config.h | 0 5 files changed, 50 insertions(+) create mode 100644 Tools/tests-host/.gitignore create mode 100644 Tools/tests-host/Makefile create mode 100644 Tools/tests-host/arch/board/board.h create mode 100644 Tools/tests-host/mixer_test.cpp create mode 100644 Tools/tests-host/nuttx/config.h diff --git a/Tools/tests-host/.gitignore b/Tools/tests-host/.gitignore new file mode 100644 index 0000000000..61e0915515 --- /dev/null +++ b/Tools/tests-host/.gitignore @@ -0,0 +1,2 @@ +./obj/* +mixer_test diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile new file mode 100644 index 0000000000..c603b2aa2a --- /dev/null +++ b/Tools/tests-host/Makefile @@ -0,0 +1,36 @@ + +CC=g++ +CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers -I../../src -D__EXPORT="" -Dnullptr="0" + +ODIR=obj +LDIR =../lib + +LIBS=-lm + +#_DEPS = test.h +#DEPS = $(patsubst %,$(IDIR)/%,$(_DEPS)) + +_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o mixer.o mixer_group.o +OBJ = $(patsubst %,$(ODIR)/%,$(_OBJ)) + +#$(DEPS) +$(ODIR)/%.o: %.cpp + $(CC) -c -o $@ $< $(CFLAGS) + +$(ODIR)/%.o: ../../src/systemcmds/tests/%.c + $(CC) -c -o $@ $< $(CFLAGS) + +$(ODIR)/%.o: ../../src/modules/systemlib/%.cpp + $(CC) -c -o $@ $< $(CFLAGS) + +$(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.cpp + $(CC) -c -o $@ $< $(CFLAGS) + +# +mixer_test: $(OBJ) + g++ -o $@ $^ $(CFLAGS) $(LIBS) + +.PHONY: clean + +clean: + rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ \ No newline at end of file diff --git a/Tools/tests-host/arch/board/board.h b/Tools/tests-host/arch/board/board.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/Tools/tests-host/mixer_test.cpp b/Tools/tests-host/mixer_test.cpp new file mode 100644 index 0000000000..5d92040f11 --- /dev/null +++ b/Tools/tests-host/mixer_test.cpp @@ -0,0 +1,12 @@ +#include +#include + +extern int test_mixer(int argc, char *argv[]); + +int main(int argc, char *argv[]) { + warnx("Host execution started"); + + char* args[] = {argv[0], "../../ROMFS/px4fmu_common/mixers/IO_pass.mix"}; + + test_mixer(2, args); +} \ No newline at end of file diff --git a/Tools/tests-host/nuttx/config.h b/Tools/tests-host/nuttx/config.h new file mode 100644 index 0000000000..e69de29bb2 From 1dc9569e31717aefab8e05b858122f433dab1698 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Oct 2013 11:44:26 +0200 Subject: [PATCH 15/27] Fixed mixer chunk load and line ending detection for good. --- src/modules/systemlib/mixer/mixer.cpp | 27 ++++ src/modules/systemlib/mixer/mixer.h | 23 ++++ src/modules/systemlib/mixer/mixer_group.cpp | 15 ++ src/modules/systemlib/mixer/mixer_load.c | 15 +- src/modules/systemlib/mixer/mixer_load.h | 2 +- .../systemlib/mixer/mixer_multirotor.cpp | 10 +- src/modules/systemlib/mixer/mixer_simple.cpp | 62 +++------ src/systemcmds/mixer/mixer.cpp | 2 +- src/systemcmds/tests/test_mixer.cpp | 129 +++++++++++++++++- 9 files changed, 229 insertions(+), 56 deletions(-) diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp index b4b82c539f..cce46bf5fc 100644 --- a/src/modules/systemlib/mixer/mixer.cpp +++ b/src/modules/systemlib/mixer/mixer.cpp @@ -116,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 6c322ff92e..723bf9f3b7 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -212,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: }; @@ -240,6 +258,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. * diff --git a/src/modules/systemlib/mixer/mixer_group.cpp b/src/modules/systemlib/mixer/mixer_group.cpp index 1dbc512cdb..3ed99fba09 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 index 18c4e474a6..a55ddf8a35 100644 --- a/src/modules/systemlib/mixer/mixer_load.c +++ b/src/modules/systemlib/mixer/mixer_load.c @@ -38,22 +38,22 @@ */ #include -#include #include #include #include #include "mixer_load.h" -int load_mixer_file(const char *fname, char *buf) +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) - err(1, "can't open %s", fname); + if (fp == NULL) { + return 1; + } /* read valid lines from the file into a buffer */ buf[0] = '\0'; @@ -70,7 +70,7 @@ int load_mixer_file(const char *fname, char *buf) /* compact whitespace in the buffer */ char *t, *f; - for (f = buf; *f != '\0'; f++) { + for (f = line; *f != '\0'; f++) { /* scan for space characters */ if (*f == ' ') { /* look for additional spaces */ @@ -87,8 +87,9 @@ int load_mixer_file(const char *fname, char *buf) } /* if the line is too long to fit in the buffer, bail */ - if ((strlen(line) + strlen(buf) + 1) >= sizeof(buf)) - break; + if ((strlen(line) + strlen(buf) + 1) >= maxlen) { + return 1; + } /* add the line to the buffer */ strcat(buf, line); diff --git a/src/modules/systemlib/mixer/mixer_load.h b/src/modules/systemlib/mixer/mixer_load.h index b2327a4f72..4b7091d5b2 100644 --- a/src/modules/systemlib/mixer/mixer_load.h +++ b/src/modules/systemlib/mixer/mixer_load.h @@ -44,7 +44,7 @@ __BEGIN_DECLS -__EXPORT int load_mixer_file(const char *fname, char *buf); +__EXPORT int load_mixer_file(const char *fname, char *buf, unsigned maxlen); __END_DECLS diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 89afc272c0..b89f341b60 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -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 c8434f991b..c3985b5de6 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/systemcmds/mixer/mixer.cpp b/src/systemcmds/mixer/mixer.cpp index b1ebebbb44..6da39d371b 100644 --- a/src/systemcmds/mixer/mixer.cpp +++ b/src/systemcmds/mixer/mixer.cpp @@ -102,7 +102,7 @@ load(const char *devname, const char *fname) if (ioctl(dev, MIXERIOCRESET, 0)) err(1, "can't reset mixers on %s", devname); - load_mixer_file(fname, &buf[0]); + load_mixer_file(fname, &buf[0], sizeof(buf)); /* XXX pass the buffer to the device */ int ret = ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf); diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index acb7bd88f6..4da86042d2 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -46,6 +46,7 @@ #include #include #include +#include #include #include @@ -53,6 +54,11 @@ #include "tests.h" +static int mixer_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &control); + int test_mixer(int argc, char *argv[]) { warnx("testing mixer"); @@ -66,9 +72,128 @@ int test_mixer(int argc, char *argv[]) char buf[2048]; - load_mixer_file(filename, &buf[0]); + load_mixer_file(filename, &buf[0], sizeof(buf)); + unsigned loaded = strlen(buf); - warnx("loaded: %s", &buf[0]); + warnx("loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded); + + /* load the mixer in chunks, like + * in the case of a remote load, + * e.g. on PX4IO. + */ + + unsigned nused = 0; + + const unsigned chunk_size = 64; + + MixerGroup mixer_group(mixer_callback, 0); + + /* load at once test */ + unsigned xx = loaded; + mixer_group.load_from_buf(&buf[0], xx); + warnx("complete buffer load: loaded %u mixers", mixer_group.count()); + if (mixer_group.count() != 8) + return 1; + + unsigned empty_load = 2; + char empty_buf[2]; + empty_buf[0] = ' '; + empty_buf[1] = '\0'; + mixer_group.reset(); + mixer_group.load_from_buf(&empty_buf[0], empty_load); + warnx("empty buffer load: loaded %u mixers, used: %u", mixer_group.count(), empty_load); + if (empty_load != 0) + return 1; + + /* FIRST mark the mixer as invalid */ + bool mixer_ok = false; + /* THEN actually delete it */ + mixer_group.reset(); + char mixer_text[256]; /* large enough for one mixer */ + unsigned mixer_text_length = 0; + + unsigned transmitted = 0; + + warnx("transmitted: %d, loaded: %d", transmitted, loaded); + + while (transmitted < loaded) { + + unsigned text_length = (loaded - transmitted > chunk_size) ? chunk_size : loaded - transmitted; + + /* check for overflow - this would be really fatal */ + if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { + bool mixer_ok = false; + return 1; + } + + /* append mixer text and nul-terminate */ + memcpy(&mixer_text[mixer_text_length], &buf[transmitted], text_length); + mixer_text_length += text_length; + mixer_text[mixer_text_length] = '\0'; + warnx("buflen %u, text:\n\"%s\"", mixer_text_length, &mixer_text[0]); + + /* process the text buffer, adding new mixers as their descriptions can be parsed */ + unsigned resid = mixer_text_length; + mixer_group.load_from_buf(&mixer_text[0], resid); + + /* if anything was parsed */ + if (resid != mixer_text_length) { + + /* only set mixer ok if no residual is left over */ + if (resid == 0) { + mixer_ok = true; + } else { + /* not yet reached the end of the mixer, set as not ok */ + mixer_ok = false; + } + + warnx("used %u", mixer_text_length - resid); + + /* copy any leftover text to the base of the buffer for re-use */ + if (resid > 0) + memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid); + + mixer_text_length = resid; + } + + transmitted += text_length; + } + + warnx("chunked load: loaded %u mixers", mixer_group.count()); + + if (mixer_group.count() != 8) + return 1; + + /* load multirotor at once test */ + mixer_group.reset(); + + if (argc > 2) + filename = argv[2]; + else + filename = "/etc/mixers/FMU_quad_w.mix"; + + load_mixer_file(filename, &buf[0], sizeof(buf)); + loaded = strlen(buf); + + warnx("loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded); + + unsigned mc_loaded = loaded; + mixer_group.load_from_buf(&buf[0], mc_loaded); + warnx("complete buffer load: loaded %u mixers", mixer_group.count()); + if (mixer_group.count() != 8) + return 1; +} + +static int +mixer_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &control) +{ + if (control_group != 0) + return -1; + + control = 0.0f; return 0; } From 5671df0af4e258ef0a83377cdbd50e59734aa00b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Oct 2013 11:44:42 +0200 Subject: [PATCH 16/27] Improved mixer tests --- Tools/tests-host/Makefile | 7 +++++-- Tools/tests-host/mixer_test.cpp | 8 ++++---- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile index c603b2aa2a..97410ff47c 100644 --- a/Tools/tests-host/Makefile +++ b/Tools/tests-host/Makefile @@ -10,14 +10,14 @@ LIBS=-lm #_DEPS = test.h #DEPS = $(patsubst %,$(IDIR)/%,$(_DEPS)) -_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o mixer.o mixer_group.o +_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o mixer.o mixer_group.o mixer_load.o OBJ = $(patsubst %,$(ODIR)/%,$(_OBJ)) #$(DEPS) $(ODIR)/%.o: %.cpp $(CC) -c -o $@ $< $(CFLAGS) -$(ODIR)/%.o: ../../src/systemcmds/tests/%.c +$(ODIR)/%.o: ../../src/systemcmds/tests/%.cpp $(CC) -c -o $@ $< $(CFLAGS) $(ODIR)/%.o: ../../src/modules/systemlib/%.cpp @@ -26,6 +26,9 @@ $(ODIR)/%.o: ../../src/modules/systemlib/%.cpp $(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.cpp $(CC) -c -o $@ $< $(CFLAGS) +$(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.c + $(CC) -c -o $@ $< $(CFLAGS) + # mixer_test: $(OBJ) g++ -o $@ $^ $(CFLAGS) $(LIBS) diff --git a/Tools/tests-host/mixer_test.cpp b/Tools/tests-host/mixer_test.cpp index 5d92040f11..042322aadc 100644 --- a/Tools/tests-host/mixer_test.cpp +++ b/Tools/tests-host/mixer_test.cpp @@ -1,12 +1,12 @@ #include #include - -extern int test_mixer(int argc, char *argv[]); +#include "../../src/systemcmds/tests/tests.h" int main(int argc, char *argv[]) { warnx("Host execution started"); - char* args[] = {argv[0], "../../ROMFS/px4fmu_common/mixers/IO_pass.mix"}; + char* args[] = {argv[0], "../../ROMFS/px4fmu_common/mixers/IO_pass.mix", + "../../ROMFS/px4fmu_common/mixers/FMU_quad_w.mix"}; - test_mixer(2, args); + test_mixer(3, args); } \ No newline at end of file From 8aaf285ac5eca73853300d38c8121e9c2757fef2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Oct 2013 12:38:26 +0200 Subject: [PATCH 17/27] Python exception handling from muggenhor --- Tools/px_uploader.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 64af672a34..a2d7d371df 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -430,7 +430,7 @@ while True: # Windows, don't open POSIX ports if not "/" in port: up = uploader(port, args.baud) - except: + except Exception: # open failed, rate-limit our attempts time.sleep(0.05) @@ -443,7 +443,7 @@ while True: up.identify() print("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port)) - except: + except Exception: # most probably a timeout talking to the port, no bootloader, try to reboot the board print("attempting reboot on %s..." % port) up.send_reboot() From f475ffda04e49255d99eb7d7da537432d00c8151 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Oct 2013 15:54:02 +0200 Subject: [PATCH 18/27] Set better default gains for Iris and F330 --- ROMFS/px4fmu_common/init.d/10_dji_f330 | 6 +++--- ROMFS/px4fmu_common/init.d/16_3dr_iris | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10_dji_f330 b/ROMFS/px4fmu_common/init.d/10_dji_f330 index d21759507d..b8fe5fc31c 100644 --- a/ROMFS/px4fmu_common/init.d/10_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/10_dji_f330 @@ -10,12 +10,12 @@ then # Set all params here, then disable autoconfig param set SYS_AUTOCONFIG 0 - param set MC_ATTRATE_D 0.002 + param set MC_ATTRATE_D 0.004 param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.09 + param set MC_ATTRATE_P 0.12 param set MC_ATT_D 0.0 param set MC_ATT_I 0.0 - param set MC_ATT_P 6.8 + param set MC_ATT_P 7.0 param set MC_YAWPOS_D 0.0 param set MC_YAWPOS_I 0.0 param set MC_YAWPOS_P 2.0 diff --git a/ROMFS/px4fmu_common/init.d/16_3dr_iris b/ROMFS/px4fmu_common/init.d/16_3dr_iris index d47ecb3932..3ac086b915 100644 --- a/ROMFS/px4fmu_common/init.d/16_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/16_3dr_iris @@ -10,12 +10,12 @@ then # Set all params here, then disable autoconfig param set SYS_AUTOCONFIG 0 - param set MC_ATTRATE_D 0.006 + param set MC_ATTRATE_D 0.004 param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.17 + param set MC_ATTRATE_P 0.13 param set MC_ATT_D 0.0 param set MC_ATT_I 0.0 - param set MC_ATT_P 5.0 + param set MC_ATT_P 9.0 param set MC_YAWPOS_D 0.0 param set MC_YAWPOS_I 0.15 param set MC_YAWPOS_P 0.5 From af7288ed936d642f3141a3e8792799909d351885 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Oct 2013 09:56:57 +0200 Subject: [PATCH 19/27] Enable position control for Easystar type planes --- ROMFS/px4fmu_common/init.d/100_mpx_easystar | 3 +-- ROMFS/px4fmu_common/init.d/101_hk_bixler | 3 +-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/100_mpx_easystar b/ROMFS/px4fmu_common/init.d/100_mpx_easystar index 828540ad90..1aa1ef494e 100644 --- a/ROMFS/px4fmu_common/init.d/100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/100_mpx_easystar @@ -98,8 +98,7 @@ else mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix fi fw_att_control start -# Not ready yet for prime-time -#fw_pos_control_l1 start +fw_pos_control_l1 start if [ $EXIT_ON_END == yes ] then diff --git a/ROMFS/px4fmu_common/init.d/101_hk_bixler b/ROMFS/px4fmu_common/init.d/101_hk_bixler index 5a9cb2171a..4d8a24c4ae 100644 --- a/ROMFS/px4fmu_common/init.d/101_hk_bixler +++ b/ROMFS/px4fmu_common/init.d/101_hk_bixler @@ -91,8 +91,7 @@ att_pos_estimator_ekf start # mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix fw_att_control start -# Not ready yet for prime-time -#fw_pos_control_l1 start +fw_pos_control_l1 start if [ $EXIT_ON_END == yes ] then From 57b8dee7092eb84e8f4f31dcee85736eedb2ca8f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Oct 2013 13:41:37 +0200 Subject: [PATCH 20/27] Bring back proper log conversion copy operation --- src/modules/sdlog2/sdlog2.c | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index ec8033202d..080aa550cb 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) From c6b58491bbd7390650723c65b4d4e9ec0922c8de Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Oct 2013 22:18:44 +0200 Subject: [PATCH 21/27] Work queue in RGB driver without work_cancel() --- src/drivers/rgbled/rgbled.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index fedff769b8..aeb7e2f787 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -248,6 +248,11 @@ RGBLED::led_trampoline(void *arg) void RGBLED::led() { + if (!_should_run) { + _running = false; + return; + } + switch (_mode) { case RGBLED_MODE_BLINK_SLOW: case RGBLED_MODE_BLINK_NORMAL: @@ -471,11 +476,6 @@ RGBLED::set_mode(rgbled_mode_t mode) work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1); } - /* if it should stop, then cancel the workq */ - if (!should_run && _running) { - _running = false; - work_cancel(LPWORK, &_work); - } } } From fbe595a591734bffa95d28125b8e0bda117d7314 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Oct 2013 23:10:12 +0200 Subject: [PATCH 22/27] Fixed some stupid compile errors --- src/drivers/rgbled/rgbled.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index aeb7e2f787..ea87b37d94 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -103,6 +103,7 @@ private: bool _running; int _led_interval; + bool _should_run; int _counter; void set_color(rgbled_color_t ledcolor); @@ -136,6 +137,7 @@ RGBLED::RGBLED(int bus, int rgbled) : _brightness(1.0f), _running(false), _led_interval(0), + _should_run(false), _counter(0) { memset(&_work, 0, sizeof(_work)); @@ -414,10 +416,10 @@ RGBLED::set_mode(rgbled_mode_t mode) { if (mode != _mode) { _mode = mode; - bool should_run = false; switch (mode) { case RGBLED_MODE_OFF: + _should_run = false; send_led_enable(false); break; @@ -428,7 +430,7 @@ RGBLED::set_mode(rgbled_mode_t mode) break; case RGBLED_MODE_BLINK_SLOW: - should_run = true; + _should_run = true; _counter = 0; _led_interval = 2000; _brightness = 1.0f; @@ -436,7 +438,7 @@ RGBLED::set_mode(rgbled_mode_t mode) break; case RGBLED_MODE_BLINK_NORMAL: - should_run = true; + _should_run = true; _counter = 0; _led_interval = 500; _brightness = 1.0f; @@ -444,7 +446,7 @@ RGBLED::set_mode(rgbled_mode_t mode) break; case RGBLED_MODE_BLINK_FAST: - should_run = true; + _should_run = true; _counter = 0; _led_interval = 100; _brightness = 1.0f; @@ -452,14 +454,14 @@ RGBLED::set_mode(rgbled_mode_t mode) break; case RGBLED_MODE_BREATHE: - should_run = true; + _should_run = true; _counter = 0; _led_interval = 25; send_led_enable(true); break; case RGBLED_MODE_PATTERN: - should_run = true; + _should_run = true; _counter = 0; _brightness = 1.0f; send_led_enable(true); @@ -471,7 +473,7 @@ RGBLED::set_mode(rgbled_mode_t mode) } /* if it should run now, start the workq */ - if (should_run && !_running) { + if (_should_run && !_running) { _running = true; work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1); } From 39336fd58533c85ef6bad93b80dd51e62b6eba3a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Oct 2013 10:46:28 +0200 Subject: [PATCH 23/27] Better defaults for RC SCALE --- ROMFS/px4fmu_common/init.d/1000_rc_fw.hil | 2 ++ ROMFS/px4fmu_common/init.d/100_mpx_easystar | 2 ++ ROMFS/px4fmu_common/init.d/101_hk_bixler | 2 ++ ROMFS/px4fmu_common/init.d/31_io_phantom | 2 ++ 4 files changed, 8 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil index 6e29bd6f80..5e4028bbb8 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil @@ -33,6 +33,8 @@ then param set FW_T_SINK_MIN 4.0 param set FW_Y_ROLLFF 1.1 param set FW_L1_PERIOD 16 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 param set SYS_AUTOCONFIG 0 param save diff --git a/ROMFS/px4fmu_common/init.d/100_mpx_easystar b/ROMFS/px4fmu_common/init.d/100_mpx_easystar index 1aa1ef494e..4f843e9aa1 100644 --- a/ROMFS/px4fmu_common/init.d/100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/100_mpx_easystar @@ -29,6 +29,8 @@ then param set FW_T_SINK_MIN 4.0 param set FW_Y_ROLLFF 1.1 param set FW_L1_PERIOD 16 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 param set SYS_AUTOCONFIG 0 param save diff --git a/ROMFS/px4fmu_common/init.d/101_hk_bixler b/ROMFS/px4fmu_common/init.d/101_hk_bixler index 4d8a24c4ae..cef86c34d0 100644 --- a/ROMFS/px4fmu_common/init.d/101_hk_bixler +++ b/ROMFS/px4fmu_common/init.d/101_hk_bixler @@ -29,6 +29,8 @@ then param set FW_T_SINK_MIN 4.0 param set FW_Y_ROLLFF 1.1 param set FW_L1_PERIOD 16 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 param set SYS_AUTOCONFIG 0 param save diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom index 8fe94452f8..6528337454 100644 --- a/ROMFS/px4fmu_common/init.d/31_io_phantom +++ b/ROMFS/px4fmu_common/init.d/31_io_phantom @@ -29,6 +29,8 @@ then param set FW_T_SINK_MIN 4.0 param set FW_Y_ROLLFF 1.1 param set FW_L1_PERIOD 17 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 param set SYS_AUTOCONFIG 0 param save From 8ed0796448e49ae34dbfdf31c056e402834c0b8f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Oct 2013 22:17:53 +0200 Subject: [PATCH 24/27] L1 control diagram illustrating corner case --- Documentation/l1_control.odg | Bin 0 -> 11753 bytes 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 Documentation/l1_control.odg diff --git a/Documentation/l1_control.odg b/Documentation/l1_control.odg new file mode 100644 index 0000000000000000000000000000000000000000..69910c677459aae4664a08b1bbeb6aaed48721ad GIT binary patch literal 11753 zcmeHtbzGEd)Bg&BfOJYIEgefY(jwhR!?H9i>@Hn`Gzf?wAkxw$-5^RRh)8#LhjjiH z&+*acJf7!w-uLf!{4CykUo+pCYwqiwz2=%xRY1N;2mqh~03lk6QmM%7tt*v4e+-GzI71zTzS|?@U}_JB3Bf@^zZxd=E4R>h zMSt9Jv$J-v6{WF)!ySbA6$47K~tWe(y1 z+kvmA4&wl_137;1UR(H2jk@Oj)g(tK7={4+?&p7s@vF?QSP%&0Uq)a1;}3UzH=T=< zlZWGv??0@D*<0KFQ;pZWY#@leBf?HIJMgbcO&zT5|0&Eh@AsMh3SbE}H%A!qpAQ9L zGdG7Nhq)=-l+DE&>`ME?@;~PB-)AfMm)V*_O_*Vs*fkLs5pV@D{&1`L98fVl)sxqv)uTxMoq zbAB#1PChQ6Ihc>lRFI#SO8~?sV8O}9Xa1+^pIZLV`mc8zcD?XnaBC3NcUM8ouCWmd z&DjAiO2f@Z!yzW}=W`VL!}CAB{R`py?LPt#8XDU7h=vGa{~}Dp6$}i&b|dUM2uW^P{*3dBL}#D(%V2 z-=Zn2_Pq-RZI1e5x>QPT2)o~)X4;GF7bp^Dcqh8gWDI|uvzq3z<+XO{82U}{AZ;^~ z1m&HUlE1n~h)8dLtzC(+>+6R*eE9O@j6o|jrgSFHp?Wv;N2b=#1+x;?*lsaHU+Xe=EnSlQ>i4@vV^ObbCOji}iS^Lp$>^*(LFcS)#Si z{D;Tu@fpXJPXc&5z1>iJ<8RMxbU#{JIVLQiA(j%^cl{juXyu_}-Fn@F!$46JK`(%zFlZx!Hbioe1S57SDI#^Eoasg#TaZ5 zMq9ILdPw4CL$&{*^KpAFjv8+`Y5Bt`{4MTiQQ{X`fwQ)fI#*D(k7gNBWCiA)4tKB^ z88F&EYKKnCp8BnkHf^+OmW;DorWS+|Ws!5yuo#}W?1lzJS?Y|Tl|t(rW=pZN)1tDNt@T75_onIAd+KnvUv|=C zc*9wHx^E^ZThEa1WFp~!@iAOibY{ZR8`>WV*Y@Ndi3I96&-YI-8wA~_Ug1DDen(pj zAtq{F2vDP3xYJ78KAL30#P>}9Gqz$ZJG1mgka7%+?Z_3`JIba6PJuieD-lt)XlreK zF5bu;%YV6(JUd~;bo3kzy>^Iytt#+}atFC8UXs#k)8pd~^y96fs$f2UUf^le86!Y~ zFbqSm*bhY01l5gvw|ii$+c9i21dI)x;pO3noIK0lQ|Wcp4wdSWW|j@xN?=>46U}lBo4hoYnA)ra=Ra>?R-mG7AfV} z=!z*$?WOA;U37*Cs^sw16LTY6wt93B8!fSaK$kq>O)dIix)`5uv`m}fO>mtQI%`OJ z@#`g!qalHudFAx|F=>JiHQ9meWgl6~3Bm1o?CezP5$L&AI!rzgh?k_%OxLN#(XA;< z#sYx?f+)`1C9c;GD4V**Z-@sr6|2-Cp=zK>x}<3@TAobY&Z$32#d4Us>V#jRsw&(< z!&QkJ!$3jo`Dg)vzmK7}5it}5ae!ZM`4Ay=({LO*{t?f+M@P}LJnvwv<})TLGUH3l z`pU|TrTH+21|XYqX2&Q13e0r^1%D42WgXf$w!0XxR3l_DaRf5{t zCM7h>MHnBTIOxzVRRq=XHJ;Z!R{T<&)OIhS(Uw2;wWWG(D)T^U3iH7B82jX&hl-c| zg(%u$*O_ZlLub6k8GfB;Zi7hY{b`G=$x4;}3TLf5nokYu6fi#w6!hHPJjgv2it41= z&h^Y(OCk4Un_GpFSfvF!e^q@-{(Ntf3jYD2CuWFIsQ%*n>G6H>>E^SxdhC~5vVuZ| zg|hN$b)QfZ%}Co`WCH3uV}_J6KIKutZ%Hwbe0odbl)WtRatTbH=IDX<#OPsouXAn0 zkw!u^Uw-(+k!!xcu;wixWG*#GhRMaVz5?a>&{aG7>^G-U_to?CZ_8r6O~0M|QkhDN zKaj<^qLNopC)Btt9f`B1+g}8qhX197Wh1c%5k1CSkj2=gD=%_Neiu?1wye&pWaq*& z6lUWoWn1Nl+yR_w{RuOY5=N_97B|YP^xKKdYuuV?bva*P=xXe*f}_LW7>$iH#hsew z#W|hK>IMx(`JrAigWK;{VQoEc)s4{%@9?@h=H++UEU_cWOhf8E-pUT2=q{!$seN5r z>&6p9Inenq!g{*fJIlJ7PmTK>BWnwmP{Ro$!RDnZ)-9j z5N@kd2!KyJ~wju{0Fhg|mP9?Nnkjv#=GD`@2numWP$(6CUQA|TMH zHdEhL+1L0c3^CrLhVMnwo|8JGF%v#5hIiKgEXkMV3j-K?2ZZ$5Sn`SQv#tp&Pc<OvTk>=Z)y(D$Oaf z-a9%L&!&*Be0^z$G8IN`tGSsfVwEH3or&WUz@;6P7lnCs&0>6#3FWO!vw`WY!HST4 z-fc4!v(^nroMm{9C~;5ndnJ)g&o&q*KwLp2RU~!MM}vaMd2z#Z2b;w(P@o2VuI`;t z(VPII8a?4~x_8#8Zm4Eyi(~QmcJq}raGj@5LU}AiP;0};?@dvg6fIRowO1x%bbs=eS&Qj^6o+AO+LFM6Gzx`bDfB2#5x(^y0oJ z6O<45OiN>)N+-Etg@)HHA|FtgNiD5{Pw6A#r5u~HnMrM)!l)uFC%wYsPDU>GG{pO{ z^d#PJ&)DhI=8T?W+^Oqku*dY#T#>;NUVZbRzsi9fyV$%-pi}u0J%99iU|t3$wAS9B z%LhZ!l_*?3LeuKip*G5KCh@QXF(!HJc;}{vCd7MFiZ*mo*eL;Hs!!{2k!6)Buy)dJ zf%mp%jUfdKjGrmL|AD(+9r#I;MyPCJ;N5;W<_2!35p^Ds0@JW!hdzy5Ijc#mPWL%o z|GFo7jIGndwqQZ+I?T>zMk<){z@rU>V$hc>V zPE08E)_phoj0QUlQGw9~ zPB!TMZRWkj=v9p7dnQAkbX&8AJth-r@=MHiiOdY+g{n3*I} z@hz*opo&iqd}UCBX@A^ObFs3ojv_r5xY_-zD>`GXgHXKjW0(w&NsNHufr=Hm zD7Bw8o@zWB$^KO9M>nv<=m4V=MbKF-;N(ljae`vs)Ja4GL+@- z#)->l57O?A_Vyo*d<_+JkgMKgm-06PeTi@qhE=E1(98$fjimd*xg5;x$v+jkvF(8- zqPN$Sil8^kdYFI%k36F54Vb!R zO~=+8Kedijk9_zPZsUPEt*>#%!pETB^uW^+Ot@F2$eqQ26662KtKfa6LWM!7VZ12x zs47MWa{&L_liGVNBMWK~N?5R@Qor`Q>v)CsVOWf6Lrh}!Lly}FWWYQr!SsOcLecWW z&tu%S;vZm#-VA1$5tCm&gnZ-u@Zc-pNlf0^`-y$kjE!|ec6myZdYMu4$J8IHFyK zrueLewq=&?AAE7{NRHC2Jx-s(CMN41avYl2&*t@pb#QQHar*Uj2)$K(hu356TvFk$ z3H6|QMOGx?r3<}qBl`rleuIoLd?y9HKnBM)NKm(V{0nQgpyElhlUEFtHMw2ikhn_}T zM+t`Yi!bwLE5?Qob8K6o;F3_%vQOcu2ENGK(7dTPt{0|wqHak<0tSzLhF-8N&Yx!MoFO#y%jUQ1x%oK1BY{m@&<=MWFT6( z6PJjz{~;)Uz$LAFYlTk!aKI}Px&M;AfbKCxo8KQ{_G4fj)u$>5?fMvmGG8Odk+&#s2ID(ibiQAMK z-6OcKK7*El%ZFkjv^SR|&|2|0ZPEPN z6GdxI!%7CmyprN;=$dmBw!OpSo0fgwG#raxXB7q6a#nW{2QPilA6dnZv*yKzx+W>8 z0KoO<4kD}2v~spLb1=2GgK@xrT(UblSca-9%VA-VVIWwrjE6#6zS`Xj((HH2a#;FfsyMIi1r% z_Xg7Jd&O?@t~!$vkduvr?;XAJRGO0Rl&beusMy1OiHsk&M5m^v-dlH_)=#$O>+eMw zhl`=|2G^d~9TW|5b?eu44YLnF6GJ_S1oMh6fZy-MXGYzdRD(=$4Rr5&*5e{zLD`hz zK5)qS<;B@$l)1*Kn?!l&6rn6h4TZO&R^-J>G(Sl~+4;#T$bgrnL1<)g@0-t#=04Eu z6OZbJNlkFPOB_yml({^`;7EL?krLhH*@@eg+tv2P`mhe{WX?Q&1@!ejR>%@34!t@v z0n?Sfj{0j>LT|gQ6!9S9Kut9KXZ#D4(t)F+Hhq;lS8XLSJnj1)tvR z{a#R}VSjq1qV1TKkQc`F{Kxo2iwBb>>*{NAgr*CkKmp0IdoOLm-{@^q)Q9zU_{bW& zxqO{}v|m~@Mf-+8!|I->aG2O-=}^0zfa6p>NxxpUyVV;3xJz$`R}tD$vf`)UUhGuK z{nCfYZ86;Tja~dXxZ^oGgZ1QnlM;8yW*S)Z??aDW3dEMk$!fPYi?GGS#S8pW_x6tH zqZ|djKl6K_77yuVPl#zH+vGApF&2L!GKeN=Tpz`C0k^x zfVfvck)r(Ma@7_=O_iOr107vMLcV;S=Z6is8kF|#1m?5T-l^gDv_+5$qd<0by>I74 z*E2g5Jc0d{Td;5e?PeA4eey@gFr0mxs__Vxi={AhIjq}Y4OmU#$J9VaD zXuHR_4;&ZG4puuI=~=uVgD%wH$?l@yL!A-W~3QxUrtTSEw#MqtAXI?rHE^p7{`|c(cX}tO}CAaQA zDvVFLO=;ZRq)nO5l<6~!V0EONM8=uK>Kn3@s|qSQw>(%nJQELvh}aKAu4Z1oUd{Nn za67(WxR&Quc37r+^|0SD5VK;j4k^Q2WHcm#t$r6KSlj{a-h)Jp7KuvsE zub$j?@+w!!cbI#g@#RtPWUcOL$o;GK&8_C6t2sX>!h+9~^%t}GM@dsBY@aBFVVW-w zOM56Cs#SK@x5P1wAPTE$_e~ z+mRD{Om9x+RfN9)7Z85ija!_g*KzMl60{*%$>ef+S2d37p53(%oNSPOAFLoYigl(d zNx^osF+a%k#$7of+w)NEvtd_)h@Qi^oTxpu??nE`5)wkWlB6Tk z7qIlCBVcEWg3C9t`;m=tUC~KLYOT_CiIH`_l!2?JescRjY5vT6!yVWxZc{$7~uH^Pn( zM`uSw!O{7DC>N!Uz;^#bx$An2GtA5s`d{QIerG2XVu>hZ{(mH|fAat8o@-10 zyT0E^UAyF`)b9#E48PH33IZYOx8KWM93W>X^v?rLWW?F$t?NoP;Cg#;O@IH8erJq@ z9RyJU{Nb|S8FRgnfx+Do)wu69DE;op`5L^YWhP8EzK7TqJ{u|4!u^dS8)l_9Tu@uS zY}869#J1giM97#wnWFQNI+$XQ*!^jPVswLCn0O46e%WF~y=R~BJG0h&`~opz@v_cW z{L>-}c2E^*8F#1o!2Wkh(KJiwY(xB09F#r}hNt+eo83S3$F`d%?$msdr&gn{j?g6F zWY5pSn{umUd`Y2Gp+_vhFsR z1rH11xp5Eat|Da@yLWbwiECnJQM8G0l)mva65-VIYS`ss>dM-N@FlHU@GY8c28Xx3 zAMP8Ptr=voZK=@4KMTO)eY_al{}lp<(d_D8#TKutt1Yw8$Vxq>7tWO3C1zl>0cdptL`q-%k^?z!2Fl72Zop6cRGzgG%l zn#9&DO

yd{$-KBM33nLbWKjb_j9GK5s16nwR3-p{lKyLhc z3rl)StFzi^Irdp38T?kvO%gcI{-Yx*tw1DS&i5?F(J{A2kzyhl3=`;Wm}p-Jl4oPm z+iMQf$a`qDBq!qR-$){v)F?aYTz`$)L10=MkM>GWCDDpL-ZP3Uz^?>8ws||RRG6`~ zPP9|qh$Qs!Eo3Tp#?Z7!0f~J2E>mA;1M**e5_1$g-(VqCU&i9h7u|ae;@fjb;vS^P zs)oc?za4Iqqq%>&-I+|jtxqi=eWSHPlk@7i_=H;vU4nFrIdOuM$xD#{AYpzfD~6bR zleaNF%GEQkZ>lRW5@bJcsEViAF`6`tD?7j^p=hww3V@)ojVwgq=N2S(L1GY5t-t8c{68F zT1kwB!~i82ZxhIIu|L@{2M9+E&Y(y6nySxK%)6AN7EQg$gl8wU@YG<;H{7l)Aj8V5 zlHi@kjTbB?h?#pe+BMS}k#%Rp$#cF4g`Jzs^I5UCY}S)xX1c%ywLII>P{DaeTSk(Qzks~LCmbq0U7_)rxz8U(@mAVinSbv2>{~r&qG5>&XBk`rL4qa;4`}!s zLf&4^wHAw7pKt}Ei&^BaM!fEFd@oYnBX4t$guYwJE2#8xsIMd}SeyD^lM-g;z`Gh-RFGS^;S!EI3CmciD z1>swFu@*s@CL)yz4$1cBsb#WejSe@b8`ohtM% zi&j*&8w|Ic58YqDx3c-Av)pJ}Xm-EkAO$nbioWh`g{_)O5hy8!#VVHu2}-o+)c92g z_2({@M~Va`=VB0*UkG8mRf#Rvjt*L_m@OpXQ^p-f& zTh|SqWweIox8v>jLFprkW4R+u+Ojm`C&fr<^9F8g+`egO8QIGEAc(-5ItN`<(-OA@ zoJE5#3h!OPsBV1Us8#2k8SaSMfguzlRZRA@UWmKXN{d@|p%rbgMyzc=6OUTUbJFeV z^vrlX0A@6|@Ve@gwcj1YL95}q&K6#^O3_)v$vy3z3qE6~rDUo4pxetjr!g~>;ro~r z89nbLQbVj{((ch=p>g1(kGA1rD5W$D1T2wAubj%zz@32s8LQ_qH2Z62Xx&D1VkI2G zJ+~KE*TboKeA7i5Yp5hSUgUh3F~c>y@Wrd7ts+Vi*L1sU|LWa@>Sm$VyX>&H^Z|G6 zm_xR5^VI7=_I_r;&g-Zf?_B5mP6ypA~< zq}LLja`~}Z)uz15Me@DvI64s2y+NZx!0X0CpSauU)QFlEuu6S!(pb8}w_Q%ELZ?}X zhA|@Pnru+$;lXxw(qVjg(bFs{Q_^NKi{UJ*Z_SU!&_s)ImQ2#$b($Ax?a#37q?T;w zU|2)Mjcdk2i4A=qlXK%!7iN+dR6{!H_IpNoPiXM;QI>M5|D;C!#UeaV8UFNmO$WaV zEACzW5=q2VwrW4DE#smPaJK?UtQx|DRf4J~=XY6>!#i-f1AUXXf9j_3X`{F$rSG%q zM6DH>tec_$+u}rj*jlVlEZ^5FTtVDgn`}|#l^o~?p2K}*(oMwgiAwa5#hK5`c$O5q z)c*c?R|Zd`CL|k4N8*NXDOc8S1)9~{o-PSXQ~Qgk?(H&Vo;Y?6!?d$$ zeS!haU!_xedLEe*sUK4Su`a{#YMQG>MxB5@B#Fpw8&K(G4!QjmOI1$=G#Bw6_z;!S zyj$sU*^tTS`~2k_!{PmoX=ZjPqU)YUy(5PEqmP5-WgRoNKJ@gkGNe^oeaWO2$aEBu zUAB)v&9qFJ=RJc%p@x7$N%VdK?XE zM&384UGujTDKcQp=lKI8)za%VO|f))WXg?e4nQ6HX|J=MD7s=ey@ zt+zjt#pR^7%V=3cae=kC8QC32q4|okZ{m{VdY5#gQ+=c6x^s~G$1dr2`yC?0en&}0 zQ-V!iS(d{d(e-5khJCLc7iqLOe4xT?JYu2Q@+-L>8w|gO_rTqp3wOob|tR9V!LTK>Yy=s>;Xz=|K`--d^9>H#3)py zIDuR%D!dYrZ;knti^!i>PtBTuVdI!Jfx~%^I^Pk|k5wugjI7>J=oTD5qiDulf~537 z)9azit)Y9?V+W%(!5|mo+z2Iqc4{1f32LA+7FG5s^GnyLMpEc&dqw^93e?;52Wl%6 z^V3NeQR^-Bk4PENMaYZe(O52Wdf#Vm@y`uYPI@C>J2fA$*nxio0GL9)cIpizLcrg* zXI^hlf6|X;&A&?hwFmR3gc{=Vx@YQoBl<(?$0Pi;9pJig>L>LfdO3e>qx!4luSb5@ z?E*iE7_s;LrFr0o;NP7H{uHz${`>R6zoPt<0syXau%Gnc?ti8n|B;dXyT^YD0|3`K z*-!HR4cM<4+CRmq{0+{p8QVX_+58R8uNmCm<6P%(KZ*X{e>w@#=l4rS_xDKG3DHk7 z{teR44DX+!#Qg^4XU6vj%8xaBt@}sXcAfM6q^{qf{7nY<-`3N=Pml(&y#AWP{>=aW z-Lzk0<#h)5lXMX%Kl8yK+J8*x*KlwhNq^Eq#=nWHstPEGBXs}(7x9k|@vbH_U4Q$3 DeGk#2 literal 0 HcmV?d00001 From 4532eca4ef6f6170765062ccd2ca7f29814e0b3a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Oct 2013 22:55:16 +0200 Subject: [PATCH 25/27] Covered corner case in L1 controller not adressed so far in existing concepts --- src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 33 +++++++++++++++++------- 1 file changed, 23 insertions(+), 10 deletions(-) diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp index c5c0c7a3c1..daf136d495 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -130,8 +130,12 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c float alongTrackDist = vector_A_to_airplane * vector_AB; /* estimate airplane position WRT to B */ - math::Vector2f vector_B_to_airplane_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized(); - float bearing_wp_b = atan2f(-vector_B_to_airplane_unit.getY() , -vector_B_to_airplane_unit.getX()); + math::Vector2f vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized(); + + /* calculate angle of airplane position vector relative to line) */ + + // XXX this could probably also be based solely on the dot product + float AB_to_BP_bearing = atan2f(vector_B_to_P_unit % vector_AB, vector_B_to_P_unit * vector_AB); /* extension from [2], fly directly to A */ if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane , 1.0f) < -0.7071f) { @@ -148,21 +152,30 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c /* bearing from current position to L1 point */ _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); -// XXX this can be useful as last-resort guard, but is currently not needed -#if 0 - } else if (absf(bearing_wp_b) > math::radians(80.0f)) { - /* extension, fly back to waypoint */ + /* + * If the AB vector and the vector from B to airplane point in the same + * direction, we have missed the waypoint. At +- 90 degrees we are just passing it. + */ + } else if (fabsf(AB_to_BP_bearing) < math::radians(100.0f)) { + /* + * Extension, fly back to waypoint. + * + * This corner case is possible if the system was following + * the AB line from waypoint A to waypoint B, then is + * switched to manual mode (or otherwise misses the waypoint) + * and behind the waypoint continues to follow the AB line. + */ /* calculate eta to fly to waypoint B */ /* velocity across / orthogonal to line */ - xtrack_vel = ground_speed_vector % (-vector_B_to_airplane_unit); + xtrack_vel = ground_speed_vector % (-vector_B_to_P_unit); /* velocity along line */ - ltrack_vel = ground_speed_vector * (-vector_B_to_airplane_unit); + ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit); eta = atan2f(xtrack_vel, ltrack_vel); /* bearing from current position to L1 point */ - _nav_bearing = bearing_wp_b; -#endif + _nav_bearing = atan2f(-vector_B_to_P_unit.getY() , -vector_B_to_P_unit.getX()); + } else { /* calculate eta to fly along the line between A and B */ From 3ca94e7943fc76053114ab97d4b63dc6902fd5bf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 17 Oct 2013 13:38:21 +0200 Subject: [PATCH 26/27] Prevent warnings by data conversion --- mavlink/include/mavlink/v1.0/mavlink_conversions.h | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/mavlink/include/mavlink/v1.0/mavlink_conversions.h b/mavlink/include/mavlink/v1.0/mavlink_conversions.h index d893635770..51afac87c3 100644 --- a/mavlink/include/mavlink/v1.0/mavlink_conversions.h +++ b/mavlink/include/mavlink/v1.0/mavlink_conversions.h @@ -9,6 +9,10 @@ #endif #include +#ifndef M_PI_2 + #define M_PI_2 ((float)asin(1)) +#endif + /** * @file mavlink_conversions.h * @@ -49,12 +53,12 @@ MAVLINK_HELPER void mavlink_dcm_to_euler(const float dcm[3][3], float* roll, flo float phi, theta, psi; theta = asin(-dcm[2][0]); - if (fabs(theta - M_PI_2) < 1.0e-3f) { + if (fabsf(theta - (float)M_PI_2) < 1.0e-3f) { phi = 0.0f; - psi = (atan2(dcm[1][2] - dcm[0][1], + psi = (atan2f(dcm[1][2] - dcm[0][1], dcm[0][2] + dcm[1][1]) + phi); - } else if (fabs(theta + M_PI_2) < 1.0e-3f) { + } else if (fabsf(theta + (float)M_PI_2) < 1.0e-3f) { phi = 0.0f; psi = atan2f(dcm[1][2] - dcm[0][1], dcm[0][2] + dcm[1][1] - phi); @@ -128,4 +132,4 @@ MAVLINK_HELPER void mavlink_euler_to_dcm(float roll, float pitch, float yaw, flo dcm[2][2] = cosPhi * cosThe; } -#endif \ No newline at end of file +#endif From 0f67c5cbb0f69e5b9dda4e4e75120e63e466e1f8 Mon Sep 17 00:00:00 2001 From: Alexander Lourier Date: Fri, 18 Oct 2013 03:47:15 +0400 Subject: [PATCH 27/27] Parameters list generator --- Tools/px4params/README.md | 9 ++ Tools/px4params/dokuwikiout.py | 27 ++++ Tools/px4params/output.py | 5 + Tools/px4params/parser.py | 220 +++++++++++++++++++++++++++ Tools/px4params/px_process_params.py | 61 ++++++++ Tools/px4params/scanner.py | 34 +++++ Tools/px4params/xmlout.py | 22 +++ src/modules/sensors/sensor_params.c | 26 ++++ 8 files changed, 404 insertions(+) create mode 100644 Tools/px4params/README.md create mode 100644 Tools/px4params/dokuwikiout.py create mode 100644 Tools/px4params/output.py create mode 100644 Tools/px4params/parser.py create mode 100755 Tools/px4params/px_process_params.py create mode 100644 Tools/px4params/scanner.py create mode 100644 Tools/px4params/xmlout.py diff --git a/Tools/px4params/README.md b/Tools/px4params/README.md new file mode 100644 index 0000000000..a23b447995 --- /dev/null +++ b/Tools/px4params/README.md @@ -0,0 +1,9 @@ +h1. PX4 Parameters Processor + +It's designed to scan PX4 source codes, find declarations of tunable parameters, +and generate the list in various formats. + +Currently supported formats are: + +* XML for the parametric UI generator +* Human-readable description in DokuWiki format diff --git a/Tools/px4params/dokuwikiout.py b/Tools/px4params/dokuwikiout.py new file mode 100644 index 0000000000..33f76b415e --- /dev/null +++ b/Tools/px4params/dokuwikiout.py @@ -0,0 +1,27 @@ +import output + +class DokuWikiOutput(output.Output): + def Generate(self, groups): + result = "" + for group in groups: + result += "==== %s ====\n\n" % group.GetName() + for param in group.GetParams(): + code = param.GetFieldValue("code") + name = param.GetFieldValue("short_desc") + if code != name: + name = "%s (%s)" % (name, code) + result += "=== %s ===\n\n" % name + long_desc = param.GetFieldValue("long_desc") + if long_desc is not None: + result += "%s\n\n" % long_desc + min_val = param.GetFieldValue("min") + if min_val is not None: + result += "* Minimal value: %s\n" % min_val + max_val = param.GetFieldValue("max") + if max_val is not None: + result += "* Maximal value: %s\n" % max_val + def_val = param.GetFieldValue("default") + if def_val is not None: + result += "* Default value: %s\n" % def_val + result += "\n" + return result diff --git a/Tools/px4params/output.py b/Tools/px4params/output.py new file mode 100644 index 0000000000..c092468713 --- /dev/null +++ b/Tools/px4params/output.py @@ -0,0 +1,5 @@ +class Output(object): + def Save(self, groups, fn): + data = self.Generate(groups) + with open(fn, 'w') as f: + f.write(data) diff --git a/Tools/px4params/parser.py b/Tools/px4params/parser.py new file mode 100644 index 0000000000..251be672f2 --- /dev/null +++ b/Tools/px4params/parser.py @@ -0,0 +1,220 @@ +import sys +import re + +class ParameterGroup(object): + """ + Single parameter group + """ + def __init__(self, name): + self.name = name + self.params = [] + + def AddParameter(self, param): + """ + Add parameter to the group + """ + self.params.append(param) + + def GetName(self): + """ + Get parameter group name + """ + return self.name + + def GetParams(self): + """ + Returns the parsed list of parameters. Every parameter is a Parameter + object. Note that returned object is not a copy. Modifications affect + state of the parser. + """ + return sorted(self.params, + cmp=lambda x, y: cmp(x.GetFieldValue("code"), + y.GetFieldValue("code"))) + +class Parameter(object): + """ + Single parameter + """ + + # Define sorting order of the fields + priority = { + "code": 10, + "type": 9, + "short_desc": 8, + "long_desc": 7, + "default": 6, + "min": 5, + "max": 4, + # all others == 0 (sorted alphabetically) + } + + def __init__(self): + self.fields = {} + + def SetField(self, code, value): + """ + Set named field value + """ + self.fields[code] = value + + def GetFieldCodes(self): + """ + Return list of existing field codes in convenient order + """ + return sorted(self.fields.keys(), + cmp=lambda x, y: cmp(self.priority.get(y, 0), + self.priority.get(x, 0)) or cmp(x, y)) + + def GetFieldValue(self, code): + """ + Return value of the given field code or None if not found. + """ + return self.fields.get(code) + +class Parser(object): + """ + Parses provided data and stores all found parameters internally. + """ + + re_split_lines = re.compile(r'[\r\n]+') + re_comment_start = re.compile(r'^\/\*\*') + re_comment_content = re.compile(r'^\*\s*(.*)') + re_comment_tag = re.compile(r'@([a-zA-Z][a-zA-Z0-9_]*)\s*(.*)') + re_comment_end = re.compile(r'(.*?)\s*\*\/') + re_parameter_definition = re.compile(r'PARAM_DEFINE_([A-Z_][A-Z0-9_]*)\s*\(([A-Z_][A-Z0-9_]*)\s*,\s*([^ ,\)]+)\s*\)\s*;') + re_cut_type_specifier = re.compile(r'[a-z]+$') + re_is_a_number = re.compile(r'^-?[0-9\.]') + re_remove_dots = re.compile(r'\.+$') + + valid_tags = set(["min", "max", "group"]) + + # Order of parameter groups + priority = { + # All other groups = 0 (sort alphabetically) + "Miscellaneous": -10 + } + + def __init__(self): + self.param_groups = {} + + def GetSupportedExtensions(self): + """ + Returns list of supported file extensions that can be parsed by this + parser. + """ + return ["cpp", "c"] + + def Parse(self, contents): + """ + Incrementally parse program contents and append all found parameters + to the list. + """ + # This code is essentially a comment-parsing grammar. "state" + # represents parser state. It contains human-readable state + # names. + state = None + for line in self.re_split_lines.split(contents): + line = line.strip() + # Ignore empty lines + if line == "": + continue + if self.re_comment_start.match(line): + state = "wait-short" + short_desc = None + long_desc = None + tags = {} + elif state is not None and state != "comment-processed": + m = self.re_comment_end.search(line) + if m: + line = m.group(1) + last_comment_line = True + else: + last_comment_line = False + m = self.re_comment_content.match(line) + if m: + comment_content = m.group(1) + if comment_content == "": + # When short comment ends with empty comment line, + # start waiting for the next part - long comment. + if state == "wait-short-end": + state = "wait-long" + else: + m = self.re_comment_tag.match(comment_content) + if m: + tag, desc = m.group(1, 2) + tags[tag] = desc + current_tag = tag + state = "wait-tag-end" + elif state == "wait-short": + # Store first line of the short description + short_desc = comment_content + state = "wait-short-end" + elif state == "wait-short-end": + # Append comment line to the short description + short_desc += "\n" + comment_content + elif state == "wait-long": + # Store first line of the long description + long_desc = comment_content + state = "wait-long-end" + elif state == "wait-long-end": + # Append comment line to the long description + long_desc += "\n" + comment_content + elif state == "wait-tag-end": + # Append comment line to the tag text + tags[current_tag] += "\n" + comment_content + else: + raise AssertionError( + "Invalid parser state: %s" % state) + elif not last_comment_line: + # Invalid comment line (inside comment, but not starting with + # "*" or "*/". Reset parsed content. + state = None + if last_comment_line: + state = "comment-processed" + else: + # Non-empty line outside the comment + m = self.re_parameter_definition.match(line) + if m: + tp, code, defval = m.group(1, 2, 3) + # Remove trailing type specifier from numbers: 0.1f => 0.1 + if self.re_is_a_number.match(defval): + defval = self.re_cut_type_specifier.sub('', defval) + param = Parameter() + param.SetField("code", code) + param.SetField("short_desc", code) + param.SetField("type", tp) + param.SetField("default", defval) + # If comment was found before the parameter declaration, + # inject its data into the newly created parameter. + group = "Miscellaneous" + if state == "comment-processed": + if short_desc is not None: + param.SetField("short_desc", + self.re_remove_dots.sub('', short_desc)) + if long_desc is not None: + param.SetField("long_desc", long_desc) + for tag in tags: + if tag == "group": + group = tags[tag] + elif tag not in self.valid_tags: + sys.stderr.write("Skipping invalid" + "documentation tag: '%s'\n" % tag) + else: + param.SetField(tag, tags[tag]) + # Store the parameter + if group not in self.param_groups: + self.param_groups[group] = ParameterGroup(group) + self.param_groups[group].AddParameter(param) + # Reset parsed comment. + state = None + + def GetParamGroups(self): + """ + Returns the parsed list of parameters. Every parameter is a Parameter + object. Note that returned object is not a copy. Modifications affect + state of the parser. + """ + return sorted(self.param_groups.values(), + cmp=lambda x, y: cmp(self.priority.get(y.GetName(), 0), + self.priority.get(x.GetName(), 0)) or cmp(x.GetName(), + y.GetName())) diff --git a/Tools/px4params/px_process_params.py b/Tools/px4params/px_process_params.py new file mode 100755 index 0000000000..cdf5aba7c0 --- /dev/null +++ b/Tools/px4params/px_process_params.py @@ -0,0 +1,61 @@ +#!/usr/bin/env python +############################################################################ +# +# Copyright (C) 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# PX4 paramers processor (main executable file) +# +# It scans src/ subdirectory of the project, collects all parameters +# definitions, and outputs list of parameters in XML and DokuWiki formats. +# + +import scanner +import parser +import xmlout +import dokuwikiout + +# Initialize parser +prs = parser.Parser() + +# Scan directories, and parse the files +sc = scanner.Scanner() +sc.ScanDir("../../src", prs) +output = prs.GetParamGroups() + +# Output into XML +out = xmlout.XMLOutput() +out.Save(output, "parameters.xml") + +# Output into DokuWiki +out = dokuwikiout.DokuWikiOutput() +out.Save(output, "parameters.wiki") diff --git a/Tools/px4params/scanner.py b/Tools/px4params/scanner.py new file mode 100644 index 0000000000..b5a1af47c3 --- /dev/null +++ b/Tools/px4params/scanner.py @@ -0,0 +1,34 @@ +import os +import re + +class Scanner(object): + """ + Traverses directory tree, reads all source files, and passes their contents + to the Parser. + """ + + re_file_extension = re.compile(r'\.([^\.]+)$') + + def ScanDir(self, srcdir, parser): + """ + Scans provided path and passes all found contents to the parser using + parser.Parse method. + """ + extensions = set(parser.GetSupportedExtensions()) + for dirname, dirnames, filenames in os.walk(srcdir): + for filename in filenames: + m = self.re_file_extension.search(filename) + if m: + ext = m.group(1) + if ext in extensions: + path = os.path.join(dirname, filename) + self.ScanFile(path, parser) + + def ScanFile(self, path, parser): + """ + Scans provided file and passes its contents to the parser using + parser.Parse method. + """ + with open(path, 'r') as f: + contents = f.read() + parser.Parse(contents) diff --git a/Tools/px4params/xmlout.py b/Tools/px4params/xmlout.py new file mode 100644 index 0000000000..d56802b158 --- /dev/null +++ b/Tools/px4params/xmlout.py @@ -0,0 +1,22 @@ +import output +from xml.dom.minidom import getDOMImplementation + +class XMLOutput(output.Output): + def Generate(self, groups): + impl = getDOMImplementation() + xml_document = impl.createDocument(None, "parameters", None) + xml_parameters = xml_document.documentElement + for group in groups: + xml_group = xml_document.createElement("group") + xml_group.setAttribute("name", group.GetName()) + xml_parameters.appendChild(xml_group) + for param in group.GetParams(): + xml_param = xml_document.createElement("parameter") + xml_group.appendChild(xml_param) + for code in param.GetFieldCodes(): + value = param.GetFieldValue(code) + xml_field = xml_document.createElement(code) + xml_param.appendChild(xml_field) + xml_value = xml_document.createTextNode(value) + xml_field.appendChild(xml_value) + return xml_document.toprettyxml(indent=" ", newl="\n", encoding="utf-8") diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 4d719e0e15..8875f65fc6 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -44,8 +44,34 @@ #include +/** + * 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);