diff options
98 files changed, 6629 insertions, 2294 deletions
diff --git a/Debug/ARMv7M b/Debug/ARMv7M new file mode 100644 index 000000000..803d96453 --- /dev/null +++ b/Debug/ARMv7M @@ -0,0 +1,164 @@ +# +# Assorted ARMv7M macros +# + +echo Loading ARMv7M GDB macros. Use 'help armv7m' for more information.\n + +define armv7m + echo Use 'help armv7m' for more information.\n +end + +document armv7m +. Various macros for working with the ARMv7M family of processors. +. +. vecstate +. Print information about the current exception handling state. +. +. Use 'help <macro>' for more specific help. +end + + +define vecstate + set $icsr = *(unsigned *)0xe000ed04 + set $vect = $icsr & 0x1ff + set $pend = ($icsr & 0x1ff000) >> 12 + set $shcsr = *(unsigned *)0xe000ed24 + set $cfsr = *(unsigned *)0xe000ed28 + set $mmfsr = $cfsr & 0xff + set $bfsr = ($cfsr >> 8) & 0xff + set $ufsr = ($cfsr >> 16) & 0xffff + set $hfsr = *(unsigned *)0xe000ed2c + set $bfar = *(unsigned *)0xe000ed38 + set $mmfar = *(unsigned *)0xe000ed34 + + if $vect < 15 + + if $hfsr != 0 + printf "HardFault:" + if $hfsr & (1<<1) + printf " due to vector table read fault\n" + end + if $hfsr & (1<<30) + printf " forced due to escalated or disabled configurable fault (see below)\n" + end + if $hfsr & (1<<31) + printf " due to an unexpected debug event\n" + end + end + if $mmfsr != 0 + printf "MemManage:" + if $mmfsr & (1<<5) + printf " during lazy FP state save" + end + if $mmfsr & (1<<4) + printf " during exception entry" + end + if $mmfsr & (1<<3) + printf " during exception return" + end + if $mmfsr & (1<<0) + printf " during data access" + end + if $mmfsr & (1<<0) + printf " during instruction prefetch" + end + if $mmfsr & (1<<7) + printf " accessing 0x%08x", $mmfar + end + printf "\n" + end + if $bfsr != 0 + printf "BusFault:" + if $bfsr & (1<<2) + printf " (imprecise)" + end + if $bfsr & (1<<1) + printf " (precise)" + end + if $bfsr & (1<<5) + printf " during lazy FP state save" + end + if $bfsr & (1<<4) + printf " during exception entry" + end + if $bfsr & (1<<3) + printf " during exception return" + end + if $bfsr & (1<<0) + printf " during instruction prefetch" + end + if $bfsr & (1<<7) + printf " accessing 0x%08x", $bfar + end + printf "\n" + end + if $ufsr != 0 + printf "UsageFault" + if $ufsr & (1<<9) + printf " due to divide-by-zero" + end + if $ufsr & (1<<8) + printf " due to unaligned memory access" + end + if $ufsr & (1<<3) + printf " due to access to disabled/absent coprocessor" + end + if $ufsr & (1<<2) + printf " due to a bad EXC_RETURN value" + end + if $ufsr & (1<<1) + printf " due to bad T or IT bits in EPSR" + end + if $ufsr & (1<<0) + printf " due to executing an undefined instruction" + end + printf "\n" + end + else + if $vect >= 15 + printf "Handling vector %u\n", $vect + end + end + if ((unsigned)$lr & 0xf0000000) == 0xf0000000 + if ($lr & 1) + printf "exception frame is on MSP\n" + #set $frame_ptr = (unsigned *)$msp + set $frame_ptr = (unsigned *)$sp + else + printf "exception frame is on PSP, backtrace may not be possible\n" + #set $frame_ptr = (unsigned *)$psp + set $frame_ptr = (unsigned *)$sp + end + if $lr & 0x10 + set $fault_sp = $frame_ptr + (8 * 4) + else + set $fault_sp = $frame_ptr + (26 * 4) + end + + + printf " r0: %08x r1: %08x r2: %08x r3: %08x\n", $frame_ptr[0], $frame_ptr[1], $frame_ptr[2], $frame_ptr[3] + printf " r4: %08x r5: %08x r6: %08x r7: %08x\n", $r4, $r5, $r6, $r7 + printf " r8: %08x r9: %08x r10: %08x r11: %08x\n", $r8, $r9, $r10, $r11 + printf " r12: %08x\n", $frame_ptr[4] + printf " sp: %08x lr: %08x pc: %08x PSR: %08x\n", $fault_sp, $frame_ptr[5], $frame_ptr[6], $frame_ptr[7] + + # Swap to the context of the faulting code and try to print a backtrace + set $saved_sp = $sp + set $sp = $fault_sp + set $saved_lr = $lr + set $lr = $frame_ptr[5] + set $saved_pc = $pc + set $pc = $frame_ptr[6] + bt + set $sp = $saved_sp + set $lr = $saved_lr + set $pc = $saved_pc + else + printf "(not currently in exception handler)\n" + end +end + +document vecstate +. vecstate +. Print information about the current exception handling state. +end diff --git a/Debug/NuttX b/Debug/NuttX index 8e6544842..3b95e96b2 100644 --- a/Debug/NuttX +++ b/Debug/NuttX @@ -174,11 +174,15 @@ end define showtaskstack set $task = (struct _TCB *)$arg0 - set $stack_free = 0 - while ($stack_free < $task->adj_stack_size) && *(uint8_t *)($task->stack_alloc_ptr + $stack_free) - set $stack_free = $stack_free + 1 + if $task == &g_idletcb + printf "can't measure idle stack\n" + else + set $stack_free = 0 + while ($stack_free < $task->adj_stack_size) && *(uint8_t *)($task->stack_alloc_ptr + $stack_free) + set $stack_free = $stack_free + 1 + end + printf" stack 0x%08x-0x%08x (%d) %d free\n", $task->stack_alloc_ptr, $task->adj_stack_ptr, $task->adj_stack_size, $stack_free end - printf" stack 0x%08x-0x%08x (%d) %d free\n", $task->stack_alloc_ptr, $task->adj_stack_ptr, $task->adj_stack_size, $stack_free end # @@ -2,6 +2,7 @@ # Various PX4-specific macros # source Debug/NuttX +source Debug/ARMv7M echo Loading PX4 GDB macros. Use 'help px4' for more information.\n @@ -21,7 +22,7 @@ end define _perf_print set $hdr = (struct perf_ctr_header *)$arg0 - printf "%p\n", $hdr + #printf "%p\n", $hdr printf "%s: ", $hdr->name # PC_COUNT if $hdr->type == 0 diff --git a/Firmware.sublime-project b/Firmware.sublime-project index a316ae2fa..72bacee9f 100644 --- a/Firmware.sublime-project +++ b/Firmware.sublime-project @@ -7,6 +7,7 @@ [ "*.o", "*.a", + "*.d", ".built", ".context", ".depend", diff --git a/ROMFS/px4fmu_default/logging/logconv.m b/ROMFS/px4fmu_default/logging/logconv.m index 5ea2aeb95..92ee01413 100755 --- a/ROMFS/px4fmu_default/logging/logconv.m +++ b/ROMFS/px4fmu_default/logging/logconv.m @@ -33,7 +33,7 @@ end % float vbat; //battery voltage in [volt] % float bat_current - current drawn from battery at this time instant % float bat_discharged - discharged energy in mAh -% float adc[3]; //remaining auxiliary ADC ports [volt] +% float adc[4]; //ADC ports [volt] % float local_position[3]; //tangent plane mapping into x,y,z [m] % int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter] % float attitude[3]; //pitch, roll, yaw [rad] @@ -57,7 +57,7 @@ logFormat{9} = struct('name', 'actuators', 'bytes', 4, 'array', 8, ' logFormat{10} = struct('name', 'vbat', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); logFormat{11} = struct('name', 'bat_current', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); logFormat{12} = struct('name', 'bat_discharged', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{13} = struct('name', 'adc', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{13} = struct('name', 'adc', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le'); logFormat{14} = struct('name', 'local_position', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); logFormat{15} = struct('name', 'gps_raw_position', 'bytes', 4, 'array', 3, 'precision', 'uint32', 'machineformat', 'ieee-le'); logFormat{16} = struct('name', 'attitude', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); diff --git a/ROMFS/px4fmu_default/mixers/FMU_Q.mix b/ROMFS/px4fmu_default/mixers/FMU_Q.mix index a35d299fd..ebcb66b24 100644 --- a/ROMFS/px4fmu_default/mixers/FMU_Q.mix +++ b/ROMFS/px4fmu_default/mixers/FMU_Q.mix @@ -26,12 +26,12 @@ for the elevons. M: 2 O: 10000 10000 0 -10000 10000 S: 0 0 -5000 -8000 0 -10000 10000 -S: 0 1 -8000 -8000 0 -10000 10000 +S: 0 1 8000 8000 0 -10000 10000 M: 2 O: 10000 10000 0 -10000 10000 S: 0 0 -8000 -5000 0 -10000 10000 -S: 0 1 8000 8000 0 -10000 10000 +S: 0 1 -8000 -8000 0 -10000 10000 Output 2 -------- diff --git a/ROMFS/px4fmu_default/mixers/FMU_X5.mix b/ROMFS/px4fmu_default/mixers/FMU_X5.mix index 981466704..9f81e1dc3 100644 --- a/ROMFS/px4fmu_default/mixers/FMU_X5.mix +++ b/ROMFS/px4fmu_default/mixers/FMU_X5.mix @@ -23,13 +23,13 @@ for the elevons. M: 2 O: 10000 10000 0 -10000 10000 -S: 0 0 3000 5000 0 -10000 10000 -S: 0 1 5000 5000 0 -10000 10000 +S: 0 0 -3000 -5000 0 -10000 10000 +S: 0 1 -5000 -5000 0 -10000 10000 M: 2 O: 10000 10000 0 -10000 10000 -S: 0 0 5000 3000 0 -10000 10000 -S: 0 1 -5000 -5000 0 -10000 10000 +S: 0 0 -5000 -3000 0 -10000 10000 +S: 0 1 5000 5000 0 -10000 10000 Output 2 -------- diff --git a/Tools/px_mkfw.py b/Tools/px_mkfw.py index 9f4ddad62..faef9ca33 100755 --- a/Tools/px_mkfw.py +++ b/Tools/px_mkfw.py @@ -107,4 +107,4 @@ if args.image != None: desc['image_size'] = len(bytes) desc['image'] = base64.b64encode(zlib.compress(bytes,9)) -print json.dumps(desc, indent=4) +print(json.dumps(desc, indent=4)) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 3b23f4f83..cce388d71 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -262,8 +262,8 @@ class uploader(object): self.port.flush() programmed = self.__recv(len(data)) if programmed != data: - print("got " + binascii.hexlify(programmed)) - print("expect " + binascii.hexlify(data)) + print(("got " + binascii.hexlify(programmed))) + print(("expect " + binascii.hexlify(data))) return False self.__getSync() return True @@ -307,8 +307,8 @@ class uploader(object): report_crc = self.__recv_int() self.__getSync() if report_crc != expect_crc: - print("Expected 0x%x" % expect_crc) - print("Got 0x%x" % report_crc) + print(("Expected 0x%x" % expect_crc)) + print(("Got 0x%x" % report_crc)) raise RuntimeError("Program CRC failed") # get basic data about the board @@ -319,7 +319,7 @@ class uploader(object): # get the bootloader protocol ID first self.bl_rev = self.__getInfo(uploader.INFO_BL_REV) if (self.bl_rev < uploader.BL_REV_MIN) or (self.bl_rev > uploader.BL_REV_MAX): - print("Unsupported bootloader protocol %d" % uploader.INFO_BL_REV) + print(("Unsupported bootloader protocol %d" % uploader.INFO_BL_REV)) raise RuntimeError("Bootloader protocol mismatch") self.board_type = self.__getInfo(uploader.INFO_BOARD_ID) @@ -360,7 +360,7 @@ args = parser.parse_args() # Load the firmware file fw = firmware(args.firmware) -print("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision'))) +print(("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision')))) # Spin waiting for a device to show up while True: @@ -393,7 +393,7 @@ while True: try: # identify the bootloader up.identify() - print("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port)) + print(("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port))) except: # most probably a timeout talking to the port, no bootloader @@ -406,7 +406,7 @@ while True: except RuntimeError as ex: # print the error - print("ERROR: %s" % ex.args) + print(("ERROR: %s" % ex.args)) finally: # always close the port diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 7ca77e513..bd972f03f 100755 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -72,38 +72,6 @@ __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]); -static unsigned int loop_interval_alarm = 6500; // loop interval in microseconds - -static float dt = 0.005f; -/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ -static float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */ -static float x_aposteriori_k[12]; /**< states */ -static float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f, - }; /**< init: diagonal matrix with big values */ - -static float x_aposteriori[12]; -static float P_aposteriori[144]; - -/* output euler angles */ -static float euler[3] = {0.0f, 0.0f, 0.0f}; - -static float Rot_matrix[9] = {1.f, 0, 0, - 0, 1.f, 0, - 0, 0, 1.f - }; /**< init: identity matrix */ - - static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int attitude_estimator_ekf_task; /**< Handle of deamon task / thread */ @@ -153,7 +121,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 12000, + 12400, attitude_estimator_ekf_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); @@ -193,6 +161,38 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) */ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) { + +const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds + + float dt = 0.005f; +/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ + float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */ + float x_aposteriori_k[12]; /**< states */ + float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f, + }; /**< init: diagonal matrix with big values */ + + float x_aposteriori[12]; + float P_aposteriori[144]; + + /* output euler angles */ + float euler[3] = {0.0f, 0.0f, 0.0f}; + + float Rot_matrix[9] = {1.f, 0, 0, + 0, 1.f, 0, + 0, 0, 1.f + }; /**< init: identity matrix */ + // print text printf("Extended Kalman Filter Attitude Estimator initialized..\n\n"); fflush(stdout); diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 42ca10f55..7c0a25f86 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -80,6 +80,7 @@ #include <uORB/topics/subsystem_info.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/parameter_update.h> +#include <uORB/topics/differential_pressure.h> #include <mavlink/mavlink_log.h> #include <systemlib/param/param.h> @@ -242,11 +243,11 @@ static int led_off(int led) } enum AUDIO_PATTERN { - AUDIO_PATTERN_ERROR = 1, - AUDIO_PATTERN_NOTIFY_POSITIVE = 2, - AUDIO_PATTERN_NOTIFY_NEUTRAL = 3, - AUDIO_PATTERN_NOTIFY_NEGATIVE = 4, - AUDIO_PATTERN_TETRIS = 5 + AUDIO_PATTERN_ERROR = 2, + AUDIO_PATTERN_NOTIFY_POSITIVE = 3, + AUDIO_PATTERN_NOTIFY_NEUTRAL = 4, + AUDIO_PATTERN_NOTIFY_NEGATIVE = 5, + AUDIO_PATTERN_NOTIFY_CHARGE = 6 }; int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state) @@ -785,6 +786,79 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) close(sub_sensor_combined); } +void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status) +{ + /* announce change */ + + mavlink_log_info(mavlink_fd, "keep it still"); + /* set to accel calibration mode */ + status->flag_preflight_airspeed_calibration = true; + state_machine_publish(status_pub, status, mavlink_fd); + + const int calibration_count = 2500; + + int sub_differential_pressure = orb_subscribe(ORB_ID(differential_pressure)); + struct differential_pressure_s differential_pressure; + + int calibration_counter = 0; + float airspeed_offset = 0.0f; + + while (calibration_counter < calibration_count) { + + /* wait blocking for new data */ + struct pollfd fds[1] = { { .fd = sub_differential_pressure, .events = POLLIN } }; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(differential_pressure), sub_differential_pressure, &differential_pressure); + airspeed_offset += differential_pressure.voltage; + calibration_counter++; + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); + return; + } + } + + airspeed_offset = airspeed_offset / calibration_count; + + if (isfinite(airspeed_offset)) { + + if (param_set(param_find("SENS_VAIR_OFF"), &(airspeed_offset))) { + mavlink_log_critical(mavlink_fd, "Setting offs failed!"); + } + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + } + + //char buf[50]; + //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); + //mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "airspeed calibration done"); + + tune_confirm(); + sleep(2); + tune_confirm(); + sleep(2); + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); + } + + /* exit airspeed calibration mode */ + status->flag_preflight_airspeed_calibration = false; + state_machine_publish(status_pub, status, mavlink_fd); + + close(sub_differential_pressure); +} + void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd) @@ -980,6 +1054,28 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta handled = true; } + /* airspeed calibration */ + if ((int)(cmd->param6) == 1) { //xxx: this is not defined by the mavlink protocol + /* transition to calibration state */ + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + + if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + mavlink_log_info(mavlink_fd, "CMD starting airspeed cal"); + tune_confirm(); + do_airspeed_calibration(status_pub, ¤t_status); + tune_confirm(); + mavlink_log_info(mavlink_fd, "CMD finished airspeed cal"); + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_critical(mavlink_fd, "REJECTING airspeed cal"); + result = VEHICLE_CMD_RESULT_DENIED; + } + + handled = true; + } + /* none found */ if (!handled) { //warnx("refusing unsupported calibration request\n"); @@ -1265,6 +1361,8 @@ int commander_thread_main(int argc, char *argv[]) param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms); param_t _param_sys_type = param_find("MAV_TYPE"); + param_t _param_system_id = param_find("MAV_SYS_ID"); + param_t _param_component_id = param_find("MAV_COMP_ID"); /* welcome user */ warnx("I am in command now!\n"); @@ -1379,6 +1477,11 @@ int commander_thread_main(int argc, char *argv[]) struct sensor_combined_s sensors; memset(&sensors, 0, sizeof(sensors)); + int differential_pressure_sub = orb_subscribe(ORB_ID(differential_pressure)); + struct differential_pressure_s differential_pressure; + memset(&differential_pressure, 0, sizeof(differential_pressure)); + uint64_t last_differential_pressure_time = 0; + /* Subscribe to command topic */ int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); struct vehicle_command_s cmd; @@ -1432,6 +1535,13 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); } + orb_check(differential_pressure_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(differential_pressure), differential_pressure_sub, &differential_pressure); + last_differential_pressure_time = differential_pressure.timestamp; + } + orb_check(cmd_sub, &new_data); if (new_data) { @@ -1450,6 +1560,7 @@ int commander_thread_main(int argc, char *argv[]) /* parameters changed */ orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); + /* update parameters */ if (!current_status.flag_system_armed) { if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { @@ -1465,6 +1576,11 @@ int commander_thread_main(int argc, char *argv[]) } else { current_status.flag_external_manual_override_ok = true; } + + /* check and update system / component ID */ + param_get(_param_system_id, &(current_status.system_id)); + param_get(_param_component_id, &(current_status.component_id)); + } } @@ -1618,6 +1734,7 @@ int commander_thread_main(int argc, char *argv[]) bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; bool global_pos_valid = current_status.flag_global_position_valid; bool local_pos_valid = current_status.flag_local_position_valid; + bool airspeed_valid = current_status.flag_airspeed_valid; /* check for global or local position updates, set a timeout of 2s */ if (hrt_absolute_time() - last_global_position_time < 2000000) { @@ -1636,6 +1753,14 @@ int commander_thread_main(int argc, char *argv[]) current_status.flag_local_position_valid = false; } + /* Check for valid airspeed/differential pressure measurements */ + if (hrt_absolute_time() - last_differential_pressure_time < 2000000) { + current_status.flag_airspeed_valid = true; + + } else { + current_status.flag_airspeed_valid = false; + } + /* * Consolidate global position and local position valid flags * for vector flight mode. @@ -1651,7 +1776,8 @@ int commander_thread_main(int argc, char *argv[]) /* consolidate state change, flag as changed if required */ if (vector_flight_mode_ok != current_status.flag_vector_flight_mode_ok || global_pos_valid != current_status.flag_global_position_valid || - local_pos_valid != current_status.flag_local_position_valid) { + local_pos_valid != current_status.flag_local_position_valid || + airspeed_valid != current_status.flag_airspeed_valid) { state_changed = true; } diff --git a/apps/controllib/block/BlockParam.cpp b/apps/controllib/block/BlockParam.cpp index b3f49f7db..fd12e365d 100644 --- a/apps/controllib/block/BlockParam.cpp +++ b/apps/controllib/block/BlockParam.cpp @@ -46,7 +46,7 @@ namespace control { -BlockParamBase::BlockParamBase(Block *parent, const char *name) : +BlockParamBase::BlockParamBase(Block *parent, const char *name, bool parent_prefix) : _handle(PARAM_INVALID) { char fullname[blockNameLengthMax]; @@ -61,8 +61,10 @@ BlockParamBase::BlockParamBase(Block *parent, const char *name) : if (!strcmp(name, "")) { strncpy(fullname, parentName, blockNameLengthMax); - } else { + } else if (parent_prefix) { snprintf(fullname, blockNameLengthMax, "%s_%s", parentName, name); + } else { + strncpy(fullname, name, blockNameLengthMax); } parent->getParams().add(this); diff --git a/apps/controllib/block/BlockParam.hpp b/apps/controllib/block/BlockParam.hpp index 7f86d1717..58a9bfc0d 100644 --- a/apps/controllib/block/BlockParam.hpp +++ b/apps/controllib/block/BlockParam.hpp @@ -53,7 +53,12 @@ namespace control class __EXPORT BlockParamBase : public ListNode<BlockParamBase *> { public: - BlockParamBase(Block *parent, const char *name); + /** + * Instantiate a block param base. + * + * @param parent_prefix Set to true to include the parent name in the parameter name + */ + BlockParamBase(Block *parent, const char *name, bool parent_prefix=true); virtual ~BlockParamBase() {}; virtual void update() = 0; const char *getName() { return param_name(_handle); } @@ -68,8 +73,8 @@ template<class T> class __EXPORT BlockParam : public BlockParamBase { public: - BlockParam(Block *block, const char *name) : - BlockParamBase(block, name), + BlockParam(Block *block, const char *name, bool parent_prefix=true) : + BlockParamBase(block, name, parent_prefix), _val() { update(); } diff --git a/apps/controllib/fixedwing.cpp b/apps/controllib/fixedwing.cpp index d9637b4a7..7f5711c47 100644 --- a/apps/controllib/fixedwing.cpp +++ b/apps/controllib/fixedwing.cpp @@ -171,10 +171,10 @@ BlockBacksideAutopilot::BlockBacksideAutopilot(SuperBlock *parent, _headingHold(this, ""), _velocityHold(this, ""), _altitudeHold(this, ""), - _trimAil(this, "TRIM_AIL"), - _trimElv(this, "TRIM_ELV"), - _trimRdr(this, "TRIM_RDR"), - _trimThr(this, "TRIM_THR") + _trimAil(this, "TRIM_ROLL", false), /* general roll trim (full name: TRIM_ROLL) */ + _trimElv(this, "TRIM_PITCH", false), /* general pitch trim */ + _trimRdr(this, "TRIM_YAW", false), /* general yaw trim */ + _trimThr(this, "TRIM_THR", true) /* FWB_ specific throttle trim (full name: FWB_TRIM_THR) */ { } @@ -322,8 +322,8 @@ void BlockMultiModeBacksideAutopilot::update() _att.roll, _att.pitch, _att.yaw, _att.rollspeed, _att.pitchspeed, _att.yawspeed ); - _actuators.control[CH_AIL] = - _backsideAutopilot.getAileron(); - _actuators.control[CH_ELV] = - _backsideAutopilot.getElevator(); + _actuators.control[CH_AIL] = _backsideAutopilot.getAileron(); + _actuators.control[CH_ELV] = _backsideAutopilot.getElevator(); _actuators.control[CH_RDR] = _backsideAutopilot.getRudder(); _actuators.control[CH_THR] = _backsideAutopilot.getThrottle(); @@ -355,7 +355,7 @@ void BlockMultiModeBacksideAutopilot::update() _att.rollspeed, _att.pitchspeed, _att.yawspeed); _actuators.control[CH_AIL] = _stabilization.getAileron(); - _actuators.control[CH_ELV] = - _stabilization.getElevator(); + _actuators.control[CH_ELV] = _stabilization.getElevator(); _actuators.control[CH_RDR] = _stabilization.getRudder(); _actuators.control[CH_THR] = _manual.throttle; } diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp index fc929284c..56265995f 100644 --- a/apps/drivers/blinkm/blinkm.cpp +++ b/apps/drivers/blinkm/blinkm.cpp @@ -127,7 +127,7 @@ class BlinkM : public device::I2C { public: BlinkM(int bus, int blinkm); - ~BlinkM(); + virtual ~BlinkM(); virtual int init(); @@ -135,7 +135,7 @@ public: virtual int setMode(int mode); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - static const char *script_names[]; + static const char *const script_names[]; private: enum ScriptID { @@ -219,7 +219,7 @@ namespace } /* list of script names, must match script ID numbers */ -const char *BlinkM::script_names[] = { +const char *const BlinkM::script_names[] = { "USER", "RGB", "WHITE_FLASH", @@ -499,7 +499,7 @@ BlinkM::led() for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) { if(vehicle_status_raw.voltage_battery < num_of_cells * MAX_CELL_VOLTAGE) break; } - printf("<blinkm> cells found:%u\n", num_of_cells); + printf("<blinkm> cells found:%d\n", num_of_cells); } else { if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { @@ -827,7 +827,7 @@ BlinkM::get_firmware_version(uint8_t version[2]) { const uint8_t msg = 'Z'; - return transfer(&msg, sizeof(msg), version, sizeof(version)); + return transfer(&msg, sizeof(msg), version, 2); } void blinkm_usage() { diff --git a/apps/drivers/bma180/bma180.cpp b/apps/drivers/bma180/bma180.cpp index 32eb5333e..4409a8a9c 100644 --- a/apps/drivers/bma180/bma180.cpp +++ b/apps/drivers/bma180/bma180.cpp @@ -126,7 +126,7 @@ class BMA180 : public device::SPI { public: BMA180(int bus, spi_dev_e device); - ~BMA180(); + virtual ~BMA180(); virtual int init(); diff --git a/apps/drivers/boards/px4fmu/px4fmu_spi.c b/apps/drivers/boards/px4fmu/px4fmu_spi.c index 70245a3ec..7a02eaeb7 100644 --- a/apps/drivers/boards/px4fmu/px4fmu_spi.c +++ b/apps/drivers/boards/px4fmu/px4fmu_spi.c @@ -92,21 +92,21 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, case PX4_SPIDEV_GYRO: /* Making sure the other peripherals are not selected */ stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); - stm32_gpiowrite(GPIO_SPI_CS_MPU, selected); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL, selected); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1); break; case PX4_SPIDEV_ACCEL: /* Making sure the other peripherals are not selected */ stm32_gpiowrite(GPIO_SPI_CS_ACCEL, !selected); - stm32_gpiowrite(GPIO_SPI_CS_MPU, selected); - stm32_gpiowrite(GPIO_SPI_CS_GYRO, selected); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); break; case PX4_SPIDEV_MPU: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_ACCEL, selected); - stm32_gpiowrite(GPIO_SPI_CS_GYRO, selected); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1); + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected); break; @@ -125,7 +125,7 @@ __EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devi __EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) { /* there can only be one device on this bus, so always select it */ - stm32_gpiowrite(GPIO_SPI_CS_SDCARD, 0); + stm32_gpiowrite(GPIO_SPI_CS_SDCARD, !selected); } __EXPORT uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) diff --git a/apps/drivers/boards/px4io/px4io_internal.h b/apps/drivers/boards/px4io/px4io_internal.h index f77d237a7..44bb98513 100644 --- a/apps/drivers/boards/px4io/px4io_internal.h +++ b/apps/drivers/boards/px4io/px4io_internal.h @@ -74,8 +74,8 @@ #define GPIO_ACC_OC_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN12) #define GPIO_SERVO_OC_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN13) -#define GPIO_RELAY1_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN13) -#define GPIO_RELAY2_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN12) +#define GPIO_RELAY1_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN12) +#define GPIO_RELAY2_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN11) /* Analog inputs ********************************************************************/ diff --git a/apps/drivers/device/i2c.cpp b/apps/drivers/device/i2c.cpp index 474190d83..a416801eb 100644 --- a/apps/drivers/device/i2c.cpp +++ b/apps/drivers/device/i2c.cpp @@ -120,7 +120,7 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re struct i2c_msg_s msgv[2]; unsigned msgs; int ret; - unsigned tries = 0; + unsigned retry_count = 0; do { // debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); @@ -154,16 +154,51 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re I2C_SETFREQUENCY(_dev, _frequency); ret = I2C_TRANSFER(_dev, &msgv[0], msgs); + /* success */ if (ret == OK) break; - // reset the I2C bus to unwedge on error - up_i2creset(_dev); + /* if we have already retried once, or we are going to give up, then reset the bus */ + if ((retry_count >= 1) || (retry_count >= _retries)) + up_i2creset(_dev); - } while (tries++ < _retries); + } while (retry_count++ < _retries); return ret; } +int +I2C::transfer(i2c_msg_s *msgv, unsigned msgs) +{ + int ret; + unsigned retry_count = 0; + + /* force the device address into the message vector */ + for (unsigned i = 0; i < msgs; i++) + msgv[i].addr = _address; + + + do { + /* + * I2C architecture means there is an unavoidable race here + * if there are any devices on the bus with a different frequency + * preference. Really, this is pointless. + */ + I2C_SETFREQUENCY(_dev, _frequency); + ret = I2C_TRANSFER(_dev, msgv, msgs); + + /* success */ + if (ret == OK) + break; + + /* if we have already retried once, or we are going to give up, then reset the bus */ + if ((retry_count >= 1) || (retry_count >= _retries)) + up_i2creset(_dev); + + } while (retry_count++ < _retries); + + return ret; +} + } // namespace device
\ No newline at end of file diff --git a/apps/drivers/device/i2c.h b/apps/drivers/device/i2c.h index 4d630b8a8..66c34dd7c 100644 --- a/apps/drivers/device/i2c.h +++ b/apps/drivers/device/i2c.h @@ -101,6 +101,16 @@ protected: uint8_t *recv, unsigned recv_len); /** + * Perform a multi-part I2C transaction to the device. + * + * @param msgv An I2C message vector. + * @param msgs The number of entries in the message vector. + * @return OK if the transfer was successful, -errno + * otherwise. + */ + int transfer(i2c_msg_s *msgv, unsigned msgs); + + /** * Change the bus address. * * Most often useful during probe() when the driver is testing diff --git a/apps/drivers/drv_hrt.h b/apps/drivers/drv_hrt.h index 3b493a81a..8a99eeca7 100644 --- a/apps/drivers/drv_hrt.h +++ b/apps/drivers/drv_hrt.h @@ -92,6 +92,22 @@ __EXPORT extern hrt_abstime ts_to_abstime(struct timespec *ts); __EXPORT extern void abstime_to_ts(struct timespec *ts, hrt_abstime abstime); /* + * Compute the delta between a timestamp taken in the past + * and now. + * + * This function is safe to use even if the timestamp is updated + * by an interrupt during execution. + */ +__EXPORT extern hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then); + +/* + * Store the absolute time in an interrupt-safe fashion. + * + * This function ensures that the timestamp cannot be seen half-written by an interrupt handler. + */ +__EXPORT extern hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now); + +/* * Call callout(arg) after delay has elapsed. * * If callout is NULL, this can be used to implement a timeout by testing the call diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h index c110cd5cb..07831f20c 100644 --- a/apps/drivers/drv_pwm_output.h +++ b/apps/drivers/drv_pwm_output.h @@ -36,7 +36,7 @@ * * Servo values can be set with the PWM_SERVO_SET ioctl, by writing a * pwm_output_values structure to the device, or by publishing to the - * output_pwm ObjDev. + * output_pwm ORB topic. * Writing a value of 0 to a channel suppresses any output for that * channel. */ @@ -60,7 +60,7 @@ __BEGIN_DECLS #define PWM_OUTPUT_DEVICE_PATH "/dev/pwm_output" /** - * Maximum number of PWM output channels in the system. + * Maximum number of PWM output channels supported by the device. */ #define PWM_OUTPUT_MAX_CHANNELS 16 @@ -77,22 +77,19 @@ typedef uint16_t servo_position_t; * device. */ struct pwm_output_values { - /** desired servo update rate in Hz */ - uint32_t update_rate; - /** desired pulse widths for each of the supported channels */ servo_position_t values[PWM_OUTPUT_MAX_CHANNELS]; }; /* - * ObjDev tag for PWM outputs. + * ORB tag for PWM outputs. */ ORB_DECLARE(output_pwm); /* * ioctl() definitions * - * Note that ioctls and ObjDev updates should not be mixed, as the + * Note that ioctls and ORB updates should not be mixed, as the * behaviour of the system in this case is not defined. */ #define _PWM_SERVO_BASE 0x2a00 @@ -103,8 +100,14 @@ ORB_DECLARE(output_pwm); /** disarm all servo outputs (stop generating pulses) */ #define PWM_SERVO_DISARM _IOC(_PWM_SERVO_BASE, 1) -/** set update rate in Hz */ -#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2) +/** set alternate servo update rate */ +#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2) + +/** get the number of servos in *(unsigned *)arg */ +#define PWM_SERVO_GET_COUNT _IOC(_PWM_SERVO_BASE, 3) + +/** selects servo update rates, one bit per servo. 0 = default (50Hz), 1 = alternate */ +#define PWM_SERVO_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4) /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) @@ -112,6 +115,10 @@ ORB_DECLARE(output_pwm); /** get a single specific servo value */ #define PWM_SERVO_GET(_servo) _IOC(_PWM_SERVO_BASE, 0x40 + _servo) +/** get the _n'th rate group's channels; *(uint32_t *)arg returns a bitmap of channels + * whose update rates must be the same. + */ +#define PWM_SERVO_GET_RATEGROUP(_n) _IOC(_PWM_SERVO_BASE, 0x60 + _n) /* * Low-level PWM output interface. @@ -148,7 +155,7 @@ __EXPORT extern void up_pwm_servo_deinit(void); __EXPORT extern void up_pwm_servo_arm(bool armed); /** - * Set the servo update rate + * Set the servo update rate for all rate groups. * * @param rate The update rate in Hz to set. * @return OK on success, -ERANGE if an unsupported update rate is set. @@ -156,6 +163,25 @@ __EXPORT extern void up_pwm_servo_arm(bool armed); __EXPORT extern int up_pwm_servo_set_rate(unsigned rate); /** + * Get a bitmap of output channels assigned to a given rate group. + * + * @param group The rate group to query. Rate groups are assigned contiguously + * starting from zero. + * @return A bitmap of channels assigned to the rate group, or zero if + * the group number has no channels. + */ +__EXPORT extern uint32_t up_pwm_servo_get_rate_group(unsigned group); + +/** + * Set the update rate for a given rate group. + * + * @param group The rate group whose update rate will be changed. + * @param rate The update rate in Hz. + * @return OK if the group was adjusted, -ERANGE if an unsupported update rate is set. + */ +__EXPORT extern int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate); + +/** * Set the current output value for a channel. * * @param channel The channel to set. diff --git a/apps/drivers/drv_range_finder.h b/apps/drivers/drv_range_finder.h new file mode 100644 index 000000000..03a82ec5d --- /dev/null +++ b/apps/drivers/drv_range_finder.h @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * 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 Rangefinder driver interface. + */ + +#ifndef _DRV_RANGEFINDER_H +#define _DRV_RANGEFINDER_H + +#include <stdint.h> +#include <sys/ioctl.h> + +#include "drv_sensor.h" +#include "drv_orb_dev.h" + +#define RANGE_FINDER_DEVICE_PATH "/dev/range_finder" + +/** + * range finder report structure. Reads from the device must be in multiples of this + * structure. + */ +struct range_finder_report { + uint64_t timestamp; + float distance; /** in meters */ + uint8_t valid; /** 1 == within sensor range, 0 = outside sensor range */ +}; + +/* + * ObjDev tag for raw range finder data. + */ +ORB_DECLARE(sensor_range_finder); + +/* + * ioctl() definitions + * + * Rangefinder drivers also implement the generic sensor driver + * interfaces from drv_sensor.h + */ + +#define _RANGEFINDERIOCBASE (0x7900) +#define __RANGEFINDERIOC(_n) (_IOC(_RANGEFINDERIOCBASE, _n)) + +/** set the minimum effective distance of the device */ +#define RANGEFINDERIOCSETMINIUMDISTANCE __RANGEFINDERIOC(1) + +/** set the maximum effective distance of the device */ +#define RANGEFINDERIOCSETMAXIUMDISTANCE __RANGEFINDERIOC(2) + + +#endif /* _DRV_RANGEFINDER_H */ diff --git a/apps/drivers/drv_rc_input.h b/apps/drivers/drv_rc_input.h index 927406108..4decc324e 100644 --- a/apps/drivers/drv_rc_input.h +++ b/apps/drivers/drv_rc_input.h @@ -100,4 +100,11 @@ struct rc_input_values { */ ORB_DECLARE(input_rc); +#define _RC_INPUT_BASE 0x2b00 + +/** Fetch R/C input values into (rc_input_values *)arg */ + +#define RC_INPUT_GET _IOC(_RC_INPUT_BASE, 0) + + #endif /* _DRV_RC_INPUT_H */ diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index 6d7cfcc68..e35bdb944 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -67,7 +67,7 @@ #include "mtk.h" -#define TIMEOUT_5HZ 400 +#define TIMEOUT_5HZ 500 #define RATE_MEASUREMENT_PERIOD 5000000 /* oddly, ERROR is not defined for c++ */ @@ -86,7 +86,7 @@ class GPS : public device::CDev { public: GPS(const char* uart_path); - ~GPS(); + virtual ~GPS(); virtual int init(); diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp index fe9b281f6..d9aa772d4 100644 --- a/apps/drivers/hil/hil.cpp +++ b/apps/drivers/hil/hil.cpp @@ -91,7 +91,7 @@ public: MODE_NONE }; HIL(); - ~HIL(); + virtual ~HIL(); virtual int ioctl(file *filp, int cmd, unsigned long arg); @@ -158,6 +158,7 @@ HIL::HIL() : CDev("hilservo", PWM_OUTPUT_DEVICE_PATH/*"/dev/hil" XXXL*/), _mode(MODE_NONE), _update_rate(50), + _current_update_rate(0), _task(-1), _t_actuators(-1), _t_armed(-1), @@ -511,9 +512,14 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; case PWM_SERVO_SET_UPDATE_RATE: + // HIL always outputs at the alternate (usually faster) rate g_hil->set_pwm_rate(arg); break; + case PWM_SERVO_SELECT_UPDATE_RATE: + // HIL always outputs at the alternate (usually faster) rate + break; + case PWM_SERVO_SET(2): case PWM_SERVO_SET(3): if (_mode != MODE_4PWM) { @@ -549,6 +555,14 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } + case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): { + // no restrictions on output grouping + unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0); + + *(uint32_t *)arg = (1 << channel); + break; + } + case MIXERIOCGETOUTPUTCOUNT: if (_mode == MODE_4PWM) { *(unsigned *)arg = 4; @@ -641,7 +655,7 @@ enum PortMode { PortMode g_port_mode; int -hil_new_mode(PortMode new_mode, int update_rate) +hil_new_mode(PortMode new_mode) { // uint32_t gpio_bits; @@ -699,8 +713,6 @@ hil_new_mode(PortMode new_mode, int update_rate) /* (re)set the PWM output mode */ g_hil->set_mode(servo_mode); - if ((servo_mode != HIL::MODE_NONE) && (update_rate != 0)) - g_hil->set_pwm_rate(update_rate); return OK; } @@ -786,59 +798,34 @@ int hil_main(int argc, char *argv[]) { PortMode new_mode = PORT_MODE_UNDEFINED; - int pwm_update_rate_in_hz = 0; - - if (!strcmp(argv[1], "test")) - test(); - - if (!strcmp(argv[1], "fake")) - fake(argc - 1, argv + 1); + const char *verb = argv[1]; if (hil_start() != OK) - errx(1, "failed to start the FMU driver"); + errx(1, "failed to start the HIL driver"); /* * Mode switches. - * - * XXX use getopt? */ - for (int i = 1; i < argc; i++) { /* argv[0] is "fmu" */ - if (!strcmp(argv[i], "mode_pwm")) { - new_mode = PORT1_FULL_PWM; + // this was all cut-and-pasted from the FMU driver; it's junk + if (!strcmp(verb, "mode_pwm")) { + new_mode = PORT1_FULL_PWM; - } else if (!strcmp(argv[i], "mode_pwm_serial")) { - new_mode = PORT1_PWM_AND_SERIAL; + } else if (!strcmp(verb, "mode_pwm_serial")) { + new_mode = PORT1_PWM_AND_SERIAL; - } else if (!strcmp(argv[i], "mode_pwm_gpio")) { - new_mode = PORT1_PWM_AND_GPIO; - - } else if (!strcmp(argv[i], "mode_port2_pwm8")) { - new_mode = PORT2_8PWM; + } else if (!strcmp(verb, "mode_pwm_gpio")) { + new_mode = PORT1_PWM_AND_GPIO; - } else if (!strcmp(argv[i], "mode_port2_pwm12")) { - new_mode = PORT2_8PWM; + } else if (!strcmp(verb, "mode_port2_pwm8")) { + new_mode = PORT2_8PWM; - } else if (!strcmp(argv[i], "mode_port2_pwm16")) { - new_mode = PORT2_8PWM; - } + } else if (!strcmp(verb, "mode_port2_pwm12")) { + new_mode = PORT2_8PWM; - /* look for the optional pwm update rate for the supported modes */ - if (strcmp(argv[i], "-u") == 0 || strcmp(argv[i], "--update-rate") == 0) { - // if (new_mode == PORT1_FULL_PWM || new_mode == PORT1_PWM_AND_GPIO) { - // XXX all modes have PWM settings - if (argc > i + 1) { - pwm_update_rate_in_hz = atoi(argv[i + 1]); - printf("pwm update rate: %d Hz\n", pwm_update_rate_in_hz); - } else { - fprintf(stderr, "missing argument for pwm update rate (-u)\n"); - return 1; - } - // } else { - // fprintf(stderr, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio\n"); - // } - } - } + } else if (!strcmp(verb, "mode_port2_pwm16")) { + new_mode = PORT2_8PWM; + } /* was a new mode set? */ if (new_mode != PORT_MODE_UNDEFINED) { @@ -848,12 +835,17 @@ hil_main(int argc, char *argv[]) return OK; /* switch modes */ - return hil_new_mode(new_mode, pwm_update_rate_in_hz); + return hil_new_mode(new_mode); } - /* test, etc. here */ + if (!strcmp(verb, "test")) + test(); + + if (!strcmp(verb, "fake")) + fake(argc - 1, argv + 1); + fprintf(stderr, "HIL: unrecognized command, try:\n"); - fprintf(stderr, " mode_pwm [-u pwm_update_rate_in_hz], mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16\n"); + fprintf(stderr, " mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16\n"); return -EINVAL; } diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index 3734d7755..8ab568282 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -130,7 +130,7 @@ class HMC5883 : public device::I2C { public: HMC5883(int bus); - ~HMC5883(); + virtual ~HMC5883(); virtual int init(); @@ -465,7 +465,7 @@ HMC5883::probe() read_reg(ADDR_ID_C, data[2])) debug("read_reg fail"); - _retries = 1; + _retries = 2; if ((data[0] != ID_A_WHO_AM_I) || (data[1] != ID_B_WHO_AM_I) || diff --git a/apps/drivers/l3gd20/l3gd20.cpp b/apps/drivers/l3gd20/l3gd20.cpp index f2f585f42..6227df72a 100644 --- a/apps/drivers/l3gd20/l3gd20.cpp +++ b/apps/drivers/l3gd20/l3gd20.cpp @@ -152,7 +152,7 @@ class L3GD20 : public device::SPI { public: L3GD20(int bus, const char* path, spi_dev_e device); - ~L3GD20(); + virtual ~L3GD20(); virtual int init(); diff --git a/apps/drivers/led/led.cpp b/apps/drivers/led/led.cpp index 12d864be2..c7c45525e 100644 --- a/apps/drivers/led/led.cpp +++ b/apps/drivers/led/led.cpp @@ -53,7 +53,7 @@ class LED : device::CDev { public: LED(); - ~LED(); + virtual ~LED(); virtual int init(); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); diff --git a/apps/drivers/mb12xx/Makefile b/apps/drivers/mb12xx/Makefile new file mode 100644 index 000000000..0d2405787 --- /dev/null +++ b/apps/drivers/mb12xx/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Makefile to build the Maxbotix Sonar driver. +# + +APPNAME = mb12xx +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/mb12xx/mb12xx.cpp b/apps/drivers/mb12xx/mb12xx.cpp new file mode 100644 index 000000000..9d0f6bddc --- /dev/null +++ b/apps/drivers/mb12xx/mb12xx.cpp @@ -0,0 +1,840 @@ +/**************************************************************************** + * + * 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 mb12xx.cpp + * @author Greg Hulands + * + * Driver for the Maxbotix sonar range finders connected via I2C. + */ + +#include <nuttx/config.h> + +#include <drivers/device/i2c.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdlib.h> +#include <stdbool.h> +#include <semaphore.h> +#include <string.h> +#include <fcntl.h> +#include <poll.h> +#include <errno.h> +#include <stdio.h> +#include <math.h> +#include <unistd.h> + +#include <nuttx/arch.h> +#include <nuttx/wqueue.h> +#include <nuttx/clock.h> + +#include <arch/board/board.h> + +#include <systemlib/perf_counter.h> +#include <systemlib/err.h> + +#include <drivers/drv_hrt.h> +#include <drivers/drv_range_finder.h> + +#include <uORB/uORB.h> +#include <uORB/topics/subsystem_info.h> + +/* Configuration Constants */ +#define MB12XX_BUS PX4_I2C_BUS_EXPANSION +#define MB12XX_BASEADDR 0x70 /* 7-bit address. 8-bit address is 0xE0 */ + +/* MB12xx Registers addresses */ + +#define MB12XX_TAKE_RANGE_REG 0x51 /* Measure range Register */ +#define MB12XX_SET_ADDRESS_1 0xAA /* Change address 1 Register */ +#define MB12XX_SET_ADDRESS_2 0xA5 /* Change address 2 Register */ + +/* Device limits */ +#define MB12XX_MIN_DISTANCE (0.20f) +#define MB12XX_MAX_DISTANCE (7.65f) + +#define MB12XX_CONVERSION_INTERVAL 60000 /* 60ms */ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +class MB12XX : public device::I2C +{ +public: + MB12XX(int bus = MB12XX_BUS, int address = MB12XX_BASEADDR); + virtual ~MB12XX(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + +private: + float _min_distance; + float _max_distance; + work_s _work; + unsigned _num_reports; + volatile unsigned _next_report; + volatile unsigned _oldest_report; + range_finder_report *_reports; + bool _sensor_ok; + int _measure_ticks; + bool _collect_phase; + + orb_advert_t _range_finder_topic; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + /** + * Test whether the device supported by the driver is present at a + * specific address. + * + * @param address The I2C bus address to probe. + * @return True if the device is present. + */ + int probe_address(uint8_t address); + + /** + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + /** + * Set the min and max distance thresholds if you want the end points of the sensors + * range to be brought in at all, otherwise it will use the defaults MB12XX_MIN_DISTANCE + * and MB12XX_MAX_DISTANCE + */ + void set_minimum_distance(float min); + void set_maximum_distance(float max); + float get_minimum_distance(); + float get_maximum_distance(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void cycle(); + int measure(); + int collect(); + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + +}; + +/* helper macro for handling report buffer indices */ +#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int mb12xx_main(int argc, char *argv[]); + +MB12XX::MB12XX(int bus, int address) : + I2C("MB12xx", RANGE_FINDER_DEVICE_PATH, bus, address, 100000), + _min_distance(MB12XX_MIN_DISTANCE), + _max_distance(MB12XX_MAX_DISTANCE), + _num_reports(0), + _next_report(0), + _oldest_report(0), + _reports(nullptr), + _sensor_ok(false), + _measure_ticks(0), + _collect_phase(false), + _range_finder_topic(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")), + _comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")) +{ + // enable debug() calls + _debug_enabled = true; + + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +MB12XX::~MB12XX() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) + delete[] _reports; +} + +int +MB12XX::init() +{ + int ret = ERROR; + + /* do I2C init (and probe) first */ + if (I2C::init() != OK) + goto out; + + /* allocate basic report buffers */ + _num_reports = 2; + _reports = new struct range_finder_report[_num_reports]; + + if (_reports == nullptr) + goto out; + + _oldest_report = _next_report = 0; + + /* get a publish handle on the range finder topic */ + memset(&_reports[0], 0, sizeof(_reports[0])); + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &_reports[0]); + + if (_range_finder_topic < 0) + debug("failed to create sensor_range_finder object. Did you start uOrb?"); + + ret = OK; + /* sensor is ok, but we don't really know if it is within range */ + _sensor_ok = true; +out: + return ret; +} + +int +MB12XX::probe() +{ + return measure(); +} + +void +MB12XX::set_minimum_distance(float min) +{ + _min_distance = min; +} + +void +MB12XX::set_maximum_distance(float max) +{ + _max_distance = max; +} + +float +MB12XX::get_minimum_distance() +{ + return _min_distance; +} + +float +MB12XX::get_maximum_distance() +{ + return _max_distance; +} + +int +MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; + + /* external signalling (DRDY) not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(MB12XX_CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL)) + return -EINVAL; + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) + return SENSOR_POLLRATE_MANUAL; + + return (1000 / _measure_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* add one to account for the sentinel in the ring */ + arg++; + + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + struct range_finder_report *buf = new struct range_finder_report[arg]; + + if (nullptr == buf) + return -ENOMEM; + + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete[] _reports; + _num_reports = arg; + _reports = buf; + start(); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _num_reports - 1; + + case SENSORIOCRESET: + /* XXX implement this */ + return -EINVAL; + + case RANGEFINDERIOCSETMINIUMDISTANCE: + { + set_minimum_distance(*(float *)arg); + return 0; + } + break; + case RANGEFINDERIOCSETMAXIUMDISTANCE: + { + set_maximum_distance(*(float *)arg); + return 0; + } + break; + default: + /* give it to the superclass */ + return I2C::ioctl(filp, cmd, arg); + } +} + +ssize_t +MB12XX::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct range_finder_report); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_measure_ticks > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_oldest_report != _next_report) { + memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); + ret += sizeof(_reports[0]); + INCREMENT(_oldest_report, _num_reports); + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement - run one conversion */ + /* XXX really it'd be nice to lock against other readers here */ + do { + _oldest_report = _next_report = 0; + + /* trigger a measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + /* wait for it to complete */ + usleep(MB12XX_CONVERSION_INTERVAL); + + /* run the collection phase */ + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + memcpy(buffer, _reports, sizeof(*_reports)); + ret = sizeof(*_reports); + + } while (0); + + return ret; +} + +int +MB12XX::measure() +{ + int ret; + + /* + * Send the command to begin a measurement. + */ + uint8_t cmd = MB12XX_TAKE_RANGE_REG; + ret = transfer(&cmd, 1, nullptr, 0); + + if (OK != ret) + { + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + return ret; + } + ret = OK; + + return ret; +} + +int +MB12XX::collect() +{ + int ret = -EIO; + + /* read from the sensor */ + uint8_t val[2] = {0, 0}; + + perf_begin(_sample_perf); + + ret = transfer(nullptr, 0, &val[0], 2); + + if (ret < 0) + { + log("error reading from sensor: %d", ret); + return ret; + } + + uint16_t distance = val[0] << 8 | val[1]; + float si_units = (distance * 1.0f)/ 100.0f; /* cm to m */ + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ + _reports[_next_report].timestamp = hrt_absolute_time(); + _reports[_next_report].distance = si_units; + _reports[_next_report].valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; + + /* publish it */ + orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &_reports[_next_report]); + + /* post a report to the ring - note, not locked */ + INCREMENT(_next_report, _num_reports); + + /* if we are running up against the oldest report, toss it */ + if (_next_report == _oldest_report) { + perf_count(_buffer_overflows); + INCREMENT(_oldest_report, _num_reports); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + ret = OK; + +out: + perf_end(_sample_perf); + return ret; + + return ret; +} + +void +MB12XX::start() +{ + /* reset the report ring and state machine */ + _collect_phase = false; + _oldest_report = _next_report = 0; + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1); + + /* notify about state change */ + struct subsystem_info_s info = { + true, + true, + true, + SUBSYSTEM_TYPE_RANGEFINDER}; + static orb_advert_t pub = -1; + + if (pub > 0) { + orb_publish(ORB_ID(subsystem_info), pub, &info); + } else { + pub = orb_advertise(ORB_ID(subsystem_info), &info); + } +} + +void +MB12XX::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +MB12XX::cycle_trampoline(void *arg) +{ + MB12XX *dev = (MB12XX *)arg; + + dev->cycle(); +} + +void +MB12XX::cycle() +{ + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + if (OK != collect()) { + log("collection error"); + /* restart the measurement state machine */ + start(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + */ + if (_measure_ticks > USEC2TICK(MB12XX_CONVERSION_INTERVAL)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&MB12XX::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(MB12XX_CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + if (OK != measure()) + log("measure error"); + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&MB12XX::cycle_trampoline, + this, + USEC2TICK(MB12XX_CONVERSION_INTERVAL)); +} + +void +MB12XX::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + printf("poll interval: %u ticks\n", _measure_ticks); + printf("report queue: %u (%u/%u @ %p)\n", + _num_reports, _oldest_report, _next_report, _reports); +} + +/** + * Local functions in support of the shell command. + */ +namespace mb12xx +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +MB12XX *g_dev; + +void start(); +void stop(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + int fd; + + if (g_dev != nullptr) + errx(1, "already started"); + + /* create the driver */ + g_dev = new MB12XX(MB12XX_BUS); + + if (g_dev == nullptr) + goto fail; + + if (OK != g_dev->init()) + goto fail; + + /* set the poll rate to default, starts automatic data collection */ + fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + goto fail; + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + goto fail; + + exit(0); + +fail: + + if (g_dev != nullptr) + { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Stop the driver + */ +void stop() +{ + if (g_dev != nullptr) + { + delete g_dev; + g_dev = nullptr; + } + else + { + errx(1, "driver not running"); + } + exit(0); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + struct range_finder_report report; + ssize_t sz; + int ret; + + int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "%s open failed (try 'mb12xx start' if the driver is not running", RANGE_FINDER_DEVICE_PATH); + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + err(1, "immediate read failed"); + + warnx("single read"); + warnx("measurement: %0.2f m", (double)report.distance); + warnx("time: %lld", report.timestamp); + + /* start the sensor polling at 2Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) + errx(1, "failed to set 2Hz poll rate"); + + /* read the sensor 5x and report each value */ + for (unsigned i = 0; i < 5; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) + errx(1, "timed out waiting for sensor data"); + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + err(1, "periodic read failed"); + + warnx("periodic read %u", i); + warnx("measurement: %0.3f", (double)report.distance); + warnx("time: %lld", report.timestamp); + } + + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "failed "); + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) + err(1, "driver reset failed"); + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + err(1, "driver poll restart failed"); + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) + errx(1, "driver not running"); + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + +} // namespace + +int +mb12xx_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) + mb12xx::start(); + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) + mb12xx::stop(); + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + mb12xx::test(); + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) + mb12xx::reset(); + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) + mb12xx::info(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp index 27e200e40..ce7062046 100644 --- a/apps/drivers/mpu6000/mpu6000.cpp +++ b/apps/drivers/mpu6000/mpu6000.cpp @@ -151,7 +151,7 @@ class MPU6000 : public device::SPI { public: MPU6000(int bus, spi_dev_e device); - ~MPU6000(); + virtual ~MPU6000(); virtual int init(); diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp index b8845aaf1..59ab5936e 100644 --- a/apps/drivers/ms5611/ms5611.cpp +++ b/apps/drivers/ms5611/ms5611.cpp @@ -104,7 +104,7 @@ class MS5611 : public device::I2C { public: MS5611(int bus); - ~MS5611(); + virtual ~MS5611(); virtual int init(); @@ -144,6 +144,7 @@ private: orb_advert_t _baro_topic; perf_counter_t _sample_perf; + perf_counter_t _measure_perf; perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; @@ -162,12 +163,12 @@ private: * @note This function is called at open and error time. It might make sense * to make it more aggressive about resetting the bus in case of errors. */ - void start(); + void start_cycle(); /** * Stop the automatic measurement state machine. */ - void stop(); + void stop_cycle(); /** * Perform a poll cycle; collect from the previous measurement @@ -274,6 +275,7 @@ MS5611::MS5611(int bus) : _msl_pressure(101325), _baro_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")), + _measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")), _comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows")) { @@ -287,7 +289,7 @@ MS5611::MS5611(int bus) : MS5611::~MS5611() { /* make sure we are truly inactive */ - stop(); + stop_cycle(); /* free any existing reports */ if (_reports != nullptr) @@ -331,7 +333,11 @@ MS5611::probe() if ((OK == probe_address(MS5611_ADDRESS_1)) || (OK == probe_address(MS5611_ADDRESS_2))) { - _retries = 1; + /* + * Disable retries; we may enable them selectively in some cases, + * but the device gets confused if we retry some of the commands. + */ + _retries = 0; return OK; } @@ -436,7 +442,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: - stop(); + stop_cycle(); _measure_ticks = 0; return OK; @@ -458,7 +464,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) /* if we need to start the poll state machine, do it */ if (want_start) - start(); + start_cycle(); return OK; } @@ -480,7 +486,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) /* if we need to start the poll state machine, do it */ if (want_start) - start(); + start_cycle(); return OK; } @@ -508,11 +514,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) return -ENOMEM; /* reset the measurement state machine with the new buffer, free the old */ - stop(); + stop_cycle(); delete[] _reports; _num_reports = arg; _reports = buf; - start(); + start_cycle(); return OK; } @@ -545,7 +551,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) } void -MS5611::start() +MS5611::start_cycle() { /* reset the report ring and state machine */ @@ -558,7 +564,7 @@ MS5611::start() } void -MS5611::stop() +MS5611::stop_cycle() { work_cancel(HPWORK, &_work); } @@ -574,15 +580,25 @@ MS5611::cycle_trampoline(void *arg) void MS5611::cycle() { + int ret; /* collection phase? */ if (_collect_phase) { /* perform collection */ - if (OK != collect()) { - log("collection error"); + ret = collect(); + if (ret != OK) { + if (ret == -6) { + /* + * The ms5611 seems to regularly fail to respond to + * its address; this happens often enough that we'd rather not + * spam the console with the message. + */ + } else { + //log("collection error %d", ret); + } /* reset the collection state machine and try again */ - start(); + start_cycle(); return; } @@ -609,8 +625,13 @@ MS5611::cycle() } /* measurement phase */ - if (OK != measure()) - log("measure error"); + ret = measure(); + if (ret != OK) { + //log("measure error %d", ret); + /* reset the collection state machine and try again */ + start_cycle(); + return; + } /* next phase is collection */ _collect_phase = true; @@ -628,6 +649,8 @@ MS5611::measure() { int ret; + perf_begin(_measure_perf); + /* * In phase zero, request temperature; in other phases, request pressure. */ @@ -635,18 +658,25 @@ MS5611::measure() /* * Send the command to begin measuring. + * + * Disable retries on this command; we can't know whether failure + * means the device did or did not see the write. */ + _retries = 0; ret = transfer(&cmd_data, 1, nullptr, 0); if (OK != ret) perf_count(_comms_errors); + perf_end(_measure_perf); + return ret; } int MS5611::collect() { + int ret; uint8_t cmd; uint8_t data[3]; union { @@ -662,9 +692,11 @@ MS5611::collect() /* this should be fairly close to the end of the conversion, so the best approximation of the time */ _reports[_next_report].timestamp = hrt_absolute_time(); - if (OK != transfer(&cmd, 1, &data[0], 3)) { + ret = transfer(&cmd, 1, &data[0], 3); + if (ret != OK) { perf_count(_comms_errors); - return -EIO; + perf_end(_sample_perf); + return ret; } /* fetch the raw value */ diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index 430d18c6d..e54724536 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -64,12 +64,14 @@ #include <systemlib/err.h> #include <systemlib/mixer/mixer.h> #include <drivers/drv_mixer.h> +#include <drivers/drv_rc_input.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_controls_effective.h> #include <uORB/topics/actuator_outputs.h> #include <systemlib/err.h> +#include <systemlib/ppm_decode.h> class PX4FMU : public device::CDev { @@ -80,21 +82,26 @@ public: MODE_NONE }; PX4FMU(); - ~PX4FMU(); + virtual ~PX4FMU(); virtual int ioctl(file *filp, int cmd, unsigned long arg); + virtual ssize_t write(file *filp, const char *buffer, size_t len); virtual int init(); int set_mode(Mode mode); - int set_pwm_rate(unsigned rate); + + int set_pwm_alt_rate(unsigned rate); + int set_pwm_alt_channels(uint32_t channels); private: static const unsigned _max_actuators = 4; Mode _mode; - int _update_rate; - int _current_update_rate; + unsigned _pwm_default_rate; + unsigned _pwm_alt_rate; + uint32_t _pwm_alt_rate_channels; + unsigned _current_update_rate; int _task; int _t_actuators; int _t_armed; @@ -118,6 +125,7 @@ private: uint8_t control_index, float &input); + int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate); int pwm_ioctl(file *filp, int cmd, unsigned long arg); struct GPIOConfig { @@ -160,7 +168,10 @@ PX4FMU *g_fmu; PX4FMU::PX4FMU() : CDev("fmuservo", "/dev/px4fmu"), _mode(MODE_NONE), - _update_rate(50), + _pwm_default_rate(50), + _pwm_alt_rate(50), + _pwm_alt_rate_channels(0), + _current_update_rate(0), _task(-1), _t_actuators(-1), _t_armed(-1), @@ -260,27 +271,31 @@ PX4FMU::set_mode(Mode mode) * are presented on the output pins. */ switch (mode) { - case MODE_2PWM: - debug("MODE_2PWM"); - /* multi-port with flow control lines as PWM */ - /* XXX magic numbers */ - up_pwm_servo_init(0x3); - _update_rate = 50; /* default output rate */ - break; + case MODE_2PWM: // multi-port with flow control lines as PWM + case MODE_4PWM: // multi-port as 4 PWM outs + debug("MODE_%dPWM", (mode == MODE_2PWM) ? 2 : 4); + + /* default output rates */ + _pwm_default_rate = 50; + _pwm_alt_rate = 50; + _pwm_alt_rate_channels = 0; - case MODE_4PWM: - debug("MODE_4PWM"); - /* multi-port as 4 PWM outs */ /* XXX magic numbers */ - up_pwm_servo_init(0xf); - _update_rate = 50; /* default output rate */ + up_pwm_servo_init((mode == MODE_2PWM) ? 0x3 : 0xf); + set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + break; case MODE_NONE: debug("MODE_NONE"); - /* disable servo outputs and set a very low update rate */ + + _pwm_default_rate = 10; /* artificially reduced output rate */ + _pwm_alt_rate = 10; + _pwm_alt_rate_channels = 0; + + /* disable servo outputs - no need to set rates */ up_pwm_servo_deinit(); - _update_rate = 10; + break; default: @@ -292,15 +307,63 @@ PX4FMU::set_mode(Mode mode) } int -PX4FMU::set_pwm_rate(unsigned rate) +PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate) { - if ((rate > 500) || (rate < 10)) - return -EINVAL; + debug("set_pwm_rate %x %u %u", rate_map, default_rate, alt_rate); + + for (unsigned pass = 0; pass < 2; pass++) { + for (unsigned group = 0; group < _max_actuators; group++) { + + // get the channel mask for this rate group + uint32_t mask = up_pwm_servo_get_rate_group(group); + if (mask == 0) + continue; + + // all channels in the group must be either default or alt-rate + uint32_t alt = rate_map & mask; + + if (pass == 0) { + // preflight + if ((alt != 0) && (alt != mask)) { + warn("rate group %u mask %x bad overlap %x", group, mask, alt); + // not a legal map, bail + return -EINVAL; + } + } else { + // set it - errors here are unexpected + if (alt != 0) { + if (up_pwm_servo_set_rate_group_update(group, _pwm_alt_rate) != OK) { + warn("rate group set alt failed"); + return -EINVAL; + } + } else { + if (up_pwm_servo_set_rate_group_update(group, _pwm_default_rate) != OK) { + warn("rate group set default failed"); + return -EINVAL; + } + } + } + } + } + _pwm_alt_rate_channels = rate_map; + _pwm_default_rate = default_rate; + _pwm_alt_rate = alt_rate; - _update_rate = rate; return OK; } +int +PX4FMU::set_pwm_alt_rate(unsigned rate) +{ + return set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, rate); +} + +int +PX4FMU::set_pwm_alt_channels(uint32_t channels) +{ + return set_pwm_rate(channels, _pwm_default_rate, _pwm_alt_rate); +} + void PX4FMU::task_main() { @@ -338,33 +401,48 @@ PX4FMU::task_main() unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4; + // rc input, published to ORB + struct rc_input_values rc_in; + orb_advert_t to_input_rc = 0; + + memset(&rc_in, 0, sizeof(rc_in)); + rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM; + log("starting"); /* loop until killed */ while (!_task_should_exit) { - /* handle update rate changes */ - if (_current_update_rate != _update_rate) { - int update_rate_in_ms = int(1000 / _update_rate); + /* + * Adjust actuator topic update rate to keep up with + * the highest servo update rate configured. + * + * We always mix at max rate; some channels may update slower. + */ + unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate; + if (_current_update_rate != max_rate) { + _current_update_rate = max_rate; + int update_rate_in_ms = int(1000 / _current_update_rate); /* reject faster than 500 Hz updates */ if (update_rate_in_ms < 2) { update_rate_in_ms = 2; - _update_rate = 500; } - /* reject slower than 50 Hz updates */ - if (update_rate_in_ms > 20) { - update_rate_in_ms = 20; - _update_rate = 50; + /* reject slower than 10 Hz updates */ + if (update_rate_in_ms > 100) { + update_rate_in_ms = 100; } + debug("adjusted actuator update interval to %ums", update_rate_in_ms); orb_set_interval(_t_actuators, update_rate_in_ms); - up_pwm_servo_set_rate(_update_rate); - _current_update_rate = _update_rate; + + // set to current max rate, even if we are actually checking slower/faster + _current_update_rate = max_rate; } - /* sleep waiting for data, but no more than a second */ - int ret = ::poll(&fds[0], 2, 1000); + /* sleep waiting for data, stopping to check for PPM + * input at 100Hz */ + int ret = ::poll(&fds[0], 2, 10); /* this would be bad... */ if (ret < 0) { @@ -429,6 +507,26 @@ PX4FMU::task_main() /* update PWM servo armed status if armed and not locked down */ up_pwm_servo_arm(aa.armed && !aa.lockdown); } + + // see if we have new PPM input data + if (ppm_last_valid_decode != rc_in.timestamp) { + // we have a new PPM frame. Publish it. + rc_in.channel_count = ppm_decoded_channels; + if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) { + rc_in.channel_count = RC_INPUT_MAX_CHANNELS; + } + for (uint8_t i=0; i<rc_in.channel_count; i++) { + rc_in.values[i] = ppm_buffer[i]; + } + rc_in.timestamp = ppm_last_valid_decode; + + /* lazily advertise on first publication */ + if (to_input_rc == 0) { + to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_in); + } else { + orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in); + } + } } ::close(_t_actuators); @@ -496,7 +594,6 @@ int PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) { int ret = OK; - int channel; lock(); @@ -510,7 +607,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; case PWM_SERVO_SET_UPDATE_RATE: - set_pwm_rate(arg); + ret = set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, arg); + break; + + case PWM_SERVO_SELECT_UPDATE_RATE: + ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate); break; case PWM_SERVO_SET(2): @@ -524,9 +625,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_SET(0): case PWM_SERVO_SET(1): if (arg < 2100) { - channel = cmd - PWM_SERVO_SET(0); - up_pwm_servo_set(channel, arg); - + up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg); } else { ret = -EINVAL; } @@ -542,12 +641,18 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) /* FALLTHROUGH */ case PWM_SERVO_GET(0): - case PWM_SERVO_GET(1): { - channel = cmd - PWM_SERVO_GET(0); - *(servo_position_t *)arg = up_pwm_servo_get(channel); - break; - } + case PWM_SERVO_GET(1): + *(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0)); + break; + + case PWM_SERVO_GET_RATEGROUP(0): + case PWM_SERVO_GET_RATEGROUP(1): + case PWM_SERVO_GET_RATEGROUP(2): + case PWM_SERVO_GET_RATEGROUP(3): + *(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0)); + break; + case PWM_SERVO_GET_COUNT: case MIXERIOCGETOUTPUTCOUNT: if (_mode == MODE_4PWM) { *(unsigned *)arg = 4; @@ -621,6 +726,30 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) return ret; } +/* + this implements PWM output via a write() method, for compatibility + with px4io + */ +ssize_t +PX4FMU::write(file *filp, const char *buffer, size_t len) +{ + unsigned count = len / 2; + uint16_t values[4]; + + if (count > 4) { + // we only have 4 PWM outputs on the FMU + count = 4; + } + + // allow for misaligned values + memcpy(values, buffer, count*2); + + for (uint8_t i=0; i<count; i++) { + up_pwm_servo_set(i, values[i]); + } + return count * 2; +} + void PX4FMU::gpio_reset(void) { @@ -757,7 +886,7 @@ enum PortMode { PortMode g_port_mode; int -fmu_new_mode(PortMode new_mode, int update_rate) +fmu_new_mode(PortMode new_mode) { uint32_t gpio_bits; PX4FMU::Mode servo_mode; @@ -809,9 +938,6 @@ fmu_new_mode(PortMode new_mode, int update_rate) /* (re)set the PWM output mode */ g_fmu->set_mode(servo_mode); - if ((servo_mode != PX4FMU::MODE_NONE) && (update_rate != 0)) - g_fmu->set_pwm_rate(update_rate); - return OK; } @@ -907,57 +1033,31 @@ int fmu_main(int argc, char *argv[]) { PortMode new_mode = PORT_MODE_UNSET; - int pwm_update_rate_in_hz = 0; - - if (!strcmp(argv[1], "test")) - test(); - - if (!strcmp(argv[1], "fake")) - fake(argc - 1, argv + 1); + const char *verb = argv[1]; if (fmu_start() != OK) errx(1, "failed to start the FMU driver"); /* * Mode switches. - * - * XXX use getopt? */ - for (int i = 1; i < argc; i++) { /* argv[0] is "fmu" */ - if (!strcmp(argv[i], "mode_gpio")) { - new_mode = PORT_FULL_GPIO; - - } else if (!strcmp(argv[i], "mode_serial")) { - new_mode = PORT_FULL_SERIAL; - - } else if (!strcmp(argv[i], "mode_pwm")) { - new_mode = PORT_FULL_PWM; - - } else if (!strcmp(argv[i], "mode_gpio_serial")) { - new_mode = PORT_GPIO_AND_SERIAL; + if (!strcmp(verb, "mode_gpio")) { + new_mode = PORT_FULL_GPIO; - } else if (!strcmp(argv[i], "mode_pwm_serial")) { - new_mode = PORT_PWM_AND_SERIAL; + } else if (!strcmp(verb, "mode_serial")) { + new_mode = PORT_FULL_SERIAL; - } else if (!strcmp(argv[i], "mode_pwm_gpio")) { - new_mode = PORT_PWM_AND_GPIO; - } + } else if (!strcmp(verb, "mode_pwm")) { + new_mode = PORT_FULL_PWM; - /* look for the optional pwm update rate for the supported modes */ - if (strcmp(argv[i], "-u") == 0 || strcmp(argv[i], "--update-rate") == 0) { - if (new_mode == PORT_FULL_PWM || new_mode == PORT_PWM_AND_GPIO) { - if (argc > i + 1) { - pwm_update_rate_in_hz = atoi(argv[i + 1]); + } else if (!strcmp(verb, "mode_gpio_serial")) { + new_mode = PORT_GPIO_AND_SERIAL; - } else { - errx(1, "missing argument for pwm update rate (-u)"); - return 1; - } + } else if (!strcmp(verb, "mode_pwm_serial")) { + new_mode = PORT_PWM_AND_SERIAL; - } else { - errx(1, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio"); - } - } + } else if (!strcmp(verb, "mode_pwm_gpio")) { + new_mode = PORT_PWM_AND_GPIO; } /* was a new mode set? */ @@ -968,12 +1068,17 @@ fmu_main(int argc, char *argv[]) return OK; /* switch modes */ - return fmu_new_mode(new_mode, pwm_update_rate_in_hz); + int ret = fmu_new_mode(new_mode); + exit(ret == OK ? 0 : 1); } - /* test, etc. here */ + if (!strcmp(verb, "test")) + test(); + + if (!strcmp(verb, "fake")) + fake(argc - 1, argv + 1); fprintf(stderr, "FMU: unrecognised command, try:\n"); - fprintf(stderr, " mode_gpio, mode_serial, mode_pwm [-u pwm_update_rate_in_hz], mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n"); + fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n"); exit(1); } diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 2c2c236ca..4f6938a14 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2012,2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,8 +35,7 @@ * @file px4io.cpp * Driver for the PX4IO board. * - * PX4IO is connected via serial (or possibly some other interface at a later - * point). + * PX4IO is connected via I2C. */ #include <nuttx/config.h> @@ -53,13 +52,13 @@ #include <stdio.h> #include <stdlib.h> #include <unistd.h> -#include <termios.h> #include <fcntl.h> #include <math.h> #include <arch/board/board.h> #include <drivers/device/device.h> +#include <drivers/device/i2c.h> #include <drivers/drv_rc_input.h> #include <drivers/drv_pwm_output.h> #include <drivers/drv_gpio.h> @@ -68,7 +67,6 @@ #include <systemlib/mixer/mixer.h> #include <systemlib/perf_counter.h> -#include <systemlib/hx_stream.h> #include <systemlib/err.h> #include <systemlib/systemlib.h> #include <systemlib/scheduling_priorities.h> @@ -78,75 +76,81 @@ #include <uORB/topics/actuator_controls_effective.h> #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/vehicle_status.h> +#include <uORB/topics/vehicle_command.h> #include <uORB/topics/rc_channels.h> #include <uORB/topics/battery_status.h> +#include <uORB/topics/parameter_update.h> #include <px4io/protocol.h> +#include <mavlink/mavlink_log.h> #include "uploader.h" +#include <debug.h> +#define PX4IO_SET_DEBUG _IOC(0xff00, 0) +#define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1) -class PX4IO : public device::CDev +class PX4IO : public device::I2C { public: PX4IO(); - ~PX4IO(); + virtual ~PX4IO(); virtual int init(); virtual int ioctl(file *filp, int cmd, unsigned long arg); + virtual ssize_t write(file *filp, const char *buffer, size_t len); /** - * Set the PWM via serial update rate - * @warning this directly affects CPU load - */ - int set_pwm_rate(int hz); + * Set the update rate for actuator outputs from FMU to IO. + * + * @param rate The rate in Hz actuator outpus are sent to IO. + * Min 10 Hz, max 400 Hz + */ + int set_update_rate(int rate); - bool dump_one; + /** + * Print the current status of IO + */ + void print_status(); private: // XXX - static const unsigned _max_actuators = PX4IO_CONTROL_CHANNELS; - unsigned _update_rate; ///< serial send rate in Hz + unsigned _max_actuators; + unsigned _max_controls; + unsigned _max_rc_input; + unsigned _max_relays; + unsigned _max_transfer; - int _serial_fd; ///< serial interface to PX4IO - hx_stream_t _io_stream; ///< HX protocol stream + unsigned _update_interval; ///< subscription interval limiting send rate volatile int _task; ///< worker task volatile bool _task_should_exit; - volatile bool _connected; ///< true once we have received a valid frame - int _t_actuators; ///< actuator output topic - actuator_controls_s _controls; ///< actuator outputs + int _mavlink_fd; - orb_advert_t _t_actuators_effective; ///< effective actuator controls topic - actuator_controls_effective_s _controls_effective; ///< effective controls + perf_counter_t _perf_update; + /* cached IO state */ + uint16_t _status; + uint16_t _alarms; + + /* subscribed topics */ + int _t_actuators; ///< actuator controls topic int _t_armed; ///< system armed control topic - actuator_armed_s _armed; ///< system armed state int _t_vstatus; ///< system / vehicle status - vehicle_status_s _vstatus; ///< overall system state + int _t_param; ///< parameter update topic + /* advertised topics */ orb_advert_t _to_input_rc; ///< rc inputs from io - rc_input_values _input_rc; ///< rc input values - + orb_advert_t _to_actuators_effective; ///< effective actuator controls topic + orb_advert_t _to_outputs; ///< mixed servo outputs topic orb_advert_t _to_battery; ///< battery status / voltage - battery_status_s _battery_status;///< battery status data - orb_advert_t _t_outputs; ///< mixed outputs topic actuator_outputs_s _outputs; ///< mixed outputs - - const char *volatile _mix_buf; ///< mixer text buffer - volatile unsigned _mix_buf_len; ///< size of the mixer text buffer + actuator_controls_effective_s _controls_effective; ///< effective controls bool _primary_pwm_device; ///< true if we are the default PWM output - uint32_t _relays; ///< state of the PX4IO relays, one bit per relay - - volatile bool _switch_armed; ///< PX4IO switch armed state - // XXX how should this work? - - bool _send_needed; ///< If true, we need to send a packet to IO - bool _config_needed; ///< if true, we need to set a config update to IO /** * Trampoline to the worker task @@ -159,43 +163,125 @@ private: void task_main(); /** - * Handle receiving bytes from PX4IO + * Send controls to IO + */ + int io_set_control_state(); + + /** + * Update IO's arming-related state + */ + int io_set_arming_state(); + + /** + * Push RC channel configuration to IO. + */ + int io_set_rc_config(); + + /** + * Fetch status and alarms from IO + * + * Also publishes battery voltage/current. + */ + int io_get_status(); + + /** + * Fetch RC inputs from IO. + * + * @param input_rc Input structure to populate. + * @return OK if data was returned. + */ + int io_get_raw_rc_input(rc_input_values &input_rc); + + /** + * Fetch and publish raw RC input data. + */ + int io_publish_raw_rc(); + + /** + * Fetch and publish the mixed control values. */ - void io_recv(); + int io_publish_mixed_controls(); /** - * HX protocol callback trampoline. + * Fetch and publish the PWM servo outputs. */ - static void rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_received); + int io_publish_pwm_outputs(); /** - * Callback invoked when we receive a whole packet from PX4IO + * write register(s) + * + * @param page Register page to write to. + * @param offset Register offset to start writing at. + * @param values Pointer to array of values to write. + * @param num_values The number of values to write. + * @return Zero if all values were successfully written. */ - void rx_callback(const uint8_t *buffer, size_t bytes_received); + int io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); /** - * Send an update packet to PX4IO + * write a register + * + * @param page Register page to write to. + * @param offset Register offset to write to. + * @param value Value to write. + * @return Zero if the value was written successfully. */ - void io_send(); + int io_reg_set(uint8_t page, uint8_t offset, const uint16_t value); /** - * Send a config packet to PX4IO + * read register(s) + * + * @param page Register page to read from. + * @param offset Register offset to start reading from. + * @param values Pointer to array where values should be stored. + * @param num_values The number of values to read. + * @return Zero if all values were successfully read. */ - void config_send(); + int io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values); /** - * Send a buffer containing mixer text to PX4IO + * read a register + * + * @param page Register page to read from. + * @param offset Register offset to start reading from. + * @return Register value that was read, or _io_reg_get_error on error. + */ + uint32_t io_reg_get(uint8_t page, uint8_t offset); + static const uint32_t _io_reg_get_error = 0x80000000; + + /** + * modify a register + * + * @param page Register page to modify. + * @param offset Register offset to modify. + * @param clearbits Bits to clear in the register. + * @param setbits Bits to set in the register. + */ + int io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t setbits); + + /** + * Send mixer definition text to IO */ int mixer_send(const char *buf, unsigned buflen); /** - * Mixer control callback; invoked to fetch a control from a specific - * group/index during mixing. + * Handle a status update from IO. + * + * Publish IO status information if necessary. + * + * @param status The status register + */ + int io_handle_status(uint16_t status); + + /** + * Handle an alarm update from IO. + * + * Publish IO alarm information if necessary. + * + * @param alarm The status register */ - static int control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input); + int io_handle_alarms(uint16_t alarms); + }; @@ -207,30 +293,35 @@ PX4IO *g_dev; } PX4IO::PX4IO() : - CDev("px4io", "/dev/px4io"), - dump_one(false), - _update_rate(50), - _serial_fd(-1), - _io_stream(nullptr), + I2C("px4io", "/dev/px4io", PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000), + _max_actuators(0), + _max_controls(0), + _max_rc_input(0), + _max_relays(0), + _max_transfer(16), /* sensible default */ + _update_interval(0), _task(-1), _task_should_exit(false), - _connected(false), + _mavlink_fd(-1), + _perf_update(perf_alloc(PC_ELAPSED, "px4io update")), + _status(0), + _alarms(0), _t_actuators(-1), - _t_actuators_effective(-1), _t_armed(-1), _t_vstatus(-1), - _t_outputs(-1), - _mix_buf(nullptr), - _mix_buf_len(0), - _primary_pwm_device(false), - _relays(0), - _switch_armed(false), - _send_needed(false), - _config_needed(true) + _t_param(-1), + _to_input_rc(0), + _to_actuators_effective(0), + _to_outputs(0), + _to_battery(0), + _primary_pwm_device(false) { /* we need this potentially before it could be set in task_main */ g_dev = this; + /* open MAVLink text channel */ + _mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); + _debug_enabled = true; } @@ -260,11 +351,154 @@ PX4IO::init() ASSERT(_task == -1); /* do regular cdev init */ - ret = CDev::init(); + ret = I2C::init(); + if (ret != OK) + return ret; + /* + * Enable a couple of retries for operations to IO. + * + * Register read/write operations are intentionally idempotent + * so this is safe as designed. + */ + _retries = 2; + + /* get some parameters */ + _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 < 1) || (_max_relays > 255) || + (_max_transfer < 16) || (_max_transfer > 255) || + (_max_rc_input < 1) || (_max_rc_input > 255)) { + + log("failed getting parameters from PX4IO"); + mavlink_log_emergency(_mavlink_fd, "[IO] param read fail, abort."); + return -1; + } + if (_max_rc_input > RC_INPUT_MAX_CHANNELS) + _max_rc_input = RC_INPUT_MAX_CHANNELS; + + /* + * Check for IO flight state - if FMU was flagged to be in + * armed state, FMU is recovering from an in-air reset. + * Read back status and request the commander to arm + * in this case. + */ + + uint16_t reg; + + /* 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; + /* + * in-air restart is only tried if the IO board reports it is + * already armed, and has been configured for in-air restart + */ + if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) && + (reg & PX4IO_P_SETUP_ARMING_ARM_OK)) { + + mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART"); + + /* WARNING: COMMANDER app/vehicle status must be initialized. + * If this fails (or the app is not started), worst-case IO + * remains untouched (so manual override is still available). + */ + + int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + /* fill with initial values, clear updated flag */ + vehicle_status_s status; + uint64_t try_start_time = hrt_absolute_time(); + bool updated = false; + + /* keep checking for an update, ensure we got a recent state, + not something that was published a long time ago. */ + do { + orb_check(vstatus_sub, &updated); + + if (updated) { + /* got data, copy and exit loop */ + orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status); + break; + } + + /* wait 10 ms */ + usleep(10000); + + /* abort after 5s */ + if ((hrt_absolute_time() - try_start_time)/1000 > 50000) { + log("failed to recover from in-air restart (1), aborting IO driver init."); + return 1; + } + + } while (true); + + /* send command to arm system via command API */ + vehicle_command_s cmd; + /* request arming */ + cmd.param1 = 1.0f; + cmd.param2 = 0; + cmd.param3 = 0; + cmd.param4 = 0; + cmd.param5 = 0; + cmd.param6 = 0; + cmd.param7 = 0; + cmd.command = VEHICLE_CMD_COMPONENT_ARM_DISARM; + cmd.target_system = status.system_id; + cmd.target_component = status.component_id; + cmd.source_system = status.system_id; + cmd.source_component = status.component_id; + /* ask to confirm command */ + cmd.confirmation = 1; + + /* send command once */ + (void)orb_advertise(ORB_ID(vehicle_command), &cmd); + + /* spin here until IO's state has propagated into the system */ + do { + orb_check(vstatus_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status); + } + + /* wait 10 ms */ + usleep(10000); + + /* abort after 5s */ + if ((hrt_absolute_time() - try_start_time)/1000 > 50000) { + log("failed to recover from in-air restart (2), aborting IO driver init."); + return 1; + } + + /* keep waiting for state change for 10 s */ + } while (!status.flag_system_armed); + + /* 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_ARM_OK | + PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | + PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | + PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK, 0); + + /* 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"); + return ret; + } + + } + /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); @@ -281,36 +515,11 @@ PX4IO::init() return -errno; } - /* wait a second for it to detect IO */ - for (unsigned i = 0; i < 10; i++) { - if (_connected) { - debug("PX4IO connected"); - break; - } - - usleep(100000); - } - - if (!_connected) { - /* error here will result in everything being torn down */ - log("PX4IO not responding"); - return -EIO; - } + mavlink_log_info(_mavlink_fd, "[IO] init ok"); return OK; } -int -PX4IO::set_pwm_rate(int hz) -{ - if (hz > 0 && hz <= 400) { - _update_rate = hz; - return OK; - } else { - return -EINVAL; - } -} - void PX4IO::task_main_trampoline(int argc, char *argv[]) { @@ -320,39 +529,9 @@ PX4IO::task_main_trampoline(int argc, char *argv[]) void PX4IO::task_main() { - log("starting"); - unsigned update_rate_in_ms; - - /* open the serial port */ - _serial_fd = ::open("/dev/ttyS2", O_RDWR); + hrt_abstime last_poll_time = 0; - if (_serial_fd < 0) { - log("failed to open serial port: %d", errno); - goto out; - } - - /* 115200bps, no parity, one stop bit */ - { - struct termios t; - - tcgetattr(_serial_fd, &t); - cfsetspeed(&t, 115200); - t.c_cflag &= ~(CSTOPB | PARENB); - tcsetattr(_serial_fd, TCSANOW, &t); - } - - /* protocol stream */ - _io_stream = hx_stream_init(_serial_fd, &PX4IO::rx_callback_trampoline, this); - - if (_io_stream == nullptr) { - log("failed to allocate HX protocol stream"); - goto out; - } - - hx_stream_set_counters(_io_stream, - perf_alloc(PC_COUNT, "PX4IO frames transmitted"), - perf_alloc(PC_COUNT, "PX4IO frames received"), - perf_alloc(PC_COUNT, "PX4IO receive errors")); + log("starting"); /* * Subscribe to the appropriate PWM output topic based on whether we are the @@ -360,9 +539,7 @@ PX4IO::task_main() */ _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1)); - /* convert the update rate in hz to milliseconds, rounding down if necessary */ - update_rate_in_ms = 1000 / _update_rate; - orb_set_interval(_t_actuators, update_rate_in_ms); + orb_set_interval(_t_actuators, 20); /* default to 50Hz */ _t_armed = orb_subscribe(ORB_ID(actuator_armed)); orb_set_interval(_t_armed, 200); /* 5Hz update rate */ @@ -370,33 +547,18 @@ PX4IO::task_main() _t_vstatus = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */ - /* advertise the limited control inputs */ - memset(&_controls_effective, 0, sizeof(_controls_effective)); - _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_1), - &_controls_effective); - - /* advertise the mixed control outputs */ - memset(&_outputs, 0, sizeof(_outputs)); - _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), - &_outputs); - - /* advertise the rc inputs */ - memset(&_input_rc, 0, sizeof(_input_rc)); - _to_input_rc = orb_advertise(ORB_ID(input_rc), &_input_rc); - - /* do not advertise the battery status until its clear that a battery is connected */ - memset(&_battery_status, 0, sizeof(_battery_status)); - _to_battery = -1; + _t_param = orb_subscribe(ORB_ID(parameter_update)); + orb_set_interval(_t_param, 500); /* 2Hz update rate max. */ /* poll descriptor */ pollfd fds[4]; - fds[0].fd = _serial_fd; + fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_actuators; + fds[1].fd = _t_armed; fds[1].events = POLLIN; - fds[2].fd = _t_armed; + fds[2].fd = _t_vstatus; fds[2].events = POLLIN; - fds[3].fd = _t_vstatus; + fds[3].fd = _t_param; fds[3].events = POLLIN; debug("ready"); @@ -404,12 +566,22 @@ PX4IO::task_main() /* lock against the ioctl handler */ lock(); - /* loop handling received serial bytes */ + /* loop talking to IO */ while (!_task_should_exit) { - /* sleep waiting for data, but no more than 100ms */ + /* adjust update interval */ + if (_update_interval != 0) { + if (_update_interval < 5) + _update_interval = 5; + if (_update_interval > 100) + _update_interval = 100; + orb_set_interval(_t_actuators, _update_interval); + _update_interval = 0; + } + + /* sleep waiting for topic updates, but no more than 20ms */ unlock(); - int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 100); + int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 20); lock(); /* this would be bad... */ @@ -418,76 +590,64 @@ PX4IO::task_main() continue; } - /* if we timed out waiting, we should send an update */ - if (ret == 0) - _send_needed = true; - - if (ret > 0) { - /* if we have new data from IO, go handle it */ - if (fds[0].revents & POLLIN) - io_recv(); - - /* if we have new control data from the ORB, handle it */ - if (fds[1].revents & POLLIN) { - - /* get controls */ - orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - ORB_ID(actuator_controls_1), _t_actuators, &_controls); - - /* scale controls to PWM (temporary measure) */ - for (unsigned i = 0; i < _max_actuators; i++) - _outputs.output[i] = 1500 + (600 * _controls.control[i]); - - /* and flag for update */ - _send_needed = true; - } - - /* if we have an arming state update, handle it */ - if (fds[2].revents & POLLIN) { - - orb_copy(ORB_ID(actuator_armed), _t_armed, &_armed); - _send_needed = true; - } - + perf_begin(_perf_update); + hrt_abstime now = hrt_absolute_time(); + + /* if we have new control data from the ORB, handle it */ + 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 it's time for another tick of the polling status machine, + * try it now. + */ + if ((now - last_poll_time) >= 20000) { + + /* + * Pull status and alarms from IO. + */ + io_get_status(); + + /* + * 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. + */ + io_publish_mixed_controls(); + io_publish_pwm_outputs(); + + /* + * If parameters have changed, re-send RC mappings to IO + * + * XXX this may be a bit spammy + */ if (fds[3].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_status), _t_vstatus, &_vstatus); - _send_needed = true; - } - } - - /* send a config packet to IO if required */ - if (_config_needed) { - _config_needed = false; - config_send(); - } + parameter_update_s pupdate; - /* send a mixer update if needed */ - if (_mix_buf != nullptr) { - mixer_send(_mix_buf, _mix_buf_len); + /* copy to reset the notification */ + orb_copy(ORB_ID(parameter_update), _t_param, &pupdate); - /* clear the buffer record so the ioctl handler knows we're done */ - _mix_buf = nullptr; - _mix_buf_len = 0; + /* re-upload RC input config as it may have changed */ + io_set_rc_config(); + } } - /* send an update to IO if required */ - if (_send_needed) { - _send_needed = false; - io_send(); - } + perf_end(_perf_update); } unlock(); -out: debug("exiting"); - /* kill the HX stream */ - if (_io_stream != nullptr) - hx_stream_free(_io_stream); - - ::close(_serial_fd); - /* clean up the alternate device node */ if (_primary_pwm_device) unregister_driver(PWM_OUTPUT_DEVICE_PATH); @@ -498,208 +658,496 @@ out: } int -PX4IO::control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input) +PX4IO::io_set_control_state() { - const actuator_controls_s *controls = (actuator_controls_s *)handle; + actuator_controls_s controls; ///< actuator outputs + uint16_t regs[_max_actuators]; - input = controls->control[control_index]; - return 0; + /* get controls */ + orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_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]); + + /* copy values to registers in IO */ + return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls); } -void -PX4IO::io_recv() +int +PX4IO::io_set_arming_state() { - uint8_t buf[32]; - int count; + actuator_armed_s armed; ///< system armed state + vehicle_status_s vstatus; ///< overall system state - /* - * We are here because poll says there is some data, so this - * won't block even on a blocking device. If more bytes are - * available, we'll go back to poll() again... - */ - count = ::read(_serial_fd, buf, sizeof(buf)); + orb_copy(ORB_ID(actuator_armed), _t_armed, &armed); + orb_copy(ORB_ID(vehicle_status), _t_vstatus, &vstatus); - /* pass received bytes to the packet decoder */ - for (int i = 0; i < count; i++) - hx_stream_rx(_io_stream, buf[i]); + uint16_t set = 0; + uint16_t clear = 0; + + if (armed.armed) { + set |= PX4IO_P_SETUP_ARMING_ARM_OK; + } else { + clear |= PX4IO_P_SETUP_ARMING_ARM_OK; + } + if (vstatus.flag_vector_flight_mode_ok) { + set |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK; + } else { + clear |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK; + } + if (vstatus.flag_external_manual_override_ok) { + set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; + } else { + clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; + } + + return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set); } -void -PX4IO::rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_received) +int +PX4IO::io_set_rc_config() { - g_dev->rx_callback((const uint8_t *)buffer, bytes_received); + unsigned offset = 0; + int input_map[_max_rc_input]; + int32_t ichan; + int ret = OK; + + /* + * Generate the input channel -> control channel mapping table; + * assign RC_MAP_ROLL/PITCH/YAW/THROTTLE to the canonical + * controls. + */ + for (unsigned i = 0; i < _max_rc_input; i++) + input_map[i] = -1; + + /* + * NOTE: The indices for mapped channels are 1-based + * for compatibility reasons with existing + * 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++; + + /* + * Iterate all possible RC inputs. + */ + for (unsigned i = 0; i < _max_rc_input; i++) { + uint16_t regs[PX4IO_P_RC_CONFIG_STRIDE]; + char pname[16]; + float fval; + + /* + * RC params are floats, but do only + * contain integer values. Do not scale + * or cast them, let the auto-typeconversion + * do its job here. + * Channels: 500 - 2500 + * Inverted flag: -1 (inverted) or 1 (normal) + */ + + sprintf(pname, "RC%d_MIN", i + 1); + param_get(param_find(pname), &fval); + regs[PX4IO_P_RC_CONFIG_MIN] = fval; + + sprintf(pname, "RC%d_TRIM", i + 1); + param_get(param_find(pname), &fval); + regs[PX4IO_P_RC_CONFIG_CENTER] = fval; + + sprintf(pname, "RC%d_MAX", i + 1); + param_get(param_find(pname), &fval); + regs[PX4IO_P_RC_CONFIG_MAX] = fval; + + sprintf(pname, "RC%d_DZ", i + 1); + param_get(param_find(pname), &fval); + regs[PX4IO_P_RC_CONFIG_DEADZONE] = fval; + + regs[PX4IO_P_RC_CONFIG_ASSIGNMENT] = input_map[i]; + + regs[PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + sprintf(pname, "RC%d_REV", i + 1); + param_get(param_find(pname), &fval); + + /* + * This has been taken for the sake of compatibility + * with APM's setup / mission planner: normal: 1, + * inverted: -1 + */ + if (fval < 0) { + regs[PX4IO_P_RC_CONFIG_OPTIONS] |= PX4IO_P_RC_CONFIG_OPTIONS_REVERSE; + } + + /* 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; + } + + /* check the IO initialisation flag */ + if (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_INIT_OK)) { + log("config for RC%d rejected by IO", i + 1); + break; + } + + offset += PX4IO_P_RC_CONFIG_STRIDE; + } + + return ret; } -void -PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received) +int +PX4IO::io_handle_status(uint16_t status) { - const px4io_report *rep = (const px4io_report *)buffer; + int ret = 1; + /** + * WARNING: This section handles in-air resets. + */ -// lock(); + /* check for IO reset - force it back to armed if necessary */ + if (_status & PX4IO_P_STATUS_FLAGS_ARMED && !(status & PX4IO_P_STATUS_FLAGS_ARMED) + && !(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_ARMED | PX4IO_P_STATUS_FLAGS_ARM_SYNC); - /* sanity-check the received frame size */ - if (bytes_received != sizeof(px4io_report)) { - debug("got %u expected %u", bytes_received, sizeof(px4io_report)); - goto out; - } + /* set new status */ + _status = status; + _status &= PX4IO_P_STATUS_FLAGS_ARMED; + } else if (!(_status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { - if (rep->i2f_magic != I2F_MAGIC) { - debug("bad magic"); - goto out; + /* set the sync flag */ + ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARM_SYNC); + /* set new status */ + _status = status; + + } else { + ret = 0; + + /* set new status */ + _status = status; } - _connected = true; + return ret; +} + +int +PX4IO::io_handle_alarms(uint16_t alarms) +{ + + /* XXX handle alarms */ - /* publish raw rc channel values from IO if valid channels are present */ - if (rep->channel_count > 0) { - _input_rc.timestamp = hrt_absolute_time(); - _input_rc.channel_count = rep->channel_count; - for (int i = 0; i < rep->channel_count; i++) { - _input_rc.values[i] = rep->rc_channel[i]; - } + /* set new alarms state */ + _alarms = alarms; - orb_publish(ORB_ID(input_rc), _to_input_rc, &_input_rc); - } + return 0; +} - /* remember the latched arming switch state */ - _switch_armed = rep->armed; +int +PX4IO::io_get_status() +{ + uint16_t regs[4]; + int ret; + + /* 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; - /* publish battery information */ + io_handle_status(regs[0]); + io_handle_alarms(regs[1]); /* only publish if battery has a valid minimum voltage */ - if (rep->battery_mv > 3300) { - _battery_status.timestamp = hrt_absolute_time(); - _battery_status.voltage_v = rep->battery_mv / 1000.0f; - /* current and discharge are unknown */ - _battery_status.current_a = -1.0f; - _battery_status.discharged_mah = -1.0f; - /* announce the battery voltage if needed, just publish else */ + if (regs[2] > 3300) { + battery_status_s battery_status; + + battery_status.timestamp = hrt_absolute_time(); + + /* voltage is scaled to mV */ + battery_status.voltage_v = regs[2] / 1000.0f; + + /* current scaling should be to cA in order to avoid limiting at 65A */ + battery_status.current_a = regs[3] / 100.f; + + /* this requires integration over time - not currently implemented */ + battery_status.discharged_mah = -1.0f; + + /* lazily publish the battery voltage */ if (_to_battery > 0) { - orb_publish(ORB_ID(battery_status), _to_battery, &_battery_status); + orb_publish(ORB_ID(battery_status), _to_battery, &battery_status); } else { - _to_battery = orb_advertise(ORB_ID(battery_status), &_battery_status); + _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); } } + return ret; +} + +int +PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) +{ + uint32_t channel_count; + int ret = OK; + + /* we don't have the status bits, so input_source has to be set elsewhere */ + input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN; + + /* + * XXX Because the channel count and channel data are fetched + * separately, there is a risk of a race between the two + * that could leave us with channel data and a count that + * are out of sync. + * Fixing this would require a guarantee of atomicity from + * IO, and a single fetch for both count and channels. + * + * XXX Since IO has the input calibration info, we ought to be + * able to get the pre-fixed-up controls directly. + * + * XXX can we do this more cheaply? If we knew we had DMA, it would + * almost certainly be better to just get all the inputs... + */ + channel_count = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); + if (channel_count == _io_reg_get_error) + return -EIO; + if (channel_count > RC_INPUT_MAX_CHANNELS) + channel_count = RC_INPUT_MAX_CHANNELS; + input_rc.channel_count = channel_count; + + if (channel_count > 0) { + ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, input_rc.values, channel_count); + if (ret == OK) + input_rc.timestamp = hrt_absolute_time(); + } - _send_needed = true; + return ret; +} - /* if monitoring, dump the received info */ - if (dump_one) { - dump_one = false; +int +PX4IO::io_publish_raw_rc() +{ + /* if no raw RC, just don't publish */ + if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK)) + return OK; - printf("IO: %s armed ", rep->armed ? "" : "not"); + /* fetch values from IO */ + rc_input_values rc_val; + rc_val.timestamp = hrt_absolute_time(); - for (unsigned i = 0; i < rep->channel_count; i++) - printf("%d: %d ", i, rep->rc_channel[i]); + 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; + } - printf("\n"); + /* lazily advertise on first publication */ + if (_to_input_rc == 0) { + _to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_val); + } else { + orb_publish(ORB_ID(input_rc), _to_input_rc, &rc_val); } -out: -// unlock(); - return; + return OK; } -void -PX4IO::io_send() +int +PX4IO::io_publish_mixed_controls() { - px4io_command cmd; - int ret; + /* if no FMU comms(!) just don't publish */ + if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK)) + return OK; - cmd.f2i_magic = F2I_MAGIC; - - /* set outputs */ - for (unsigned i = 0; i < _max_actuators; i++) { - cmd.output_control[i] = _outputs.output[i]; - } - /* publish as we send */ - _outputs.timestamp = hrt_absolute_time(); - /* XXX needs to be based off post-mix values from the IO side */ - orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &_outputs); - - /* update relays */ - for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) - cmd.relay_state[i] = (_relays & (1<< i)) ? true : false; - - /* armed and not locked down -> arming ok */ - cmd.arm_ok = (_armed.armed && !_armed.lockdown); - /* indicate that full autonomous position control / vector flight mode is available */ - cmd.vector_flight_mode_ok = _vstatus.flag_vector_flight_mode_ok; - /* allow manual override on IO (not allowed for multirotors or other systems with SAS) */ - cmd.manual_override_ok = _vstatus.flag_external_manual_override_ok; - /* set desired PWM output rate */ - cmd.servo_rate = _update_rate; - - ret = hx_stream_send(_io_stream, &cmd, sizeof(cmd)); + /* if not taking raw PPM from us, must be mixing */ + if (_status & PX4IO_P_STATUS_FLAGS_RAW_PWM) + return OK; - if (ret) - debug("send error %d", ret); + /* data we are going to fetch */ + actuator_controls_effective_s controls_effective; + controls_effective.timestamp = hrt_absolute_time(); + + /* 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; + + /* convert from register format to float */ + for (unsigned i = 0; i < _max_actuators; i++) + controls_effective.control_effective[i] = REG_TO_FLOAT(act[i]); + + /* 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); + } else { + orb_publish((_primary_pwm_device ? + ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : + ORB_ID(actuator_controls_effective_1)), + _to_actuators_effective, &controls_effective); + } + + return OK; } -void -PX4IO::config_send() +int +PX4IO::io_publish_pwm_outputs() { - px4io_config cfg; - int ret; - - cfg.f2i_config_magic = F2I_CONFIG_MAGIC; + /* if no FMU comms(!) just don't publish */ + if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK)) + return OK; - int val; + /* data we are going to fetch */ + actuator_outputs_s outputs; + outputs.timestamp = hrt_absolute_time(); - /* maintaing the standard order of Roll, Pitch, Yaw, Throttle */ - param_get(param_find("RC_MAP_ROLL"), &val); - cfg.rc_map[0] = val; - param_get(param_find("RC_MAP_PITCH"), &val); - cfg.rc_map[1] = val; - param_get(param_find("RC_MAP_YAW"), &val); - cfg.rc_map[2] = val; - param_get(param_find("RC_MAP_THROTTLE"), &val); - cfg.rc_map[3] = val; + /* 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; - /* set the individual channel properties */ - char nbuf[16]; - float float_val; - for (unsigned i = 0; i < 4; i++) { - sprintf(nbuf, "RC%d_MIN", i + 1); - param_get(param_find(nbuf), &float_val); - cfg.rc_min[i] = float_val; - } - for (unsigned i = 0; i < 4; i++) { - sprintf(nbuf, "RC%d_TRIM", i + 1); - param_get(param_find(nbuf), &float_val); - cfg.rc_trim[i] = float_val; - } - for (unsigned i = 0; i < 4; i++) { - sprintf(nbuf, "RC%d_MAX", i + 1); - param_get(param_find(nbuf), &float_val); - cfg.rc_max[i] = float_val; - } - for (unsigned i = 0; i < 4; i++) { - sprintf(nbuf, "RC%d_REV", i + 1); - param_get(param_find(nbuf), &float_val); - cfg.rc_rev[i] = float_val; + /* 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); + } else { + orb_publish((_primary_pwm_device ? + ORB_ID_VEHICLE_CONTROLS : + ORB_ID(actuator_outputs_1)), + _to_outputs, + &outputs); } - for (unsigned i = 0; i < 4; i++) { - sprintf(nbuf, "RC%d_DZ", i + 1); - param_get(param_find(nbuf), &float_val); - cfg.rc_dz[i] = float_val; + + return OK; +} + +int +PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) +{ + /* range check the transfer */ + if (num_values > ((_max_transfer) / sizeof(*values))) { + debug("io_reg_set: too many registers (%u, max %u)", num_values, _max_transfer / 2); + return -EINVAL; } - ret = hx_stream_send(_io_stream, &cfg, sizeof(cfg)); + /* set up the transfer */ + uint8_t addr[2] = { + page, + offset + }; + i2c_msg_s msgv[2]; + + msgv[0].flags = 0; + msgv[0].buffer = addr; + msgv[0].length = 2; + msgv[1].flags = I2C_M_NORESTART; + msgv[1].buffer = (uint8_t *)values; + msgv[1].length = num_values * sizeof(*values); + + /* perform the transfer */ + int ret = transfer(msgv, 2); + if (ret != OK) + debug("io_reg_set: error %d", ret); + return ret; +} + +int +PX4IO::io_reg_set(uint8_t page, uint8_t offset, uint16_t value) +{ + return io_reg_set(page, offset, &value, 1); +} + +int +PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) +{ + /* set up the transfer */ + uint8_t addr[2] = { + page, + offset + }; + i2c_msg_s msgv[2]; + + msgv[0].flags = 0; + msgv[0].buffer = addr; + msgv[0].length = 2; + msgv[1].flags = I2C_M_READ; + msgv[1].buffer = (uint8_t *)values; + msgv[1].length = num_values * sizeof(*values); + + /* perform the transfer */ + int ret = transfer(msgv, 2); + if (ret != OK) + debug("io_reg_get: data error %d", ret); + return ret; +} + +uint32_t +PX4IO::io_reg_get(uint8_t page, uint8_t offset) +{ + uint16_t value; + + if (io_reg_get(page, offset, &value, 1)) + return _io_reg_get_error; + + return value; +} + +int +PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t setbits) +{ + int ret; + uint16_t value; + ret = io_reg_get(page, offset, &value, 1); if (ret) - debug("config error %d", ret); + return ret; + value &= ~clearbits; + value |= setbits; + + return io_reg_set(page, offset, value); } int PX4IO::mixer_send(const char *buf, unsigned buflen) { - uint8_t frame[HX_STREAM_MAX_FRAME]; + uint8_t frame[_max_transfer]; px4io_mixdata *msg = (px4io_mixdata *)&frame[0]; + unsigned max_len = _max_transfer - sizeof(px4io_mixdata); msg->f2i_mixer_magic = F2I_MIXER_MAGIC; msg->action = F2I_MIXER_ACTION_RESET; @@ -707,8 +1155,8 @@ PX4IO::mixer_send(const char *buf, unsigned buflen) do { unsigned count = buflen; - if (count > F2I_MIXER_MAX_TEXT) - count = F2I_MIXER_MAX_TEXT; + if (count > max_len) + count = max_len; if (count > 0) { memcpy(&msg->text[0], buf, count); @@ -716,7 +1164,20 @@ PX4IO::mixer_send(const char *buf, unsigned buflen) buflen -= count; } - int ret = hx_stream_send(_io_stream, msg, sizeof(px4io_mixdata) + count); + /* + * We have to send an even number of bytes. This + * will only happen on the very last transfer of a + * mixer, and we are guaranteed that there will be + * space left to round up as _max_transfer will be + * even. + */ + unsigned total_len = sizeof(px4io_mixdata) + count; + if (total_len % 1) { + msg->text[count] = '\0'; + total_len++; + } + + int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2); if (ret) { log("mixer send error %d", ret); @@ -727,7 +1188,133 @@ PX4IO::mixer_send(const char *buf, unsigned buflen) } while (buflen > 0); - return 0; + /* check for the mixer-OK flag */ + if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) { + debug("mixer upload OK"); + mavlink_log_info(_mavlink_fd, "[IO] mixer upload ok"); + return 0; + } else { + debug("mixer rejected by IO"); + mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail"); + } + + /* load must have failed for some reason */ + return -EINVAL; +} + +void +PX4IO::print_status() +{ + /* basic configuration */ + printf("protocol %u software %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_SOFTWARE_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)); + + /* status */ + printf("%u bytes free\n", + 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\n", + flags, + ((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " ARMED" : ""), + ((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) ? " DSM" : ""), + ((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_PPM" : ""), + ((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")); + uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS); + printf("alarms 0x%04x%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" : "")); + printf("vbatt %u ibatt %u\n", + io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT), + io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT)); + 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\n", + arming, + ((arming & PX4IO_P_SETUP_ARMING_ARM_OK) ? " ARM_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) ? " VECTOR_FLIGHT_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : "")); + 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)); + printf("vbatt scale %u ibatt scale %u ibatt bias %u\n", + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_IBATT_SCALE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_IBATT_BIAS)); + 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" : "")); + } + printf("failsafe"); + for (unsigned i = 0; i < _max_actuators; i++) + printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); + printf("\n"); } int @@ -735,141 +1322,290 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) { int ret = OK; - lock(); - /* regular ioctl? */ switch (cmd) { case PWM_SERVO_ARM: - /* fake an armed transition */ - _armed.armed = true; - _send_needed = true; + /* set the 'armed' bit */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_ARM_OK); break; case PWM_SERVO_DISARM: - /* fake a disarmed transition */ - _armed.armed = false; - _send_needed = true; + /* clear the 'armed' bit */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_ARM_OK, 0); break; case PWM_SERVO_SET_UPDATE_RATE: - // not supported yet - ret = -EINVAL; + /* set the requested alternate rate */ + if ((arg >= 50) && (arg <= 400)) { /* TODO: we could go higher for e.g. TurboPWM */ + ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg); + } else { + ret = -EINVAL; + } break; - case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1): + case PWM_SERVO_SELECT_UPDATE_RATE: { - /* fake an update to the selected 'servo' channel */ - if ((arg >= 900) && (arg <= 2100)) { - _outputs.output[cmd - PWM_SERVO_SET(0)] = arg; - _send_needed = true; + /* 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); - } else { + /* 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; + } + + case PWM_SERVO_GET_COUNT: + *(unsigned *)arg = _max_actuators; + break; + + 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); } break; + } + + case PWM_SERVO_GET(0) ... PWM_SERVO_GET(PWM_OUTPUT_MAX_CHANNELS - 1): { + + unsigned channel = cmd - PWM_SERVO_GET(0); + + 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; + } + } + break; + } + + case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): { + + unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0); - case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_actuators - 1): - /* copy the current output value from the channel */ - *(servo_position_t *)arg = _outputs.output[cmd - PWM_SERVO_GET(0)]; + uint32_t value = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel); + + *(uint32_t *)arg = value; break; + } case GPIO_RESET: - _relays = 0; - _send_needed = true; + ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0); break; case GPIO_SET: + arg &= ((1 << _max_relays) - 1); + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg); + break; + case GPIO_CLEAR: - /* make sure only valid bits are being set */ - if ((arg & ((1UL << PX4IO_RELAY_CHANNELS) - 1)) != arg) { - ret = EINVAL; - break; - } - if (cmd == GPIO_SET) { - _relays |= arg; - } else { - _relays &= ~arg; - } - _send_needed = true; + arg &= ((1 << _max_relays) - 1); + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0); break; case GPIO_GET: - *(uint32_t *)arg = _relays; + *(uint32_t *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS); + if (*(uint32_t *)arg == _io_reg_get_error) + ret = -EIO; break; case MIXERIOCGETOUTPUTCOUNT: - *(unsigned *)arg = PX4IO_CONTROL_CHANNELS; + *(unsigned *)arg = _max_actuators; break; case MIXERIOCRESET: ret = 0; /* load always resets */ break; - case MIXERIOCLOADBUF: + case MIXERIOCLOADBUF: { + const char *buf = (const char *)arg; + ret = mixer_send(buf, strnlen(buf, 1024)); + break; + } - /* set the buffer up for transfer */ - _mix_buf = (const char *)arg; - _mix_buf_len = strnlen(_mix_buf, 1024); + case RC_INPUT_GET: { + uint16_t status; + rc_input_values *rc_val = (rc_input_values *)arg; - /* drop the lock and wait for the thread to clear the transmit */ - unlock(); + ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &status, 1); + if (ret != OK) + break; - while (_mix_buf != nullptr) - usleep(1000); + /* if no R/C input, don't try to fetch anything */ + if (!(status & PX4IO_P_STATUS_FLAGS_RC_OK)) { + ret = -ENOTCONN; + break; + } - lock(); + /* 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; + } - ret = 0; + /* 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: - /* not a recognised value */ + /* not a recognized value */ ret = -ENOTTY; } - unlock(); - return ret; } +ssize_t +PX4IO::write(file *filp, const char *buffer, size_t len) +{ + unsigned count = len / 2; + + 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; +} + +int +PX4IO::set_update_rate(int rate) +{ + int interval_ms = 1000 / rate; + if (interval_ms < 5) { + interval_ms = 5; + warnx("update rate too high, limiting interval to %d ms (%d Hz).", interval_ms, 1000 / interval_ms); + } + + if (interval_ms > 100) { + interval_ms = 100; + warnx("update rate too low, limiting to %d ms (%d Hz).", interval_ms, 1000 / interval_ms); + } + + _update_interval = interval_ms; + return 0; +} + + extern "C" __EXPORT int px4io_main(int argc, char *argv[]); namespace { void -test(void) +start(int argc, char *argv[]) { - int fd; + if (g_dev != nullptr) + errx(1, "already loaded"); + + /* create the driver - it will set g_dev */ + (void)new PX4IO(); - fd = open(PWM_OUTPUT_DEVICE_PATH, 0); + if (g_dev == nullptr) + errx(1, "driver alloc failed"); - if (fd < 0) { - puts("open fail"); - exit(1); + if (OK != g_dev->init()) { + delete g_dev; + errx(1, "driver init failed"); } - ioctl(fd, PWM_SERVO_ARM, 0); - ioctl(fd, PWM_SERVO_SET(0), 1000); - ioctl(fd, PWM_SERVO_SET(1), 1100); - ioctl(fd, PWM_SERVO_SET(2), 1200); - ioctl(fd, PWM_SERVO_SET(3), 1300); - ioctl(fd, PWM_SERVO_SET(4), 1400); - ioctl(fd, PWM_SERVO_SET(5), 1500); - ioctl(fd, PWM_SERVO_SET(6), 1600); - ioctl(fd, PWM_SERVO_SET(7), 1700); + exit(0); +} - close(fd); +void +test(void) +{ + int fd; + unsigned servo_count = 0; + unsigned pwm_value = 1000; + int direction = 1; + int ret; - actuator_armed_s aa; + fd = open("/dev/px4io", O_WRONLY); - aa.armed = true; - aa.lockdown = false; + if (fd < 0) + err(1, "failed to open device"); - orb_advertise(ORB_ID(actuator_armed), &aa); + if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count)) + err(1, "failed to get servo count"); - exit(0); + if (ioctl(fd, PWM_SERVO_ARM, 0)) + err(1, "failed to arm servos"); + + for (;;) { + + /* 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; + } + } + + /* readback servo values */ + for (unsigned i = 0; i < servo_count; i++) { + servo_position_t value; + + 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]); + } + } } void @@ -893,8 +1629,10 @@ monitor(void) exit(0); } - if (g_dev != nullptr) - g_dev->dump_one = true; +#warning implement this + +// if (g_dev != nullptr) +// g_dev->dump_one = true; } } @@ -903,56 +1641,87 @@ monitor(void) int px4io_main(int argc, char *argv[]) { - if (!strcmp(argv[1], "start")) { - - if (g_dev != nullptr) - errx(1, "already loaded"); + /* check for sufficient number of arguments */ + if (argc < 2) + goto out; - /* create the driver - it will set g_dev */ - (void)new PX4IO; + if (!strcmp(argv[1], "start")) + start(argc - 1, argv + 1); - if (g_dev == nullptr) - errx(1, "driver alloc failed"); + if (!strcmp(argv[1], "limit")) { - if (OK != g_dev->init()) { - delete g_dev; - errx(1, "driver init failed"); - } + if (g_dev != nullptr) { - /* look for the optional pwm update rate for the supported modes */ - if (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0) { - if (argc > 2 + 1) { - g_dev->set_pwm_rate(atoi(argv[2 + 1])); + if ((argc > 2)) { + g_dev->set_update_rate(atoi(argv[2])); } else { - fprintf(stderr, "missing argument for pwm update rate (-u)\n"); + errx(1, "missing argument (50 - 200 Hz)"); return 1; } } + exit(0); + } + if (!strcmp(argv[1], "recovery")) { + + if (g_dev != nullptr) { + /* + * Enable in-air restart support. + * We can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ + g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1); + } else { + errx(1, "not loaded"); + } exit(0); } if (!strcmp(argv[1], "stop")) { - if (g_dev != nullptr) { - /* stop the driver */ - delete g_dev; - } else { - errx(1, "not loaded"); - } - exit(0); + if (g_dev != nullptr) { + /* stop the driver */ + delete g_dev; + } else { + errx(1, "not loaded"); } + exit(0); + } if (!strcmp(argv[1], "status")) { - if (g_dev != nullptr) - printf("[px4io] loaded\n"); - else - printf("[px4io] not loaded\n"); + if (g_dev != nullptr) { + printf("[px4io] loaded\n"); + g_dev->print_status(); + } else { + printf("[px4io] not loaded\n"); + } - exit(0); + exit(0); + } + + if (!strcmp(argv[1], "debug")) { + if (argc <= 2) { + 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(NULL, 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); + } /* note, stop not currently implemented */ @@ -1019,5 +1788,6 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "monitor")) monitor(); - errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor' or 'update'"); + out: + errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit' or 'update'"); } diff --git a/apps/drivers/px4io/uploader.h b/apps/drivers/px4io/uploader.h index 915ee9259..f983d1981 100644 --- a/apps/drivers/px4io/uploader.h +++ b/apps/drivers/px4io/uploader.h @@ -46,7 +46,7 @@ class PX4IO_Uploader { public: PX4IO_Uploader(); - ~PX4IO_Uploader(); + virtual ~PX4IO_Uploader(); int upload(const char *filenames[]); diff --git a/apps/drivers/stm32/drv_hrt.c b/apps/drivers/stm32/drv_hrt.c index cd9cb45b1..bb67d5e6d 100644 --- a/apps/drivers/stm32/drv_hrt.c +++ b/apps/drivers/stm32/drv_hrt.c @@ -646,6 +646,36 @@ abstime_to_ts(struct timespec *ts, hrt_abstime abstime) } /* + * Compare a time value with the current time. + */ +hrt_abstime +hrt_elapsed_time(const volatile hrt_abstime *then) +{ + irqstate_t flags = irqsave(); + + hrt_abstime delta = hrt_absolute_time() - *then; + + irqrestore(flags); + + return delta; +} + +/* + * Store the absolute time in an interrupt-safe fashion + */ +hrt_abstime +hrt_store_absolute_time(volatile hrt_abstime *now) +{ + irqstate_t flags = irqsave(); + + hrt_abstime ts = hrt_absolute_time(); + + irqrestore(flags); + + return ts; +} + +/* * Initalise the high-resolution timing module. */ void diff --git a/apps/drivers/stm32/drv_pwm_servo.c b/apps/drivers/stm32/drv_pwm_servo.c index 954b458f5..d7316e1f7 100644 --- a/apps/drivers/stm32/drv_pwm_servo.c +++ b/apps/drivers/stm32/drv_pwm_servo.c @@ -68,10 +68,6 @@ #include <stm32_gpio.h> #include <stm32_tim.h> - -/* default rate (in Hz) of PWM updates */ -static uint32_t pwm_update_rate = 50; - #define REG(_tmr, _reg) (*(volatile uint32_t *)(pwm_timers[_tmr].base + _reg)) #define rCR1(_tmr) REG(_tmr, STM32_GTIM_CR1_OFFSET) @@ -93,6 +89,10 @@ static uint32_t pwm_update_rate = 50; #define rDCR(_tmr) REG(_tmr, STM32_GTIM_DCR_OFFSET) #define rDMAR(_tmr) REG(_tmr, STM32_GTIM_DMAR_OFFSET) +static void pwm_timer_init(unsigned timer); +static void pwm_timer_set_rate(unsigned timer, unsigned rate); +static void pwm_channel_init(unsigned channel); + static void pwm_timer_init(unsigned timer) { @@ -113,11 +113,8 @@ pwm_timer_init(unsigned timer) /* configure the timer to free-run at 1MHz */ rPSC(timer) = (pwm_timers[timer].clock_freq / 1000000) - 1; - /* and update at the desired rate */ - rARR(timer) = (1000000 / pwm_update_rate) - 1; - - /* generate an update event; reloads the counter and all registers */ - rEGR(timer) = GTIM_EGR_UG; + /* default to updating at 50Hz */ + pwm_timer_set_rate(timer, 50); /* note that the timer is left disabled - arming is performed separately */ } @@ -272,19 +269,41 @@ up_pwm_servo_deinit(void) } int -up_pwm_servo_set_rate(unsigned rate) +up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate) { - if ((rate < 50) || (rate > 400)) + /* limit update rate to 1..10000Hz; somewhat arbitrary but safe */ + if (rate < 1) + return -ERANGE; + if (rate > 10000) return -ERANGE; - for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) { - if (pwm_timers[i].base != 0) - pwm_timer_set_rate(i, rate); - } + if ((group >= PWM_SERVO_MAX_TIMERS) || (pwm_timers[group].base == 0)) + return ERROR; + + pwm_timer_set_rate(group, rate); return OK; } +int +up_pwm_servo_set_rate(unsigned rate) +{ + for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) + up_pwm_servo_set_rate_group_update(i, rate); +} + +uint32_t +up_pwm_servo_get_rate_group(unsigned group) +{ + unsigned channels = 0; + + for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) { + if ((pwm_channels[i].gpio != 0) && (pwm_channels[i].timer_index == group)) + channels |= (1 << i); + } + return channels; +} + void up_pwm_servo_arm(bool armed) { @@ -299,8 +318,12 @@ up_pwm_servo_arm(bool armed) rCR1(i) |= GTIM_CR1_CEN | GTIM_CR1_ARPE; } else { - /* on disarm, just stop auto-reload so we don't generate runts */ - rCR1(i) &= ~GTIM_CR1_ARPE; + // XXX This leads to FMU PWM being still active + // but uncontrollable. Just disable the timer + // and risk a runt. + ///* on disarm, just stop auto-reload so we don't generate runts */ + //rCR1(i) &= ~GTIM_CR1_ARPE; + rCR1(i) = 0; } } } diff --git a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp b/apps/drivers/stm32/tone_alarm/tone_alarm.cpp index 6799a0a21..baa652d8a 100644 --- a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/apps/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -35,27 +35,59 @@ * Driver for the PX4 audio alarm port, /dev/tone_alarm. * * The tone_alarm driver supports a set of predefined "alarm" - * patterns and one user-supplied pattern. Patterns are ordered by - * priority, with a higher-priority pattern interrupting any - * lower-priority pattern that might be playing. + * tunes and one user-supplied tune. * * The TONE_SET_ALARM ioctl can be used to select a predefined - * alarm pattern, from 1 - <TBD>. Selecting pattern zero silences + * alarm tune, from 1 - <TBD>. Selecting tune zero silences * the alarm. * - * To supply a custom pattern, write an array of 1 - <TBD> tone_note - * structures to /dev/tone_alarm. The custom pattern has a priority - * of zero. + * Tunes follow the syntax of the Microsoft GWBasic/QBasic PLAY + * statement, with some exceptions and extensions. + * + * From Wikibooks: + * + * PLAY "[string expression]" + * + * Used to play notes and a score ... The tones are indicated by letters A through G. + * Accidentals are indicated with a "+" or "#" (for sharp) or "-" (for flat) + * immediately after the note letter. See this example: + * + * PLAY "C C# C C#" + * + * Whitespaces are ignored inside the string expression. There are also codes that + * set the duration, octave and tempo. They are all case-insensitive. PLAY executes + * the commands or notes the order in which they appear in the string. Any indicators + * that change the properties are effective for the notes following that indicator. + * + * Ln Sets the duration (length) of the notes. The variable n does not indicate an actual duration + * amount but rather a note type; L1 - whole note, L2 - half note, L4 - quarter note, etc. + * (L8, L16, L32, L64, ...). By default, n = 4. + * For triplets and quintets, use L3, L6, L12, ... and L5, L10, L20, ... series respectively. + * The shorthand notation of length is also provided for a note. For example, "L4 CDE L8 FG L4 AB" + * can be shortened to "L4 CDE F8G8 AB". F and G play as eighth notes while others play as quarter notes. + * On Sets the current octave. Valid values for n are 0 through 6. An octave begins with C and ends with B. + * Remember that C- is equivalent to B. + * < > Changes the current octave respectively down or up one level. + * Nn Plays a specified note in the seven-octave range. Valid values are from 0 to 84. (0 is a pause.) + * Cannot use with sharp and flat. Cannot use with the shorthand notation neither. + * MN Stand for Music Normal. Note duration is 7/8ths of the length indicated by Ln. It is the default mode. + * ML Stand for Music Legato. Note duration is full length of that indicated by Ln. + * MS Stand for Music Staccato. Note duration is 3/4ths of the length indicated by Ln. + * Pn Causes a silence (pause) for the length of note indicated (same as Ln). + * Tn Sets the number of "L4"s per minute (tempo). Valid values are from 32 to 255. The default value is T120. + * . When placed after a note, it causes the duration of the note to be 3/2 of the set duration. + * This is how to get "dotted" notes. "L4 C#." would play C sharp as a dotted quarter note. + * It can be used for a pause as well. + * + * Extensions/variations: + * + * MB MF The MF command causes the tune to play once and then stop. The MB command causes the + * tune to repeat when it ends. * - * Patterns will normally play once and then silence (if a pattern - * was overridden it will not resume). A pattern may be made to - * repeat by inserting a note with the duration set to - * DURATION_REPEAT. This pattern will loop until either a - * higher-priority pattern is started or pattern zero is requested - * via the ioctl. */ #include <nuttx/config.h> +#include <debug.h> #include <drivers/device/device.h> #include <drivers/drv_tone_alarm.h> @@ -69,6 +101,8 @@ #include <errno.h> #include <stdio.h> #include <unistd.h> +#include <math.h> +#include <ctype.h> #include <arch/board/board.h> #include <drivers/drv_hrt.h> @@ -202,141 +236,233 @@ public: virtual ssize_t write(file *filp, const char *buffer, size_t len); private: - static const unsigned _max_pattern = 6; - static const unsigned _pattern_none = _max_pattern + 1; - static const unsigned _pattern_user = 0; - static const unsigned _max_pattern_len = 40; - static const unsigned _note_max = TONE_NOTE_MAX; - - static const tone_note _patterns[_max_pattern][_max_pattern_len]; - static const uint16_t _notes[_note_max]; - - unsigned _current_pattern; - unsigned _next_note; - - hrt_call _note_end; - tone_note _user_pattern[_max_pattern_len]; - + static const unsigned _tune_max = 1024; // be reasonable about user tunes + static const char * const _default_tunes[]; + static const unsigned _default_ntunes; + static const uint8_t _note_tab[]; + + const char *_user_tune; + + const char *_tune; // current tune string + const char *_next; // next note in the string + + unsigned _tempo; + unsigned _note_length; + enum { MODE_NORMAL, MODE_LEGATO, MODE_STACCATO} _note_mode; + unsigned _octave; + unsigned _silence_length; // if nonzero, silence before next note + bool _repeat; // if true, tune restarts at end + + hrt_call _note_call; // HRT callout for note completion + + // Convert a note value in the range C1 to B7 into a divisor for + // the configured timer's clock. + // + unsigned note_to_divisor(unsigned note); + + // Calculate the duration in microseconds of play and silence for a + // note given the current tempo, length and mode and the number of + // dots following in the play string. + // + unsigned note_duration(unsigned &silence, unsigned note_length, unsigned dots); + + // Calculate the duration in microseconds of a rest corresponding to + // a given note length. + // + unsigned rest_duration(unsigned rest_length, unsigned dots); + + // Start playing the note + // + void start_note(unsigned note); + + // Stop playing the current note and make the player 'safe' + // + void stop_note(); + + // Start playing the tune + // + void start_tune(const char *tune); + + // Parse the next note out of the string and play it + // + void next_note(); + + // Find the next character in the string, discard any whitespace and + // return the canonical (uppercase) version. + // + int next_char(); + + // Extract a number from the string, consuming all the digit characters. + // + unsigned next_number(); + + // Consume dot characters from the string, returning the number consumed. + // + unsigned next_dots(); + + // hrt_call trampoline for next_note + // static void next_trampoline(void *arg); - void next(); - bool check(tone_note *pattern); - void stop(); -}; - -/* predefined patterns for alarms 1-_max_pattern */ -const tone_note ToneAlarm::_patterns[_max_pattern][_max_pattern_len] = { - { - {TONE_NOTE_A7, 12}, - {TONE_NOTE_D8, 12}, - {TONE_NOTE_C8, 12}, - {TONE_NOTE_A7, 12}, - {TONE_NOTE_D8, 12}, - {TONE_NOTE_C8, 12}, - {TONE_NOTE_D8, 4}, - {TONE_NOTE_C8, 4}, - {TONE_NOTE_D8, 4}, - {TONE_NOTE_C8, 4}, - {TONE_NOTE_D8, 4}, - {TONE_NOTE_C8, 4}, - }, - {{TONE_NOTE_B6, 100}, {TONE_NOTE_B6, DURATION_REPEAT}}, - {{TONE_NOTE_C7, 100}}, - {{TONE_NOTE_D7, 100}}, - {{TONE_NOTE_E7, 100}}, - { - //This is tetris ;) - {TONE_NOTE_C6, 40}, - {TONE_NOTE_G5, 20}, - {TONE_NOTE_G5S, 20}, - {TONE_NOTE_A5S, 40}, - {TONE_NOTE_G5S, 20}, - {TONE_NOTE_G5, 20}, - {TONE_NOTE_F5, 40}, - {TONE_NOTE_F5, 20}, - {TONE_NOTE_G5S, 20}, - {TONE_NOTE_C6, 40}, - {TONE_NOTE_A5S, 20}, - {TONE_NOTE_G5S, 20}, - {TONE_NOTE_G5, 60}, - {TONE_NOTE_G5S, 20}, - {TONE_NOTE_A5S, 40}, - {TONE_NOTE_C6, 40}, - {TONE_NOTE_G5S, 40}, - {TONE_NOTE_F5, 40}, - {TONE_NOTE_F5, 60}, - {TONE_NOTE_A5S, 40}, - {TONE_NOTE_C6S, 20}, - {TONE_NOTE_F6, 40}, - {TONE_NOTE_D6S, 20}, - {TONE_NOTE_C6S, 20}, - {TONE_NOTE_C6, 60}, - {TONE_NOTE_G5S, 20}, - {TONE_NOTE_C6, 40}, - {TONE_NOTE_A5S, 20}, - {TONE_NOTE_G5S, 20}, - {TONE_NOTE_G5, 40}, - {TONE_NOTE_G5, 20}, - {TONE_NOTE_G5S, 20}, - {TONE_NOTE_A5S, 40}, - {TONE_NOTE_C6, 40}, - {TONE_NOTE_G5S, 40}, - {TONE_NOTE_F5, 40}, - {TONE_NOTE_F5, 60}, - } }; -const uint16_t ToneAlarm::_notes[_note_max] = { - 63707, /* E4 */ - 60132, /* F4 */ - 56758, /* F#4/Gb4 */ - 53571, /* G4 */ - 50565, /* G#4/Ab4 */ - 47727, /* A4 */ - 45048, /* A#4/Bb4 */ - 42520, /* B4 */ - 40133, /* C5 */ - 37880, /* C#5/Db5 */ - 35755, /* D5 */ - 33748, /* D#5/Eb5 */ - 31853, /* E5 */ - 30066, /* F5 */ - 28378, /* F#5/Gb5 */ - 26786, /* G5 */ - 25282, /* G#5/Ab5 */ - 23863, /* A5 */ - 22524, /* A#5/Bb5 */ - 21260, /* B5 */ - 20066, /* C6 */ - 18940, /* C#6/Db6 */ - 17877, /* D6 */ - 16874, /* D#6/Eb6 */ - 15927, /* E6 */ - 15033, /* F6 */ - 14189, /* F#6/Gb6 */ - 13393, /* G6 */ - 12641, /* G#6/Ab6 */ - 11931, /* A6 */ - 11262, /* A#6/Bb6 */ - 10630, /* B6 */ - 10033, /* C7 */ - 9470, /* C#7/Db7 */ - 8938, /* D7 */ - 8437, /* D#7/Eb7 */ - 7963, /* E7 */ - 7516, /* F7 */ - 7094, /* F#7/Gb7 */ - 6696, /* G7 */ - 6320, /* G#7/Ab7 */ - 5965, /* A7 */ - 5631, /* A#7/Bb7 */ - 5315, /* B7 */ - 5016, /* C8 */ - 4735, /* C#8/Db8 */ - 4469, /* D8 */ - 4218 /* D#8/Eb8 */ +// predefined tune array +const char * const ToneAlarm::_default_tunes[] = { + "MFT240L8 O4aO5dc O4aO5dc O4aO5dc L16dcdcdcdc", // startup tune + "MBT200a8a8a8PaaaP", // ERROR tone + "MFT200e8a8a", // NotifyPositive tone + "MFT200e8e", // NotifyNeutral tone + "MFT200e8c8e8c8e8c8", // NotifyNegative tone + "MFT90O3C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C4", // charge! + "MFT60O3C32O2A32F16F16F32G32A32A+32O3C16C16C16O2A16", // dixie + "MFT90O2C16C16C16F8.A8C16C16C16F8.A4P16P8", // cucuracha + "MNT150L8O2GGABGBADGGABL4GL8F+", // yankee + "MFT200O3C4.O2A4.G4.F4.D8E8F8D4F8C2.O2G4.O3C4.O2A4.F4.D8E8F8G4A8G2P8", // daisy + "T200O2B4P8B16B16B4P8B16B16B8G+8E8G+8B8G+8B8O3E8" // william tell + "O2B8G+8E8G+8B8G+8B8O3E8O2B4P8B16B16B4P8B16" + "O2B16B4P8B16B16B4P8B16B16B8B16B16B8B8B8B16" + "O2B16B8B8B8B16B16B8B8B8B16B16B8B8B2B2B8P8" + "P4P4P8O1B16B16B8B16B16B8B16B16O2E8F+8G+8" + "O1B16B16B8B16B16O2E8G+16G+16F+8D+8O1B8B16" + "O1B16B8B16B16B8B16B16O2E8F+8G+8E16G+16B4" + "O2B16A16G+16F+16E8G+8E8O3B16B16B8B16B16B8" + "O3B16B16O4E8F+8G+8O3B16B16B8B16B16O4E8G+16" + "O4G+16F+8D+8O3B8B16B16B8B16B16B8B16B16O4E8" + "O4F+8G+8E16G+16B4B16A16G+16F+16E8G+8E8O3G+16" + "O3G+16G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8" + "O4C+8O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8" + "O3G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8" + "O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8" + "O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8" + "O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16" + "O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8O2B16" + "O2B16B8F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8" + "O2E4G+8F+8F+8F+8O3F+16F+16F+8F+16F+16F+8" + "O3G+8A8F+4A8G+8E4G+8F+8O2B16B16B8O1B16B16" + "O1B8B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16" + "O1B16O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16" + "O1B8B16B16O2E8F+8G+8E16G+16B4B16A16G+16F+16" + "O2E8G+8E8O3B16B16B8B16B16B8B16B16O4E8F+8" + "O4G+8O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8" + "O3B16B16B8B16B16B8B16B16O4E8F+8G+8E16G+16" + "O4B4B16A16G+16F+16E8G+8E8O3E64F64G64A64B64" + "O4C64D64E8E16E16E8E8G+4.F+8E8D+8E8C+8O3B16" + "O4C+16O3B16O4C+16O3B16O4C+16D+16E16O3A16" + "O3B16A16B16A16B16O4C+16D+16O3G+16A16G+16" + "O3A16G+16A16B16O4C+16O3F+16G+16F+16G+16F+16" + "O3G+16F+16G+16F+16G+16F+16D+16O2B16O3B16" + "O4C+16D+16E8D+8E8C+8O3B16O4C+16O3B16O4C+16" + "O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16" + "O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16" + "O3G+16F+16G+16F+16A16F+16E16E8P8C+4C+16O2C16" + "O3C+16O2C16O3D+16C+16O2B16A16A16G+16E16C+16" + "O2C+16C+16C+16C+16E16D+16O1C16G+16G+16G+16" + "O1G+16G+16G+16O2C+16E16G+16O3C+16C+16C+16" + "O3C+16C+16O2C16O3C+16O2C16O3D+16C+16O2B16" + "O2A16A16G+16E16C+16C+16C+16C+16C+16E16D+16" + "O1C16G+16G+16G+16G+16G+16G+16O2C+16E16G+16" + "O3C+16E16D+16C+16D+16O2C16G+16G+16G+16O3G+16" + "O3E16C+16D+16O2C16G+16G+16G+16O3G+16E16C+16" + "O3D+16O2B16G+16G+16A+16G16D+16D+16G+16G16" + "O2G+16G16G+16A16G+16F+16E16O1B16A+16B16O2E16" + "O1B16O2F+16O1B16O2G+16E16D+16E16G+16E16A16" + "O2F+16B16O3G+16F+16E16D+16F+16E16C+16O2B16" + "O3C+16O2B16O3C+16D+16E16F+16G+16O2A16B16" + "O2A16B16O3C+16D+16E16F+16O2G+16A16G+16A16" + "O2C16O3C+16D+16E16O2F+16G+16F+16G+16F+16" + "O2G+16F+16G+16F+16G+16F+16D+16O1B16C16O2C+16" + "O2D+16E16O1B16A+16B16O2E16O1B16O2F+16O1B16" + "O2G+16E16D+16E16G+16E16A16F+16B16O3G+16F+16" + "O3E16D+16F+16E16C+16O2B16O3C+16O2B16O3C+16" + "O3D+16E16F+16G+16O2A16B16A16B16O3C+16D+16" + "O3E16F+16O2G+16A16G+16A16B16O3C+16D+16E16" + "O2F+16O3C+16O2C16O3C+16D+16C+16O2A16F+16" + "O2E16O3E16F+16G+16A16B16O4C+16D+16E8E16E16" + "O4E8E8G+4.F8E8D+8E8C+8O3B16O4C+16O3B16O4C+16" + "O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16" + "O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16" + "O3G+16F+16G+16F+16G+16F+16G+16F+16G+16F+16" + "O3D+16O2B16O3B16O4C+16D+16E8E16E16E8E8G+4." + "O4F+8E8D+8E8C+8O3B16O4C+16O3B16O4C+16O3B16" + "O4C+16D+16E16O3A16B16A16B16A16B16O4C+16D+16" + "O3G+16A16G+16A16G+16A16B16O4C+16O3F+16G+16" + "O3F+16G+16F+16A16G+16F+16E8O2B8O3E8G+16G+16" + "O3G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8" + "O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8G+16" + "O3G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8" + "O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8" + "O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8" + "O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16" + "O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8A+8" + "O3B8O2F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8" + "O2E4G+8F+8B8O1B8O2F+16F+16F+8F+16F+16F+8" + "O2G+8A8F+4A8G+8E4G+8F+8B16B16B8O1B16B16B8" + "O1B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16B16" + "O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16B8B16" + "O1B16O2E8F+8G+8E16G+16B4B16A16G+16F+16E8" + "O1B8O2E8O3B16B16B8B16B16B8B16B16O4E8F+8G+8" + "O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8B16" + "O3B16B8B16B16B8B16B16O4E8F+8G+8O3E16G+16" + "O3B4B16A16G+16F+16E16F+16G+16A16G+16A16B16" + "O4C+16O3B16O4C+16D+16E16D+16E16F+16G+16A16" + "O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16" + "O3B16O4A16O3B16O4A16O3B16O4A16O3B16E16F+16" + "O3G+16A16G+16A16B16O4C+16O3B16O4C+16D+16" + "O4E16D+16E16F+16G+16A16O3B16O4A16O3B16O4A16" + "O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16" + "O3B16O4A16O3B16P16G+16O4G+16O3G+16P16D+16" + "O4D+16O3D+16P16E16O4E16O3E16P16A16O4A16O3A16" + "P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16" + "P16O3E16O4E16O3E16P16A16O4A16O3A16O4G16O3G16" + "O4G16O3G16O4G16O3G16O4G16O3G16O4G8E8C8E8" + "O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+16" + "O3G+16O4G+8E8O3B8O4E8G+16O3G+16O4G+16O3G+16" + "O4G+16O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16" + "O3A+16O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16" + "O4A+8G8E8G8B8P16A+16P16A16P16G+16P16F+16" + "P16O4E16P16D+16P16C+16P16O3B16P16A+16P16" + "O3A16P16G+16P16F+16P16E16P16D+16P16F+16E16" + "O3F+16G+16A16G+16A16B16O4C+16O3B16O4C+16" + "O4D+16E16D+16E16F+16G+16A16O3B16O4A16O3B16" + "O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" + "O4A16O3B16O4A16O3B16E16F+16G+16A16G+16A16" + "O3B16O4C+16O3B16O4C+16D+16E16D+16E16F+16" + "O4G+16A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" + "O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" + "P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16" + "P16O3E16O4E16O3E16P16A16O4A16O3A16P16G+16" + "O4G+16O3G+16P16D+16O4D+16O3D+16P16E16O4E16" + "O3E16P16A16O4A16O3A16O4G16O3G16O4G16O3G16" + "O4G16O3G16O4G16O3G16O4G8E8C8E8G+16O3G+16" + "O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+8" + "O4E8O3B8O4E8G+16O3G+16O4G+16O3G+16O4G+16" + "O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16O3A+16" + "O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16O4A+8" + "O4G8E8G8B8P16A+16P16A16P16G+16P16F+16P16" + "O4E16P16D+16P16C+16P16O3B16P16A+16P16A16" + "P16O3G+16P16F+16P16E16P16D+16P16F16E16D+16" + "O3E16D+16E8B16B16B8B16B16B8B16B16O4E8F+8" + "O4G+8O3B16B16B8B16B16B8B16B16O4G+8A8B8P8" + "O4E8F+8G+8P8O3G+8A8B8P8P2O2B16C16O3C+16D16" + "O3D+16E16F16F+16G16G+16A16A+16B16C16O4C+16" + "O4D+16E16D+16F+16D+16E16D+16F+16D+16E16D+16" + "O4F+16D+16E16D+16F+16D+16E16D+16F+16D+16" + "O4E16D+16F+16D+16E16D+16F+16D+16E16D+16F+16" + "O4D+16E8E16O3E16O4E16O3E16O4E16O3E16O4E8" + "O3B16O2B16O3B16O2B16O3B16O2B16O3B8G+16O2G+16" + "O3G+16O2G+16O3G+16O2G+16O3G8E16O2E16O3E16" + "O2E16O3E16O2E16O3E8E16E16E8E8E8O2B16B16B8" + "O2B8B8G+16G+16G+8G+8G+8E16E16E8E8E8O1B8O2E8" + "O1B8O2G+8E8B8G+8O3E8O2B8O3E8O2B8O3G+8E8B8" + "O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16" + "O2E2P64", }; +const unsigned ToneAlarm::_default_ntunes = sizeof(_default_tunes) / sizeof(_default_tunes[0]); + +// semitone offsets from C for the characters 'A'-'G' +const uint8_t ToneAlarm::_note_tab[] = {9, 11, 0, 2, 4, 5, 7}; + /* * Driver 'main' command. */ @@ -345,8 +471,9 @@ extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]); ToneAlarm::ToneAlarm() : CDev("tone_alarm", "/dev/tone_alarm"), - _current_pattern(_pattern_none), - _next_note(0) + _user_tune(nullptr), + _tune(nullptr), + _next(nullptr) { // enable debug() calls //_debug_enabled = true; @@ -386,13 +513,8 @@ ToneAlarm::init() /* toggle the CC output each time the count passes 1 */ TONE_rCCR = 1; - /* - * Configure the timebase to free-run at half max frequency. - * XXX this should be more flexible in order to get a better - * frequency range, but for the F4 with the APB1 timers based - * at 42MHz, this gets us down to ~320Hz or so. - */ - rPSC = 1; + /* default the timer to a prescale value of 1; playing notes will change this */ + rPSC = 0; /* make sure the timer is running */ rCR1 = GTIM_CR1_CEN; @@ -401,196 +523,413 @@ ToneAlarm::init() return OK; } -int -ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) +unsigned +ToneAlarm::note_to_divisor(unsigned note) { - int result = 0; - unsigned new_pattern = arg; + // compute the frequency first (Hz) + float freq = 880.0f * expf(logf(2.0f) * ((int)note - 46) / 12.0f); - debug("ioctl %i %u", cmd, new_pattern); + float period = 0.5f / freq; -// irqstate_t flags = irqsave(); + // and the divisor, rounded to the nearest integer + unsigned divisor = (period * TONE_ALARM_CLOCK) + 0.5f; - /* decide whether to increase the alarm level to cmd or leave it alone */ - switch (cmd) { - case TONE_SET_ALARM: - debug("TONE_SET_ALARM %u", arg); - - if (new_pattern == 0) { - /* cancel any current alarm */ - _current_pattern = _pattern_none; - next(); - - } else if (new_pattern > _max_pattern) { - - /* not a legal alarm value */ - result = -ERANGE; - - } else if ((_current_pattern == _pattern_none) || (new_pattern > _current_pattern)) { - - /* higher priority than the current alarm */ - _current_pattern = new_pattern; - _next_note = 0; + return divisor; +} - /* and start playing it */ - next(); +unsigned +ToneAlarm::note_duration(unsigned &silence, unsigned note_length, unsigned dots) +{ + unsigned whole_note_period = (60 * 1000000 * 4) / _tempo; - } else { - /* current pattern is higher priority than the new pattern, ignore */ - } + if (note_length == 0) + note_length = 1; + unsigned note_period = whole_note_period / note_length; + switch (_note_mode) { + case MODE_NORMAL: + silence = note_period / 8; + break; + case MODE_STACCATO: + silence = note_period / 4; break; - default: - result = -ENOTTY; + case MODE_LEGATO: + silence = 0; break; } + note_period -= silence; -// irqrestore(flags); - - /* give it to the superclass if we didn't like it */ - if (result == -ENOTTY) - result = CDev::ioctl(filp, cmd, arg); + unsigned dot_extension = note_period / 2; + while (dots--) { + note_period += dot_extension; + dot_extension /= 2; + } - return result; + return note_period; } -int -ToneAlarm::write(file *filp, const char *buffer, size_t len) +unsigned +ToneAlarm::rest_duration(unsigned rest_length, unsigned dots) { - irqstate_t flags; - - /* sanity-check the size of the write */ - if (len > (_max_pattern_len * sizeof(tone_note))) - return -EFBIG; + unsigned whole_note_period = (60 * 1000000 * 4) / _tempo; - if ((len % sizeof(tone_note)) || (len == 0)) - return -EIO; + if (rest_length == 0) + rest_length = 1; - if (!check((tone_note *)buffer)) - return -EIO; + unsigned rest_period = whole_note_period / rest_length; - flags = irqsave(); + unsigned dot_extension = rest_period / 2; + while (dots--) { + rest_period += dot_extension; + dot_extension /= 2; + } - /* if we aren't playing an alarm tone */ - if (_current_pattern <= _pattern_user) { + return rest_period; +} - /* reset the tone state to play the new user pattern */ - _current_pattern = _pattern_user; - _next_note = 0; +void +ToneAlarm::start_note(unsigned note) +{ + // compute the divisor + unsigned divisor = note_to_divisor(note); - /* copy in the new pattern */ - memset(_user_pattern, 0, sizeof(_user_pattern)); - memcpy(_user_pattern, buffer, len); + // pick the lowest prescaler value that we can use + // (note that the effective prescale value is 1 greater) + unsigned prescale = divisor / 65536; - /* and start it */ - debug("starting user pattern"); - next(); - } + // calculate the timer period for the selected prescaler value + unsigned period = (divisor / (prescale + 1)) - 1; - irqrestore(flags); + rPSC = prescale; // load new prescaler + rARR = period; // load new toggle period + rEGR = GTIM_EGR_UG; // force a reload of the period + rCCER |= TONE_CCER; // enable the output - return len; } void -ToneAlarm::next_trampoline(void *arg) +ToneAlarm::stop_note() { - ToneAlarm *ta = (ToneAlarm *)arg; + /* stop the current note */ + rCCER &= ~TONE_CCER; - ta->next(); + /* + * Make sure the GPIO is not driving the speaker. + * + * XXX this presumes PX4FMU and the onboard speaker driver FET. + */ + stm32_gpiowrite(GPIO_TONE_ALARM, 0); } void -ToneAlarm::next(void) +ToneAlarm::start_tune(const char *tune) { - const tone_note *np; + // kill any current playback + hrt_cancel(&_note_call); + + // record the tune + _tune = tune; + _next = tune; + + // initialise player state + _tempo = 120; + _note_length = 4; + _note_mode = MODE_NORMAL; + _octave = 4; + _silence_length = 0; + _repeat = false; // otherwise command-line tunes repeat forever... + + // schedule a callback to start playing + hrt_call_after(&_note_call, 0, (hrt_callout)next_trampoline, this); +} - /* stop the current note */ - rCCER &= ~TONE_CCER; +void +ToneAlarm::next_note() +{ + // do we have an inter-note gap to wait for? + if (_silence_length > 0) { + stop_note(); + hrt_call_after(&_note_call, (hrt_abstime)_silence_length, (hrt_callout)next_trampoline, this); + _silence_length = 0; + return; + } - /* if we are no longer playing a pattern, we have nothing else to do here */ - if (_current_pattern == _pattern_none) { - stop(); + // make sure we still have a tune - may be removed by the write / ioctl handler + if ((_next == nullptr) || (_tune == nullptr)) { + stop_note(); return; } - ASSERT(_next_note < _note_max); + // parse characters out of the string until we have resolved a note + unsigned note = 0; + unsigned note_length = _note_length; + unsigned duration; + + while (note == 0) { + // we always need at least one character from the string + int c = next_char(); + if (c == 0) + goto tune_end; + _next++; + + switch (c) { + case 'L': // select note length + _note_length = next_number(); + if (_note_length < 1) + goto tune_error; + break; + + case 'O': // select octave + _octave = next_number(); + if (_octave > 6) + _octave = 6; + break; - /* find the note to play */ - if (_current_pattern == _pattern_user) { - np = &_user_pattern[_next_note]; + case '<': // decrease octave + if (_octave > 0) + _octave--; + break; - } else { - np = &_patterns[_current_pattern - 1][_next_note]; - } + case '>': // increase octave + if (_octave < 6) + _octave++; + break; + + case 'M': // select inter-note gap + c = next_char(); + if (c == 0) + goto tune_error; + _next++; + switch (c) { + case 'N': + _note_mode = MODE_NORMAL; + break; + case 'L': + _note_mode = MODE_LEGATO; + break; + case 'S': + _note_mode = MODE_STACCATO; + break; + case 'F': + _repeat = false; + break; + case 'B': + _repeat = true; + break; + default: + goto tune_error; + } + break; - /* work out which note is next */ - _next_note++; + case 'P': // pause for a note length + stop_note(); + hrt_call_after(&_note_call, + (hrt_abstime)rest_duration(next_number(), next_dots()), + (hrt_callout)next_trampoline, + this); + return; + + case 'T': { // change tempo + unsigned nt = next_number(); + + if ((nt >= 32) && (nt <= 255)) { + _tempo = nt; + } else { + goto tune_error; + } + break; + } - if (_next_note >= _note_max) { - /* hit the end of the pattern, stop */ - _current_pattern = _pattern_none; + case 'N': // play an arbitrary note + note = next_number(); + if (note > 84) + goto tune_error; + if (note == 0) { + // this is a rest - pause for the current note length + hrt_call_after(&_note_call, + (hrt_abstime)rest_duration(_note_length, next_dots()), + (hrt_callout)next_trampoline, + this); + return; + } + break; - } else if (np[1].duration == DURATION_END) { - /* hit the end of the pattern, stop */ - _current_pattern = _pattern_none; + case 'A'...'G': // play a note in the current octave + note = _note_tab[c - 'A'] + (_octave * 12) + 1; + c = next_char(); + switch (c) { + case '#': // up a semitone + case '+': + if (note < 84) + note++; + _next++; + break; + case '-': // down a semitone + if (note > 1) + note--; + _next++; + break; + default: + // 0 / no next char here is OK + break; + } + // shorthand length notation + note_length = next_number(); + if (note_length == 0) + note_length = _note_length; + break; - } else if (np[1].duration == DURATION_REPEAT) { - /* next note is a repeat, rewind in preparation */ - _next_note = 0; + default: + goto tune_error; + } } - /* set the timer to play the note, if required */ - if (np->pitch <= TONE_NOTE_SILENCE) { + // compute the duration of the note and the following silence (if any) + duration = note_duration(_silence_length, note_length, next_dots()); + + // start playing the note + start_note(note); + + // and arrange a callback when the note should stop + hrt_call_after(&_note_call, (hrt_abstime)duration, (hrt_callout)next_trampoline, this); + return; - /* set reload based on the pitch */ - rARR = _notes[np->pitch]; + // tune looks bad (unexpected EOF, bad character, etc.) +tune_error: + lowsyslog("tune error\n"); + _repeat = false; // don't loop on error - /* force an update, reloads the counter and all registers */ - rEGR = GTIM_EGR_UG; + // stop (and potentially restart) the tune +tune_end: + stop_note(); + if (_repeat) + start_tune(_tune); + return; +} - /* enable the output */ - rCCER |= TONE_CCER; +int +ToneAlarm::next_char() +{ + while (isspace(*_next)) { + _next++; } + return toupper(*_next); +} - /* arrange a callback when the note/rest is done */ - hrt_call_after(&_note_end, (hrt_abstime)10000 * np->duration, (hrt_callout)next_trampoline, this); +unsigned +ToneAlarm::next_number() +{ + unsigned number = 0; + int c; + + for (;;) { + c = next_char(); + if (!isdigit(c)) + return number; + _next++; + number = (number * 10) + (c - '0'); + } } -bool -ToneAlarm::check(tone_note *pattern) +unsigned +ToneAlarm::next_dots() { - for (unsigned i = 0; i < _note_max; i++) { + unsigned dots = 0; - /* first note must not be repeat or end */ - if ((i == 0) && - ((pattern[i].duration == DURATION_END) || (pattern[i].duration == DURATION_REPEAT))) - return false; + while (next_char() == '.') { + _next++; + dots++; + } + return dots; +} - if (pattern[i].duration == DURATION_END) - break; +void +ToneAlarm::next_trampoline(void *arg) +{ + ToneAlarm *ta = (ToneAlarm *)arg; - /* pitch must be legal */ - if (pattern[i].pitch >= _note_max) - return false; + ta->next_note(); +} + + +int +ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) +{ + int result = OK; + + debug("ioctl %i %u", cmd, arg); + +// irqstate_t flags = irqsave(); + + /* decide whether to increase the alarm level to cmd or leave it alone */ + switch (cmd) { + case TONE_SET_ALARM: + debug("TONE_SET_ALARM %u", arg); + + if (arg <= _default_ntunes) { + if (arg == 0) { + // stop the tune + _tune = nullptr; + _next = nullptr; + } else { + // play the selected tune + start_tune(_default_tunes[arg - 1]); + } + } else { + result = -EINVAL; + } + + break; + + default: + result = -ENOTTY; + break; } - return true; +// irqrestore(flags); + + /* give it to the superclass if we didn't like it */ + if (result == -ENOTTY) + result = CDev::ioctl(filp, cmd, arg); + + return result; } -void -ToneAlarm::stop() +int +ToneAlarm::write(file *filp, const char *buffer, size_t len) { - /* stop the current note */ - rCCER &= ~TONE_CCER; + // sanity-check the buffer for length and nul-termination + if (len > _tune_max) + return -EFBIG; - /* - * Make sure the GPIO is not driving the speaker. - * - * XXX this presumes PX4FMU and the onboard speaker driver FET. - */ - stm32_gpiowrite(GPIO_TONE_ALARM, 0); + // if we have an existing user tune, free it + if (_user_tune != nullptr) { + + // if we are playing the user tune, stop + if (_tune == _user_tune) { + _tune = nullptr; + _next = nullptr; + } + + // free the old user tune + free((void *)_user_tune); + _user_tune = nullptr; + } + + // if the new tune is empty, we're done + if (buffer[0] == '\0') + return OK; + + // allocate a copy of the new tune + _user_tune = strndup(buffer, len); + if (_user_tune == nullptr) + return -ENOMEM; + + // and play it + start_tune(_user_tune); + + return len; } /** @@ -602,7 +941,7 @@ namespace ToneAlarm *g_dev; int -play_pattern(unsigned pattern) +play_tune(unsigned tune) { int fd, ret; @@ -611,7 +950,8 @@ play_pattern(unsigned pattern) if (fd < 0) err(1, "/dev/tone_alarm"); - ret = ioctl(fd, TONE_SET_ALARM, pattern); + ret = ioctl(fd, TONE_SET_ALARM, tune); + close(fd); if (ret != 0) err(1, "TONE_SET_ALARM"); @@ -619,12 +959,30 @@ play_pattern(unsigned pattern) exit(0); } +int +play_string(const char *str) +{ + int fd, ret; + + fd = open("/dev/tone_alarm", O_WRONLY); + + if (fd < 0) + err(1, "/dev/tone_alarm"); + + ret = write(fd, str, strlen(str) + 1); + close(fd); + + if (ret < 0) + err(1, "play tune"); + exit(0); +} + } // namespace int tone_alarm_main(int argc, char *argv[]) { - unsigned pattern; + unsigned tune; /* start the driver lazily */ if (g_dev == nullptr) { @@ -640,13 +998,21 @@ tone_alarm_main(int argc, char *argv[]) } if ((argc > 1) && !strcmp(argv[1], "start")) - play_pattern(1); + play_tune(1); if ((argc > 1) && !strcmp(argv[1], "stop")) - play_pattern(0); + play_tune(0); + + if ((tune = strtol(argv[1], nullptr, 10)) != 0) + play_tune(tune); - if ((pattern = strtol(argv[1], nullptr, 10)) != 0) - play_pattern(pattern); + /* if it looks like a PLAY string... */ + if (strlen(argv[1]) > 2) { + const char *str = argv[1]; + if (str[0] == 'M') { + play_string(str); + } + } errx(1, "unrecognised command, try 'start', 'stop' or an alarm number"); } diff --git a/apps/examples/control_demo/params.c b/apps/examples/control_demo/params.c index 8cefc298f..83e01e63c 100644 --- a/apps/examples/control_demo/params.c +++ b/apps/examples/control_demo/params.c @@ -56,8 +56,4 @@ PARAM_DEFINE_FLOAT(FWB_V_MIN, 10.0f); // minimum commanded velocity PARAM_DEFINE_FLOAT(FWB_V_CMD, 12.0f); // commanded velocity PARAM_DEFINE_FLOAT(FWB_V_MAX, 16.0f); // maximum commanded velocity -// trim -PARAM_DEFINE_FLOAT(FWB_TRIM_AIL, 0.0f); // trim aileron, normalized (-1,1) -PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.005f); // trim elevator (-1,1) -PARAM_DEFINE_FLOAT(FWB_TRIM_RDR, 0.0f); // trim rudder (-1,1) PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.8f); // trim throttle (0,1) diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index db851221b..955e77b3e 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -247,8 +247,8 @@ void KalmanNav::update() // output if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz _outTimeStamp = newTimeStamp; - printf("nav: %4d Hz, miss #: %4d\n", - _navFrames / 10, _miss / 10); + //printf("nav: %4d Hz, miss #: %4d\n", + // _navFrames / 10, _miss / 10); _navFrames = 0; _miss = 0; } diff --git a/apps/mavlink/mavlink_log.h b/apps/mavlink/mavlink_log.h index 62e6f7ca0..233a76cb3 100644 --- a/apps/mavlink/mavlink_log.h +++ b/apps/mavlink/mavlink_log.h @@ -63,7 +63,11 @@ * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); * @param _text The text to log; */ +#ifdef __cplusplus +#define mavlink_log_emergency(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, (unsigned long)_text); +#else #define mavlink_log_emergency(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, (unsigned long)_text); +#endif /** * Send a mavlink critical message. @@ -71,7 +75,11 @@ * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); * @param _text The text to log; */ +#ifdef __cplusplus +#define mavlink_log_critical(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, (unsigned long)_text); +#else #define mavlink_log_critical(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, (unsigned long)_text); +#endif /** * Send a mavlink info message. @@ -79,7 +87,11 @@ * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); * @param _text The text to log; */ +#ifdef __cplusplus +#define mavlink_log_info(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text); +#else #define mavlink_log_info(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text); +#endif struct mavlink_logmessage { char text[51]; diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 9f85d5801..1dd3fc2d8 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -91,32 +91,32 @@ static uint64_t last_sensor_timestamp; static void *uorb_receive_thread(void *arg); struct listener { - void (*callback)(struct listener *l); + void (*callback)(const struct listener *l); int *subp; uintptr_t arg; }; -static void l_sensor_combined(struct listener *l); -static void l_vehicle_attitude(struct listener *l); -static void l_vehicle_gps_position(struct listener *l); -static void l_vehicle_status(struct listener *l); -static void l_rc_channels(struct listener *l); -static void l_input_rc(struct listener *l); -static void l_global_position(struct listener *l); -static void l_local_position(struct listener *l); -static void l_global_position_setpoint(struct listener *l); -static void l_local_position_setpoint(struct listener *l); -static void l_attitude_setpoint(struct listener *l); -static void l_actuator_outputs(struct listener *l); -static void l_actuator_armed(struct listener *l); -static void l_manual_control_setpoint(struct listener *l); -static void l_vehicle_attitude_controls(struct listener *l); -static void l_debug_key_value(struct listener *l); -static void l_optical_flow(struct listener *l); -static void l_vehicle_rates_setpoint(struct listener *l); -static void l_home(struct listener *l); - -struct listener listeners[] = { +static void l_sensor_combined(const struct listener *l); +static void l_vehicle_attitude(const struct listener *l); +static void l_vehicle_gps_position(const struct listener *l); +static void l_vehicle_status(const struct listener *l); +static void l_rc_channels(const struct listener *l); +static void l_input_rc(const struct listener *l); +static void l_global_position(const struct listener *l); +static void l_local_position(const struct listener *l); +static void l_global_position_setpoint(const struct listener *l); +static void l_local_position_setpoint(const struct listener *l); +static void l_attitude_setpoint(const struct listener *l); +static void l_actuator_outputs(const struct listener *l); +static void l_actuator_armed(const struct listener *l); +static void l_manual_control_setpoint(const struct listener *l); +static void l_vehicle_attitude_controls(const struct listener *l); +static void l_debug_key_value(const struct listener *l); +static void l_optical_flow(const struct listener *l); +static void l_vehicle_rates_setpoint(const struct listener *l); +static void l_home(const struct listener *l); + +static const struct listener listeners[] = { {l_sensor_combined, &mavlink_subs.sensor_sub, 0}, {l_vehicle_attitude, &mavlink_subs.att_sub, 0}, {l_vehicle_gps_position, &mavlink_subs.gps_sub, 0}, @@ -144,7 +144,7 @@ struct listener listeners[] = { static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); void -l_sensor_combined(struct listener *l) +l_sensor_combined(const struct listener *l) { struct sensor_combined_s raw; @@ -199,7 +199,7 @@ l_sensor_combined(struct listener *l) } void -l_vehicle_attitude(struct listener *l) +l_vehicle_attitude(const struct listener *l) { struct vehicle_attitude_s att; @@ -222,7 +222,7 @@ l_vehicle_attitude(struct listener *l) } void -l_vehicle_gps_position(struct listener *l) +l_vehicle_gps_position(const struct listener *l) { struct vehicle_gps_position_s gps; @@ -256,7 +256,7 @@ l_vehicle_gps_position(struct listener *l) } void -l_vehicle_status(struct listener *l) +l_vehicle_status(const struct listener *l) { /* immediately communicate state changes back to user */ orb_copy(ORB_ID(vehicle_status), status_sub, &v_status); @@ -280,7 +280,7 @@ l_vehicle_status(struct listener *l) } void -l_rc_channels(struct listener *l) +l_rc_channels(const struct listener *l) { /* copy rc channels into local buffer */ orb_copy(ORB_ID(rc_channels), rc_sub, &rc); @@ -288,7 +288,7 @@ l_rc_channels(struct listener *l) } void -l_input_rc(struct listener *l) +l_input_rc(const struct listener *l) { /* copy rc channels into local buffer */ orb_copy(ORB_ID(input_rc), mavlink_subs.input_rc_sub, &rc_raw); @@ -310,7 +310,7 @@ l_input_rc(struct listener *l) } void -l_global_position(struct listener *l) +l_global_position(const struct listener *l) { /* copy global position data into local buffer */ orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos); @@ -340,7 +340,7 @@ l_global_position(struct listener *l) } void -l_local_position(struct listener *l) +l_local_position(const struct listener *l) { /* copy local position data into local buffer */ orb_copy(ORB_ID(vehicle_local_position), mavlink_subs.local_pos_sub, &local_pos); @@ -357,7 +357,7 @@ l_local_position(struct listener *l) } void -l_global_position_setpoint(struct listener *l) +l_global_position_setpoint(const struct listener *l) { struct vehicle_global_position_setpoint_s global_sp; @@ -379,7 +379,7 @@ l_global_position_setpoint(struct listener *l) } void -l_local_position_setpoint(struct listener *l) +l_local_position_setpoint(const struct listener *l) { struct vehicle_local_position_setpoint_s local_sp; @@ -396,7 +396,7 @@ l_local_position_setpoint(struct listener *l) } void -l_attitude_setpoint(struct listener *l) +l_attitude_setpoint(const struct listener *l) { struct vehicle_attitude_setpoint_s att_sp; @@ -413,7 +413,7 @@ l_attitude_setpoint(struct listener *l) } void -l_vehicle_rates_setpoint(struct listener *l) +l_vehicle_rates_setpoint(const struct listener *l) { struct vehicle_rates_setpoint_s rates_sp; @@ -430,7 +430,7 @@ l_vehicle_rates_setpoint(struct listener *l) } void -l_actuator_outputs(struct listener *l) +l_actuator_outputs(const struct listener *l) { struct actuator_outputs_s act_outputs; @@ -511,32 +511,16 @@ l_actuator_outputs(struct listener *l) 0); } else { - - /* - * Catch the case where no rudder is in use and throttle is not - * on output four - */ - float rudder, throttle; - - if (act_outputs.noutputs < 4) { - rudder = 0.0f; - throttle = (act_outputs.output[2] - 900.0f) / 1200.0f; - - } else { - rudder = (act_outputs.output[2] - 1500.0f) / 600.0f; - throttle = (act_outputs.output[3] - 900.0f) / 1200.0f; - } - mavlink_msg_hil_controls_send(chan, hrt_absolute_time(), - (act_outputs.output[0] - 1500.0f) / 600.0f, - (act_outputs.output[1] - 1500.0f) / 600.0f, - rudder, - throttle, - (act_outputs.output[4] - 1500.0f) / 600.0f, - (act_outputs.output[5] - 1500.0f) / 600.0f, - (act_outputs.output[6] - 1500.0f) / 600.0f, - (act_outputs.output[7] - 1500.0f) / 600.0f, + (act_outputs.output[0] - 1500.0f) / 500.0f, + (act_outputs.output[1] - 1500.0f) / 500.0f, + (act_outputs.output[2] - 1500.0f) / 500.0f, + (act_outputs.output[3] - 1000.0f) / 1000.0f, + (act_outputs.output[4] - 1500.0f) / 500.0f, + (act_outputs.output[5] - 1500.0f) / 500.0f, + (act_outputs.output[6] - 1500.0f) / 500.0f, + (act_outputs.output[7] - 1500.0f) / 500.0f, mavlink_mode, 0); } @@ -545,13 +529,13 @@ l_actuator_outputs(struct listener *l) } void -l_actuator_armed(struct listener *l) +l_actuator_armed(const struct listener *l) { orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); } void -l_manual_control_setpoint(struct listener *l) +l_manual_control_setpoint(const struct listener *l) { struct manual_control_setpoint_s man_control; @@ -569,7 +553,7 @@ l_manual_control_setpoint(struct listener *l) } void -l_vehicle_attitude_controls(struct listener *l) +l_vehicle_attitude_controls(const struct listener *l) { struct actuator_controls_effective_s actuators; @@ -597,7 +581,7 @@ l_vehicle_attitude_controls(struct listener *l) } void -l_debug_key_value(struct listener *l) +l_debug_key_value(const struct listener *l) { struct debug_key_value_s debug; @@ -613,7 +597,7 @@ l_debug_key_value(struct listener *l) } void -l_optical_flow(struct listener *l) +l_optical_flow(const struct listener *l) { struct optical_flow_s flow; @@ -624,7 +608,7 @@ l_optical_flow(struct listener *l) } void -l_home(struct listener *l) +l_home(const struct listener *l) { struct home_position_s home; diff --git a/apps/mavlink_onboard/mavlink.c b/apps/mavlink_onboard/mavlink.c index 33ebe8600..5a2685560 100644 --- a/apps/mavlink_onboard/mavlink.c +++ b/apps/mavlink_onboard/mavlink.c @@ -498,7 +498,7 @@ int mavlink_onboard_main(int argc, char *argv[]) mavlink_task = task_spawn("mavlink_onboard", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 6000 /* XXX probably excessive */, + 2048, mavlink_thread_main, (const char**)argv); exit(0); diff --git a/apps/namedapp/namedapp_list.h b/apps/namedapp/namedapp_list.h deleted file mode 100644 index 72d1fa52d..000000000 --- a/apps/namedapp/namedapp_list.h +++ /dev/null @@ -1,42 +0,0 @@ -/* List of application requirements, generated during make context. */ -{ "math_demo", SCHED_PRIORITY_DEFAULT, 8192, math_demo_main }, -{ "control_demo", SCHED_PRIORITY_DEFAULT, 2048, control_demo_main }, -{ "kalman_demo", SCHED_PRIORITY_MAX - 30, 2048, kalman_demo_main }, -{ "reboot", SCHED_PRIORITY_DEFAULT, 2048, reboot_main }, -{ "perf", SCHED_PRIORITY_DEFAULT, 2048, perf_main }, -{ "top", SCHED_PRIORITY_DEFAULT - 10, 3000, top_main }, -{ "boardinfo", SCHED_PRIORITY_DEFAULT, 2048, boardinfo_main }, -{ "mixer", SCHED_PRIORITY_DEFAULT, 4096, mixer_main }, -{ "eeprom", SCHED_PRIORITY_DEFAULT, 4096, eeprom_main }, -{ "param", SCHED_PRIORITY_DEFAULT, 4096, param_main }, -{ "bl_update", SCHED_PRIORITY_DEFAULT, 4096, bl_update_main }, -{ "preflight_check", SCHED_PRIORITY_DEFAULT, 2048, preflight_check_main }, -{ "delay_test", SCHED_PRIORITY_DEFAULT, 2048, delay_test_main }, -{ "uorb", SCHED_PRIORITY_DEFAULT, 4096, uorb_main }, -{ "mavlink", SCHED_PRIORITY_DEFAULT, 2048, mavlink_main }, -{ "mavlink_onboard", SCHED_PRIORITY_DEFAULT, 2048, mavlink_onboard_main }, -{ "gps", SCHED_PRIORITY_DEFAULT, 2048, gps_main }, -{ "commander", SCHED_PRIORITY_MAX - 30, 2048, commander_main }, -{ "sdlog", SCHED_PRIORITY_MAX - 30, 2048, sdlog_main }, -{ "sensors", SCHED_PRIORITY_MAX-5, 4096, sensors_main }, -{ "ardrone_interface", SCHED_PRIORITY_MAX - 15, 2048, ardrone_interface_main }, -{ "multirotor_att_control", SCHED_PRIORITY_MAX - 15, 2048, multirotor_att_control_main }, -{ "multirotor_pos_control", SCHED_PRIORITY_MAX - 25, 2048, multirotor_pos_control_main }, -{ "fixedwing_att_control", SCHED_PRIORITY_MAX - 30, 2048, fixedwing_att_control_main }, -{ "fixedwing_pos_control", SCHED_PRIORITY_MAX - 30, 2048, fixedwing_pos_control_main }, -{ "position_estimator", SCHED_PRIORITY_DEFAULT, 4096, position_estimator_main }, -{ "attitude_estimator_ekf", SCHED_PRIORITY_DEFAULT, 2048, attitude_estimator_ekf_main }, -{ "ms5611", SCHED_PRIORITY_DEFAULT, 2048, ms5611_main }, -{ "hmc5883", SCHED_PRIORITY_DEFAULT, 4096, hmc5883_main }, -{ "mpu6000", SCHED_PRIORITY_DEFAULT, 4096, mpu6000_main }, -{ "bma180", SCHED_PRIORITY_DEFAULT, 2048, bma180_main }, -{ "l3gd20", SCHED_PRIORITY_DEFAULT, 2048, l3gd20_main }, -{ "px4io", SCHED_PRIORITY_DEFAULT, 2048, px4io_main }, -{ "blinkm", SCHED_PRIORITY_DEFAULT, 2048, blinkm_main }, -{ "tone_alarm", SCHED_PRIORITY_DEFAULT, 2048, tone_alarm_main }, -{ "adc", SCHED_PRIORITY_DEFAULT, 2048, adc_main }, -{ "fmu", SCHED_PRIORITY_DEFAULT, 2048, fmu_main }, -{ "hil", SCHED_PRIORITY_DEFAULT, 2048, hil_main }, -{ "tests", SCHED_PRIORITY_DEFAULT, 12000, tests_main }, -{ "sercon", SCHED_PRIORITY_DEFAULT, 2048, sercon_main }, -{ "serdis", SCHED_PRIORITY_DEFAULT, 2048, serdis_main }, diff --git a/apps/namedapp/namedapp_proto.h b/apps/namedapp/namedapp_proto.h deleted file mode 100644 index 09f5b4156..000000000 --- a/apps/namedapp/namedapp_proto.h +++ /dev/null @@ -1,42 +0,0 @@ -/* List of application entry points, generated during make context. */ -EXTERN int math_demo_main(int argc, char *argv[]); -EXTERN int control_demo_main(int argc, char *argv[]); -EXTERN int kalman_demo_main(int argc, char *argv[]); -EXTERN int reboot_main(int argc, char *argv[]); -EXTERN int perf_main(int argc, char *argv[]); -EXTERN int top_main(int argc, char *argv[]); -EXTERN int boardinfo_main(int argc, char *argv[]); -EXTERN int mixer_main(int argc, char *argv[]); -EXTERN int eeprom_main(int argc, char *argv[]); -EXTERN int param_main(int argc, char *argv[]); -EXTERN int bl_update_main(int argc, char *argv[]); -EXTERN int preflight_check_main(int argc, char *argv[]); -EXTERN int delay_test_main(int argc, char *argv[]); -EXTERN int uorb_main(int argc, char *argv[]); -EXTERN int mavlink_main(int argc, char *argv[]); -EXTERN int mavlink_onboard_main(int argc, char *argv[]); -EXTERN int gps_main(int argc, char *argv[]); -EXTERN int commander_main(int argc, char *argv[]); -EXTERN int sdlog_main(int argc, char *argv[]); -EXTERN int sensors_main(int argc, char *argv[]); -EXTERN int ardrone_interface_main(int argc, char *argv[]); -EXTERN int multirotor_att_control_main(int argc, char *argv[]); -EXTERN int multirotor_pos_control_main(int argc, char *argv[]); -EXTERN int fixedwing_att_control_main(int argc, char *argv[]); -EXTERN int fixedwing_pos_control_main(int argc, char *argv[]); -EXTERN int position_estimator_main(int argc, char *argv[]); -EXTERN int attitude_estimator_ekf_main(int argc, char *argv[]); -EXTERN int ms5611_main(int argc, char *argv[]); -EXTERN int hmc5883_main(int argc, char *argv[]); -EXTERN int mpu6000_main(int argc, char *argv[]); -EXTERN int bma180_main(int argc, char *argv[]); -EXTERN int l3gd20_main(int argc, char *argv[]); -EXTERN int px4io_main(int argc, char *argv[]); -EXTERN int blinkm_main(int argc, char *argv[]); -EXTERN int tone_alarm_main(int argc, char *argv[]); -EXTERN int adc_main(int argc, char *argv[]); -EXTERN int fmu_main(int argc, char *argv[]); -EXTERN int hil_main(int argc, char *argv[]); -EXTERN int tests_main(int argc, char *argv[]); -EXTERN int sercon_main(int argc, char *argv[]); -EXTERN int serdis_main(int argc, char *argv[]); diff --git a/apps/nshlib/Kconfig b/apps/nshlib/Kconfig index 92bc83cfd..d7a7b8a99 100644 --- a/apps/nshlib/Kconfig +++ b/apps/nshlib/Kconfig @@ -55,6 +55,10 @@ config NSH_DISABLE_CP bool "Disable cp" default n +config NSH_DISABLE_CMP + bool "Disable cmp" + default n + config NSH_DISABLE_DD bool "Disable dd" default n diff --git a/apps/nshlib/nsh.h b/apps/nshlib/nsh.h index 23209dba5..83cf25aa7 100644 --- a/apps/nshlib/nsh.h +++ b/apps/nshlib/nsh.h @@ -603,6 +603,9 @@ void nsh_usbtrace(void); # ifndef CONFIG_NSH_DISABLE_CP int cmd_cp(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); # endif +# ifndef CONFIG_NSH_DISABLE_CMP + int cmd_cmp(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif # ifndef CONFIG_NSH_DISABLE_DD int cmd_dd(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); # endif diff --git a/apps/nshlib/nsh_fscmds.c b/apps/nshlib/nsh_fscmds.c index f47dca896..83717e416 100644 --- a/apps/nshlib/nsh_fscmds.c +++ b/apps/nshlib/nsh_fscmds.c @@ -1232,3 +1232,84 @@ int cmd_sh(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) } #endif #endif + + +#if CONFIG_NFILE_DESCRIPTORS > 0 +#ifndef CONFIG_NSH_DISABLE_CMP +int cmd_cmp(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + char *path1 = NULL; + char *path2 = NULL; + int fd1 = -1, fd2 = -1; + int ret = ERROR; + unsigned total_read = 0; + + /* Get the full path to the two files */ + + path1 = nsh_getfullpath(vtbl, argv[1]); + if (!path1) + { + goto errout; + } + + path2 = nsh_getfullpath(vtbl, argv[2]); + if (!path2) + { + goto errout; + } + + /* Open the files for reading */ + fd1 = open(path1, O_RDONLY); + if (fd1 < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "open", NSH_ERRNO); + goto errout; + } + + fd2 = open(path2, O_RDONLY); + if (fd2 < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "open", NSH_ERRNO); + goto errout; + } + + for (;;) + { + char buf1[128]; + char buf2[128]; + + int nbytesread1 = read(fd1, buf1, sizeof(buf1)); + int nbytesread2 = read(fd2, buf2, sizeof(buf2)); + + if (nbytesread1 < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "read", NSH_ERRNO); + goto errout; + } + + if (nbytesread2 < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "read", NSH_ERRNO); + goto errout; + } + + total_read += nbytesread1>nbytesread2?nbytesread2:nbytesread1; + + if (nbytesread1 != nbytesread2 || memcmp(buf1, buf2, nbytesread1) != 0) + { + nsh_output(vtbl, "files differ: byte %u\n", total_read); + goto errout; + } + + if (nbytesread1 < sizeof(buf1)) break; + } + + ret = OK; + +errout: + if (fd1 != -1) close(fd1); + if (fd2 != -1) close(fd2); + return ret; +} +#endif +#endif diff --git a/apps/nshlib/nsh_parse.c b/apps/nshlib/nsh_parse.c index f679d9b32..4d8f04b23 100644 --- a/apps/nshlib/nsh_parse.c +++ b/apps/nshlib/nsh_parse.c @@ -175,6 +175,9 @@ static const struct cmdmap_s g_cmdmap[] = # ifndef CONFIG_NSH_DISABLE_CP { "cp", cmd_cp, 3, 3, "<source-path> <dest-path>" }, # endif +# ifndef CONFIG_NSH_DISABLE_CMP + { "cmp", cmd_cmp, 3, 3, "<path1> <path2>" }, +# endif #endif #if defined (CONFIG_RTC) && !defined(CONFIG_DISABLE_CLOCK) && !defined(CONFIG_NSH_DISABLE_DATE) diff --git a/apps/px4/tests/test_bson.c b/apps/px4/tests/test_bson.c index a42c462ca..6130fe763 100644 --- a/apps/px4/tests/test_bson.c +++ b/apps/px4/tests/test_bson.c @@ -51,7 +51,7 @@ static const int32_t sample_small_int = 123; static const int64_t sample_big_int = (int64_t)INT_MAX + 123LL; static const double sample_double = 2.5f; static const char *sample_string = "this is a test"; -static const uint8_t sample_data[256]; +static const uint8_t sample_data[256] = {0}; //static const char *sample_filename = "/fs/microsd/bson.test"; static int diff --git a/apps/px4io/Makefile b/apps/px4io/Makefile index d97f963ab..0eb3ffcf7 100644 --- a/apps/px4io/Makefile +++ b/apps/px4io/Makefile @@ -39,11 +39,19 @@ # Note that we pull a couple of specific files from the systemlib, since # we can't support it all. # -CSRCS = $(wildcard *.c) \ +CSRCS = adc.c \ + controls.c \ + dsm.c \ + i2c.c \ + px4io.c \ + registers.c \ + safety.c \ + sbus.c \ ../systemlib/hx_stream.c \ ../systemlib/perf_counter.c \ ../systemlib/up_cxxinitialize.c -CXXSRCS = $(wildcard *.cpp) + +CXXSRCS = mixer.cpp INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common diff --git a/apps/px4io/adc.c b/apps/px4io/adc.c index e06b269dc..670b8d635 100644 --- a/apps/px4io/adc.c +++ b/apps/px4io/adc.c @@ -154,7 +154,7 @@ adc_measure(unsigned channel) while (!(rSR & ADC_SR_EOC)) { /* never spin forever - this will give a bogus result though */ - if ((hrt_absolute_time() - now) > 1000) { + if (hrt_elapsed_time(&now) > 1000) { debug("adc timeout"); break; } diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c deleted file mode 100644 index e51c2771a..000000000 --- a/apps/px4io/comms.c +++ /dev/null @@ -1,329 +0,0 @@ -/**************************************************************************** - * - * 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 comms.c - * - * FMU communication for the PX4IO module. - */ - -#include <nuttx/config.h> -#include <stdio.h> -#include <stdbool.h> -#include <fcntl.h> -#include <unistd.h> -#include <debug.h> -#include <stdlib.h> -#include <errno.h> -#include <string.h> -#include <poll.h> -#include <termios.h> - -#include <nuttx/clock.h> - -#include <drivers/drv_hrt.h> -#include <drivers/drv_pwm_output.h> -#include <systemlib/hx_stream.h> -#include <systemlib/perf_counter.h> - -#define DEBUG -#include "px4io.h" - -#define FMU_MIN_REPORT_INTERVAL 5000 /* 5ms */ -#define FMU_MAX_REPORT_INTERVAL 100000 /* 100ms */ - -#define FMU_STATUS_INTERVAL 1000000 /* 100ms */ - -static int fmu_fd; -static hx_stream_t stream; -static struct px4io_report report; - -static void comms_handle_frame(void *arg, const void *buffer, size_t length); - -perf_counter_t comms_rx_errors; - -static void -comms_init(void) -{ - /* initialise the FMU interface */ - fmu_fd = open("/dev/ttyS1", O_RDWR); - stream = hx_stream_init(fmu_fd, comms_handle_frame, NULL); - - comms_rx_errors = perf_alloc(PC_COUNT, "rx_err"); - hx_stream_set_counters(stream, 0, 0, comms_rx_errors); - - /* default state in the report to FMU */ - report.i2f_magic = I2F_MAGIC; - - struct termios t; - - /* 115200bps, no parity, one stop bit */ - tcgetattr(fmu_fd, &t); - cfsetspeed(&t, 115200); - t.c_cflag &= ~(CSTOPB | PARENB); - tcsetattr(fmu_fd, TCSANOW, &t); - - /* init the ADC */ - adc_init(); -} - -void -comms_main(void) -{ - comms_init(); - - struct pollfd fds; - fds.fd = fmu_fd; - fds.events = POLLIN; - debug("FMU: ready"); - - for (;;) { - /* wait for serial data, but no more than 10ms */ - poll(&fds, 1, 10); - - /* - * Pull bytes from FMU and feed them to the HX engine. - * Limit the number of bytes we actually process on any one iteration. - */ - if (fds.revents & POLLIN) { - char buf[32]; - ssize_t count = read(fmu_fd, buf, sizeof(buf)); - - for (int i = 0; i < count; i++) - hx_stream_rx(stream, buf[i]); - } - - /* - * Decide if it's time to send an update to the FMU. - */ - static hrt_abstime last_report_time; - hrt_abstime now, delta; - - /* should we send a report to the FMU? */ - now = hrt_absolute_time(); - delta = now - last_report_time; - - if ((delta > FMU_MIN_REPORT_INTERVAL) && - (system_state.fmu_report_due || (delta > FMU_MAX_REPORT_INTERVAL))) { - - system_state.fmu_report_due = false; - last_report_time = now; - - /* populate the report */ - for (unsigned i = 0; i < system_state.rc_channels; i++) { - report.rc_channel[i] = system_state.rc_channel_data[i]; - } - - report.channel_count = system_state.rc_channels; - report.armed = system_state.armed; - - report.battery_mv = system_state.battery_mv; - report.adc_in = system_state.adc_in5; - report.overcurrent = system_state.overcurrent; - - /* and send it */ - hx_stream_send(stream, &report, sizeof(report)); - } - - /* - * Fetch ADC values, check overcurrent flags, etc. - */ - static hrt_abstime last_status_time; - - if ((now - last_status_time) > FMU_STATUS_INTERVAL) { - - /* - * Coefficients here derived by measurement of the 5-16V - * range on one unit: - * - * V counts - * 5 1001 - * 6 1219 - * 7 1436 - * 8 1653 - * 9 1870 - * 10 2086 - * 11 2303 - * 12 2522 - * 13 2738 - * 14 2956 - * 15 3172 - * 16 3389 - * - * slope = 0.0046067 - * intercept = 0.3863 - * - * Intercept corrected for best results @ 12V. - */ - unsigned counts = adc_measure(ADC_VBATT); - system_state.battery_mv = (4150 + (counts * 46)) / 10; - - system_state.adc_in5 = adc_measure(ADC_IN5); - - system_state.overcurrent = - (OVERCURRENT_SERVO ? (1 << 0) : 0) | - (OVERCURRENT_ACC ? (1 << 1) : 0); - - last_status_time = now; - } - } -} - -static void -comms_handle_config(const void *buffer, size_t length) -{ - const struct px4io_config *cfg = (struct px4io_config *)buffer; - - if (length != sizeof(*cfg)) - return; - - /* fetch the rc mappings */ - for (unsigned i = 0; i < 4; i++) { - system_state.rc_map[i] = cfg->rc_map[i]; - } - - /* fetch the rc channel attributes */ - for (unsigned i = 0; i < 4; i++) { - system_state.rc_min[i] = cfg->rc_min[i]; - system_state.rc_trim[i] = cfg->rc_trim[i]; - system_state.rc_max[i] = cfg->rc_max[i]; - system_state.rc_rev[i] = cfg->rc_rev[i]; - system_state.rc_dz[i] = cfg->rc_dz[i]; - } -} - -static void -comms_handle_command(const void *buffer, size_t length) -{ - const struct px4io_command *cmd = (struct px4io_command *)buffer; - - if (length != sizeof(*cmd)) - return; - - irqstate_t flags = irqsave(); - - /* fetch new PWM output values */ - for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) - system_state.fmu_channel_data[i] = cmd->output_control[i]; - - /* if the IO is armed and the FMU gets disarmed, the IO must also disarm */ - if (system_state.arm_ok && !cmd->arm_ok) - system_state.armed = false; - - system_state.arm_ok = cmd->arm_ok; - system_state.vector_flight_mode_ok = cmd->vector_flight_mode_ok; - system_state.manual_override_ok = cmd->manual_override_ok; - system_state.mixer_fmu_available = true; - system_state.fmu_data_received_time = hrt_absolute_time(); - - /* set PWM update rate if changed (after limiting) */ - uint16_t new_servo_rate = cmd->servo_rate; - - /* reject faster than 500 Hz updates */ - if (new_servo_rate > 500) { - new_servo_rate = 500; - } - - /* reject slower than 50 Hz updates */ - if (new_servo_rate < 50) { - new_servo_rate = 50; - } - - if (system_state.servo_rate != new_servo_rate) { - up_pwm_servo_set_rate(new_servo_rate); - system_state.servo_rate = new_servo_rate; - } - - /* - * update servo values immediately. - * the updates are done in addition also - * in the mainloop, since this function will only - * update with a connected FMU. - */ - mixer_tick(); - - /* handle relay state changes here */ - for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) { - if (system_state.relays[i] != cmd->relay_state[i]) { - switch (i) { - case 0: - POWER_ACC1(cmd->relay_state[i]); - break; - - case 1: - POWER_ACC2(cmd->relay_state[i]); - break; - - case 2: - POWER_RELAY1(cmd->relay_state[i]); - break; - - case 3: - POWER_RELAY2(cmd->relay_state[i]); - break; - } - } - - system_state.relays[i] = cmd->relay_state[i]; - } - - irqrestore(flags); -} - -static void -comms_handle_frame(void *arg, const void *buffer, size_t length) -{ - const uint16_t *type = (const uint16_t *)buffer; - - - /* make sure it's what we are expecting */ - if (length > 2) { - switch (*type) { - case F2I_MAGIC: - comms_handle_command(buffer, length); - break; - - case F2I_CONFIG_MAGIC: - comms_handle_config(buffer, length); - break; - - case F2I_MIXER_MAGIC: - mixer_handle_text(buffer, length); - break; - - default: - break; - } - } -} - diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 169d5bb81..e80a41f15 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -37,148 +37,296 @@ * R/C inputs and servo outputs. */ - #include <nuttx/config.h> -#include <stdio.h> #include <stdbool.h> -#include <fcntl.h> -#include <unistd.h> -#include <debug.h> -#include <stdlib.h> -#include <errno.h> -#include <termios.h> -#include <string.h> -#include <poll.h> - -#include <nuttx/clock.h> #include <drivers/drv_hrt.h> -#include <systemlib/hx_stream.h> #include <systemlib/perf_counter.h> #include <systemlib/ppm_decode.h> -#define DEBUG #include "px4io.h" #define RC_FAILSAFE_TIMEOUT 2000000 /**< two seconds failsafe timeout */ -#define RC_CHANNEL_HIGH_THRESH 1700 -#define RC_CHANNEL_LOW_THRESH 1300 +#define RC_CHANNEL_HIGH_THRESH 5000 +#define RC_CHANNEL_LOW_THRESH -5000 + +static bool ppm_input(uint16_t *values, uint16_t *num_values); -static void ppm_input(void); +static perf_counter_t c_gather_dsm; +static perf_counter_t c_gather_sbus; +static perf_counter_t c_gather_ppm; void -controls_main(void) +controls_init(void) { - struct pollfd fds[2]; - /* DSM input */ - fds[0].fd = dsm_init("/dev/ttyS0"); - fds[0].events = POLLIN; + dsm_init("/dev/ttyS0"); /* S.bus input */ - fds[1].fd = sbus_init("/dev/ttyS2"); - fds[1].events = POLLIN; + sbus_init("/dev/ttyS2"); - for (;;) { - /* run this loop at ~100Hz */ - poll(fds, 2, 10); + /* default to a 1:1 input map, all enabled */ + for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) { + unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; - /* - * Gather R/C control inputs from supported sources. - * - * Note that if you're silly enough to connect more than - * one control input source, they're going to fight each - * other. Don't do that. - */ - bool locked = false; + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = 0; + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MIN] = 1000; + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_CENTER] = 1500; + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MAX] = 2000; + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_DEADZONE] = 30; + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_ASSIGNMENT] = i; + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + } - /* - * Store RC channel count to detect switch to RC loss sooner - * than just by timeout - */ - unsigned rc_channels = system_state.rc_channels; + c_gather_dsm = perf_alloc(PC_ELAPSED, "c_gather_dsm"); + c_gather_sbus = perf_alloc(PC_ELAPSED, "c_gather_sbus"); + c_gather_ppm = perf_alloc(PC_ELAPSED, "c_gather_ppm"); +} + +void +controls_tick() { + + /* + * Gather R/C control inputs from supported sources. + * + * Note that if you're silly enough to connect more than + * one control input source, they're going to fight each + * other. Don't do that. + */ + + perf_begin(c_gather_dsm); + bool dsm_updated = dsm_input(r_raw_rc_values, &r_raw_rc_count); + if (dsm_updated) + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; + perf_end(c_gather_dsm); + + perf_begin(c_gather_sbus); + bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count); + if (sbus_updated) + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; + perf_end(c_gather_sbus); + + /* + * XXX each S.bus frame will cause a PPM decoder interrupt + * storm (lots of edges). It might be sensible to actually + * disable the PPM decoder completely if we have S.bus signal. + */ + perf_begin(c_gather_ppm); + bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count); + if (ppm_updated) + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; + perf_end(c_gather_ppm); + + ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS); + + /* + * In some cases we may have received a frame, but input has still + * been lost. + */ + bool rc_input_lost = false; + + /* + * If we received a new frame from any of the RC sources, process it. + */ + if (dsm_updated || sbus_updated || ppm_updated) { + + /* update RC-received timestamp */ + system_state.rc_channels_timestamp = hrt_absolute_time(); + + /* record a bitmask of channels assigned */ + unsigned assigned_channels = 0; + + /* map raw inputs to mapped inputs */ + /* XXX mapping should be atomic relative to protocol */ + for (unsigned i = 0; i < r_raw_rc_count; i++) { + + /* map the input channel */ + uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; + + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { + + uint16_t raw = r_raw_rc_values[i]; + + int16_t scaled; + + /* + * 1) Constrain to min/max values, as later processing depends on bounds. + */ + if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) + raw = conf[PX4IO_P_RC_CONFIG_MIN]; + if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) + raw = conf[PX4IO_P_RC_CONFIG_MAX]; + + /* + * 2) Scale around the mid point differently for lower and upper range. + * + * This is necessary as they don't share the same endpoints and slope. + * + * First normalize to 0..1 range with correct sign (below or above center), + * then scale to 20000 range (if center is an actual center, -10000..10000, + * if parameters only support half range, scale to 10000 range, e.g. if + * center == min 0..10000, if center == max -10000..0). + * + * As the min and max bounds were enforced in step 1), division by zero + * cannot occur, as for the case of center == min or center == max the if + * statement is mutually exclusive with the arithmetic NaN case. + * + * DO NOT REMOVE OR ALTER STEP 1! + */ + if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); + + } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); - if (fds[0].revents & POLLIN) - locked |= dsm_input(); + } else { + /* in the configured dead zone, output zero */ + scaled = 0; + } - if (fds[1].revents & POLLIN) - locked |= sbus_input(); + /* invert channel if requested */ + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) + scaled = -scaled; + + /* and update the scaled/mapped version */ + unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; + ASSERT(mapped < MAX_CONTROL_CHANNELS); + + /* invert channel if pitch - pulling the lever down means pitching up by convention */ + if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ + scaled = -scaled; + + r_rc_values[mapped] = SIGNED_TO_REG(scaled); + assigned_channels |= (1 << mapped); + } + } + + /* set un-assigned controls to zero */ + for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) { + if (!(assigned_channels & (1 << i))) + r_rc_values[i] = 0; + } /* - * If we don't have lock from one of the serial receivers, - * look for PPM. It shares an input with S.bus, so there's - * a possibility it will mis-parse an S.bus frame. + * If we got an update with zero channels, treat it as + * a loss of input. * - * XXX each S.bus frame will cause a PPM decoder interrupt - * storm (lots of edges). It might be sensible to actually - * disable the PPM decoder completely if we have an alternate - * receiver lock. + * This might happen if a protocol-based receiver returns an update + * that contains no channels that we have mapped. */ - if (!locked) - ppm_input(); - - /* check for manual override status */ - if (system_state.rc_channel_data[4] > RC_CHANNEL_HIGH_THRESH) { - /* force manual input override */ - system_state.mixer_manual_override = true; - + if (assigned_channels == 0) { + rc_input_lost = true; } else { - /* override not engaged, use FMU */ - system_state.mixer_manual_override = false; + /* set RC OK flag and clear RC lost alarm */ + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; + r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_RC_LOST; } /* - * If we haven't seen any new control data in 200ms, assume we - * have lost input and tell FMU. + * Export the valid channel bitmap */ - if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) { + r_rc_valid = assigned_channels; + } - if (system_state.rc_channels > 0) { - /* - * If the RC signal status (lost / present) has - * just changed, request an update immediately. - */ - system_state.fmu_report_due = true; - } + /* + * If we haven't seen any new control data in 200ms, assume we + * have lost input. + */ + if (hrt_elapsed_time(&system_state.rc_channels_timestamp) > 200000) { + rc_input_lost = true; - /* set the number of channels to zero - no inputs */ - system_state.rc_channels = 0; - } + /* clear the input-kind flags here */ + r_status_flags &= ~( + PX4IO_P_STATUS_FLAGS_RC_PPM | + PX4IO_P_STATUS_FLAGS_RC_DSM | + PX4IO_P_STATUS_FLAGS_RC_SBUS); + } + + /* + * Handle losing RC input + */ + if (rc_input_lost) { + + /* Clear the RC input status flag, clear manual override flag */ + r_status_flags &= ~( + PX4IO_P_STATUS_FLAGS_OVERRIDE | + PX4IO_P_STATUS_FLAGS_RC_OK); + + /* Set the RC_LOST alarm */ + r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST; + + /* Mark the arrays as empty */ + r_raw_rc_count = 0; + r_rc_valid = 0; + } + + /* + * Check for manual override. + * + * The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we + * must have R/C input. + * Override is enabled if either the hardcoded channel / value combination + * is selected, or the AP has requested it. + */ + if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) { + + bool override = false; /* - * PWM output updates are performed in addition on each comm update. - * the updates here are required to ensure operation if FMU is not started - * or stopped responding. + * Check mapped channel 5 (can be any remote channel, + * depends on RC_MAP_OVER parameter); + * If the value is 'high' then the pilot has + * requested override. + * */ - mixer_tick(); + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) > RC_CHANNEL_HIGH_THRESH)) + override = true; + + if (override) { + + r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; + + /* mix new RC input control values to servos */ + if (dsm_updated || sbus_updated || ppm_updated) + mixer_tick(); + + } else { + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE; + } } } -static void -ppm_input(void) +static bool +ppm_input(uint16_t *values, uint16_t *num_values) { + bool result = false; + + /* avoid racing with PPM updates */ + irqstate_t state = irqsave(); + /* - * Look for new PPM input. + * If we have received a new PPM frame within the last 200ms, accept it + * and then invalidate it. */ - if (ppm_last_valid_decode != 0) { - - /* avoid racing with PPM updates */ - irqstate_t state = irqsave(); + if (hrt_elapsed_time(&ppm_last_valid_decode) < 200000) { /* PPM data exists, copy it */ - system_state.rc_channels = ppm_decoded_channels; + *num_values = ppm_decoded_channels; + if (*num_values > MAX_CONTROL_CHANNELS) + *num_values = MAX_CONTROL_CHANNELS; - for (unsigned i = 0; i < ppm_decoded_channels; i++) { - system_state.rc_channel_data[i] = ppm_buffer[i]; - } + for (unsigned i = 0; i < *num_values; i++) + values[i] = ppm_buffer[i]; - /* copy the timestamp and clear it */ - system_state.rc_channels_timestamp = ppm_last_valid_decode; + /* clear validity */ ppm_last_valid_decode = 0; - irqrestore(state); - - /* trigger an immediate report to the FMU */ - system_state.fmu_report_due = true; + /* good if we got any channels */ + result = (*num_values > 0); } + + irqrestore(state); + + return result; } diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c index 869c27b1b..ea35e5513 100644 --- a/apps/px4io/dsm.c +++ b/apps/px4io/dsm.c @@ -43,17 +43,13 @@ #include <fcntl.h> #include <unistd.h> -#include <string.h> #include <termios.h> -#include <systemlib/ppm_decode.h> - #include <drivers/drv_hrt.h> #define DEBUG #include "px4io.h" -#include "protocol.h" #define DSM_FRAME_SIZE 16 #define DSM_FRAME_CHANNELS 7 @@ -72,13 +68,13 @@ unsigned dsm_frame_drops; static bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value); static void dsm_guess_format(bool reset); -static void dsm_decode(hrt_abstime now); +static bool dsm_decode(hrt_abstime now, uint16_t *values, uint16_t *num_values); int dsm_init(const char *device) { if (dsm_fd < 0) - dsm_fd = open(device, O_RDONLY); + dsm_fd = open(device, O_RDONLY | O_NONBLOCK); if (dsm_fd >= 0) { struct termios t; @@ -106,7 +102,7 @@ dsm_init(const char *device) } bool -dsm_input(void) +dsm_input(uint16_t *values, uint16_t *num_values) { ssize_t ret; hrt_abstime now; @@ -143,7 +139,7 @@ dsm_input(void) /* if the read failed for any reason, just give up here */ if (ret < 1) - goto out; + return false; last_rx_time = now; @@ -156,20 +152,14 @@ dsm_input(void) * If we don't have a full frame, return */ if (partial_frame_count < DSM_FRAME_SIZE) - goto out; + return false; /* * Great, it looks like we might have a frame. Go ahead and * decode it. */ - dsm_decode(now); partial_frame_count = 0; - -out: - /* - * If we have seen a frame in the last 200ms, we consider ourselves 'locked' - */ - return (now - last_frame_time) < 200000; + return dsm_decode(now, values, num_values); } static bool @@ -259,23 +249,23 @@ dsm_guess_format(bool reset) if ((votes11 == 1) && (votes10 == 0)) { channel_shift = 11; - debug("DSM: detected 11-bit format"); + debug("DSM: 11-bit format"); return; } if ((votes10 == 1) && (votes11 == 0)) { channel_shift = 10; - debug("DSM: detected 10-bit format"); + debug("DSM: 10-bit format"); return; } /* call ourselves to reset our state ... we have to try again */ - debug("DSM: format detector failed, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11); + debug("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11); dsm_guess_format(true); } -static void -dsm_decode(hrt_abstime frame_time) +static bool +dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) { /* @@ -296,7 +286,7 @@ dsm_decode(hrt_abstime frame_time) /* if we don't know the frame format, update the guessing state machine */ if (channel_shift == 0) { dsm_guess_format(false); - return; + return false; } /* @@ -324,8 +314,8 @@ dsm_decode(hrt_abstime frame_time) continue; /* update the decoded channel count */ - if (channel >= system_state.rc_channels) - system_state.rc_channels = channel + 1; + if (channel >= *num_values) + *num_values = channel + 1; /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ if (channel_shift == 11) @@ -356,13 +346,11 @@ dsm_decode(hrt_abstime frame_time) break; } - system_state.rc_channel_data[channel] = value; + values[channel] = value; } - /* and note that we have received data from the R/C controller */ - /* XXX failsafe will cause problems here - need a strategy for detecting it */ - system_state.rc_channels_timestamp = frame_time; - - /* trigger an immediate report to the FMU */ - system_state.fmu_report_due = true; + /* + * XXX Note that we may be in failsafe here; we need to work out how to detect that. + */ + return true; } diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c new file mode 100644 index 000000000..4485daa5b --- /dev/null +++ b/apps/px4io/i2c.c @@ -0,0 +1,340 @@ +/**************************************************************************** + * + * 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 i2c.c + * + * I2C communication for the PX4IO module. + */ + +#include <stdint.h> + +#include <nuttx/arch.h> +#include <arch/board/board.h> +#include <stm32_i2c.h> +#include <stm32_dma.h> + +//#define DEBUG +#include "px4io.h" + +/* + * I2C register definitions. + */ +#define I2C_BASE STM32_I2C1_BASE + +#define REG(_reg) (*(volatile uint32_t *)(I2C_BASE + _reg)) + +#define rCR1 REG(STM32_I2C_CR1_OFFSET) +#define rCR2 REG(STM32_I2C_CR2_OFFSET) +#define rOAR1 REG(STM32_I2C_OAR1_OFFSET) +#define rOAR2 REG(STM32_I2C_OAR2_OFFSET) +#define rDR REG(STM32_I2C_DR_OFFSET) +#define rSR1 REG(STM32_I2C_SR1_OFFSET) +#define rSR2 REG(STM32_I2C_SR2_OFFSET) +#define rCCR REG(STM32_I2C_CCR_OFFSET) +#define rTRISE REG(STM32_I2C_TRISE_OFFSET) + +static int i2c_interrupt(int irq, void *context); +static void i2c_rx_setup(void); +static void i2c_tx_setup(void); +static void i2c_rx_complete(void); +static void i2c_tx_complete(void); + +static DMA_HANDLE rx_dma; +static DMA_HANDLE tx_dma; + +static uint8_t rx_buf[68]; +static unsigned rx_len; + +static const uint8_t junk_buf[] = { 0xff, 0xff, 0xff, 0xff }; + +static const uint8_t *tx_buf = junk_buf; +static unsigned tx_len = sizeof(junk_buf); +unsigned tx_count; + +static uint8_t selected_page; +static uint8_t selected_offset; + +enum { + DIR_NONE = 0, + DIR_TX = 1, + DIR_RX = 2 +} direction; + +void +i2c_init(void) +{ + debug("i2c init"); + + /* allocate DMA handles and initialise DMA */ + rx_dma = stm32_dmachannel(DMACHAN_I2C1_RX); + i2c_rx_setup(); + tx_dma = stm32_dmachannel(DMACHAN_I2C1_TX); + i2c_tx_setup(); + + /* enable the i2c block clock and reset it */ + modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_I2C1EN); + modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_I2C1RST); + modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_I2C1RST, 0); + + /* configure the i2c GPIOs */ + stm32_configgpio(GPIO_I2C1_SCL); + stm32_configgpio(GPIO_I2C1_SDA); + + /* soft-reset the block */ + rCR1 |= I2C_CR1_SWRST; + rCR1 = 0; + + /* set for DMA operation */ + rCR2 |= I2C_CR2_ITEVFEN |I2C_CR2_ITERREN | I2C_CR2_DMAEN; + + /* set the frequency value in CR2 */ + rCR2 &= ~I2C_CR2_FREQ_MASK; + rCR2 |= STM32_PCLK1_FREQUENCY / 1000000; + + /* set divisor and risetime for fast mode */ + uint16_t result = STM32_PCLK1_FREQUENCY / (400000 * 25); + if (result < 1) + result = 1; + result = 3; + rCCR &= ~I2C_CCR_CCR_MASK; + rCCR |= I2C_CCR_DUTY | I2C_CCR_FS | result; + rTRISE = (uint16_t)((((STM32_PCLK1_FREQUENCY / 1000000) * 300) / 1000) + 1); + + /* set our device address */ + rOAR1 = 0x1a << 1; + + /* enable event interrupts */ + irq_attach(STM32_IRQ_I2C1EV, i2c_interrupt); + irq_attach(STM32_IRQ_I2C1ER, i2c_interrupt); + up_enable_irq(STM32_IRQ_I2C1EV); + up_enable_irq(STM32_IRQ_I2C1ER); + + /* and enable the I2C port */ + rCR1 |= I2C_CR1_ACK | I2C_CR1_PE; + +#ifdef DEBUG + i2c_dump(); +#endif +} + + +/* + reset the I2C bus + used to recover from lockups + */ +void i2c_reset(void) +{ + rCR1 |= I2C_CR1_SWRST; + rCR1 = 0; + + /* set for DMA operation */ + rCR2 |= I2C_CR2_ITEVFEN |I2C_CR2_ITERREN | I2C_CR2_DMAEN; + + /* set the frequency value in CR2 */ + rCR2 &= ~I2C_CR2_FREQ_MASK; + rCR2 |= STM32_PCLK1_FREQUENCY / 1000000; + + /* set divisor and risetime for fast mode */ + uint16_t result = STM32_PCLK1_FREQUENCY / (400000 * 25); + if (result < 1) + result = 1; + result = 3; + rCCR &= ~I2C_CCR_CCR_MASK; + rCCR |= I2C_CCR_DUTY | I2C_CCR_FS | result; + rTRISE = (uint16_t)((((STM32_PCLK1_FREQUENCY / 1000000) * 300) / 1000) + 1); + + /* set our device address */ + rOAR1 = 0x1a << 1; + + /* and enable the I2C port */ + rCR1 |= I2C_CR1_ACK | I2C_CR1_PE; +} + +static int +i2c_interrupt(int irq, FAR void *context) +{ + uint16_t sr1 = rSR1; + + if (sr1 & (I2C_SR1_STOPF | I2C_SR1_AF | I2C_SR1_ADDR)) { + + if (sr1 & I2C_SR1_STOPF) { + /* write to CR1 to clear STOPF */ + (void)rSR1; /* as recommended, re-read SR1 */ + rCR1 |= I2C_CR1_PE; + } + + /* DMA never stops, so we should do that now */ + switch (direction) { + case DIR_TX: + i2c_tx_complete(); + break; + case DIR_RX: + i2c_rx_complete(); + break; + default: + /* not currently transferring - must be a new txn */ + break; + } + direction = DIR_NONE; + } + + if (sr1 & I2C_SR1_ADDR) { + + /* clear ADDR to ack our selection and get direction */ + (void)rSR1; /* as recommended, re-read SR1 */ + uint16_t sr2 = rSR2; + + if (sr2 & I2C_SR2_TRA) { + /* we are the transmitter */ + + direction = DIR_TX; + + } else { + /* we are the receiver */ + + direction = DIR_RX; + } + } + + /* clear any errors that might need it (this handles AF as well */ + if (sr1 & I2C_SR1_ERRORMASK) + rSR1 = 0; + + return 0; +} + +static void +i2c_rx_setup(void) +{ + /* + * Note that we configure DMA in circular mode; this means that a too-long + * transfer will overwrite the buffer, but that avoids us having to deal with + * bailing out of a transaction while the master is still babbling at us. + */ + rx_len = 0; + stm32_dmasetup(rx_dma, (uintptr_t)&rDR, (uintptr_t)&rx_buf[0], sizeof(rx_buf), + DMA_CCR_CIRC | + DMA_CCR_MINC | + DMA_CCR_PSIZE_32BITS | + DMA_CCR_MSIZE_8BITS | + DMA_CCR_PRIMED); + + stm32_dmastart(rx_dma, NULL, NULL, false); +} + +static void +i2c_rx_complete(void) +{ + rx_len = sizeof(rx_buf) - stm32_dmaresidual(rx_dma); + stm32_dmastop(rx_dma); + + if (rx_len >= 2) { + selected_page = rx_buf[0]; + selected_offset = rx_buf[1]; + + /* work out how many registers are being written */ + unsigned count = (rx_len - 2) / 2; + if (count > 0) { + registers_set(selected_page, selected_offset, (const uint16_t *)&rx_buf[2], count); + } else { + /* no registers written, must be an address cycle */ + uint16_t *regs; + unsigned reg_count; + + /* work out which registers are being addressed */ + int ret = registers_get(selected_page, selected_offset, ®s, ®_count); + if (ret == 0) { + tx_buf = (uint8_t *)regs; + tx_len = reg_count * 2; + } else { + tx_buf = junk_buf; + tx_len = sizeof(junk_buf); + } + + /* disable interrupts while reconfiguring DMA for the selected registers */ + irqstate_t flags = irqsave(); + + stm32_dmastop(tx_dma); + i2c_tx_setup(); + + irqrestore(flags); + } + } + + /* prepare for the next transaction */ + i2c_rx_setup(); +} + +static void +i2c_tx_setup(void) +{ + /* + * Note that we configure DMA in circular mode; this means that a too-long + * transfer will copy the buffer more than once, but that avoids us having + * to deal with bailing out of a transaction while the master is still + * babbling at us. + */ + stm32_dmasetup(tx_dma, (uintptr_t)&rDR, (uintptr_t)&tx_buf[0], tx_len, + DMA_CCR_DIR | + DMA_CCR_CIRC | + DMA_CCR_MINC | + DMA_CCR_PSIZE_8BITS | + DMA_CCR_MSIZE_8BITS | + DMA_CCR_PRIMED); + + stm32_dmastart(tx_dma, NULL, NULL, false); +} + +static void +i2c_tx_complete(void) +{ + tx_count = tx_len - stm32_dmaresidual(tx_dma); + stm32_dmastop(tx_dma); + + /* for debug purposes, save the length of the last transmit as seen by the DMA */ + + /* leave tx_buf/tx_len alone, so that a retry will see the same data */ + + /* prepare for the next transaction */ + i2c_tx_setup(); +} + +void +i2c_dump(void) +{ + debug("CR1 0x%08x CR2 0x%08x", rCR1, rCR2); + debug("OAR1 0x%08x OAR2 0x%08x", rOAR1, rOAR2); + debug("CCR 0x%08x TRISE 0x%08x", rCCR, rTRISE); + debug("SR1 0x%08x SR2 0x%08x", rSR1, rSR2); +} diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 8e00781a0..f38593d2a 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -38,22 +38,14 @@ */ #include <nuttx/config.h> -#include <nuttx/arch.h> +#include <syslog.h> #include <sys/types.h> #include <stdbool.h> #include <string.h> -#include <assert.h> -#include <errno.h> -#include <unistd.h> -#include <fcntl.h> -#include <sched.h> - -#include <debug.h> #include <drivers/drv_pwm_output.h> #include <drivers/drv_hrt.h> -#include <drivers/device/device.h> #include <systemlib/mixer/mixer.h> @@ -75,13 +67,16 @@ extern "C" { #define OVERRIDE 4 /* current servo arm/disarm state */ -bool mixer_servos_armed = false; +static bool mixer_servos_armed = false; /* selected control values and count for mixing */ -static uint16_t *control_values; -static int control_count; - -static uint16_t rc_channel_data[PX4IO_INPUT_CHANNELS]; +enum mixer_source { + MIX_NONE, + MIX_FMU, + MIX_OVERRIDE, + MIX_FAILSAFE +}; +static mixer_source source; static int mixer_callback(uintptr_t handle, uint8_t control_group, @@ -93,87 +88,62 @@ static MixerGroup mixer_group(mixer_callback, 0); void mixer_tick(void) { - bool should_arm; - /* check that we are receiving fresh data from the FMU */ - if ((hrt_absolute_time() - system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { - /* too many frames without FMU input, time to go to failsafe */ - system_state.mixer_manual_override = true; - system_state.mixer_fmu_available = false; + if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { + + /* too long without FMU input, time to go to failsafe */ + if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { + isr_debug(1, "AP RX timeout"); + } + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM); + r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST; + + } else { + r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; + r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_FMU_LOST; } + source = MIX_FAILSAFE; + /* - * Decide which set of inputs we're using. + * Decide which set of controls we're using. */ - - /* this is for planes, where manual override makes sense */ - if (system_state.manual_override_ok) { - /* if everything is ok */ - if (!system_state.mixer_manual_override && system_state.mixer_fmu_available) { - /* we have recent control data from the FMU */ - control_count = PX4IO_CONTROL_CHANNELS; - control_values = &system_state.fmu_channel_data[0]; - - } else if (system_state.rc_channels > 0) { - /* when override is on or the fmu is not available, but RC is present */ - control_count = system_state.rc_channels; - - sched_lock(); - - /* remap roll, pitch, yaw and throttle from RC specific to internal ordering */ - rc_channel_data[ROLL] = system_state.rc_channel_data[system_state.rc_map[ROLL] - 1]; - rc_channel_data[PITCH] = system_state.rc_channel_data[system_state.rc_map[PITCH] - 1]; - rc_channel_data[YAW] = system_state.rc_channel_data[system_state.rc_map[YAW] - 1]; - rc_channel_data[THROTTLE] = system_state.rc_channel_data[system_state.rc_map[THROTTLE] - 1]; - //rc_channel_data[OVERRIDE] = system_state.rc_channel_data[system_state.rc_map[OVERRIDE] - 1]; - - /* get the remaining channels, no remapping needed */ - for (unsigned i = 4; i < system_state.rc_channels; i++) { - rc_channel_data[i] = system_state.rc_channel_data[i]; - } - - /* scale the control inputs */ - rc_channel_data[THROTTLE] = ((float)(rc_channel_data[THROTTLE] - system_state.rc_min[THROTTLE]) / - (float)(system_state.rc_max[THROTTLE] - system_state.rc_min[THROTTLE])) * 1000.0f + 1000; - - if (rc_channel_data[THROTTLE] > 2000) { - rc_channel_data[THROTTLE] = 2000; - } - - if (rc_channel_data[THROTTLE] < 1000) { - rc_channel_data[THROTTLE] = 1000; - } - - // lowsyslog("Tmin: %d Ttrim: %d Tmax: %d T: %d \n", - // (int)(system_state.rc_min[THROTTLE]), (int)(system_state.rc_trim[THROTTLE]), - // (int)(system_state.rc_max[THROTTLE]), (int)(rc_channel_data[THROTTLE])); - - control_values = &rc_channel_data[0]; - sched_unlock(); - } else { - /* we have no control input (no FMU, no RC) */ - - // XXX builtin failsafe would activate here - control_count = 0; - } - //lowsyslog("R: %d P: %d Y: %d T: %d \n", control_values[0], control_values[1], control_values[2], control_values[3]); + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) || + !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { + + /* don't actually mix anything - we already have raw PWM values or + not a valid mixer. */ + source = MIX_NONE; - /* this is for multicopters, etc. where manual override does not make sense */ } else { - /* if the fmu is available whe are good */ - if (system_state.mixer_fmu_available) { - control_count = PX4IO_CONTROL_CHANNELS; - control_values = &system_state.fmu_channel_data[0]; - /* we better shut everything off */ - } else { - control_count = 0; + + if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { + + /* mix from FMU controls */ + source = MIX_FMU; + } + + if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { + + /* if allowed, mix from RC inputs directly */ + source = MIX_OVERRIDE; } } /* - * Run the mixers if we have any control data at all. + * Run the mixers. */ - if (control_count > 0) { + if (source == MIX_FAILSAFE) { + + /* copy failsafe values to the servo outputs */ + for (unsigned i = 0; i < IO_SERVO_COUNT; i++) + r_page_servos[i] = r_page_servo_failsafe[i]; + + } else if (source != MIX_NONE) { + float outputs[IO_SERVO_COUNT]; unsigned mixed; @@ -181,31 +151,36 @@ mixer_tick(void) mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT); /* scale to PWM and update the servo outputs as required */ - for (unsigned i = 0; i < IO_SERVO_COUNT; i++) { - if (i < mixed) { - /* scale to servo output */ - system_state.servos[i] = (outputs[i] * 500.0f) + 1500; - - } else { - /* set to zero to inhibit PWM pulse output */ - system_state.servos[i] = 0; - } - - /* - * If we are armed, update the servo output. - */ - if (system_state.armed && system_state.arm_ok) { - up_pwm_servo_set(i, system_state.servos[i]); - } + for (unsigned i = 0; i < mixed; i++) { + + /* save actuator values for FMU readback */ + r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); + + /* scale to servo output */ + r_page_servos[i] = (outputs[i] * 500.0f) + 1500; + } + for (unsigned i = mixed; i < IO_SERVO_COUNT; i++) + r_page_servos[i] = 0; } /* * Decide whether the servos should be armed right now. - * A sufficient reason is armed state and either FMU or RC control inputs + * + * We must be armed, and we must have a PWM source; either raw from + * FMU or from the mixer. + * + * XXX correct behaviour for failsafe may require an additional case + * here. */ - - should_arm = system_state.armed && system_state.arm_ok && (control_count > 0); + bool should_arm = ( + /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && + /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && + /* there is valid input */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) && + /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) && + /* FMU is available or FMU is not available but override is an option */ + ((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) )) + ); if (should_arm && !mixer_servos_armed) { /* need to arm, but not armed */ @@ -217,6 +192,12 @@ mixer_tick(void) up_pwm_servo_arm(false); mixer_servos_armed = false; } + + if (mixer_servos_armed) { + /* update the servo outputs. */ + for (unsigned i = 0; i < IO_SERVO_COUNT; i++) + up_pwm_servo_set(i, r_page_servos[i]); + } } static int @@ -225,30 +206,54 @@ mixer_callback(uintptr_t handle, uint8_t control_index, float &control) { - /* if the control index refers to an input that's not valid, we can't return it */ - if (control_index >= control_count) + if (control_group != 0) return -1; - /* scale from current PWM units (1000-2000) to mixer input values */ - if (system_state.manual_override_ok && system_state.mixer_manual_override && control_index == 3) { - control = ((float)control_values[control_index] - 1000.0f) / 1000.0f; - } else { - control = ((float)control_values[control_index] - 1500.0f) / 500.0f; + switch (source) { + case MIX_FMU: + if (control_index < PX4IO_CONTROL_CHANNELS) { + control = REG_TO_FLOAT(r_page_controls[control_index]); + break; + } + return -1; + + case MIX_OVERRIDE: + if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << control_index)) { + control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); + break; + } + return -1; + + case MIX_FAILSAFE: + case MIX_NONE: + /* XXX we could allow for configuration of per-output failsafe values */ + return -1; } return 0; } -static char mixer_text[256]; +/* + * XXX error handling here should be more aggressive; currently it is + * possible to get STATUS_FLAGS_MIXER_OK set even though the mixer has + * not loaded faithfully. + */ + +static char mixer_text[256]; /* large enough for one mixer */ static unsigned mixer_text_length = 0; void mixer_handle_text(const void *buffer, size_t length) { + /* do not allow a mixer change while fully armed */ + if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && + /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + return; + } px4io_mixdata *msg = (px4io_mixdata *)buffer; - debug("mixer text %u", length); + isr_debug(2, "mix txt %u", length); if (length < sizeof(px4io_mixdata)) return; @@ -257,23 +262,30 @@ mixer_handle_text(const void *buffer, size_t length) switch (msg->action) { case F2I_MIXER_ACTION_RESET: - debug("reset"); + isr_debug(2, "reset"); + + /* FIRST mark the mixer as invalid */ + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; + /* THEN actually delete it */ mixer_group.reset(); mixer_text_length = 0; /* FALLTHROUGH */ case F2I_MIXER_ACTION_APPEND: - debug("append %d", length); + isr_debug(2, "append %d", length); /* check for overflow - this is really fatal */ - if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) + /* XXX could add just what will fit & try to parse, then repeat... */ + if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; return; + } /* append mixer text and nul-terminate */ memcpy(&mixer_text[mixer_text_length], msg->text, text_length); mixer_text_length += text_length; mixer_text[mixer_text_length] = '\0'; - debug("buflen %u", mixer_text_length); + isr_debug(2, "buflen %u", mixer_text_length); /* process the text buffer, adding new mixers as their descriptions can be parsed */ unsigned resid = mixer_text_length; @@ -281,7 +293,11 @@ mixer_handle_text(const void *buffer, size_t length) /* if anything was parsed */ if (resid != mixer_text_length) { - debug("used %u", mixer_text_length - resid); + + /* ideally, this should test resid == 0 ? */ + r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK; + + isr_debug(2, "used %u", mixer_text_length - resid); /* copy any leftover text to the base of the buffer for re-use */ if (resid > 0) diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 90236b40c..8d8b7e941 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -31,67 +31,157 @@ * ****************************************************************************/ -/** - * @file PX4FMU <-> PX4IO messaging protocol. - * - * This initial version of the protocol is very simple; each side transmits a - * complete update with each frame. This avoids the sending of many small - * messages and the corresponding complexity involved. - */ - #pragma once -#define PX4IO_CONTROL_CHANNELS 8 -#define PX4IO_INPUT_CHANNELS 12 -#define PX4IO_RELAY_CHANNELS 4 - -#pragma pack(push, 1) - /** - * Periodic command from FMU to IO. - */ -struct px4io_command { - uint16_t f2i_magic; -#define F2I_MAGIC 0x636d - - uint16_t servo_rate; - uint16_t output_control[PX4IO_CONTROL_CHANNELS]; /**< PWM output rate in Hz */ - bool relay_state[PX4IO_RELAY_CHANNELS]; /**< relay states as requested by FMU */ - bool arm_ok; /**< FMU allows full arming */ - bool vector_flight_mode_ok; /**< FMU aquired a valid position lock, ready for pos control */ - bool manual_override_ok; /**< if true, IO performs a direct manual override */ -}; - -/** - * Periodic report from IO to FMU + * @file protocol.h + * + * PX4IO I2C interface protocol. + * + * Communication is performed via writes to and reads from 16-bit virtual + * registers organised into pages of 255 registers each. + * + * The first two bytes of each write select a page and offset address + * respectively. Subsequent reads and writes increment the offset within + * the page. + * + * Most pages are readable or writable but not both. + * + * Note that some pages may permit offset values greater than 255, which + * can only be achieved by long writes. The offset does not wrap. + * + * Writes to unimplemented registers are ignored. Reads from unimplemented + * registers return undefined values. + * + * As convention, values that would be floating point in other parts of + * the PX4 system are expressed as signed integer values scaled by 10000, + * e.g. control values range from -10000..10000. Use the REG_TO_SIGNED and + * SIGNED_TO_REG macros to convert between register representation and + * the signed version, and REG_TO_FLOAT/FLOAT_TO_REG to convert to float. + * + * Note that the implementation of readable pages prefers registers within + * readable pages to be densely packed. Page numbers do not need to be + * packed. */ -struct px4io_report { - uint16_t i2f_magic; -#define I2F_MAGIC 0x7570 - - uint16_t rc_channel[PX4IO_INPUT_CHANNELS]; - bool armed; - uint8_t channel_count; - uint16_t battery_mv; - uint16_t adc_in; - uint8_t overcurrent; -}; - -/** - * As-needed config message from FMU to IO - */ -struct px4io_config { - uint16_t f2i_config_magic; -#define F2I_CONFIG_MAGIC 0x6366 - - uint8_t rc_map[4]; /**< channel ordering of roll, pitch, yaw, throttle */ - uint16_t rc_min[4]; /**< min value for each channel */ - uint16_t rc_trim[4]; /**< trim value for each channel */ - uint16_t rc_max[4]; /**< max value for each channel */ - int8_t rc_rev[4]; /**< rev value for each channel */ - uint16_t rc_dz[4]; /**< dz value for each channel */ -}; +#define PX4IO_CONTROL_CHANNELS 8 +#define PX4IO_INPUT_CHANNELS 12 +#define PX4IO_RELAY_CHANNELS 4 + +/* Per C, this is safe for all 2's complement systems */ +#define REG_TO_SIGNED(_reg) ((int16_t)(_reg)) +#define SIGNED_TO_REG(_signed) ((uint16_t)(_signed)) + +#define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f) +#define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f)) + +/* static configuration page */ +#define PX4IO_PAGE_CONFIG 0 +#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers TBD */ +#define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */ +#define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */ +#define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */ +#define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */ +#define PX4IO_P_CONFIG_ACTUATOR_COUNT 5 /* hardcoded max actuator output count */ +#define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */ +#define PX4IO_P_CONFIG_ADC_INPUT_COUNT 7 /* hardcoded max ADC inputs */ +#define PX4IO_P_CONFIG_RELAY_COUNT 8 /* harcoded # of relay outputs */ + +/* dynamic status page */ +#define PX4IO_PAGE_STATUS 1 +#define PX4IO_P_STATUS_FREEMEM 0 +#define PX4IO_P_STATUS_CPULOAD 1 + +#define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */ +#define PX4IO_P_STATUS_FLAGS_ARMED (1 << 0) /* arm-ok and locally armed */ +#define PX4IO_P_STATUS_FLAGS_OVERRIDE (1 << 1) /* in manual override */ +#define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 2) /* RC input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 3) /* PPM input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_DSM (1 << 4) /* DSM input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_SBUS (1 << 5) /* SBUS input is valid */ +#define PX4IO_P_STATUS_FLAGS_FMU_OK (1 << 6) /* controls from FMU are valid */ +#define PX4IO_P_STATUS_FLAGS_RAW_PWM (1 << 7) /* raw PWM from FMU is bypassing the mixer */ +#define PX4IO_P_STATUS_FLAGS_MIXER_OK (1 << 8) /* mixer is OK */ +#define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */ +#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */ + +#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ +#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */ +#define PX4IO_P_STATUS_ALARMS_TEMPERATURE (1 << 1) /* board temperature is high */ +#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT (1 << 2) /* servo current limit was exceeded */ +#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT (1 << 3) /* accessory current limit was exceeded */ +#define PX4IO_P_STATUS_ALARMS_FMU_LOST (1 << 4) /* timed out waiting for controls from FMU */ +#define PX4IO_P_STATUS_ALARMS_RC_LOST (1 << 5) /* timed out waiting for RC input */ +#define PX4IO_P_STATUS_ALARMS_PWM_ERROR (1 << 6) /* PWM configuration or output was bad */ + +#define PX4IO_P_STATUS_VBATT 4 /* battery voltage in mV */ +#define PX4IO_P_STATUS_IBATT 5 /* battery current in cA */ + +/* array of post-mix actuator outputs, -10000..10000 */ +#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* array of PWM servo output values, microseconds */ +#define PX4IO_PAGE_SERVOS 3 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* array of raw RC input values, microseconds */ +#define PX4IO_PAGE_RAW_RC_INPUT 4 +#define PX4IO_P_RAW_RC_COUNT 0 /* number of valid channels */ +#define PX4IO_P_RAW_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT channels from here */ + +/* array of scaled RC input values, -10000..10000 */ +#define PX4IO_PAGE_RC_INPUT 5 +#define PX4IO_P_RC_VALID 0 /* bitmask of valid controls */ +#define PX4IO_P_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT controls from here */ + +/* array of raw ADC values */ +#define PX4IO_PAGE_RAW_ADC_INPUT 6 /* 0..CONFIG_ADC_INPUT_COUNT-1 */ + +/* PWM servo information */ +#define PX4IO_PAGE_PWM_INFO 7 +#define PX4IO_RATE_MAP_BASE 0 /* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */ + +/* setup page */ +#define PX4IO_PAGE_SETUP 100 +#define PX4IO_P_SETUP_FEATURES 0 + +#define PX4IO_P_SETUP_ARMING 1 /* arming controls */ +#define PX4IO_P_SETUP_ARMING_ARM_OK (1 << 0) /* OK to arm */ +#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 2) /* OK to switch to manual override via override RC channel */ +#define PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK (1 << 3) /* OK to perform position / vector control (= position lock) */ +#define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */ + +#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ +#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ +#define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */ +#define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */ +#define PX4IO_P_SETUP_VBATT_SCALE 6 /* battery voltage correction factor (float) */ +#define PX4IO_P_SETUP_IBATT_SCALE 7 /* battery current scaling factor (float) */ +#define PX4IO_P_SETUP_IBATT_BIAS 8 /* battery current bias value */ +#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ + +/* autopilot control values, -10000..10000 */ +#define PX4IO_PAGE_CONTROLS 101 /* 0..CONFIG_CONTROL_COUNT */ + +/* raw text load to the mixer parser - ignores offset */ +#define PX4IO_PAGE_MIXERLOAD 102 + +/* R/C channel config */ +#define PX4IO_PAGE_RC_CONFIG 103 /* R/C input configuration */ +#define PX4IO_P_RC_CONFIG_MIN 0 /* lowest input value */ +#define PX4IO_P_RC_CONFIG_CENTER 1 /* center input value */ +#define PX4IO_P_RC_CONFIG_MAX 2 /* highest input value */ +#define PX4IO_P_RC_CONFIG_DEADZONE 3 /* band around center that is ignored */ +#define PX4IO_P_RC_CONFIG_ASSIGNMENT 4 /* mapped input value */ +#define PX4IO_P_RC_CONFIG_OPTIONS 5 /* channel options bitmask */ +#define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED (1 << 0) +#define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE (1 << 1) +#define PX4IO_P_RC_CONFIG_STRIDE 6 /* spacing between channel config data */ + +/* PWM output - overrides mixer */ +#define PX4IO_PAGE_DIRECT_PWM 104 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM failsafe values - zero disables the output */ +#define PX4IO_PAGE_FAILSAFE_PWM 105 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /** * As-needed mixer data upload. @@ -99,18 +189,16 @@ struct px4io_config { * This message adds text to the mixer text buffer; the text * buffer is drained as the definitions are consumed. */ +#pragma pack(push, 1) struct px4io_mixdata { uint16_t f2i_mixer_magic; #define F2I_MIXER_MAGIC 0x6d74 uint8_t action; -#define F2I_MIXER_ACTION_RESET 0 -#define F2I_MIXER_ACTION_APPEND 1 +#define F2I_MIXER_ACTION_RESET 0 +#define F2I_MIXER_ACTION_APPEND 1 char text[0]; /* actual text size may vary */ }; +#pragma pack(pop) -/* maximum size is limited by the HX frame size */ -#define F2I_MIXER_MAX_TEXT (HX_STREAM_MAX_FRAME - sizeof(struct px4io_mixdata)) - -#pragma pack(pop)
\ No newline at end of file diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 88342816e..9de37e118 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -37,20 +37,23 @@ */ #include <nuttx/config.h> -#include <stdio.h> + +#include <stdio.h> // required for task_create #include <stdbool.h> -#include <fcntl.h> -#include <unistd.h> -#include <debug.h> #include <stdlib.h> #include <errno.h> #include <string.h> - -#include <nuttx/clock.h> +#include <poll.h> +#include <signal.h> #include <drivers/drv_pwm_output.h> #include <drivers/drv_hrt.h> +#include <systemlib/perf_counter.h> + +#include <stm32_uart.h> + +#define DEBUG #include "px4io.h" __EXPORT int user_start(int argc, char *argv[]); @@ -59,19 +62,79 @@ extern void up_cxxinitialize(void); struct sys_state_s system_state; -int user_start(int argc, char *argv[]) +static struct hrt_call serial_dma_call; + +/* store i2c reset count XXX this should be a register, together with other error counters */ +volatile uint32_t i2c_loop_resets = 0; + +/* + * a set of debug buffers to allow us to send debug information from ISRs + */ + +static volatile uint32_t msg_counter; +static volatile uint32_t last_msg_counter; +static volatile uint8_t msg_next_out, msg_next_in; + +/* + * WARNING: too large buffers here consume the memory required + * for mixer handling. Do not allocate more than 80 bytes for + * output. + */ +#define NUM_MSG 2 +static char msg[NUM_MSG][40]; + +/* + * add a debug message to be printed on the console + */ +void +isr_debug(uint8_t level, const char *fmt, ...) +{ + if (level > r_page_setup[PX4IO_P_SETUP_SET_DEBUG]) { + return; + } + va_list ap; + va_start(ap, fmt); + vsnprintf(msg[msg_next_in], sizeof(msg[0]), fmt, ap); + va_end(ap); + msg_next_in = (msg_next_in+1) % NUM_MSG; + msg_counter++; +} + +/* + * show all pending debug messages + */ +static void +show_debug_messages(void) +{ + if (msg_counter != last_msg_counter) { + uint32_t n = msg_counter - last_msg_counter; + if (n > NUM_MSG) n = NUM_MSG; + last_msg_counter = msg_counter; + while (n--) { + debug("%s", msg[msg_next_out]); + msg_next_out = (msg_next_out+1) % NUM_MSG; + } + } +} + +int +user_start(int argc, char *argv[]) { /* run C++ ctors before we go any further */ up_cxxinitialize(); /* reset all to zero */ memset(&system_state, 0, sizeof(system_state)); - /* default to 50 Hz PWM outputs */ - system_state.servo_rate = 50; /* configure the high-resolution time/callout interface */ hrt_init(); + /* + * Poll at 1ms intervals for received bytes that have not triggered + * a DMA event. + */ + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); + /* print some startup info */ lowsyslog("\nPX4IO: starting\n"); @@ -89,17 +152,80 @@ int user_start(int argc, char *argv[]) /* configure the first 8 PWM outputs (i.e. all of them) */ up_pwm_servo_init(0xff); - /* start the flight control signal handler */ - task_create("FCon", - SCHED_PRIORITY_DEFAULT, - 1024, - (main_t)controls_main, - NULL); + /* initialise the control inputs */ + controls_init(); + + /* start the i2c handler */ + i2c_init(); + + /* add a performance counter for mixing */ + perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); + + /* add a performance counter for controls */ + perf_counter_t controls_perf = perf_alloc(PC_ELAPSED, "controls"); + /* and one for measuring the loop rate */ + perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop"); struct mallinfo minfo = mallinfo(); - lowsyslog("free %u largest %u\n", minfo.mxordblk, minfo.fordblks); + lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks); + +#if 0 + /* not enough memory, lock down */ + if (minfo.mxordblk < 500) { + lowsyslog("ERR: not enough MEM"); + bool phase = false; + + if (phase) { + LED_AMBER(true); + LED_BLUE(false); + } else { + LED_AMBER(false); + LED_BLUE(true); + } + + phase = !phase; + usleep(300000); + } +#endif + + /* + * Run everything in a tight loop. + */ + + uint64_t last_debug_time = 0; + for (;;) { + + /* track the rate at which the loop is running */ + perf_count(loop_perf); + + /* kick the mixer */ + perf_begin(mixer_perf); + mixer_tick(); + perf_end(mixer_perf); + + /* kick the control inputs */ + perf_begin(controls_perf); + controls_tick(); + perf_end(controls_perf); + + /* check for debug activity */ + show_debug_messages(); + + /* post debug state at ~1Hz */ + if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { + + struct mallinfo minfo = mallinfo(); + + isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u m=%u", + (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG], + (unsigned)r_status_flags, + (unsigned)r_setup_arming, + (unsigned)r_setup_features, + (unsigned)i2c_loop_resets, + (unsigned)minfo.mxordblk); + last_debug_time = hrt_absolute_time(); + } + } +} - /* we're done here, go run the communications loop */ - comms_main(); -}
\ No newline at end of file diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 3ce6afc31..202e9d9d9 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -64,129 +64,58 @@ #endif /* - * System state structure. + * Registers. */ -struct sys_state_s { - - bool armed; /* IO armed */ - bool arm_ok; /* FMU says OK to arm */ - uint16_t servo_rate; /* output rate of servos in Hz */ - - /** - * Remote control input(s) channel mappings - */ - uint8_t rc_map[4]; +extern uint16_t r_page_status[]; /* PX4IO_PAGE_STATUS */ +extern uint16_t r_page_actuators[]; /* PX4IO_PAGE_ACTUATORS */ +extern uint16_t r_page_servos[]; /* PX4IO_PAGE_SERVOS */ +extern uint16_t r_page_raw_rc_input[]; /* PX4IO_PAGE_RAW_RC_INPUT */ +extern uint16_t r_page_rc_input[]; /* PX4IO_PAGE_RC_INPUT */ +extern uint16_t r_page_adc[]; /* PX4IO_PAGE_RAW_ADC_INPUT */ + +extern volatile uint16_t r_page_setup[]; /* PX4IO_PAGE_SETUP */ +extern volatile uint16_t r_page_controls[]; /* PX4IO_PAGE_CONTROLS */ +extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */ +extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */ - /** - * Remote control channel attributes - */ - uint16_t rc_min[4]; - uint16_t rc_trim[4]; - uint16_t rc_max[4]; - int16_t rc_rev[4]; - uint16_t rc_dz[4]; +/* + * Register aliases. + * + * Handy aliases for registers that are widely used. + */ +#define r_status_flags r_page_status[PX4IO_P_STATUS_FLAGS] +#define r_status_alarms r_page_status[PX4IO_P_STATUS_ALARMS] - /** - * Data from the remote control input(s) - */ - unsigned rc_channels; - uint16_t rc_channel_data[PX4IO_INPUT_CHANNELS]; - uint64_t rc_channels_timestamp; +#define r_raw_rc_count r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT] +#define r_raw_rc_values (&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE]) +#define r_rc_valid r_page_rc_input[PX4IO_P_RC_VALID] +#define r_rc_values (&r_page_rc_input[PX4IO_P_RAW_RC_BASE]) - /** - * Control signals from FMU. - */ - uint16_t fmu_channel_data[PX4IO_CONTROL_CHANNELS]; +#define r_setup_features r_page_setup[PX4IO_P_SETUP_FEATURES] +#define r_setup_arming r_page_setup[PX4IO_P_SETUP_ARMING] +#define r_setup_pwm_rates r_page_setup[PX4IO_P_SETUP_PWM_RATES] +#define r_setup_pwm_defaultrate r_page_setup[PX4IO_P_SETUP_PWM_DEFAULTRATE] +#define r_setup_pwm_altrate r_page_setup[PX4IO_P_SETUP_PWM_ALTRATE] +#define r_setup_relays r_page_setup[PX4IO_P_SETUP_RELAYS] - /** - * Mixed servo outputs - */ - uint16_t servos[IO_SERVO_COUNT]; +#define r_control_values (&r_page_controls[0]) - /** - * Relay controls - */ - bool relays[PX4IO_RELAY_CHANNELS]; - - /** - * If true, we are using the FMU controls, else RC input if available. - */ - bool mixer_manual_override; - - /** - * If true, FMU input is available. - */ - bool mixer_fmu_available; +/* + * System state structure. + */ +struct sys_state_s { - /** - * If true, state that should be reported to FMU has been updated. - */ - bool fmu_report_due; + volatile uint64_t rc_channels_timestamp; /** * Last FMU receive time, in microseconds since system boot */ - uint64_t fmu_data_received_time; - - /** - * Current serial interface mode, per the serial_rx_mode parameter - * in the config packet. - */ - uint8_t serial_rx_mode; - - /** - * If true, the RC signal has been lost for more than a timeout interval - */ - bool rc_lost; - - /** - * If true, the connection to FMU has been lost for more than a timeout interval - */ - bool fmu_lost; - - /** - * If true, FMU is ready for autonomous position control. Only used for LED indication - */ - bool vector_flight_mode_ok; - - /** - * If true, IO performs an on-board manual override with the RC channel values - */ - bool manual_override_ok; - - /* - * Measured battery voltage in mV - */ - uint16_t battery_mv; - - /* - * ADC IN5 measurement - */ - uint16_t adc_in5; - - /* - * Power supply overcurrent status bits. - */ - uint8_t overcurrent; + volatile uint64_t fmu_data_received_time; }; extern struct sys_state_s system_state; -#if 0 -/* - * Software countdown timers. - * - * Each timer counts down to zero at one tick per ms. - */ -#define TIMER_BLINK_AMBER 0 -#define TIMER_BLINK_BLUE 1 -#define TIMER_STATUS_PRINT 2 -#define TIMER_SANITY 7 -#define TIMER_NUM_TIMERS 8 -extern volatile int timers[TIMER_NUM_TIMERS]; -#endif - /* * GPIO handling. */ @@ -206,6 +135,7 @@ extern volatile int timers[TIMER_NUM_TIMERS]; #define ADC_VBATT 4 #define ADC_IN5 5 +#define ADC_CHANNEL_COUNT 2 /* * Mixer @@ -213,34 +143,46 @@ extern volatile int timers[TIMER_NUM_TIMERS]; extern void mixer_tick(void); extern void mixer_handle_text(const void *buffer, size_t length); -/* +/** * Safety switch/LED. */ extern void safety_init(void); -/* +/** * FMU communications */ -extern void comms_main(void) __attribute__((noreturn)); +extern void i2c_init(void); -/* +/** + * Register space + */ +extern void registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); +extern int registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values); + +/** * Sensors/misc inputs */ extern int adc_init(void); extern uint16_t adc_measure(unsigned channel); -/* +/** * R/C receiver handling. + * + * Input functions return true when they receive an update from the RC controller. */ -extern void controls_main(void); +extern void controls_init(void); +extern void controls_tick(void); extern int dsm_init(const char *device); -extern bool dsm_input(void); +extern bool dsm_input(uint16_t *values, uint16_t *num_values); extern int sbus_init(const char *device); -extern bool sbus_input(void); +extern bool sbus_input(uint16_t *values, uint16_t *num_values); + +/** global debug level for isr_debug() */ +extern volatile uint8_t debug_level; + +/* send a debug message to the console */ +extern void isr_debug(uint8_t level, const char *fmt, ...); + +void i2c_dump(void); +void i2c_reset(void); -/* - * Assertion codes - */ -#define A_GPIO_OPEN_FAIL 100 -#define A_SERVO_OPEN_FAIL 101 -#define A_INPUTQ_OPEN_FAIL 102 diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c new file mode 100644 index 000000000..6c09def9e --- /dev/null +++ b/apps/px4io/registers.c @@ -0,0 +1,644 @@ +/**************************************************************************** + * + * 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 registers.c + * + * Implementation of the PX4IO register space. + */ + +#include <nuttx/config.h> + +#include <stdbool.h> +#include <stdlib.h> + +#include <drivers/drv_hrt.h> + +#include "px4io.h" +#include "protocol.h" + +static int registers_set_one(uint8_t page, uint8_t offset, uint16_t value); +static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate); + +/** + * PAGE 0 + * + * Static configuration parameters. + */ +static const uint16_t r_page_config[] = { + [PX4IO_P_CONFIG_PROTOCOL_VERSION] = 1, /* XXX hardcoded magic number */ + [PX4IO_P_CONFIG_SOFTWARE_VERSION] = 1, /* XXX hardcoded magic number */ + [PX4IO_P_CONFIG_BOOTLOADER_VERSION] = 3, /* XXX hardcoded magic number */ + [PX4IO_P_CONFIG_MAX_TRANSFER] = 64, /* XXX hardcoded magic number */ + [PX4IO_P_CONFIG_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS, + [PX4IO_P_CONFIG_ACTUATOR_COUNT] = IO_SERVO_COUNT, + [PX4IO_P_CONFIG_RC_INPUT_COUNT] = MAX_CONTROL_CHANNELS, + [PX4IO_P_CONFIG_ADC_INPUT_COUNT] = ADC_CHANNEL_COUNT, + [PX4IO_P_CONFIG_RELAY_COUNT] = PX4IO_RELAY_CHANNELS, +}; + +/** + * PAGE 1 + * + * Status values. + */ +uint16_t r_page_status[] = { + [PX4IO_P_STATUS_FREEMEM] = 0, + [PX4IO_P_STATUS_CPULOAD] = 0, + [PX4IO_P_STATUS_FLAGS] = 0, + [PX4IO_P_STATUS_ALARMS] = 0, + [PX4IO_P_STATUS_VBATT] = 0, + [PX4IO_P_STATUS_IBATT] = 0 +}; + +/** + * PAGE 2 + * + * Post-mixed actuator values. + */ +uint16_t r_page_actuators[IO_SERVO_COUNT]; + +/** + * PAGE 3 + * + * Servo PWM values + */ +uint16_t r_page_servos[IO_SERVO_COUNT]; + +/** + * PAGE 4 + * + * Raw RC input + */ +uint16_t r_page_raw_rc_input[] = +{ + [PX4IO_P_RAW_RC_COUNT] = 0, + [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + MAX_CONTROL_CHANNELS)] = 0 +}; + +/** + * PAGE 5 + * + * Scaled/routed RC input + */ +uint16_t r_page_rc_input[] = { + [PX4IO_P_RC_VALID] = 0, + [PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + MAX_CONTROL_CHANNELS)] = 0 +}; + +/** + * Scratch page; used for registers that are constructed as-read. + * + * PAGE 6 Raw ADC input. + * PAGE 7 PWM rate maps. + */ +uint16_t r_page_scratch[32]; + +/** + * PAGE 100 + * + * Setup registers + */ +volatile uint16_t r_page_setup[] = +{ + [PX4IO_P_SETUP_FEATURES] = 0, + [PX4IO_P_SETUP_ARMING] = 0, + [PX4IO_P_SETUP_PWM_RATES] = 0, + [PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50, + [PX4IO_P_SETUP_PWM_ALTRATE] = 200, + [PX4IO_P_SETUP_RELAYS] = 0, + [PX4IO_P_SETUP_VBATT_SCALE] = 10000, + [PX4IO_P_SETUP_IBATT_SCALE] = 0, + [PX4IO_P_SETUP_IBATT_BIAS] = 0, + [PX4IO_P_SETUP_SET_DEBUG] = 0, +}; + +#define PX4IO_P_SETUP_FEATURES_VALID (0) +#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_ARM_OK | \ + PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \ + PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) +#define PX4IO_P_SETUP_RATES_VALID ((1 << IO_SERVO_COUNT) - 1) +#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) + +/** + * PAGE 101 + * + * Control values from the FMU. + */ +volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS]; + +/* + * PAGE 102 does not have a buffer. + */ + +/** + * PAGE 103 + * + * R/C channel input configuration. + */ +uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE]; + +/* valid options */ +#define PX4IO_P_RC_CONFIG_OPTIONS_VALID (PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) + +/* + * PAGE 104 uses r_page_servos. + */ + +/** + * PAGE 105 + * + * Failsafe servo PWM values + */ +uint16_t r_page_servo_failsafe[IO_SERVO_COUNT]; + +void +registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) +{ + + switch (page) { + + /* handle bulk controls input */ + case PX4IO_PAGE_CONTROLS: + + /* copy channel data */ + while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { + + /* XXX range-check value? */ + r_page_controls[offset] = *values; + + offset++; + num_values--; + values++; + } + + system_state.fmu_data_received_time = hrt_absolute_time(); + r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; + r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_FMU_LOST; + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM; + + break; + + /* handle raw PWM input */ + case PX4IO_PAGE_DIRECT_PWM: + + /* copy channel data */ + while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { + + /* XXX range-check value? */ + r_page_servos[offset] = *values; + + offset++; + num_values--; + values++; + } + + system_state.fmu_data_received_time = hrt_absolute_time(); + r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM; + + break; + + /* handle setup for servo failsafe values */ + case PX4IO_PAGE_FAILSAFE_PWM: + + /* copy channel data */ + while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { + + /* XXX range-check value? */ + r_page_servo_failsafe[offset] = *values; + + offset++; + num_values--; + values++; + } + break; + + /* handle text going to the mixer parser */ + case PX4IO_PAGE_MIXERLOAD: + mixer_handle_text(values, num_values * sizeof(*values)); + break; + + default: + /* avoid offset wrap */ + if ((offset + num_values) > 255) + num_values = 255 - offset; + + /* iterate individual registers, set each in turn */ + while (num_values--) { + if (registers_set_one(page, offset, *values)) + break; + offset++; + values++; + } + } +} + +static int +registers_set_one(uint8_t page, uint8_t offset, uint16_t value) +{ + switch (page) { + + case PX4IO_PAGE_STATUS: + switch (offset) { + case PX4IO_P_STATUS_ALARMS: + /* clear bits being written */ + r_status_alarms &= ~value; + break; + + case PX4IO_P_STATUS_FLAGS: + /* + * Allow FMU override of arming state (to allow in-air restores), + * but only if the arming state is not in sync on the IO side. + */ + if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { + r_status_flags = value; + } + break; + + default: + /* just ignore writes to other registers in this page */ + break; + } + break; + + case PX4IO_PAGE_SETUP: + switch (offset) { + case PX4IO_P_SETUP_FEATURES: + + value &= PX4IO_P_SETUP_FEATURES_VALID; + r_setup_features = value; + + /* no implemented feature selection at this point */ + + break; + + case PX4IO_P_SETUP_ARMING: + + value &= PX4IO_P_SETUP_ARMING_VALID; + + /* + * Update arming state - disarm if no longer OK. + * This builds on the requirement that the FMU driver + * asks about the FMU arming state on initialization, + * so that an in-air reset of FMU can not lead to a + * lockup of the IO arming state. + */ + if ((r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && !(value & PX4IO_P_SETUP_ARMING_ARM_OK)) { + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; + } + + r_setup_arming = value; + + break; + + case PX4IO_P_SETUP_PWM_RATES: + value &= PX4IO_P_SETUP_RATES_VALID; + pwm_configure_rates(value, r_setup_pwm_defaultrate, r_setup_pwm_altrate); + break; + + case PX4IO_P_SETUP_PWM_DEFAULTRATE: + if (value < 50) + value = 50; + if (value > 400) + value = 400; + pwm_configure_rates(r_setup_pwm_rates, value, r_setup_pwm_altrate); + break; + + case PX4IO_P_SETUP_PWM_ALTRATE: + if (value < 50) + value = 50; + if (value > 400) + value = 400; + pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value); + break; + + case PX4IO_P_SETUP_RELAYS: + value &= PX4IO_P_SETUP_RELAYS_VALID; + r_setup_relays = value; + POWER_RELAY1(value & (1 << 0) ? 1 : 0); + POWER_RELAY2(value & (1 << 1) ? 1 : 0); + POWER_ACC1(value & (1 << 2) ? 1 : 0); + POWER_ACC2(value & (1 << 3) ? 1 : 0); + break; + + case PX4IO_P_SETUP_SET_DEBUG: + r_page_setup[PX4IO_P_SETUP_SET_DEBUG] = value; + isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]); + break; + + default: + return -1; + } + break; + + case PX4IO_PAGE_RC_CONFIG: { + + /* do not allow a RC config change while fully armed */ + if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && + /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + break; + } + + unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE; + unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE; + uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE]; + + if (channel >= MAX_CONTROL_CHANNELS) + return -1; + + /* disable the channel until we have a chance to sanity-check it */ + conf[PX4IO_P_RC_CONFIG_OPTIONS] &= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + + switch (index) { + + case PX4IO_P_RC_CONFIG_MIN: + case PX4IO_P_RC_CONFIG_CENTER: + case PX4IO_P_RC_CONFIG_MAX: + case PX4IO_P_RC_CONFIG_DEADZONE: + case PX4IO_P_RC_CONFIG_ASSIGNMENT: + conf[index] = value; + break; + + case PX4IO_P_RC_CONFIG_OPTIONS: + value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID; + r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; + + /* set all options except the enabled option */ + conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + + /* should the channel be enabled? */ + /* this option is normally set last */ + if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { + uint8_t count = 0; + + /* assert min..center..max ordering */ + if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) { + count++; + } + if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) { + count++; + } + if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) { + count++; + } + if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) { + count++; + } + + /* assert deadzone is sane */ + if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) { + count++; + } + // The following check isn't needed as constraint checks in controls.c will catch this. + //if (conf[PX4IO_P_RC_CONFIG_MIN] > (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + // count++; + //} + //if (conf[PX4IO_P_RC_CONFIG_MAX] < (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + // count++; + //} + if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) { + count++; + } + + /* sanity checks pass, enable channel */ + if (count) { + isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1)); + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK; + } else { + conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + } + } + break; + /* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */ + + } + break; + /* case PX4IO_RC_PAGE_CONFIG */ + } + + default: + return -1; + } + return 0; +} + +uint8_t last_page; +uint8_t last_offset; + +int +registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values) +{ +#define SELECT_PAGE(_page_name) \ + do { \ + *values = &_page_name[0]; \ + *num_values = sizeof(_page_name) / sizeof(_page_name[0]); \ + } while(0) + + switch (page) { + + /* + * Handle pages that are updated dynamically at read time. + */ + case PX4IO_PAGE_STATUS: + /* PX4IO_P_STATUS_FREEMEM */ + { + struct mallinfo minfo = mallinfo(); + r_page_status[PX4IO_P_STATUS_FREEMEM] = minfo.fordblks; + } + + /* XXX PX4IO_P_STATUS_CPULOAD */ + + /* PX4IO_P_STATUS_FLAGS maintained externally */ + + /* PX4IO_P_STATUS_ALARMS maintained externally */ + + /* PX4IO_P_STATUS_VBATT */ + { + /* + * Coefficients here derived by measurement of the 5-16V + * range on one unit: + * + * V counts + * 5 1001 + * 6 1219 + * 7 1436 + * 8 1653 + * 9 1870 + * 10 2086 + * 11 2303 + * 12 2522 + * 13 2738 + * 14 2956 + * 15 3172 + * 16 3389 + * + * slope = 0.0046067 + * intercept = 0.3863 + * + * Intercept corrected for best results @ 12V. + */ + unsigned counts = adc_measure(ADC_VBATT); + unsigned mV = (4150 + (counts * 46)) / 10 - 200; + unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000; + + r_page_status[PX4IO_P_STATUS_VBATT] = corrected; + } + + /* PX4IO_P_STATUS_IBATT */ + { + unsigned counts = adc_measure(ADC_VBATT); + unsigned scaled = (counts * r_page_setup[PX4IO_P_SETUP_IBATT_SCALE]) / 10000; + int corrected = scaled + REG_TO_SIGNED(r_page_setup[PX4IO_P_SETUP_IBATT_BIAS]); + if (corrected < 0) + corrected = 0; + r_page_status[PX4IO_P_STATUS_IBATT] = corrected; + } + + SELECT_PAGE(r_page_status); + break; + + case PX4IO_PAGE_RAW_ADC_INPUT: + memset(r_page_scratch, 0, sizeof(r_page_scratch)); + r_page_scratch[0] = adc_measure(ADC_VBATT); + r_page_scratch[1] = adc_measure(ADC_IN5); + + SELECT_PAGE(r_page_scratch); + break; + + case PX4IO_PAGE_PWM_INFO: + memset(r_page_scratch, 0, sizeof(r_page_scratch)); + for (unsigned i = 0; i < IO_SERVO_COUNT; i++) + r_page_scratch[PX4IO_RATE_MAP_BASE + i] = up_pwm_servo_get_rate_group(i); + + SELECT_PAGE(r_page_scratch); + break; + + /* + * Pages that are just a straight read of the register state. + */ + + /* status pages */ + case PX4IO_PAGE_CONFIG: + SELECT_PAGE(r_page_config); + break; + case PX4IO_PAGE_ACTUATORS: + SELECT_PAGE(r_page_actuators); + break; + case PX4IO_PAGE_SERVOS: + SELECT_PAGE(r_page_servos); + break; + case PX4IO_PAGE_RAW_RC_INPUT: + SELECT_PAGE(r_page_raw_rc_input); + break; + case PX4IO_PAGE_RC_INPUT: + SELECT_PAGE(r_page_rc_input); + break; + + /* readback of input pages */ + case PX4IO_PAGE_SETUP: + SELECT_PAGE(r_page_setup); + break; + case PX4IO_PAGE_CONTROLS: + SELECT_PAGE(r_page_controls); + break; + case PX4IO_PAGE_RC_CONFIG: + SELECT_PAGE(r_page_rc_input_config); + break; + case PX4IO_PAGE_DIRECT_PWM: + SELECT_PAGE(r_page_servos); + break; + case PX4IO_PAGE_FAILSAFE_PWM: + SELECT_PAGE(r_page_servo_failsafe); + break; + + default: + return -1; + } + +#undef SELECT_PAGE +#undef COPY_PAGE + +last_page = page; +last_offset = offset; + + /* if the offset is at or beyond the end of the page, we have no data */ + if (offset >= *num_values) + return -1; + + /* correct the data pointer and count for the offset */ + *values += offset; + *num_values -= offset; + + return 0; +} + +/* + * Helper function to handle changes to the PWM rate control registers. + */ +static void +pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate) +{ + for (unsigned pass = 0; pass < 2; pass++) { + for (unsigned group = 0; group < IO_SERVO_COUNT; group++) { + + /* get the channel mask for this rate group */ + uint32_t mask = up_pwm_servo_get_rate_group(group); + if (mask == 0) + continue; + + /* all channels in the group must be either default or alt-rate */ + uint32_t alt = map & mask; + + if (pass == 0) { + /* preflight */ + if ((alt != 0) && (alt != mask)) { + /* not a legal map, bail with an alarm */ + r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; + return; + } + } else { + /* set it - errors here are unexpected */ + if (alt != 0) { + if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_altrate) != OK) + r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; + } else { + if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_defaultrate) != OK) + r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; + } + } + } + } + r_setup_pwm_rates = map; + r_setup_pwm_defaultrate = defaultrate; + r_setup_pwm_altrate = altrate; +} diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c index 0cae29ac9..5495d5ae1 100644 --- a/apps/px4io/safety.c +++ b/apps/px4io/safety.c @@ -36,15 +36,8 @@ */ #include <nuttx/config.h> -#include <stdio.h> -#include <stdbool.h> -#include <fcntl.h> -#include <unistd.h> -#include <debug.h> -#include <stdlib.h> -#include <errno.h> -#include <nuttx/clock.h> +#include <stdbool.h> #include <drivers/drv_hrt.h> @@ -116,30 +109,28 @@ safety_check_button(void *arg) * state machine, keep ARM_COUNTER_THRESHOLD the same * length in all cases of the if/else struct below. */ - if (safety_button_pressed && !system_state.armed) { + if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { if (counter < ARM_COUNTER_THRESHOLD) { counter++; } else if (counter == ARM_COUNTER_THRESHOLD) { - /* change to armed state and notify the FMU */ - system_state.armed = true; + /* switch to armed state */ + r_status_flags |= PX4IO_P_STATUS_FLAGS_ARMED; counter++; - system_state.fmu_report_due = true; } /* Disarm quickly */ - } else if (safety_button_pressed && system_state.armed) { + } else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { if (counter < ARM_COUNTER_THRESHOLD) { counter++; } else if (counter == ARM_COUNTER_THRESHOLD) { /* change to disarmed state and notify the FMU */ - system_state.armed = false; + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; counter++; - system_state.fmu_report_due = true; } } else { @@ -149,18 +140,18 @@ safety_check_button(void *arg) /* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */ uint16_t pattern = LED_PATTERN_SAFE; - if (system_state.armed) { - if (system_state.arm_ok) { + if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) { + if (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) { pattern = LED_PATTERN_IO_FMU_ARMED; } else { pattern = LED_PATTERN_IO_ARMED; } - } else if (system_state.arm_ok) { + } else if (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) { pattern = LED_PATTERN_FMU_ARMED; - } else if (system_state.vector_flight_mode_ok) { + } else if (r_setup_arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) { pattern = LED_PATTERN_VECTOR_FLIGHT_MODE_OK; } @@ -185,12 +176,17 @@ heartbeat_blink(void *arg) static void failsafe_blink(void *arg) { + /* indicate that a serious initialisation error occured */ + if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)) { + LED_AMBER(true); + return; + } + static bool failsafe = false; /* blink the failsafe LED if we don't have FMU input */ - if (!system_state.mixer_fmu_available) { + if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { failsafe = !failsafe; - } else { failsafe = false; } diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c index 568ef8091..073ddeaba 100644 --- a/apps/px4io/sbus.c +++ b/apps/px4io/sbus.c @@ -53,7 +53,7 @@ #include "debug.h" #define SBUS_FRAME_SIZE 25 -#define SBUS_INPUT_CHANNELS 18 +#define SBUS_INPUT_CHANNELS 16 static int sbus_fd = -1; @@ -66,13 +66,13 @@ static unsigned partial_frame_count; unsigned sbus_frame_drops; -static void sbus_decode(hrt_abstime frame_time); +static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values); int sbus_init(const char *device) { if (sbus_fd < 0) - sbus_fd = open(device, O_RDONLY); + sbus_fd = open(device, O_RDONLY | O_NONBLOCK); if (sbus_fd >= 0) { struct termios t; @@ -97,7 +97,7 @@ sbus_init(const char *device) } bool -sbus_input(void) +sbus_input(uint16_t *values, uint16_t *num_values) { ssize_t ret; hrt_abstime now; @@ -134,7 +134,7 @@ sbus_input(void) /* if the read failed for any reason, just give up here */ if (ret < 1) - goto out; + return false; last_rx_time = now; @@ -147,20 +147,14 @@ sbus_input(void) * If we don't have a full frame, return */ if (partial_frame_count < SBUS_FRAME_SIZE) - goto out; + return false; /* * Great, it looks like we might have a frame. Go ahead and * decode it. */ - sbus_decode(now); partial_frame_count = 0; - -out: - /* - * If we have seen a frame in the last 200ms, we consider ourselves 'locked' - */ - return (now - last_frame_time) < 200000; + return sbus_decode(now, values, num_values); } /* @@ -199,13 +193,13 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { /* 15 */ { {20, 5, 0x07, 0}, {21, 0, 0xff, 3}, { 0, 0, 0x00, 0} } }; -static void -sbus_decode(hrt_abstime frame_time) +static bool +sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) { /* check frame boundary markers to avoid out-of-sync cases */ if ((frame[0] != 0x0f) || (frame[24] != 0x00)) { sbus_frame_drops++; - return; + return false; } /* if the failsafe or connection lost bit is set, we consider the frame invalid */ @@ -213,8 +207,8 @@ sbus_decode(hrt_abstime frame_time) (frame[23] & (1 << 3))) { /* failsafe */ /* actively announce signal loss */ - system_state.rc_channels = 0; - return 1; + *values = 0; + return false; } /* we have received something we think is a frame */ @@ -241,23 +235,21 @@ sbus_decode(hrt_abstime frame_time) } /* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ - system_state.rc_channel_data[channel] = (value / 2) + 998; + values[channel] = (value / 2) + 998; } /* decode switch channels if data fields are wide enough */ - if (chancount > 17) { + if (PX4IO_INPUT_CHANNELS > 17 && chancount > 15) { + chancount = 18; + /* channel 17 (index 16) */ - system_state.rc_channel_data[16] = (frame[23] & (1 << 0)) * 1000 + 998; + values[16] = (frame[23] & (1 << 0)) * 1000 + 998; /* channel 18 (index 17) */ - system_state.rc_channel_data[17] = (frame[23] & (1 << 1)) * 1000 + 998; + values[17] = (frame[23] & (1 << 1)) * 1000 + 998; } /* note the number of channels decoded */ - system_state.rc_channels = chancount; - - /* and note that we have received data from the R/C controller */ - system_state.rc_channels_timestamp = frame_time; + *num_values = chancount; - /* trigger an immediate report to the FMU */ - system_state.fmu_report_due = true; + return true; } diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index 4fd5fea1b..df8745d9f 100644 --- a/apps/sdlog/sdlog.c +++ b/apps/sdlog/sdlog.c @@ -683,7 +683,7 @@ int sdlog_thread_main(int argc, char *argv[]) .vbat = buf.batt.voltage_v, .bat_current = buf.batt.current_a, .bat_discharged = buf.batt.discharged_mah, - .adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]}, + .adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2], buf.raw.adc_voltage_v[3]}, .local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z}, .gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt}, .attitude = {buf.att.pitch, buf.att.roll, buf.att.yaw}, diff --git a/apps/sdlog/sdlog_ringbuffer.h b/apps/sdlog/sdlog_ringbuffer.h index f7fc0d450..b65916459 100644 --- a/apps/sdlog/sdlog_ringbuffer.h +++ b/apps/sdlog/sdlog_ringbuffer.h @@ -56,7 +56,7 @@ struct sdlog_sysvector { float vbat; /**< battery voltage in [volt] */ float bat_current; /**< battery discharge current */ float bat_discharged; /**< discharged energy in mAh */ - float adc[3]; /**< remaining auxiliary ADC ports [volt] */ + float adc[4]; /**< ADC ports [volt] */ float local_position[3]; /**< tangent plane mapping into x,y,z [m] */ int32_t gps_raw_position[3]; /**< latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter] */ float attitude[3]; /**< roll, pitch, yaw [rad] */ @@ -88,4 +88,4 @@ void sdlog_logbuffer_write(struct sdlog_logbuffer *lb, const struct sdlog_sysvec int sdlog_logbuffer_read(struct sdlog_logbuffer *lb, struct sdlog_sysvector *elem); -#endif
\ No newline at end of file +#endif diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c index 9f23ebbba..a1ef9d136 100644 --- a/apps/sensors/sensor_params.c +++ b/apps/sensors/sensor_params.c @@ -64,6 +64,8 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f); +PARAM_DEFINE_FLOAT(SENS_VAIR_OFF, 2.5f); + PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f); diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index d8d200ea9..d725c7727 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -187,6 +187,7 @@ private: float mag_scale[3]; float accel_offset[3]; float accel_scale[3]; + float airspeed_offset; int rc_type; @@ -235,6 +236,7 @@ private: param_t accel_scale[3]; param_t mag_offset[3]; param_t mag_scale[3]; + param_t airspeed_offset; param_t rc_map_roll; param_t rc_map_pitch; @@ -480,6 +482,9 @@ Sensors::Sensors() : _parameter_handles.mag_scale[1] = param_find("SENS_MAG_YSCALE"); _parameter_handles.mag_scale[2] = param_find("SENS_MAG_ZSCALE"); + /*Airspeed offset */ + _parameter_handles.airspeed_offset = param_find("SENS_VAIR_OFF"); + _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); /* fetch initial parameter values */ @@ -687,6 +692,9 @@ Sensors::parameters_update() param_get(_parameter_handles.mag_scale[1], &(_parameters.mag_scale[1])); param_get(_parameter_handles.mag_scale[2], &(_parameters.mag_scale[2])); + /* Airspeed offset */ + param_get(_parameter_handles.airspeed_offset, &(_parameters.airspeed_offset)); + /* scaling of ADC ticks to battery voltage */ if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { warnx("Failed updating voltage scaling param"); @@ -990,12 +998,16 @@ Sensors::adc_poll(struct sensor_combined_s &raw) /* read all channels available */ int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc)); - /* look for battery channel */ - 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); + } + + /* look for specific channels and process the raw voltage to measurement data */ if (ADC_BATTERY_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { /* Voltage in volts */ float voltage = (buf_adc[i].am_data * _parameters.battery_voltage_scaling); @@ -1025,8 +1037,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { /* calculate airspeed, raw is the difference from */ - - float voltage = buf_adc[i].am_data / 4096.0f; + 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 @@ -1034,24 +1045,24 @@ Sensors::adc_poll(struct sensor_combined_s &raw) */ if (voltage > 0.4f) { - float pres_raw = fabsf(voltage - (3.3f / 2.0f)); - float pres_mbar = pres_raw * (3.3f / 5.0f) * 10.0f; + float diff_pres_pa = (voltage - _parameters.airspeed_offset) * 1000.0f; //for MPXV7002DP sensor - float airspeed_true = calc_true_airspeed(pres_mbar + _barometer.pressure, - _barometer.pressure, _barometer.temperature - 5.0f); + float airspeed_true = calc_true_airspeed(diff_pres_pa + _barometer.pressure*1e2f, + _barometer.pressure*1e2f, _barometer.temperature - 5.0f); //factor 1e2 for conversion from mBar to Pa // XXX HACK - true temperature is much less than indicated temperature in baro, // subtract 5 degrees in an attempt to account for the electrical upheating of the PCB - float airspeed_indicated = calc_indicated_airspeed(pres_mbar + _barometer.pressure, - _barometer.pressure, _barometer.temperature - 5.0f); - // XXX HACK + float airspeed_indicated = calc_indicated_airspeed(diff_pres_pa); + + //printf("voltage: %.4f, diff_pres_pa %.4f, baro press %.4f Pa, v_ind %.4f, v_true %.4f\n", (double)voltage, (double)diff_pres_pa, (double)_barometer.pressure*1e2f, (double)airspeed_indicated, (double)airspeed_true); _differential_pressure.timestamp = hrt_absolute_time(); _differential_pressure.static_pressure_mbar = _barometer.pressure; - _differential_pressure.differential_pressure_mbar = pres_mbar; + _differential_pressure.differential_pressure_mbar = diff_pres_pa*1e-2f; _differential_pressure.temperature_celcius = _barometer.temperature; _differential_pressure.indicated_airspeed_m_s = airspeed_indicated; _differential_pressure.true_airspeed_m_s = airspeed_true; + _differential_pressure.voltage = voltage; /* announce the airspeed if needed, just publish else */ if (_airspeed_pub > 0) { @@ -1064,7 +1075,6 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } _last_adc = hrt_absolute_time(); - break; } } } @@ -1074,36 +1084,6 @@ Sensors::adc_poll(struct sensor_combined_s &raw) void Sensors::ppm_poll() { - /* fake low-level driver, directly pulling from driver variables */ - static orb_advert_t rc_input_pub = -1; - struct rc_input_values raw; - - raw.timestamp = ppm_last_valid_decode; - /* we are accepting this message */ - _ppm_last_valid = ppm_last_valid_decode; - - /* - * relying on two decoded channels is very noise-prone, - * in particular if nothing is connected to the pins. - * requiring a minimum of four channels - */ - if (ppm_decoded_channels > 4 && hrt_absolute_time() - _ppm_last_valid < PPM_INPUT_TIMEOUT_INTERVAL) { - - for (unsigned i = 0; i < ppm_decoded_channels; i++) { - raw.values[i] = ppm_buffer[i]; - } - - raw.channel_count = ppm_decoded_channels; - - /* publish to object request broker */ - if (rc_input_pub <= 0) { - rc_input_pub = orb_advertise(ORB_ID(input_rc), &raw); - - } else { - orb_publish(ORB_ID(input_rc), rc_input_pub, &raw); - } - } - /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ bool rc_updated; @@ -1149,31 +1129,45 @@ Sensors::ppm_poll() /* Read out values from raw message */ for (unsigned int i = 0; i < channel_limit; i++) { - /* scale around the mid point differently for lower and upper range */ + /* + * 1) Constrain to min/max values, as later processing depends on bounds. + */ + 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]; + + /* + * 2) Scale around the mid point differently for lower and upper range. + * + * This is necessary as they don't share the same endpoints and slope. + * + * First normalize to 0..1 range with correct sign (below or above center), + * the total range is 2 (-1..1). + * If center (trim) == min, scale to 0..1, if center (trim) == max, + * scale to -1..0. + * + * As the min and max bounds were enforced in step 1), division by zero + * cannot occur, as for the case of center == min or center == max the if + * statement is mutually exclusive with the arithmetic NaN case. + * + * DO NOT REMOVE OR ALTER STEP 1! + */ if (rc_input.values[i] > (_parameters.trim[i] + _parameters.dz[i])) { - _rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]); + _rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]); } else if (rc_input.values[i] < (_parameters.trim[i] - _parameters.dz[i])) { - /* division by zero impossible for trim == min (as for throttle), as this falls in the above if clause */ - _rc.chan[i].scaled = -((_parameters.trim[i] - rc_input.values[i]) / (float)(_parameters.trim[i] - _parameters.min[i])); + _rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]); } else { /* in the configured dead zone, output zero */ _rc.chan[i].scaled = 0.0f; } - /* reverse channel if required */ - if (i == (int)_rc.function[THROTTLE]) { - if ((int)_parameters.rev[i] == -1) { - _rc.chan[i].scaled = 1.0f + -1.0f * _rc.chan[i].scaled; - } - - } else { - _rc.chan[i].scaled *= _parameters.rev[i]; - } + _rc.chan[i].scaled *= _parameters.rev[i]; /* handle any parameter-induced blowups */ - if (isnan(_rc.chan[i].scaled) || isinf(_rc.chan[i].scaled)) + if (!isfinite(_rc.chan[i].scaled)) _rc.chan[i].scaled = 0.0f; } diff --git a/apps/system/readline/readline.c b/apps/system/readline/readline.c index bac7eee8c..a8240a62a 100644 --- a/apps/system/readline/readline.c +++ b/apps/system/readline/readline.c @@ -126,7 +126,7 @@ static inline int readline_rawgetc(int infd) * error occurs). */ - do + for (;;) { /* Read one character from the incoming stream */ @@ -154,13 +154,21 @@ static inline int readline_rawgetc(int infd) { return -errcode; } + + continue; } - } - while (nread < 1); - /* On success, return the character that was read */ + else if (buffer == '\0') + { + /* Ignore NUL characters, since they look like EOF to our caller */ - return (int)buffer; + continue; + } + + /* Success, return the character that was read */ + + return (int)buffer; + } } /**************************************************************************** diff --git a/apps/systemcmds/i2c/Makefile b/apps/systemcmds/i2c/Makefile new file mode 100644 index 000000000..046e74757 --- /dev/null +++ b/apps/systemcmds/i2c/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Build the i2c test tool. +# + +APPNAME = i2c +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 4096 + +include $(APPDIR)/mk/app.mk diff --git a/apps/systemcmds/i2c/i2c.c b/apps/systemcmds/i2c/i2c.c new file mode 100644 index 000000000..e1babdc12 --- /dev/null +++ b/apps/systemcmds/i2c/i2c.c @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file i2c.c + * + * i2c hacking tool + */ + +#include <nuttx/config.h> + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <stdbool.h> +#include <unistd.h> +#include <fcntl.h> +#include <sys/mount.h> +#include <sys/ioctl.h> +#include <sys/stat.h> + +#include <nuttx/i2c.h> + +#include <arch/board/board.h> + +#include "systemlib/systemlib.h" +#include "systemlib/err.h" + +#ifndef PX4_I2C_BUS_ONBOARD +# error PX4_I2C_BUS_ONBOARD not defined, no device interface +#endif +#ifndef PX4_I2C_OBDEV_PX4IO +# error PX4_I2C_OBDEV_PX4IO not defined +#endif + +__EXPORT int i2c_main(int argc, char *argv[]); + +static int transfer(uint8_t address, uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len); + +static struct i2c_dev_s *i2c; + +int i2c_main(int argc, char *argv[]) +{ + /* find the right I2C */ + i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD); + + if (i2c == NULL) + errx(1, "failed to locate I2C bus"); + + usleep(100000); + + uint8_t buf[] = { 0, 4}; + int ret = transfer(PX4_I2C_OBDEV_PX4IO, buf, sizeof(buf), NULL, 0); + + if (ret) + errx(1, "send failed - %d", ret); + + uint32_t val; + ret = transfer(PX4_I2C_OBDEV_PX4IO, NULL, 0, (uint8_t *)&val, sizeof(val)); + if (ret) + errx(1, "recive failed - %d", ret); + + errx(0, "got 0x%08x", val); +} + +static int +transfer(uint8_t address, uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) +{ + struct i2c_msg_s msgv[2]; + unsigned msgs; + int ret; + + // debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); + + msgs = 0; + + if (send_len > 0) { + msgv[msgs].addr = address; + msgv[msgs].flags = 0; + msgv[msgs].buffer = send; + msgv[msgs].length = send_len; + msgs++; + } + + if (recv_len > 0) { + msgv[msgs].addr = address; + msgv[msgs].flags = I2C_M_READ; + msgv[msgs].buffer = recv; + msgv[msgs].length = recv_len; + msgs++; + } + + if (msgs == 0) + return -1; + + /* + * I2C architecture means there is an unavoidable race here + * if there are any devices on the bus with a different frequency + * preference. Really, this is pointless. + */ + I2C_SETFREQUENCY(i2c, 400000); + ret = I2C_TRANSFER(i2c, &msgv[0], msgs); + + // reset the I2C bus to unwedge on error + if (ret != OK) + up_i2creset(i2c); + + return ret; +} diff --git a/apps/systemcmds/mixer/mixer.c b/apps/systemcmds/mixer/mixer.c index e2f7b5bd5..55c4f0836 100644 --- a/apps/systemcmds/mixer/mixer.c +++ b/apps/systemcmds/mixer/mixer.c @@ -117,7 +117,23 @@ load(const char *devname, const char *fname) if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) continue; - /* XXX an optimisation here would be to strip extra whitespace */ + /* 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)) diff --git a/apps/systemcmds/preflight_check/preflight_check.c b/apps/systemcmds/preflight_check/preflight_check.c index f25f812ae..9ac6f0b5e 100644 --- a/apps/systemcmds/preflight_check/preflight_check.c +++ b/apps/systemcmds/preflight_check/preflight_check.c @@ -54,6 +54,8 @@ #include <drivers/drv_accel.h> #include <drivers/drv_baro.h> +#include <mavlink/mavlink_log.h> + __EXPORT int preflight_check_main(int argc, char *argv[]); static int led_toggle(int leds, int led); static int led_on(int leds, int led); @@ -75,6 +77,8 @@ int preflight_check_main(int argc, char *argv[]) bool system_ok = true; int fd; + /* open text message output path */ + int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); int ret; /* give the system some time to sample the sensors in the background */ @@ -86,6 +90,7 @@ int preflight_check_main(int argc, char *argv[]) fd = open(MAG_DEVICE_PATH, 0); if (fd < 0) { warn("failed to open magnetometer - start with 'hmc5883 start' or 'lsm303d start'"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: NO MAG"); system_ok = false; goto system_eval; } @@ -93,6 +98,7 @@ int preflight_check_main(int argc, char *argv[]) if (ret != OK) { warnx("magnetometer calibration missing or bad - calibrate magnetometer first"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CALIBRATION"); system_ok = false; goto system_eval; } @@ -105,6 +111,7 @@ int preflight_check_main(int argc, char *argv[]) if (ret != OK) { warnx("accel self test failed"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK"); system_ok = false; goto system_eval; } @@ -117,6 +124,7 @@ int preflight_check_main(int argc, char *argv[]) if (ret != OK) { warnx("gyro self test failed"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK"); system_ok = false; goto system_eval; } diff --git a/apps/systemcmds/pwm/Makefile b/apps/systemcmds/pwm/Makefile new file mode 100644 index 000000000..5ab105ed1 --- /dev/null +++ b/apps/systemcmds/pwm/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Build the pwm tool. +# + +APPNAME = pwm +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 4096 + +include $(APPDIR)/mk/app.mk diff --git a/apps/systemcmds/pwm/pwm.c b/apps/systemcmds/pwm/pwm.c new file mode 100644 index 000000000..de7a53199 --- /dev/null +++ b/apps/systemcmds/pwm/pwm.c @@ -0,0 +1,233 @@ +/**************************************************************************** + * + * 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 pwm.c + * + * PWM servo output configuration and monitoring tool. + */ + +#include <nuttx/config.h> + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <stdbool.h> +#include <unistd.h> +#include <fcntl.h> +#include <sys/mount.h> +#include <sys/ioctl.h> +#include <sys/stat.h> + +#include <nuttx/i2c.h> +#include <nuttx/mtd.h> +#include <nuttx/fs/nxffs.h> +#include <nuttx/fs/ioctl.h> + +#include <arch/board/board.h> + +#include "systemlib/systemlib.h" +#include "systemlib/err.h" +#include "drivers/drv_pwm_output.h" + +static void usage(const char *reason); +__EXPORT int pwm_main(int argc, char *argv[]); + + +static void +usage(const char *reason) +{ + if (reason != NULL) + warnx("%s", reason); + errx(1, + "usage:\n" + "pwm [-v] [-d <device>] [-u <alt_rate>] [-c <channel group>] [arm|disarm] [<channel_value> ...]\n" + " -v Print information about the PWM device\n" + " <device> PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" + " <alt_rate> PWM update rate for channels in <alt_channel_mask>\n" + " <channel_group> Channel group that should update at the alternate rate (may be specified more than once)\n" + " arm | disarm Arm or disarm the ouptut\n" + " <channel_value>... PWM output values in microseconds to assign to the PWM outputs\n" + "\n" + "When -c is specified, any channel groups not listed with -c will update at the default rate.\n" + ); + +} + +int +pwm_main(int argc, char *argv[]) +{ + const char *dev = PWM_OUTPUT_DEVICE_PATH; + unsigned alt_rate = 0; + uint32_t alt_channel_groups = 0; + bool alt_channels_set = false; + bool print_info = false; + int ch; + int ret; + char *ep; + unsigned group; + + if (argc < 2) + usage(NULL); + + while ((ch = getopt(argc, argv, "c:d:u:v")) != EOF) { + switch (ch) { + case 'c': + group = strtoul(optarg, &ep, 0); + if ((*ep != '\0') || (group >= 32)) + usage("bad channel_group value"); + alt_channel_groups |= (1 << group); + alt_channels_set = true; + break; + + case 'd': + dev = optarg; + break; + + case 'u': + alt_rate = strtol(optarg, &ep, 0); + if (*ep != '\0') + usage("bad alt_rate value"); + break; + + case 'v': + print_info = true; + break; + + default: + usage(NULL); + } + } + argc -= optind; + argv += optind; + + /* open for ioctl only */ + int fd = open(dev, 0); + if (fd < 0) + err(1, "can't open %s", dev); + + /* change alternate PWM rate */ + if (alt_rate > 0) { + ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate); + if (ret != OK) + err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)"); + } + + /* assign alternate rate to channel groups */ + if (alt_channels_set) { + uint32_t mask = 0; + + for (unsigned group = 0; group < 32; group++) { + if ((1 << group) & alt_channel_groups) { + uint32_t group_mask; + + ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask); + if (ret != OK) + err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group); + + mask |= group_mask; + } + } + + ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, mask); + if (ret != OK) + err(1, "PWM_SERVO_SELECT_UPDATE_RATE"); + } + + /* iterate remaining arguments */ + unsigned channel = 0; + while (argc--) { + const char *arg = argv[0]; + argv++; + if (!strcmp(arg, "arm")) { + ret = ioctl(fd, PWM_SERVO_ARM, 0); + if (ret != OK) + err(1, "PWM_SERVO_ARM"); + continue; + } + if (!strcmp(arg, "disarm")) { + ret = ioctl(fd, PWM_SERVO_DISARM, 0); + if (ret != OK) + err(1, "PWM_SERVO_DISARM"); + continue; + } + unsigned pwm_value = strtol(arg, &ep, 0); + if (*ep == '\0') { + ret = ioctl(fd, PWM_SERVO_SET(channel), pwm_value); + if (ret != OK) + err(1, "PWM_SERVO_SET(%d)", channel); + channel++; + continue; + } + usage("unrecognised option"); + } + + /* print verbose info */ + if (print_info) { + /* get the number of servo channels */ + unsigned count; + ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&count); + if (ret != OK) + err(1, "PWM_SERVO_GET_COUNT"); + + /* print current servo values */ + for (unsigned i = 0; i < count; i++) { + servo_position_t spos; + + ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos); + if (ret == OK) { + printf("channel %u: %uus\n", i, spos); + } else { + printf("%u: ERROR\n", i); + } + } + + /* print rate groups */ + for (unsigned i = 0; i < count; i++) { + uint32_t group_mask; + + ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(i), (unsigned long)&group_mask); + if (ret != OK) + break; + if (group_mask != 0) { + printf("channel group %u: channels", i); + for (unsigned j = 0; j < 32; j++) + if (group_mask & (1 << j)) + printf(" %u", j); + printf("\n"); + } + } + fflush(stdout); + } + exit(0); +}
\ No newline at end of file diff --git a/apps/systemlib/airspeed.c b/apps/systemlib/airspeed.c index 5c68f8ea5..264287b10 100644 --- a/apps/systemlib/airspeed.c +++ b/apps/systemlib/airspeed.c @@ -40,14 +40,31 @@ * */ -#include "math.h" +#include <stdio.h> +#include <math.h> #include "conversions.h" #include "airspeed.h" -float calc_indicated_airspeed(float pressure_front, float pressure_ambient, float temperature) +/** + * Calculate indicated airspeed. + * + * Note that the indicated airspeed is not the true airspeed because it + * lacks the air density compensation. Use the calc_true_airspeed functions to get + * the true airspeed. + * + * @param differential_pressure total_ pressure - static pressure + * @return indicated airspeed in m/s + */ +float calc_indicated_airspeed(float differential_pressure) { - return sqrtf((2.0f*(pressure_front - pressure_ambient)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + + if (differential_pressure > 0) { + return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + } else { + return -sqrtf((2.0f*fabs(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + } + } /** @@ -55,14 +72,14 @@ float calc_indicated_airspeed(float pressure_front, float pressure_ambient, floa * * Note that the true airspeed is NOT the groundspeed, because of the effects of wind * - * @param speed current indicated airspeed + * @param speed_indicated current indicated airspeed * @param pressure_ambient pressure at the side of the tube/airplane - * @param temperature air temperature in degrees celcius + * @param temperature_celsius air temperature in degrees celcius * @return true airspeed in m/s */ -float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, float temperature) +float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_ambient, float temperature_celsius) { - return speed * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient, temperature)); + return speed_indicated * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient, temperature_celsius)); } /** @@ -70,12 +87,25 @@ float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, flo * * Note that the true airspeed is NOT the groundspeed, because of the effects of wind * - * @param pressure_front pressure inside the pitot/prandl tube - * @param pressure_ambient pressure at the side of the tube/airplane - * @param temperature air temperature in degrees celcius + * @param total_pressure pressure inside the pitot/prandtl tube + * @param static_pressure pressure at the side of the tube/airplane + * @param temperature_celsius air temperature in degrees celcius * @return true airspeed in m/s */ -float calc_true_airspeed(float pressure_front, float pressure_ambient, float temperature) +float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius) { - return sqrtf((2.0f*(pressure_front - pressure_ambient)) / get_air_density(pressure_ambient, temperature)); + float density = get_air_density(static_pressure, temperature_celsius); + if (density < 0.0001f || !isfinite(density)) { + density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C; + printf("[airspeed] Invalid air density, using density at sea level\n"); + } + + float pressure_difference = total_pressure - static_pressure; + + if(pressure_difference > 0) { + return sqrtf((2.0f*(pressure_difference)) / density); + } else + { + return -sqrtf((2.0f*fabs(pressure_difference)) / density); + } } diff --git a/apps/systemlib/airspeed.h b/apps/systemlib/airspeed.h index b1beb79ae..def53f0c1 100644 --- a/apps/systemlib/airspeed.h +++ b/apps/systemlib/airspeed.h @@ -48,43 +48,42 @@ __BEGIN_DECLS -/** - * Calculate indicated airspeed. - * - * Note that the indicated airspeed is not the true airspeed because it - * lacks the air density compensation. Use the calc_true_airspeed functions to get - * the true airspeed. - * - * @param pressure_front pressure inside the pitot/prandl tube - * @param pressure_ambient pressure at the side of the tube/airplane - * @param temperature air temperature in degrees celcius - * @return indicated airspeed in m/s - */ -__EXPORT float calc_indicated_airspeed(float pressure_front, float pressure_ambient, float temperature); + /** + * Calculate indicated airspeed. + * + * Note that the indicated airspeed is not the true airspeed because it + * lacks the air density compensation. Use the calc_true_airspeed functions to get + * the true airspeed. + * + * @param total_pressure pressure inside the pitot/prandtl tube + * @param static_pressure pressure at the side of the tube/airplane + * @return indicated airspeed in m/s + */ + __EXPORT float calc_indicated_airspeed(float differential_pressure); -/** - * Calculate true airspeed from indicated airspeed. - * - * Note that the true airspeed is NOT the groundspeed, because of the effects of wind - * - * @param speed current indicated airspeed - * @param pressure_ambient pressure at the side of the tube/airplane - * @param temperature air temperature in degrees celcius - * @return true airspeed in m/s - */ -__EXPORT float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, float temperature); + /** + * Calculate true airspeed from indicated airspeed. + * + * Note that the true airspeed is NOT the groundspeed, because of the effects of wind + * + * @param speed_indicated current indicated airspeed + * @param pressure_ambient pressure at the side of the tube/airplane + * @param temperature_celsius air temperature in degrees celcius + * @return true airspeed in m/s + */ + __EXPORT float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_ambient, float temperature_celsius); -/** - * Directly calculate true airspeed - * - * Note that the true airspeed is NOT the groundspeed, because of the effects of wind - * - * @param pressure_front pressure inside the pitot/prandl tube - * @param pressure_ambient pressure at the side of the tube/airplane - * @param temperature air temperature in degrees celcius - * @return true airspeed in m/s - */ -__EXPORT float calc_true_airspeed(float pressure_front, float pressure_ambient, float temperature); + /** + * Directly calculate true airspeed + * + * Note that the true airspeed is NOT the groundspeed, because of the effects of wind + * + * @param total_pressure pressure inside the pitot/prandtl tube + * @param static_pressure pressure at the side of the tube/airplane + * @param temperature_celsius air temperature in degrees celcius + * @return true airspeed in m/s + */ + __EXPORT float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius); __END_DECLS diff --git a/apps/systemlib/conversions.c b/apps/systemlib/conversions.c index 2b8003e45..ac94252c5 100644 --- a/apps/systemlib/conversions.c +++ b/apps/systemlib/conversions.c @@ -150,5 +150,5 @@ void quat2rot(const float Q[4], float R[9]) float get_air_density(float static_pressure, float temperature_celsius) { - return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius + CONSTANTS_ABSOLUTE_NULL_KELVIN)); + return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS)); } diff --git a/apps/systemlib/conversions.h b/apps/systemlib/conversions.h index c2987709b..5d485b01f 100644 --- a/apps/systemlib/conversions.h +++ b/apps/systemlib/conversions.h @@ -44,10 +44,10 @@ #include <float.h> #include <stdint.h> -#define CONSTANTS_ONE_G 9.80665f -#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C 1.225f -#define CONSTANTS_AIR_GAS_CONST 8.31432f -#define CONSTANTS_ABSOLUTE_NULL_KELVIN 273.15f +#define CONSTANTS_ONE_G 9.80665f // m/s^2 +#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C 1.225f // kg/m^3 +#define CONSTANTS_AIR_GAS_CONST 287.1f // J/(kg * K) +#define CONSTANTS_ABSOLUTE_NULL_CELSIUS -273.15f // °C __BEGIN_DECLS diff --git a/apps/systemlib/mixer/mixer.cpp b/apps/systemlib/mixer/mixer.cpp index 72f765d90..df0dfe838 100644 --- a/apps/systemlib/mixer/mixer.cpp +++ b/apps/systemlib/mixer/mixer.cpp @@ -54,6 +54,7 @@ #include "mixer.h" Mixer::Mixer(ControlCallback control_cb, uintptr_t cb_handle) : + _next(nullptr), _control_cb(control_cb), _cb_handle(cb_handle) { diff --git a/apps/systemlib/mixer/mixer.h b/apps/systemlib/mixer/mixer.h index 00ddf1581..71386cba7 100644 --- a/apps/systemlib/mixer/mixer.h +++ b/apps/systemlib/mixer/mixer.h @@ -160,7 +160,7 @@ public: * @param control_cb Callback invoked when reading controls. */ Mixer(ControlCallback control_cb, uintptr_t cb_handle); - ~Mixer() {}; + virtual ~Mixer() {}; /** * Perform the mixing function. diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp index 60eeff225..1dbc512cd 100644 --- a/apps/systemlib/mixer/mixer_group.cpp +++ b/apps/systemlib/mixer/mixer_group.cpp @@ -93,6 +93,7 @@ MixerGroup::reset() mixer = _first; _first = mixer->_next; delete mixer; + mixer = nullptr; } } diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c index ebb72ca3e..8073570d1 100644 --- a/apps/systemlib/param/param.c +++ b/apps/systemlib/param/param.c @@ -87,7 +87,7 @@ struct param_wbuf_s { UT_array *param_values; /** array info for the modified parameters array */ -UT_icd param_icd = {sizeof(struct param_wbuf_s), NULL, NULL, NULL}; +const UT_icd param_icd = {sizeof(struct param_wbuf_s), NULL, NULL, NULL}; /** parameter update topic */ ORB_DEFINE(parameter_update, struct parameter_update_s); diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp index 3f3476f62..136375140 100644 --- a/apps/uORB/objects_common.cpp +++ b/apps/uORB/objects_common.cpp @@ -53,6 +53,9 @@ ORB_DEFINE(sensor_gyro, struct gyro_report); #include <drivers/drv_baro.h> ORB_DEFINE(sensor_baro, struct baro_report); +#include <drivers/drv_range_finder.h> +ORB_DEFINE(sensor_range_finder, struct range_finder_report); + #include <drivers/drv_pwm_output.h> ORB_DEFINE(output_pwm, struct pwm_output_values); diff --git a/apps/uORB/topics/actuator_controls_effective.h b/apps/uORB/topics/actuator_controls_effective.h index 40278c56c..088c4fc8f 100644 --- a/apps/uORB/topics/actuator_controls_effective.h +++ b/apps/uORB/topics/actuator_controls_effective.h @@ -37,8 +37,8 @@ * Actuator control topics - mixer inputs. * * Values published to these topics are the outputs of the vehicle control - * system, and are expected to be mixed and used to drive the actuators - * (servos, speed controls, etc.) that operate the vehicle. + * system and mixing process; they are the control-scale values that are + * then fed to the actual actuator driver. * * Each topic can be published by a single controller */ diff --git a/apps/uORB/topics/differential_pressure.h b/apps/uORB/topics/differential_pressure.h index fd7670cbc..d5e4bf37e 100644 --- a/apps/uORB/topics/differential_pressure.h +++ b/apps/uORB/topics/differential_pressure.h @@ -49,15 +49,16 @@ */ /** - * Battery voltages and status + * Differential pressure and airspeed */ struct differential_pressure_s { - uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ + uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ float static_pressure_mbar; /**< Static / environment pressure */ float differential_pressure_mbar; /**< Differential pressure reading */ float temperature_celcius; /**< ambient temperature in celcius, -1 if unknown */ float indicated_airspeed_m_s; /**< indicated airspeed in meters per second, -1 if unknown */ - float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */ + float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */ + float voltage; /**< Voltage from the airspeed sensor (voltage divider already compensated) */ }; /** @@ -67,4 +68,4 @@ struct differential_pressure_s { /* register this as object request broker structure */ ORB_DECLARE(differential_pressure); -#endif
\ No newline at end of file +#endif diff --git a/apps/uORB/topics/subsystem_info.h b/apps/uORB/topics/subsystem_info.h index c3e039d66..c415e832e 100644 --- a/apps/uORB/topics/subsystem_info.h +++ b/apps/uORB/topics/subsystem_info.h @@ -71,7 +71,8 @@ enum SUBSYSTEM_TYPE SUBSYSTEM_TYPE_YAWPOSITION = 4096, SUBSYSTEM_TYPE_ALTITUDECONTROL = 16384, SUBSYSTEM_TYPE_POSITIONCONTROL = 32768, - SUBSYSTEM_TYPE_MOTORCONTROL = 65536 + SUBSYSTEM_TYPE_MOTORCONTROL = 65536, + SUBSYSTEM_TYPE_RANGEFINDER = 131072 }; /** diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 06b4c5ca5..c7c1048f6 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -156,7 +156,9 @@ struct vehicle_status_s enum VEHICLE_FLIGHT_MODE flight_mode; /**< current flight mode, as defined by mode switch */ enum VEHICLE_MANUAL_CONTROL_MODE manual_control_mode; /**< current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */ enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode; /**< current stabilization mode */ - int32_t system_type; /**< system type, inspired by MAVLinks VEHICLE_TYPE enum */ + int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ + int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ + int32_t component_id; /**< subsystem / component id, inspired by MAVLink's component ID field */ /* system flags - these represent the state predicates */ @@ -171,8 +173,9 @@ struct vehicle_status_s bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ - bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ + bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ bool flag_preflight_accel_calibration; + bool flag_preflight_airspeed_calibration; bool rc_signal_found_once; bool rc_signal_lost; /**< true if RC reception is terminally lost */ @@ -209,6 +212,8 @@ struct vehicle_status_s bool flag_auto_flight_mode_ok; /**< conditions of vector flight mode apply plus a valid takeoff position lock has been aquired */ bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ bool flag_valid_launch_position; /**< indicates a valid launch position */ + bool flag_valid_home_position; /**< indicates a valid home position (a valid home position is not always a valid launch) */ + bool flag_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */ }; /** diff --git a/apps/uORB/uORB.cpp b/apps/uORB/uORB.cpp index 532e54b8e..7abbf42ae 100644 --- a/apps/uORB/uORB.cpp +++ b/apps/uORB/uORB.cpp @@ -429,6 +429,12 @@ ORBDevNode::appears_updated(SubscriberData *sd) /* avoid racing between interrupt and non-interrupt context calls */ irqstate_t state = irqsave(); + /* check if this topic has been published yet, if not bail out */ + if (_data == nullptr) { + ret = false; + goto out; + } + /* * If the subscriber's generation count matches the update generation * count, there has been no update from their perspective; if they @@ -485,6 +491,7 @@ ORBDevNode::appears_updated(SubscriberData *sd) break; } +out: irqrestore(state); /* consider it updated */ diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index d78c95d96..917e8da34 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -241,6 +241,11 @@ include $(PX4_MK_DIR)/nuttx.mk ifneq ($(ROMFS_ROOT),) +# +# Note that there is no support for more than one root directory or constructing +# a root from several templates. That would be a nice feature. +# + # Add dependencies on anything in the ROMFS root ROMFS_DEPS += $(wildcard \ (ROMFS_ROOT)/* \ diff --git a/nuttx/arch/arm/src/stm32/stm32_i2c.c b/nuttx/arch/arm/src/stm32/stm32_i2c.c index 82ae6af5f..05c3f7095 100644 --- a/nuttx/arch/arm/src/stm32/stm32_i2c.c +++ b/nuttx/arch/arm/src/stm32/stm32_i2c.c @@ -185,6 +185,22 @@ enum stm32_trace_e I2CEVENT_ERROR /* Error occurred, param = 0 */ }; +#ifdef CONFIG_I2C_TRACE +static const char *stm32_trace_names[] = { + "NONE ", + "SENDADDR ", + "SENDBYTE ", + "ITBUFEN ", + "RCVBYTE ", + "REITBUFEN ", + "DISITBUFEN", + "BTFNOSTART", + "BTFRESTART", + "BTFSTOP ", + "ERROR " +}; +#endif + /* Trace data */ struct stm32_trace_s @@ -846,6 +862,7 @@ static void stm32_i2c_traceevent(FAR struct stm32_i2c_priv_s *priv, trace->event = event; trace->parm = parm; + trace->time = clock_systimer(); /* Bump up the trace index (unless we are out of trace entries) */ @@ -869,8 +886,8 @@ static void stm32_i2c_tracedump(FAR struct stm32_i2c_priv_s *priv) for (i = 0; i <= priv->tndx; i++) { trace = &priv->trace[i]; - syslog("%2d. STATUS: %08x COUNT: %3d EVENT: %2d PARM: %08x TIME: %d\n", - i+1, trace->status, trace->count, trace->event, trace->parm, + syslog("%2d. STATUS: %08x COUNT: %3d EVENT: %s PARM: %08x TIME: %d\n", + i+1, trace->status, trace->count, stm32_trace_names[trace->event], trace->parm, trace->time - priv->start_time); } } @@ -1620,8 +1637,8 @@ static int stm32_i2c_process(FAR struct i2c_dev_s *dev, FAR struct i2c_msg_s *ms status = stm32_i2c_getstatus(priv); errval = ETIMEDOUT; - i2cdbg("Timed out: CR1: %04x status: %08x\n", - stm32_i2c_getreg(priv, STM32_I2C_CR1_OFFSET), status); + i2cdbg("Timed out: CR1: %04x status: %08x after %d\n", + stm32_i2c_getreg(priv, STM32_I2C_CR1_OFFSET), status, timeout_us); /* "Note: When the STOP, START or PEC bit is set, the software must * not perform any write access to I2C_CR1 before this bit is @@ -2005,7 +2022,7 @@ int up_i2creset(FAR struct i2c_dev_s * dev) /* Give up if we have tried too hard */ - if (clock_count++ > 1000) + if (clock_count++ > CONFIG_STM32_I2CTIMEOTICKS) { goto out; } diff --git a/nuttx/arch/arm/src/stm32/stm32_otgfsdev.c b/nuttx/arch/arm/src/stm32/stm32_otgfsdev.c index bcce3ce60..2c9ae4cac 100644 --- a/nuttx/arch/arm/src/stm32/stm32_otgfsdev.c +++ b/nuttx/arch/arm/src/stm32/stm32_otgfsdev.c @@ -397,7 +397,6 @@ struct stm32_ep_s struct stm32_req_s *tail; uint8_t epphy; /* Physical EP address */ uint8_t eptype:2; /* Endpoint type */ - uint8_t configured:1; /* 1: Endpoint has been configured */ uint8_t active:1; /* 1: A request is being processed */ uint8_t stalled:1; /* 1: Endpoint is stalled */ uint8_t isin:1; /* 1: IN Endpoint */ @@ -679,6 +678,99 @@ static const struct usbdev_ops_s g_devops = .pullup = stm32_pullup, }; +/* Device error strings that may be enabled for more desciptive USB trace + * output. + */ + +#ifdef CONFIG_USBDEV_TRACE_STRINGS +const struct trace_msg_t g_usb_trace_strings_deverror[] = +{ + TRACE_STR(STM32_TRACEERR_ALLOCFAIL ), + TRACE_STR(STM32_TRACEERR_BADCLEARFEATURE ), + TRACE_STR(STM32_TRACEERR_BADDEVGETSTATUS ), + TRACE_STR(STM32_TRACEERR_BADEPNO ), + TRACE_STR(STM32_TRACEERR_BADEPGETSTATUS ), + TRACE_STR(STM32_TRACEERR_BADGETCONFIG ), + TRACE_STR(STM32_TRACEERR_BADGETSETDESC ), + TRACE_STR(STM32_TRACEERR_BADGETSTATUS ), + TRACE_STR(STM32_TRACEERR_BADSETADDRESS ), + TRACE_STR(STM32_TRACEERR_BADSETCONFIG ), + TRACE_STR(STM32_TRACEERR_BADSETFEATURE ), + TRACE_STR(STM32_TRACEERR_BADTESTMODE ), + TRACE_STR(STM32_TRACEERR_BINDFAILED ), + TRACE_STR(STM32_TRACEERR_DISPATCHSTALL ), + TRACE_STR(STM32_TRACEERR_DRIVER ), + TRACE_STR(STM32_TRACEERR_DRIVERREGISTERED), + TRACE_STR(STM32_TRACEERR_EP0NOSETUP ), + TRACE_STR(STM32_TRACEERR_EP0SETUPSTALLED ), + TRACE_STR(STM32_TRACEERR_EPINNULLPACKET ), + TRACE_STR(STM32_TRACEERR_EPOUTNULLPACKET ), + TRACE_STR(STM32_TRACEERR_INVALIDCTRLREQ ), + TRACE_STR(STM32_TRACEERR_INVALIDPARMS ), + TRACE_STR(STM32_TRACEERR_IRQREGISTRATION ), + TRACE_STR(STM32_TRACEERR_NOEP ), + TRACE_STR(STM32_TRACEERR_NOTCONFIGURED ), + TRACE_STR(STM32_TRACEERR_EPOUTQEMPTY ), + TRACE_STR(STM32_TRACEERR_EPINREQEMPTY ), + TRACE_STR(STM32_TRACEERR_NOOUTSETUP ), + TRACE_STR(STM32_TRACEERR_POLLTIMEOUT ), + TRACE_STR_END +}; +#endif + +/* Interrupt event strings that may be enabled for more desciptive USB trace + * output. + */ + +#ifdef CONFIG_USBDEV_TRACE_STRINGS +const struct trace_msg_t g_usb_trace_strings_intdecode[] = +{ + TRACE_STR(STM32_TRACEINTID_USB ), + TRACE_STR(STM32_TRACEINTID_INTPENDING ), + TRACE_STR(STM32_TRACEINTID_EPOUT ), + TRACE_STR(STM32_TRACEINTID_EPIN ), + TRACE_STR(STM32_TRACEINTID_MISMATCH ), + TRACE_STR(STM32_TRACEINTID_WAKEUP ), + TRACE_STR(STM32_TRACEINTID_SUSPEND ), + TRACE_STR(STM32_TRACEINTID_SOF ), + TRACE_STR(STM32_TRACEINTID_RXFIFO ), + TRACE_STR(STM32_TRACEINTID_DEVRESET ), + TRACE_STR(STM32_TRACEINTID_ENUMDNE ), + TRACE_STR(STM32_TRACEINTID_IISOIXFR ), + TRACE_STR(STM32_TRACEINTID_IISOOXFR ), + TRACE_STR(STM32_TRACEINTID_SRQ ), + TRACE_STR(STM32_TRACEINTID_OTG ), + TRACE_STR(STM32_TRACEINTID_EPOUT_XFRC ), + TRACE_STR(STM32_TRACEINTID_EPOUT_EPDISD), + TRACE_STR(STM32_TRACEINTID_EPOUT_SETUP ), + TRACE_STR(STM32_TRACEINTID_DISPATCH ), + TRACE_STR(STM32_TRACEINTID_GETSTATUS ), + TRACE_STR(STM32_TRACEINTID_EPGETSTATUS ), + TRACE_STR(STM32_TRACEINTID_DEVGETSTATUS), + TRACE_STR(STM32_TRACEINTID_IFGETSTATUS ), + TRACE_STR(STM32_TRACEINTID_CLEARFEATURE), + TRACE_STR(STM32_TRACEINTID_SETFEATURE ), + TRACE_STR(STM32_TRACEINTID_SETADDRESS ), + TRACE_STR(STM32_TRACEINTID_GETSETDESC ), + TRACE_STR(STM32_TRACEINTID_GETCONFIG ), + TRACE_STR(STM32_TRACEINTID_SETCONFIG ), + TRACE_STR(STM32_TRACEINTID_GETSETIF ), + TRACE_STR(STM32_TRACEINTID_SYNCHFRAME ), + TRACE_STR(STM32_TRACEINTID_EPIN_XFRC ), + TRACE_STR(STM32_TRACEINTID_EPIN_TOC ), + TRACE_STR(STM32_TRACEINTID_EPIN_ITTXFE ), + TRACE_STR(STM32_TRACEINTID_EPIN_EPDISD ), + TRACE_STR(STM32_TRACEINTID_EPIN_TXFE ), + TRACE_STR(STM32_TRACEINTID_EPIN_EMPWAIT), + TRACE_STR(STM32_TRACEINTID_OUTNAK ), + TRACE_STR(STM32_TRACEINTID_OUTRECVD ), + TRACE_STR(STM32_TRACEINTID_OUTDONE ), + TRACE_STR(STM32_TRACEINTID_SETUPDONE ), + TRACE_STR(STM32_TRACEINTID_SETUPRECVD ), + TRACE_STR_END +}; +#endif + /******************************************************************************* * Public Data *******************************************************************************/ @@ -1142,7 +1234,7 @@ static void stm32_epin_request(FAR struct stm32_usbdev_s *priv, /* Add one more packet to the TxFIFO. We will wait for the transfer * complete event before we add the next packet (or part of a packet * to the TxFIFO). - * + * * The documentation says that we can can multiple packets to the TxFIFO, * but it seems that we need to get the transfer complete event before * we can add the next (or maybe I have got something wrong?) @@ -1796,13 +1888,6 @@ static struct stm32_ep_s *stm32_ep_findbyaddr(struct stm32_usbdev_s *priv, privep = &priv->epout[epphy]; } - /* Verify that the endpoint has been configured */ - - if (!privep->configured) - { - return NULL; - } - /* Return endpoint reference */ DEBUGASSERT(privep->epphy == epphy); @@ -2459,16 +2544,16 @@ static inline void stm32_epout(FAR struct stm32_usbdev_s *priv, uint8_t epno) /* Continue processing data from the EP0 OUT request queue */ stm32_epout_complete(priv, privep); - } - /* If we are not actively processing an OUT request, then we - * need to setup to receive the next control request. - */ + /* If we are not actively processing an OUT request, then we + * need to setup to receive the next control request. + */ - if (!privep->active) - { - stm32_ep0out_ctrlsetup(priv); - priv->ep0state = EP0STATE_IDLE; + if (!privep->active) + { + stm32_ep0out_ctrlsetup(priv); + priv->ep0state = EP0STATE_IDLE; + } } } @@ -2626,16 +2711,16 @@ static inline void stm32_epin(FAR struct stm32_usbdev_s *priv, uint8_t epno) /* Continue processing data from the EP0 OUT request queue */ stm32_epin_request(priv, privep); - } - /* If we are not actively processing an OUT request, then we - * need to setup to receive the next control request. - */ + /* If we are not actively processing an OUT request, then we + * need to setup to receive the next control request. + */ - if (!privep->active) - { - stm32_ep0out_ctrlsetup(priv); - priv->ep0state = EP0STATE_IDLE; + if (!privep->active) + { + stm32_ep0out_ctrlsetup(priv); + priv->ep0state = EP0STATE_IDLE; + } } /* Test mode is another special case */ @@ -2754,7 +2839,7 @@ static inline void stm32_epin_interrupt(FAR struct stm32_usbdev_s *priv) * interrupt here; it will be re-enabled if there is still * insufficient space in the TxFIFO. */ - + empty &= ~OTGFS_DIEPEMPMSK(epno); stm32_putreg(empty, STM32_OTGFS_DIEPEMPMSK); stm32_putreg(OTGFS_DIEPINT_XFRC, STM32_OTGFS_DIEPINT(epno)); @@ -3063,6 +3148,12 @@ static inline void stm32_rxinterrupt(FAR struct stm32_usbdev_s *priv) datlen = GETUINT16(priv->ctrlreq.len); if (USB_REQ_ISOUT(priv->ctrlreq.type) && datlen > 0) { + /* Clear NAKSTS so that we can receive the data */ + + regval = stm32_getreg(STM32_OTGFS_DOEPCTL0); + regval |= OTGFS_DOEPCTL0_CNAK; + stm32_putreg(regval, STM32_OTGFS_DOEPCTL0); + /* Wait for the data phase. */ priv->ep0state = EP0STATE_SETUP_OUT; @@ -3654,7 +3745,7 @@ static int stm32_epout_configure(FAR struct stm32_ep_s *privep, uint8_t eptype, { regval |= OTGFS_DOEPCTL_CNAK; } - + regval &= ~(OTGFS_DOEPCTL_MPSIZ_MASK | OTGFS_DOEPCTL_EPTYP_MASK); regval |= mpsiz; regval |= (eptype << OTGFS_DOEPCTL_EPTYP_SHIFT); @@ -3750,7 +3841,7 @@ static int stm32_epin_configure(FAR struct stm32_ep_s *privep, uint8_t eptype, { regval |= OTGFS_DIEPCTL_CNAK; } - + regval &= ~(OTGFS_DIEPCTL_MPSIZ_MASK | OTGFS_DIEPCTL_EPTYP_MASK | OTGFS_DIEPCTL_TXFNUM_MASK); regval |= mpsiz; regval |= (eptype << OTGFS_DIEPCTL_EPTYP_SHIFT); @@ -4345,19 +4436,6 @@ static int stm32_epin_setstall(FAR struct stm32_ep_s *privep) regaddr = STM32_OTGFS_DIEPCTL(privep->epphy); regval = stm32_getreg(regaddr); - /* Is the endpoint enabled? */ - - if ((regval & OTGFS_DIEPCTL_EPENA) != 0) - { - /* Yes.. the endpoint is enabled, disable it */ - - regval = OTGFS_DIEPCTL_EPDIS; - } - else - { - regval = 0; - } - /* Then stall the endpoint */ regval |= OTGFS_DIEPCTL_STALL; diff --git a/nuttx/arch/arm/src/stm32/stm32_usbdev.c b/nuttx/arch/arm/src/stm32/stm32_usbdev.c index d13ac8f96..6036eb3d5 100644 --- a/nuttx/arch/arm/src/stm32/stm32_usbdev.c +++ b/nuttx/arch/arm/src/stm32/stm32_usbdev.c @@ -1,7 +1,7 @@ /**************************************************************************** * arch/arm/src/stm32/stm32_usbdev.c * - * Copyright (C) 2009-2012 Gregory Nutt. All rights reserved. + * Copyright (C) 2009-2013 Gregory Nutt. All rights reserved. * Author: Gregory Nutt <gnutt@nuttx.orgr> * * References: @@ -59,7 +59,9 @@ #include <arch/irq.h> #include "up_arch.h" -#include "stm32_internal.h" +#include "stm32.h" +#include "stm32_syscfg.h" +#include "stm32_gpio.h" #include "stm32_usbdev.h" #if defined(CONFIG_USBDEV) && defined(CONFIG_STM32_USB) @@ -78,6 +80,20 @@ # define CONFIG_USB_PRI NVIC_SYSH_PRIORITY_DEFAULT #endif +/* USB Interrupts. Should be re-mapped if CAN is used. */ + +#ifdef CONFIG_STM32_STM32F30XX +# ifdef CONFIG_STM32_USB_ITRMP +# define STM32_IRQ_USBHP STM32_IRQ_USBHP_2 +# define STM32_IRQ_USBLP STM32_IRQ_USBLP_2 +# define STM32_IRQ_USBWKUP STM32_IRQ_USBWKUP_2 +# else +# define STM32_IRQ_USBHP STM32_IRQ_USBHP_1 +# define STM32_IRQ_USBLP STM32_IRQ_USBLP_1 +# define STM32_IRQ_USBWKUP STM32_IRQ_USBWKUP_1 +# endif +#endif + /* Extremely detailed register debug that you would normally never want * enabled. */ @@ -250,12 +266,12 @@ /* The various states of a control pipe */ -enum stm32_devstate_e +enum stm32_ep0state_e { - DEVSTATE_IDLE = 0, /* No request in progress */ - DEVSTATE_RDREQUEST, /* Read request in progress */ - DEVSTATE_WRREQUEST, /* Write request in progress */ - DEVSTATE_STALLED /* We are stalled */ + EP0STATE_IDLE = 0, /* No request in progress */ + EP0STATE_RDREQUEST, /* Read request in progress */ + EP0STATE_WRREQUEST, /* Write request in progress */ + EP0STATE_STALLED /* We are stalled */ }; /* Resume states */ @@ -320,7 +336,7 @@ struct stm32_usbdev_s /* STM32-specific fields */ struct usb_ctrlreq_s ctrl; /* Last EP0 request */ - uint8_t devstate; /* Driver state (see enum stm32_devstate_e) */ + uint8_t ep0state; /* State of EP0 (see enum stm32_ep0state_e) */ uint8_t rsmstate; /* Resume state (see enum stm32_rsmstate_e) */ uint8_t nesofs; /* ESOF counter (for resume support) */ uint8_t rxpending:1; /* 1: OUT data in PMA, but no read requests */ @@ -1014,7 +1030,7 @@ static void stm32_copytopma(const uint8_t *buffer, uint16_t pma, uint16_t nbytes /* Copy loop. Source=user buffer, Dest=packet memory */ - dest = (uint16_t*)(STM32_USBCANRAM_BASE + ((uint32_t)pma << 1)); + dest = (uint16_t*)(STM32_USBRAM_BASE + ((uint32_t)pma << 1)); for (i = nwords; i != 0; i--) { /* Read two bytes and pack into on 16-bit word */ @@ -1044,7 +1060,7 @@ stm32_copyfrompma(uint8_t *buffer, uint16_t pma, uint16_t nbytes) /* Copy loop. Source=packet memory, Dest=user buffer */ - src = (uint32_t*)(STM32_USBCANRAM_BASE + ((uint32_t)pma << 1)); + src = (uint32_t*)(STM32_USBRAM_BASE + ((uint32_t)pma << 1)); for (i = nwords; i != 0; i--) { /* Copy 16-bits from packet memory to user buffer. */ @@ -1142,7 +1158,7 @@ static void stm32_reqcomplete(struct stm32_ep_s *privep, int16_t result) bool stalled = privep->stalled; if (USB_EPNO(privep->ep.eplog) == EP0) { - privep->stalled = (privep->dev->devstate == DEVSTATE_STALLED); + privep->stalled = (privep->dev->ep0state == EP0STATE_STALLED); } /* Save the result in the request structure */ @@ -1222,8 +1238,7 @@ static int stm32_wrrequest(struct stm32_usbdev_s *priv, struct stm32_ep_s *prive */ usbtrace(TRACE_INTDECODE(STM32_TRACEINTID_EPINQEMPTY), 0); - priv->devstate = DEVSTATE_IDLE; - return OK; + return -ENOENT; } epno = USB_EPNO(privep->ep.eplog); @@ -1267,7 +1282,6 @@ static int stm32_wrrequest(struct stm32_usbdev_s *priv, struct stm32_ep_s *prive buf = privreq->req.buf + privreq->req.xfrd; stm32_epwrite(priv, privep, buf, nbytes); - priv->devstate = DEVSTATE_WRREQUEST; /* Update for the next data IN interrupt */ @@ -1275,15 +1289,16 @@ static int stm32_wrrequest(struct stm32_usbdev_s *priv, struct stm32_ep_s *prive bytesleft = privreq->req.len - privreq->req.xfrd; /* If all of the bytes were sent (including any final null packet) - * then we are finished with the transfer + * then we are finished with the request buffer). */ if (bytesleft == 0 && !privep->txnullpkt) { + /* Return the write request to the class driver */ + usbtrace(TRACE_COMPLETE(USB_EPNO(privep->ep.eplog)), privreq->req.xfrd); privep->txnullpkt = 0; stm32_reqcomplete(privep, OK); - priv->devstate = DEVSTATE_IDLE; } return OK; @@ -1314,7 +1329,7 @@ static int stm32_rdrequest(struct stm32_usbdev_s *priv, struct stm32_ep_s *prive */ usbtrace(TRACE_INTDECODE(STM32_TRACEINTID_EPOUTQEMPTY), epno); - return OK; + return -ENOENT; } ullvdbg("EP%d: len=%d xfrd=%d\n", epno, privreq->req.len, privreq->req.xfrd); @@ -1343,22 +1358,18 @@ static int stm32_rdrequest(struct stm32_usbdev_s *priv, struct stm32_ep_s *prive /* Receive the next packet */ stm32_copyfrompma(dest, src, readlen); - priv->devstate = DEVSTATE_RDREQUEST; /* If the receive buffer is full or this is a partial packet, - * then we are finished with the transfer + * then we are finished with the request buffer). */ privreq->req.xfrd += readlen; if (pmalen < privep->ep.maxpacket || privreq->req.xfrd >= privreq->req.len) { - /* Complete the transfer and mark the state IDLE. The endpoint - * RX will be marked valid when the data phase completes. - */ + /* Return the read request to the class driver. */ usbtrace(TRACE_COMPLETE(epno), privreq->req.xfrd); stm32_reqcomplete(privep, OK); - priv->devstate = DEVSTATE_IDLE; } return OK; @@ -1400,7 +1411,7 @@ static void stm32_dispatchrequest(struct stm32_usbdev_s *priv) /* Stall on failure */ usbtrace(TRACE_DEVERROR(STM32_TRACEERR_DISPATCHSTALL), 0); - priv->devstate = DEVSTATE_STALLED; + priv->ep0state = EP0STATE_STALLED; } } } @@ -1436,7 +1447,7 @@ static void stm32_epdone(struct stm32_usbdev_s *priv, uint8_t epno) { /* Read host data into the current read request */ - stm32_rdrequest(priv, privep); + (void)stm32_rdrequest(priv, privep); /* "After the received data is processed, the application software * should set the STAT_RX bits to '11' (Valid) in the USB_EPnR, @@ -1567,7 +1578,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv) ullvdbg("SETUP: type=%02x req=%02x value=%04x index=%04x len=%04x\n", priv->ctrl.type, priv->ctrl.req, value.w, index.w, len.w); - priv->devstate = DEVSTATE_IDLE; + priv->ep0state = EP0STATE_IDLE; /* Dispatch any non-standard requests */ @@ -1600,7 +1611,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv) index.b[MSB] != 0 || value.w != 0) { usbtrace(TRACE_DEVERROR(STM32_TRACEERR_BADEPGETSTATUS), 0); - priv->devstate = DEVSTATE_STALLED; + priv->ep0state = EP0STATE_STALLED; } else { @@ -1613,7 +1624,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv) if (epno >= STM32_NENDPOINTS) { usbtrace(TRACE_DEVERROR(STM32_TRACEERR_BADEPGETSTATUS), epno); - priv->devstate = DEVSTATE_STALLED; + priv->ep0state = EP0STATE_STALLED; } else { @@ -1663,7 +1674,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv) else { usbtrace(TRACE_DEVERROR(STM32_TRACEERR_BADDEVGETSTATUS), 0); - priv->devstate = DEVSTATE_STALLED; + priv->ep0state = EP0STATE_STALLED; } } break; @@ -1679,7 +1690,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv) default: { usbtrace(TRACE_DEVERROR(STM32_TRACEERR_BADGETSTATUS), 0); - priv->devstate = DEVSTATE_STALLED; + priv->ep0state = EP0STATE_STALLED; } break; } @@ -1720,7 +1731,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv) else { usbtrace(TRACE_DEVERROR(STM32_TRACEERR_BADCLEARFEATURE), 0); - priv->devstate = DEVSTATE_STALLED; + priv->ep0state = EP0STATE_STALLED; } } } @@ -1764,7 +1775,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv) else { usbtrace(TRACE_DEVERROR(STM32_TRACEERR_BADSETFEATURE), 0); - priv->devstate = DEVSTATE_STALLED; + priv->ep0state = EP0STATE_STALLED; } } } @@ -1783,7 +1794,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv) index.w != 0 || len.w != 0 || value.w > 127) { usbtrace(TRACE_DEVERROR(STM32_TRACEERR_BADSETADDRESS), 0); - priv->devstate = DEVSTATE_STALLED; + priv->ep0state = EP0STATE_STALLED; } /* Note that setting of the device address will be deferred. A zero-length @@ -1818,7 +1829,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv) else { usbtrace(TRACE_DEVERROR(STM32_TRACEERR_BADGETSETDESC), 0); - priv->devstate = DEVSTATE_STALLED; + priv->ep0state = EP0STATE_STALLED; } } break; @@ -1843,7 +1854,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv) else { usbtrace(TRACE_DEVERROR(STM32_TRACEERR_BADGETCONFIG), 0); - priv->devstate = DEVSTATE_STALLED; + priv->ep0state = EP0STATE_STALLED; } } break; @@ -1868,7 +1879,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv) else { usbtrace(TRACE_DEVERROR(STM32_TRACEERR_BADSETCONFIG), 0); - priv->devstate = DEVSTATE_STALLED; + priv->ep0state = EP0STATE_STALLED; } } break; @@ -1910,7 +1921,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv) default: { usbtrace(TRACE_DEVERROR(STM32_TRACEERR_INVALIDCTRLREQ), priv->ctrl.req); - priv->devstate = DEVSTATE_STALLED; + priv->ep0state = EP0STATE_STALLED; } break; } @@ -1922,9 +1933,9 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv) * must be sent (may be a zero length packet). * 2. The request was successfully handled by the class implementation. In * case, the EP0 IN response has already been queued and the local variable - * 'handled' will be set to true and devstate != DEVSTATE_STALLED; + * 'handled' will be set to true and ep0state != EP0STATE_STALLED; * 3. An error was detected in either the above logic or by the class implementation - * logic. In either case, priv->state will be set DEVSTATE_STALLED + * logic. In either case, priv->state will be set EP0STATE_STALLED * to indicate this case. * * NOTE: Non-standard requests are a special case. They are handled by the @@ -1932,7 +1943,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv) * logic altogether. */ - if (priv->devstate != DEVSTATE_STALLED && !handled) + if (priv->ep0state != EP0STATE_STALLED && !handled) { /* We will response. First, restrict the data length to the length * requested in the setup packet @@ -1946,7 +1957,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv) /* Send the response (might be a zero-length packet) */ stm32_epwrite(priv, ep0, response.b, nbytes); - priv->devstate = DEVSTATE_IDLE; + priv->ep0state = EP0STATE_IDLE; } } @@ -1956,6 +1967,8 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv) static void stm32_ep0in(struct stm32_usbdev_s *priv) { + int ret; + /* There is no longer anything in the EP0 TX packet memory */ priv->eplist[EP0].txbusy = false; @@ -1964,14 +1977,15 @@ static void stm32_ep0in(struct stm32_usbdev_s *priv) * from the class driver? */ - if (priv->devstate == DEVSTATE_WRREQUEST) + if (priv->ep0state == EP0STATE_WRREQUEST) { - stm32_wrrequest(priv, &priv->eplist[EP0]); + ret = stm32_wrrequest(priv, &priv->eplist[EP0]); + priv->ep0state = ((ret == OK) ? EP0STATE_WRREQUEST : EP0STATE_IDLE); } /* No.. Are we processing the completion of a status response? */ - else if (priv->devstate == DEVSTATE_IDLE) + else if (priv->ep0state == EP0STATE_IDLE) { /* Look at the saved SETUP command. Was it a SET ADDRESS request? * If so, then now is the time to set the address. @@ -1987,7 +2001,7 @@ static void stm32_ep0in(struct stm32_usbdev_s *priv) } else { - priv->devstate = DEVSTATE_STALLED; + priv->ep0state = EP0STATE_STALLED; } } @@ -1997,12 +2011,15 @@ static void stm32_ep0in(struct stm32_usbdev_s *priv) static void stm32_ep0out(struct stm32_usbdev_s *priv) { + int ret; + struct stm32_ep_s *privep = &priv->eplist[EP0]; - switch (priv->devstate) + switch (priv->ep0state) { - case DEVSTATE_RDREQUEST: /* Write request in progress */ - case DEVSTATE_IDLE: /* No transfer in progress */ - stm32_rdrequest(priv, privep); + case EP0STATE_RDREQUEST: /* Write request in progress */ + case EP0STATE_IDLE: /* No transfer in progress */ + ret = stm32_rdrequest(priv, privep); + priv->ep0state = ((ret == OK) ? EP0STATE_RDREQUEST : EP0STATE_IDLE); break; default: @@ -2010,7 +2027,7 @@ static void stm32_ep0out(struct stm32_usbdev_s *priv) * completed, STALL the endpoint in either case */ - priv->devstate = DEVSTATE_STALLED; + priv->ep0state = EP0STATE_STALLED; break; } } @@ -2103,7 +2120,7 @@ static inline void stm32_ep0done(struct stm32_usbdev_s *priv, uint16_t istr) /* Now figure out the new RX/TX status. Here are all possible * consequences of the above EP0 operations: * - * rxstatus txstatus devstate MEANING + * rxstatus txstatus ep0state MEANING * -------- -------- --------- --------------------------------- * NAK NAK IDLE Nothing happened * NAK VALID IDLE EP0 response sent from USBDEV driver @@ -2113,9 +2130,9 @@ static inline void stm32_ep0done(struct stm32_usbdev_s *priv, uint16_t istr) * First handle the STALL condition: */ - if (priv->devstate == DEVSTATE_STALLED) + if (priv->ep0state == EP0STATE_STALLED) { - usbtrace(TRACE_DEVERROR(STM32_TRACEERR_EP0SETUPSTALLED), priv->devstate); + usbtrace(TRACE_DEVERROR(STM32_TRACEERR_EP0SETUPSTALLED), priv->ep0state); priv->rxstatus = USB_EPR_STATRX_STALL; priv->txstatus = USB_EPR_STATTX_STALL; } @@ -2843,7 +2860,7 @@ static int stm32_epsubmit(struct usbdev_ep_s *ep, struct usbdev_req_s *req) if (!privep->txbusy) { priv->txstatus = USB_EPR_STATTX_NAK; - ret = stm32_wrrequest(priv, privep); + ret = stm32_wrrequest(priv, privep); /* Set the new TX status */ @@ -2955,7 +2972,12 @@ static int stm32_epstall(struct usbdev_ep_s *ep, bool resume) if (status == 0) { usbtrace(TRACE_DEVERROR(STM32_TRACEERR_EPDISABLED), 0); - priv->devstate = DEVSTATE_STALLED; + + if (epno == 0) + { + priv->ep0state = EP0STATE_STALLED; + } + return -ENODEV; } @@ -3256,7 +3278,7 @@ static void stm32_reset(struct stm32_usbdev_s *priv) /* Reset the device state structure */ - priv->devstate = DEVSTATE_IDLE; + priv->ep0state = EP0STATE_IDLE; priv->rsmstate = RSMSTATE_IDLE; priv->rxpending = false; @@ -3466,27 +3488,48 @@ void up_usbinitialize(void) usbtrace(TRACE_DEVINIT, 0); stm32_checksetup(); + /* Configure USB GPIO alternate function pins */ + +#ifdef CONFIG_STM32_STM32F30XX + (void)stm32_configgpio(GPIO_USB_DM); + (void)stm32_configgpio(GPIO_USB_DP); +#endif + /* Power up the USB controller, but leave it in the reset state */ stm32_hwsetup(priv); + /* Remap the USB interrupt as needed (Only supported by the STM32 F3 family) */ + +#ifdef CONFIG_STM32_STM32F30XX +# ifdef CONFIG_STM32_USB_ITRMP + /* Clear the ITRMP bit to use the legacy, shared USB/CAN interrupts */ + + modifyreg32(STM32_RCC_APB1ENR, SYSCFG_CFGR1_USB_ITRMP, 0); +# else + /* Set the ITRMP bit to use the STM32 F3's dedicated USB interrupts */ + + modifyreg32(STM32_RCC_APB1ENR, 0, SYSCFG_CFGR1_USB_ITRMP); +# endif +#endif + /* Attach USB controller interrupt handlers. The hardware will not be * initialized and interrupts will not be enabled until the class device * driver is bound. Getting the IRQs here only makes sure that we have * them when we need them later. */ - if (irq_attach(STM32_IRQ_USBHPCANTX, stm32_hpinterrupt) != 0) + if (irq_attach(STM32_IRQ_USBHP, stm32_hpinterrupt) != 0) { usbtrace(TRACE_DEVERROR(STM32_TRACEERR_IRQREGISTRATION), - (uint16_t)STM32_IRQ_USBHPCANTX); + (uint16_t)STM32_IRQ_USBHP); goto errout; } - if (irq_attach(STM32_IRQ_USBLPCANRX0, stm32_lpinterrupt) != 0) + if (irq_attach(STM32_IRQ_USBLP, stm32_lpinterrupt) != 0) { usbtrace(TRACE_DEVERROR(STM32_TRACEERR_IRQREGISTRATION), - (uint16_t)STM32_IRQ_USBLPCANRX0); + (uint16_t)STM32_IRQ_USBLP); goto errout; } return; @@ -3522,10 +3565,10 @@ void up_usbuninitialize(void) /* Disable and detach the USB IRQs */ - up_disable_irq(STM32_IRQ_USBHPCANTX); - up_disable_irq(STM32_IRQ_USBLPCANRX0); - irq_detach(STM32_IRQ_USBHPCANTX); - irq_detach(STM32_IRQ_USBLPCANRX0); + up_disable_irq(STM32_IRQ_USBHP); + up_disable_irq(STM32_IRQ_USBLP); + irq_detach(STM32_IRQ_USBHP); + irq_detach(STM32_IRQ_USBLP); if (priv->driver) { @@ -3595,13 +3638,13 @@ int usbdev_register(struct usbdevclass_driver_s *driver) /* Enable USB controller interrupts at the NVIC */ - up_enable_irq(STM32_IRQ_USBHPCANTX); - up_enable_irq(STM32_IRQ_USBLPCANRX0); + up_enable_irq(STM32_IRQ_USBHP); + up_enable_irq(STM32_IRQ_USBLP); /* Set the interrrupt priority */ - up_prioritize_irq(STM32_IRQ_USBHPCANTX, CONFIG_USB_PRI); - up_prioritize_irq(STM32_IRQ_USBLPCANRX0, CONFIG_USB_PRI); + up_prioritize_irq(STM32_IRQ_USBHP, CONFIG_USB_PRI); + up_prioritize_irq(STM32_IRQ_USBLP, CONFIG_USB_PRI); /* Enable pull-up to connect the device. The host should enumerate us * some time after this @@ -3657,8 +3700,8 @@ int usbdev_unregister(struct usbdevclass_driver_s *driver) /* Disable USB controller interrupts (but keep them attached) */ - up_disable_irq(STM32_IRQ_USBHPCANTX); - up_disable_irq(STM32_IRQ_USBLPCANRX0); + up_disable_irq(STM32_IRQ_USBHP); + up_disable_irq(STM32_IRQ_USBLP); /* Put the hardware in an inactive state. Then bring the hardware back up * in the reset state (this is probably not necessary, the stm32_reset() diff --git a/nuttx/configs/px4fmu/include/board.h b/nuttx/configs/px4fmu/include/board.h index 3f0f26ba1..8ad56a4c6 100755 --- a/nuttx/configs/px4fmu/include/board.h +++ b/nuttx/configs/px4fmu/include/board.h @@ -299,7 +299,7 @@ #define PX4_I2C_OBDEV_EEPROM NOTDEFINED #define PX4_I2C_OBDEV_PX4IO_BL 0x18 -#define PX4_I2C_OBDEV_PX4IO 0x19 +#define PX4_I2C_OBDEV_PX4IO 0x1a /* * SPI diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 5518548a6..80cf6f312 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -46,14 +46,18 @@ CONFIGURED_APPS += systemlib CONFIGURED_APPS += systemlib/mixer # Math library +ifneq ($(CONFIG_APM),y) CONFIGURED_APPS += mathlib CONFIGURED_APPS += mathlib/CMSIS CONFIGURED_APPS += examples/math_demo +endif # Control library +ifneq ($(CONFIG_APM),y) CONFIGURED_APPS += controllib CONFIGURED_APPS += examples/control_demo CONFIGURED_APPS += examples/kalman_demo +endif # System utility commands CONFIGURED_APPS += systemcmds/reboot @@ -63,6 +67,7 @@ CONFIGURED_APPS += systemcmds/boardinfo CONFIGURED_APPS += systemcmds/mixer CONFIGURED_APPS += systemcmds/eeprom CONFIGURED_APPS += systemcmds/param +CONFIGURED_APPS += systemcmds/pwm CONFIGURED_APPS += systemcmds/bl_update CONFIGURED_APPS += systemcmds/preflight_check CONFIGURED_APPS += systemcmds/delay_test @@ -77,7 +82,7 @@ CONFIGURED_APPS += systemcmds/delay_test # Tutorial code from # https://pixhawk.ethz.ch/px4/dev/debug_values -CONFIGURED_APPS += examples/px4_mavlink_debug +# CONFIGURED_APPS += examples/px4_mavlink_debug # Shared object broker; required by many parts of the system. CONFIGURED_APPS += uORB @@ -87,6 +92,8 @@ CONFIGURED_APPS += mavlink_onboard CONFIGURED_APPS += commander CONFIGURED_APPS += sdlog CONFIGURED_APPS += sensors + +ifneq ($(CONFIG_APM),y) CONFIGURED_APPS += ardrone_interface CONFIGURED_APPS += multirotor_att_control CONFIGURED_APPS += multirotor_pos_control @@ -96,10 +103,11 @@ CONFIGURED_APPS += fixedwing_pos_control CONFIGURED_APPS += position_estimator CONFIGURED_APPS += attitude_estimator_ekf CONFIGURED_APPS += hott_telemetry +endif # Hacking tools #CONFIGURED_APPS += system/i2c -#CONFIGURED_APPS += tools/i2c_dev +CONFIGURED_APPS += systemcmds/i2c # Communication and Drivers CONFIGURED_APPS += drivers/boards/px4fmu @@ -118,6 +126,7 @@ CONFIGURED_APPS += drivers/stm32/adc CONFIGURED_APPS += drivers/px4fmu CONFIGURED_APPS += drivers/hil CONFIGURED_APPS += drivers/gps +CONFIGURED_APPS += drivers/mb12xx # Testing stuff CONFIGURED_APPS += px4/sensors_bringup diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig index 5a54e350c..130886aac 100755 --- a/nuttx/configs/px4fmu/nsh/defconfig +++ b/nuttx/configs/px4fmu/nsh/defconfig @@ -369,11 +369,12 @@ CONFIG_I2C_WRITEREAD=y # I2C configuration # CONFIG_I2C=y -CONFIG_I2C_POLLED=y +CONFIG_I2C_POLLED=n CONFIG_I2C_TRANSFER=y CONFIG_I2C_TRACE=n CONFIG_I2C_RESET=y - +# XXX fixed per-transaction timeout +CONFIG_STM32_I2CTIMEOMS=10 # XXX re-enable after integration testing @@ -778,6 +779,7 @@ CONFIG_NXFFS_MAXNAMLEN=32 CONFIG_NXFFS_TAILTHRESHOLD=2048 CONFIG_NXFFS_PREALLOCATED=y CONFIG_FS_ROMFS=y +CONFIG_FS_BINFS=y # # SPI-based MMC/SD driver diff --git a/nuttx/configs/px4io/io/defconfig b/nuttx/configs/px4io/io/defconfig index 1185813db..bb937cf4e 100755 --- a/nuttx/configs/px4io/io/defconfig +++ b/nuttx/configs/px4io/io/defconfig @@ -86,7 +86,7 @@ CONFIG_ARCH_BOOTLOADER=n CONFIG_ARCH_LEDS=n CONFIG_ARCH_BUTTONS=n CONFIG_ARCH_CALIBRATION=n -CONFIG_ARCH_DMA=n +CONFIG_ARCH_DMA=y CONFIG_ARCH_MATH_H=y CONFIG_ARMV7M_CMNVECTOR=y @@ -112,7 +112,7 @@ CONFIG_STM32_JTAG_SW_ENABLE=n # Individual subsystems can be enabled: # # AHB: -CONFIG_STM32_DMA1=n +CONFIG_STM32_DMA1=y CONFIG_STM32_DMA2=n CONFIG_STM32_CRC=n # APB1: @@ -189,6 +189,12 @@ CONFIG_USART1_2STOP=0 CONFIG_USART2_2STOP=0 CONFIG_USART3_2STOP=0 +CONFIG_USART1_RXDMA=y +SERIAL_HAVE_CONSOLE_DMA=y +# Conflicts with I2C1 DMA +CONFIG_USART2_RXDMA=n +CONFIG_USART3_RXDMA=y + # # PX4IO specific driver settings # @@ -399,7 +405,7 @@ CONFIG_DISABLE_SIGNALS=y CONFIG_DISABLE_MQUEUE=y CONFIG_DISABLE_MOUNTPOINT=y CONFIG_DISABLE_ENVIRON=y -CONFIG_DISABLE_POLL=n +CONFIG_DISABLE_POLL=y # # Misc libc settings @@ -469,12 +475,12 @@ CONFIG_ARCH_BZERO=n # timer structures to minimize dynamic allocations. Set to # zero for all dynamic allocations. # -CONFIG_MAX_TASKS=8 +CONFIG_MAX_TASKS=4 CONFIG_MAX_TASK_ARGS=4 -CONFIG_NPTHREAD_KEYS=4 +CONFIG_NPTHREAD_KEYS=2 CONFIG_NFILE_DESCRIPTORS=8 CONFIG_NFILE_STREAMS=0 -CONFIG_NAME_MAX=32 +CONFIG_NAME_MAX=12 CONFIG_STDIO_BUFFER_SIZE=32 CONFIG_STDIO_LINEBUFFER=n CONFIG_NUNGET_CHARS=2 @@ -535,7 +541,7 @@ CONFIG_BOOT_COPYTORAM=n CONFIG_CUSTOM_STACK=n CONFIG_STACK_POINTER= CONFIG_IDLETHREAD_STACKSIZE=1024 -CONFIG_USERMAIN_STACKSIZE=1024 +CONFIG_USERMAIN_STACKSIZE=1200 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_PTHREAD_STACK_DEFAULT=1024 CONFIG_HEAP_BASE= diff --git a/nuttx/libc/stdio/lib_sscanf.c b/nuttx/libc/stdio/lib_sscanf.c index 77a6cf212..0092fbec2 100644 --- a/nuttx/libc/stdio/lib_sscanf.c +++ b/nuttx/libc/stdio/lib_sscanf.c @@ -197,7 +197,7 @@ int vsscanf(FAR char *buf, FAR const char *fmt, va_list ap) noassign = false; lflag = false; - while (*fmt && *buf) + while (*fmt) { /* Skip over white space */ @@ -242,6 +242,33 @@ int vsscanf(FAR char *buf, FAR const char *fmt, va_list ap) fmt--; } } + + /* Process %n: Character count */ + + if (*fmt == 'n') + { + lvdbg("vsscanf: Performing character count\n"); + + if (!noassign) + { + size_t nchars = (size_t)(buf - bufstart); + + if (lflag) + { + long *plong = va_arg(ap, long*); + *plong = (long)nchars; + } + else + { + int *pint = va_arg(ap, int*); + *pint = (int)nchars; + } + } + } else { + + /* Check for valid data in input string */ + if (!(*buf)) + break; /* Process %s: String conversion */ @@ -445,28 +472,7 @@ int vsscanf(FAR char *buf, FAR const char *fmt, va_list ap) #endif } - /* Process %n: Character count */ - - else if (*fmt == 'n') - { - lvdbg("vsscanf: Performing character count\n"); - - if (!noassign) - { - size_t nchars = (size_t)(buf - bufstart); - - if (lflag) - { - long *plong = va_arg(ap, long*); - *plong = (long)nchars; - } - else - { - int *pint = va_arg(ap, int*); - *pint = (int)nchars; - } - } - } + } /* Note %n does not count as a conversion */ |