From d8a3454538e2a634513defd50b40fb49c194b670 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Apr 2013 00:10:20 +0200 Subject: Cut over MAVLink to new build system --- src/modules/mavlink_onboard/mavlink.c | 529 +++++++++++++++++++++ .../mavlink_onboard/mavlink_bridge_header.h | 81 ++++ src/modules/mavlink_onboard/mavlink_receiver.c | 331 +++++++++++++ src/modules/mavlink_onboard/module.mk | 42 ++ src/modules/mavlink_onboard/orb_topics.h | 100 ++++ src/modules/mavlink_onboard/util.h | 54 +++ 6 files changed, 1137 insertions(+) create mode 100644 src/modules/mavlink_onboard/mavlink.c create mode 100644 src/modules/mavlink_onboard/mavlink_bridge_header.h create mode 100644 src/modules/mavlink_onboard/mavlink_receiver.c create mode 100644 src/modules/mavlink_onboard/module.mk create mode 100644 src/modules/mavlink_onboard/orb_topics.h create mode 100644 src/modules/mavlink_onboard/util.h (limited to 'src/modules/mavlink_onboard') diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c new file mode 100644 index 000000000..5a2685560 --- /dev/null +++ b/src/modules/mavlink_onboard/mavlink.c @@ -0,0 +1,529 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * 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 mavlink.c + * MAVLink 1.0 protocol implementation. + * + * @author Lorenz Meier + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "mavlink_bridge_header.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "orb_topics.h" +#include "util.h" + +__EXPORT int mavlink_onboard_main(int argc, char *argv[]); + +static int mavlink_thread_main(int argc, char *argv[]); + +/* thread state */ +volatile bool thread_should_exit = false; +static volatile bool thread_running = false; +static int mavlink_task; + +/* pthreads */ +static pthread_t receive_thread; + +/* terminate MAVLink on user request - disabled by default */ +static bool mavlink_link_termination_allowed = false; + +mavlink_system_t mavlink_system = { + 100, + 50, + MAV_TYPE_QUADROTOR, + 0, + 0, + 0 +}; // System ID, 1-255, Component/Subsystem ID, 1-255 + +/* XXX not widely used */ +uint8_t chan = MAVLINK_COMM_0; + +/* XXX probably should be in a header... */ +extern pthread_t receive_start(int uart); + +bool mavlink_hil_enabled = false; + +/* protocol interface */ +static int uart; +static int baudrate; +bool gcs_link = true; + +/* interface mode */ +static enum { + MAVLINK_INTERFACE_MODE_OFFBOARD, + MAVLINK_INTERFACE_MODE_ONBOARD +} mavlink_link_mode = MAVLINK_INTERFACE_MODE_OFFBOARD; + +static void mavlink_update_system(void); +static int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); +static void usage(void); + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) +{ + /* process baud rate */ + int speed; + + switch (baud) { + case 0: speed = B0; break; + + case 50: speed = B50; break; + + case 75: speed = B75; break; + + case 110: speed = B110; break; + + case 134: speed = B134; break; + + case 150: speed = B150; break; + + case 200: speed = B200; break; + + case 300: speed = B300; break; + + case 600: speed = B600; break; + + case 1200: speed = B1200; break; + + case 1800: speed = B1800; break; + + case 2400: speed = B2400; break; + + case 4800: speed = B4800; break; + + case 9600: speed = B9600; break; + + case 19200: speed = B19200; break; + + case 38400: speed = B38400; break; + + case 57600: speed = B57600; break; + + case 115200: speed = B115200; break; + + case 230400: speed = B230400; break; + + case 460800: speed = B460800; break; + + case 921600: speed = B921600; break; + + default: + fprintf(stderr, "[mavlink] ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud); + return -EINVAL; + } + + /* open uart */ + printf("[mavlink] UART is %s, baudrate is %d\n", uart_name, baud); + uart = open(uart_name, O_RDWR | O_NOCTTY); + + /* Try to set baud rate */ + struct termios uart_config; + int termios_state; + *is_usb = false; + + if (strcmp(uart_name, "/dev/ttyACM0") != OK) { + /* Back up the original uart configuration to restore it after exit */ + if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { + fprintf(stderr, "[mavlink] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state); + close(uart); + return -1; + } + + /* Fill the struct for the new configuration */ + tcgetattr(uart, &uart_config); + + /* Clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; + + /* Set baud rate */ + if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { + fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); + close(uart); + return -1; + } + + + if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { + fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); + close(uart); + return -1; + } + + } else { + *is_usb = true; + } + + return uart; +} + +void +mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length) +{ + write(uart, ch, (size_t)(sizeof(uint8_t) * length)); +} + +/* + * Internal function to give access to the channel status for each channel + */ +mavlink_status_t* mavlink_get_channel_status(uint8_t channel) +{ + static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; + return &m_mavlink_status[channel]; +} + +/* + * Internal function to give access to the channel buffer for each channel + */ +mavlink_message_t* mavlink_get_channel_buffer(uint8_t channel) +{ + static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS]; + return &m_mavlink_buffer[channel]; +} + +void mavlink_update_system(void) +{ + static bool initialized = false; + param_t param_system_id; + param_t param_component_id; + param_t param_system_type; + + if (!initialized) { + param_system_id = param_find("MAV_SYS_ID"); + param_component_id = param_find("MAV_COMP_ID"); + param_system_type = param_find("MAV_TYPE"); + } + + /* update system and component id */ + int32_t system_id; + param_get(param_system_id, &system_id); + if (system_id > 0 && system_id < 255) { + mavlink_system.sysid = system_id; + } + + int32_t component_id; + param_get(param_component_id, &component_id); + if (component_id > 0 && component_id < 255) { + mavlink_system.compid = component_id; + } + + int32_t system_type; + param_get(param_system_type, &system_type); + if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) { + mavlink_system.type = system_type; + } +} + +void +get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed, + uint8_t *mavlink_state, uint8_t *mavlink_mode) +{ + /* reset MAVLink mode bitfield */ + *mavlink_mode = 0; + + /* set mode flags independent of system state */ + if (v_status->flag_control_manual_enabled) { + *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + } + + if (v_status->flag_hil_enabled) { + *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; + } + + /* set arming state */ + if (armed->armed) { + *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; + } else { + *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; + } + + switch (v_status->state_machine) { + case SYSTEM_STATE_PREFLIGHT: + if (v_status->flag_preflight_gyro_calibration || + v_status->flag_preflight_mag_calibration || + v_status->flag_preflight_accel_calibration) { + *mavlink_state = MAV_STATE_CALIBRATING; + } else { + *mavlink_state = MAV_STATE_UNINIT; + } + break; + + case SYSTEM_STATE_STANDBY: + *mavlink_state = MAV_STATE_STANDBY; + break; + + case SYSTEM_STATE_GROUND_READY: + *mavlink_state = MAV_STATE_ACTIVE; + break; + + case SYSTEM_STATE_MANUAL: + *mavlink_state = MAV_STATE_ACTIVE; + *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + break; + + case SYSTEM_STATE_STABILIZED: + *mavlink_state = MAV_STATE_ACTIVE; + *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; + break; + + case SYSTEM_STATE_AUTO: + *mavlink_state = MAV_STATE_ACTIVE; + *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; + break; + + case SYSTEM_STATE_MISSION_ABORT: + *mavlink_state = MAV_STATE_EMERGENCY; + break; + + case SYSTEM_STATE_EMCY_LANDING: + *mavlink_state = MAV_STATE_EMERGENCY; + break; + + case SYSTEM_STATE_EMCY_CUTOFF: + *mavlink_state = MAV_STATE_EMERGENCY; + break; + + case SYSTEM_STATE_GROUND_ERROR: + *mavlink_state = MAV_STATE_EMERGENCY; + break; + + case SYSTEM_STATE_REBOOT: + *mavlink_state = MAV_STATE_POWEROFF; + break; + } + +} + +/** + * MAVLink Protocol main function. + */ +int mavlink_thread_main(int argc, char *argv[]) +{ + int ch; + char *device_name = "/dev/ttyS1"; + baudrate = 57600; + + struct vehicle_status_s v_status; + struct actuator_armed_s armed; + + /* work around some stupidity in task_create's argv handling */ + argc -= 2; + argv += 2; + + while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) { + switch (ch) { + case 'b': + baudrate = strtoul(optarg, NULL, 10); + if (baudrate == 0) + errx(1, "invalid baud rate '%s'", optarg); + break; + + case 'd': + device_name = optarg; + break; + + case 'e': + mavlink_link_termination_allowed = true; + break; + + case 'o': + mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD; + break; + + default: + usage(); + } + } + + struct termios uart_config_original; + bool usb_uart; + + /* print welcome text */ + warnx("MAVLink v1.0 serial interface starting..."); + + /* inform about mode */ + warnx((mavlink_link_mode == MAVLINK_INTERFACE_MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE"); + + /* Flush stdout in case MAVLink is about to take it over */ + fflush(stdout); + + /* default values for arguments */ + uart = mavlink_open_uart(baudrate, device_name, &uart_config_original, &usb_uart); + if (uart < 0) + err(1, "could not open %s", device_name); + + /* Initialize system properties */ + mavlink_update_system(); + + /* start the MAVLink receiver */ + receive_thread = receive_start(uart); + + thread_running = true; + + /* arm counter to go off immediately */ + unsigned lowspeed_counter = 10; + + while (!thread_should_exit) { + + /* 1 Hz */ + if (lowspeed_counter == 10) { + mavlink_update_system(); + + /* translate the current system state to mavlink state and mode */ + uint8_t mavlink_state = 0; + uint8_t mavlink_mode = 0; + get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode); + + /* send heartbeat */ + mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state); + + /* send status (values already copied in the section above) */ + mavlink_msg_sys_status_send(chan, + v_status.onboard_control_sensors_present, + v_status.onboard_control_sensors_enabled, + v_status.onboard_control_sensors_health, + v_status.load, + v_status.voltage_battery * 1000.0f, + v_status.current_battery * 1000.0f, + v_status.battery_remaining, + v_status.drop_rate_comm, + v_status.errors_comm, + v_status.errors_count1, + v_status.errors_count2, + v_status.errors_count3, + v_status.errors_count4); + lowspeed_counter = 0; + } + lowspeed_counter++; + + /* sleep 1000 ms */ + usleep(1000000); + } + + /* wait for threads to complete */ + pthread_join(receive_thread, NULL); + + /* Reset the UART flags to original state */ + if (!usb_uart) + tcsetattr(uart, TCSANOW, &uart_config_original); + + thread_running = false; + + exit(0); +} + +static void +usage() +{ + fprintf(stderr, "usage: mavlink start [-d ] [-b ]\n" + " mavlink stop\n" + " mavlink status\n"); + exit(1); +} + +int mavlink_onboard_main(int argc, char *argv[]) +{ + + if (argc < 2) { + warnx("missing command"); + usage(); + } + + if (!strcmp(argv[1], "start")) { + + /* this is not an error */ + if (thread_running) + errx(0, "mavlink already running\n"); + + thread_should_exit = false; + mavlink_task = task_spawn("mavlink_onboard", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + mavlink_thread_main, + (const char**)argv); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + while (thread_running) { + usleep(200000); + } + warnx("terminated."); + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + errx(0, "running"); + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + usage(); + /* not getting here */ + return 0; +} + diff --git a/src/modules/mavlink_onboard/mavlink_bridge_header.h b/src/modules/mavlink_onboard/mavlink_bridge_header.h new file mode 100644 index 000000000..3ad3bb617 --- /dev/null +++ b/src/modules/mavlink_onboard/mavlink_bridge_header.h @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * 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 mavlink_bridge_header + * MAVLink bridge header for UART access. + * + * @author Lorenz Meier + */ + +/* MAVLink adapter header */ +#ifndef MAVLINK_BRIDGE_HEADER_H +#define MAVLINK_BRIDGE_HEADER_H + +#define MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/* use efficient approach, see mavlink_helpers.h */ +#define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes + +#define MAVLINK_GET_CHANNEL_BUFFER mavlink_get_channel_buffer +#define MAVLINK_GET_CHANNEL_STATUS mavlink_get_channel_status + +#include +#include + + +/* Struct that stores the communication settings of this system. + you can also define / alter these settings elsewhere, as long + as they're included BEFORE mavlink.h. + So you can set the + + mavlink_system.sysid = 100; // System ID, 1-255 + mavlink_system.compid = 50; // Component/Subsystem ID, 1-255 + + Lines also in your main.c, e.g. by reading these parameter from EEPROM. + */ +extern mavlink_system_t mavlink_system; + +/** + * @brief Send multiple chars (uint8_t) over a comm channel + * + * @param chan MAVLink channel to use, usually MAVLINK_COMM_0 = UART0 + * @param ch Character to send + */ +extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *ch, int length); + +mavlink_status_t* mavlink_get_channel_status(uint8_t chan); +mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan); + +#endif /* MAVLINK_BRIDGE_HEADER_H */ diff --git a/src/modules/mavlink_onboard/mavlink_receiver.c b/src/modules/mavlink_onboard/mavlink_receiver.c new file mode 100644 index 000000000..0acbea675 --- /dev/null +++ b/src/modules/mavlink_onboard/mavlink_receiver.c @@ -0,0 +1,331 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * 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 mavlink_receiver.c + * MAVLink protocol message receive and dispatch + * + * @author Lorenz Meier + */ + +/* XXX trim includes */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "mavlink_bridge_header.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "util.h" +#include "orb_topics.h" + +/* XXX should be in a header somewhere */ +pthread_t receive_start(int uart); + +static void handle_message(mavlink_message_t *msg); +static void *receive_thread(void *arg); + +static mavlink_status_t status; +static struct vehicle_vicon_position_s vicon_position; +static struct vehicle_command_s vcmd; +static struct offboard_control_setpoint_s offboard_control_sp; + +struct vehicle_global_position_s hil_global_pos; +struct vehicle_attitude_s hil_attitude; +orb_advert_t pub_hil_global_pos = -1; +orb_advert_t pub_hil_attitude = -1; + +static orb_advert_t cmd_pub = -1; +static orb_advert_t flow_pub = -1; + +static orb_advert_t offboard_control_sp_pub = -1; +static orb_advert_t vicon_position_pub = -1; + +extern bool gcs_link; + +static void +handle_message(mavlink_message_t *msg) +{ + if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) { + + mavlink_command_long_t cmd_mavlink; + mavlink_msg_command_long_decode(msg, &cmd_mavlink); + + if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) + || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { + //check for MAVLINK terminate command + if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { + /* This is the link shutdown command, terminate mavlink */ + printf("[mavlink] Terminating .. \n"); + fflush(stdout); + usleep(50000); + + /* terminate other threads and this thread */ + thread_should_exit = true; + + } else { + + /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ + vcmd.param1 = cmd_mavlink.param1; + vcmd.param2 = cmd_mavlink.param2; + vcmd.param3 = cmd_mavlink.param3; + vcmd.param4 = cmd_mavlink.param4; + vcmd.param5 = cmd_mavlink.param5; + vcmd.param6 = cmd_mavlink.param6; + vcmd.param7 = cmd_mavlink.param7; + vcmd.command = cmd_mavlink.command; + vcmd.target_system = cmd_mavlink.target_system; + vcmd.target_component = cmd_mavlink.target_component; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + vcmd.confirmation = cmd_mavlink.confirmation; + + /* check if topic is advertised */ + if (cmd_pub <= 0) { + cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + } + /* publish */ + orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); + } + } + } + + if (msg->msgid == MAVLINK_MSG_ID_OPTICAL_FLOW) { + mavlink_optical_flow_t flow; + mavlink_msg_optical_flow_decode(msg, &flow); + + struct optical_flow_s f; + + f.timestamp = hrt_absolute_time(); + f.flow_raw_x = flow.flow_x; + f.flow_raw_y = flow.flow_y; + f.flow_comp_x_m = flow.flow_comp_m_x; + f.flow_comp_y_m = flow.flow_comp_m_y; + f.ground_distance_m = flow.ground_distance; + f.quality = flow.quality; + f.sensor_id = flow.sensor_id; + + /* check if topic is advertised */ + if (flow_pub <= 0) { + flow_pub = orb_advertise(ORB_ID(optical_flow), &f); + } else { + /* publish */ + orb_publish(ORB_ID(optical_flow), flow_pub, &f); + } + + } + + if (msg->msgid == MAVLINK_MSG_ID_SET_MODE) { + /* Set mode on request */ + mavlink_set_mode_t new_mode; + mavlink_msg_set_mode_decode(msg, &new_mode); + + /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ + vcmd.param1 = new_mode.base_mode; + vcmd.param2 = new_mode.custom_mode; + vcmd.param3 = 0; + vcmd.param4 = 0; + vcmd.param5 = 0; + vcmd.param6 = 0; + vcmd.param7 = 0; + vcmd.command = MAV_CMD_DO_SET_MODE; + vcmd.target_system = new_mode.target_system; + vcmd.target_component = MAV_COMP_ID_ALL; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + vcmd.confirmation = 1; + + /* check if topic is advertised */ + if (cmd_pub <= 0) { + cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + } else { + /* create command */ + orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); + } + } + + /* Handle Vicon position estimates */ + if (msg->msgid == MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) { + mavlink_vicon_position_estimate_t pos; + mavlink_msg_vicon_position_estimate_decode(msg, &pos); + + vicon_position.x = pos.x; + vicon_position.y = pos.y; + vicon_position.z = pos.z; + + if (vicon_position_pub <= 0) { + vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); + } else { + orb_publish(ORB_ID(vehicle_vicon_position), vicon_position_pub, &vicon_position); + } + } + + /* Handle quadrotor motor setpoints */ + + if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) { + mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint; + mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint); + + if (mavlink_system.sysid < 4) { + + /* switch to a receiving link mode */ + gcs_link = false; + + /* + * rate control mode - defined by MAVLink + */ + + uint8_t ml_mode = 0; + bool ml_armed = false; + + switch (quad_motors_setpoint.mode) { + case 0: + ml_armed = false; + break; + case 1: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; + ml_armed = true; + + break; + case 2: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; + ml_armed = true; + + break; + case 3: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; + break; + case 4: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; + break; + } + + offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid-1] / (float)INT16_MAX; + offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid-1] / (float)INT16_MAX; + offboard_control_sp.p3= (float)quad_motors_setpoint.yaw[mavlink_system.sysid-1] / (float)INT16_MAX; + offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid-1]/(float)UINT16_MAX; + + if (quad_motors_setpoint.thrust[mavlink_system.sysid-1] == 0) { + ml_armed = false; + } + + offboard_control_sp.armed = ml_armed; + offboard_control_sp.mode = ml_mode; + + offboard_control_sp.timestamp = hrt_absolute_time(); + + /* check if topic has to be advertised */ + if (offboard_control_sp_pub <= 0) { + offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); + } else { + /* Publish */ + orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp); + } + } + } + +} + + +/** + * Receive data from UART. + */ +static void * +receive_thread(void *arg) +{ + int uart_fd = *((int*)arg); + + const int timeout = 1000; + uint8_t ch; + + mavlink_message_t msg; + + prctl(PR_SET_NAME, "mavlink offb rcv", getpid()); + + while (!thread_should_exit) { + + struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } }; + + if (poll(fds, 1, timeout) > 0) { + /* non-blocking read until buffer is empty */ + int nread = 0; + + do { + nread = read(uart_fd, &ch, 1); + + if (mavlink_parse_char(chan, ch, &msg, &status)) { //parse the char + /* handle generic messages and commands */ + handle_message(&msg); + } + } while (nread > 0); + } + } + + return NULL; +} + +pthread_t +receive_start(int uart) +{ + pthread_attr_t receiveloop_attr; + pthread_attr_init(&receiveloop_attr); + + struct sched_param param; + param.sched_priority = SCHED_PRIORITY_MAX - 40; + (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); + + pthread_attr_setstacksize(&receiveloop_attr, 2048); + + pthread_t thread; + pthread_create(&thread, &receiveloop_attr, receive_thread, &uart); + return thread; +} \ No newline at end of file diff --git a/src/modules/mavlink_onboard/module.mk b/src/modules/mavlink_onboard/module.mk new file mode 100644 index 000000000..c40fa042e --- /dev/null +++ b/src/modules/mavlink_onboard/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# MAVLink protocol to uORB interface process (XXX hack for onboard use) +# + +MODULE_COMMAND = mavlink_onboard +SRCS = mavlink.c \ + mavlink_receiver.c + +INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h new file mode 100644 index 000000000..f18f56243 --- /dev/null +++ b/src/modules/mavlink_onboard/orb_topics.h @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file orb_topics.h + * Common sets of topics subscribed to or published by the MAVLink driver, + * and structures maintained by those subscriptions. + */ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +struct mavlink_subscriptions { + int sensor_sub; + int att_sub; + int global_pos_sub; + int act_0_sub; + int act_1_sub; + int act_2_sub; + int act_3_sub; + int gps_sub; + int man_control_sp_sub; + int armed_sub; + int actuators_sub; + int local_pos_sub; + int spa_sub; + int spl_sub; + int spg_sub; + int debug_key_value; + int input_rc_sub; +}; + +extern struct mavlink_subscriptions mavlink_subs; + +/** Global position */ +extern struct vehicle_global_position_s global_pos; + +/** Local position */ +extern struct vehicle_local_position_s local_pos; + +/** Vehicle status */ +// extern struct vehicle_status_s v_status; + +/** RC channels */ +extern struct rc_channels_s rc; + +/** Actuator armed state */ +// extern struct actuator_armed_s armed; + +/** Worker thread starter */ +extern pthread_t uorb_receive_start(void); diff --git a/src/modules/mavlink_onboard/util.h b/src/modules/mavlink_onboard/util.h new file mode 100644 index 000000000..38a4db372 --- /dev/null +++ b/src/modules/mavlink_onboard/util.h @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file util.h + * Utility and helper functions and data. + */ + +#pragma once + +#include "orb_topics.h" + +/** MAVLink communications channel */ +extern uint8_t chan; + +/** Shutdown marker */ +extern volatile bool thread_should_exit; + +/** + * Translate the custom state into standard mavlink modes and state. + */ +extern void get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed, + uint8_t *mavlink_state, uint8_t *mavlink_mode); -- cgit v1.2.3 From 574e76532126fea8ab0ac5fd0595f6fb2935f0dd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Apr 2013 11:30:41 +0200 Subject: Moved all system commands to the new world --- apps/systemcmds/bl_update/Makefile | 44 --- apps/systemcmds/bl_update/bl_update.c | 215 ------------ apps/systemcmds/boardinfo/.context | 0 apps/systemcmds/boardinfo/Makefile | 44 --- apps/systemcmds/boardinfo/boardinfo.c | 409 ---------------------- apps/systemcmds/calibration/Makefile | 44 --- apps/systemcmds/calibration/calibration.c | 147 -------- apps/systemcmds/calibration/calibration.h | 60 ---- apps/systemcmds/calibration/channels_cal.c | 196 ----------- apps/systemcmds/calibration/range_cal.c | 224 ------------ apps/systemcmds/calibration/servo_cal.c | 264 -------------- apps/systemcmds/i2c/Makefile | 42 --- apps/systemcmds/i2c/i2c.c | 140 -------- apps/systemcmds/mixer/Makefile | 44 --- apps/systemcmds/mixer/mixer.c | 152 -------- apps/systemcmds/param/Makefile | 44 --- apps/systemcmds/param/param.c | 297 ---------------- apps/systemcmds/perf/.context | 0 apps/systemcmds/perf/Makefile | 44 --- apps/systemcmds/perf/perf.c | 81 ----- apps/systemcmds/preflight_check/Makefile | 44 --- apps/systemcmds/preflight_check/preflight_check.c | 206 ----------- apps/systemcmds/pwm/Makefile | 42 --- apps/systemcmds/pwm/pwm.c | 233 ------------ apps/systemcmds/reboot/.context | 0 apps/systemcmds/reboot/Makefile | 44 --- apps/systemcmds/reboot/reboot.c | 53 --- apps/systemcmds/top/.context | 0 apps/systemcmds/top/Makefile | 44 --- apps/systemcmds/top/top.c | 253 ------------- makefiles/config_px4fmu_default.mk | 22 +- src/modules/mavlink_onboard/module.mk | 2 +- src/systemcmds/bl_update/bl_update.c | 215 ++++++++++++ src/systemcmds/bl_update/module.mk | 43 +++ src/systemcmds/boardinfo/boardinfo.c | 409 ++++++++++++++++++++++ src/systemcmds/boardinfo/module.mk | 41 +++ src/systemcmds/eeprom/module.mk | 33 ++ src/systemcmds/i2c/i2c.c | 140 ++++++++ src/systemcmds/i2c/module.mk | 41 +++ src/systemcmds/mixer/mixer.c | 152 ++++++++ src/systemcmds/mixer/module.mk | 43 +++ src/systemcmds/param/module.mk | 44 +++ src/systemcmds/param/param.c | 297 ++++++++++++++++ src/systemcmds/perf/module.mk | 41 +++ src/systemcmds/perf/perf.c | 81 +++++ src/systemcmds/preflight_check/module.mk | 42 +++ src/systemcmds/preflight_check/preflight_check.c | 206 +++++++++++ src/systemcmds/pwm/module.mk | 41 +++ src/systemcmds/pwm/pwm.c | 233 ++++++++++++ src/systemcmds/reboot/module.mk | 41 +++ src/systemcmds/reboot/reboot.c | 53 +++ src/systemcmds/top/module.mk | 44 +++ src/systemcmds/top/top.c | 253 +++++++++++++ 53 files changed, 2505 insertions(+), 3422 deletions(-) delete mode 100644 apps/systemcmds/bl_update/Makefile delete mode 100644 apps/systemcmds/bl_update/bl_update.c delete mode 100644 apps/systemcmds/boardinfo/.context delete mode 100644 apps/systemcmds/boardinfo/Makefile delete mode 100644 apps/systemcmds/boardinfo/boardinfo.c delete mode 100644 apps/systemcmds/calibration/Makefile delete mode 100755 apps/systemcmds/calibration/calibration.c delete mode 100644 apps/systemcmds/calibration/calibration.h delete mode 100755 apps/systemcmds/calibration/channels_cal.c delete mode 100755 apps/systemcmds/calibration/range_cal.c delete mode 100644 apps/systemcmds/calibration/servo_cal.c delete mode 100644 apps/systemcmds/i2c/Makefile delete mode 100644 apps/systemcmds/i2c/i2c.c delete mode 100644 apps/systemcmds/mixer/Makefile delete mode 100644 apps/systemcmds/mixer/mixer.c delete mode 100644 apps/systemcmds/param/Makefile delete mode 100644 apps/systemcmds/param/param.c delete mode 100644 apps/systemcmds/perf/.context delete mode 100644 apps/systemcmds/perf/Makefile delete mode 100644 apps/systemcmds/perf/perf.c delete mode 100644 apps/systemcmds/preflight_check/Makefile delete mode 100644 apps/systemcmds/preflight_check/preflight_check.c delete mode 100644 apps/systemcmds/pwm/Makefile delete mode 100644 apps/systemcmds/pwm/pwm.c delete mode 100644 apps/systemcmds/reboot/.context delete mode 100644 apps/systemcmds/reboot/Makefile delete mode 100644 apps/systemcmds/reboot/reboot.c delete mode 100644 apps/systemcmds/top/.context delete mode 100644 apps/systemcmds/top/Makefile delete mode 100644 apps/systemcmds/top/top.c create mode 100644 src/systemcmds/bl_update/bl_update.c create mode 100644 src/systemcmds/bl_update/module.mk create mode 100644 src/systemcmds/boardinfo/boardinfo.c create mode 100644 src/systemcmds/boardinfo/module.mk create mode 100644 src/systemcmds/i2c/i2c.c create mode 100644 src/systemcmds/i2c/module.mk create mode 100644 src/systemcmds/mixer/mixer.c create mode 100644 src/systemcmds/mixer/module.mk create mode 100644 src/systemcmds/param/module.mk create mode 100644 src/systemcmds/param/param.c create mode 100644 src/systemcmds/perf/module.mk create mode 100644 src/systemcmds/perf/perf.c create mode 100644 src/systemcmds/preflight_check/module.mk create mode 100644 src/systemcmds/preflight_check/preflight_check.c create mode 100644 src/systemcmds/pwm/module.mk create mode 100644 src/systemcmds/pwm/pwm.c create mode 100644 src/systemcmds/reboot/module.mk create mode 100644 src/systemcmds/reboot/reboot.c create mode 100644 src/systemcmds/top/module.mk create mode 100644 src/systemcmds/top/top.c (limited to 'src/modules/mavlink_onboard') diff --git a/apps/systemcmds/bl_update/Makefile b/apps/systemcmds/bl_update/Makefile deleted file mode 100644 index d05493577..000000000 --- a/apps/systemcmds/bl_update/Makefile +++ /dev/null @@ -1,44 +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. -# -############################################################################ - -# -# Build the eeprom tool. -# - -APPNAME = bl_update -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 4096 - -include $(APPDIR)/mk/app.mk - -MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/bl_update/bl_update.c b/apps/systemcmds/bl_update/bl_update.c deleted file mode 100644 index 45715b791..000000000 --- a/apps/systemcmds/bl_update/bl_update.c +++ /dev/null @@ -1,215 +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 bl_update.c - * - * STM32F4 bootloader update tool. - */ - -#include - -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "systemlib/systemlib.h" -#include "systemlib/err.h" - -__EXPORT int bl_update_main(int argc, char *argv[]); - -static void setopt(void); - -int -bl_update_main(int argc, char *argv[]) -{ - if (argc != 2) - errx(1, "missing firmware filename or command"); - - if (!strcmp(argv[1], "setopt")) - setopt(); - - int fd = open(argv[1], O_RDONLY); - - if (fd < 0) - err(1, "open %s", argv[1]); - - struct stat s; - - if (stat(argv[1], &s) < 0) - err(1, "stat %s", argv[1]); - - /* sanity-check file size */ - if (s.st_size > 16384) - errx(1, "%s: file too large", argv[1]); - - uint8_t *buf = malloc(s.st_size); - - if (buf == NULL) - errx(1, "failed to allocate %u bytes for firmware buffer", s.st_size); - - if (read(fd, buf, s.st_size) != s.st_size) - err(1, "firmware read error"); - - close(fd); - - uint32_t *hdr = (uint32_t *)buf; - - if ((hdr[0] < 0x20000000) || /* stack not below RAM */ - (hdr[0] > (0x20000000 + (128 * 1024))) || /* stack not above RAM */ - (hdr[1] < 0x08000000) || /* entrypoint not below flash */ - ((hdr[1] - 0x08000000) > 16384)) { /* entrypoint not outside bootloader */ - free(buf); - errx(1, "not a bootloader image"); - } - - warnx("image validated, erasing bootloader..."); - usleep(10000); - - /* prevent other tasks from running while we do this */ - sched_lock(); - - /* unlock the control register */ - volatile uint32_t *keyr = (volatile uint32_t *)0x40023c04; - *keyr = 0x45670123U; - *keyr = 0xcdef89abU; - - volatile uint32_t *sr = (volatile uint32_t *)0x40023c0c; - volatile uint32_t *cr = (volatile uint32_t *)0x40023c10; - volatile uint8_t *base = (volatile uint8_t *)0x08000000; - - /* check the control register */ - if (*cr & 0x80000000) { - warnx("WARNING: flash unlock failed, flash aborted"); - goto flash_end; - } - - /* erase the bootloader sector */ - *cr = 0x2; - *cr = 0x10002; - - /* wait for the operation to complete */ - while (*sr & 0x1000) { - } - - if (*sr & 0xf2) { - warnx("WARNING: erase error 0x%02x", *sr); - goto flash_end; - } - - /* verify the erase */ - for (int i = 0; i < s.st_size; i++) { - if (base[i] != 0xff) { - warnx("WARNING: erase failed at %d - retry update, DO NOT reboot", i); - goto flash_end; - } - } - - warnx("flashing..."); - - /* now program the bootloader - speed is not critical so use x8 mode */ - for (int i = 0; i < s.st_size; i++) { - - /* program a byte */ - *cr = 1; - base[i] = buf[i]; - - /* wait for the operation to complete */ - while (*sr & 0x1000) { - } - - if (*sr & 0xf2) { - warnx("WARNING: program error 0x%02x", *sr); - goto flash_end; - } - } - - /* re-lock the flash control register */ - *cr = 0x80000000; - - warnx("verifying..."); - - /* now run a verify pass */ - for (int i = 0; i < s.st_size; i++) { - if (base[i] != buf[i]) { - warnx("WARNING: verify failed at %u - retry update, DO NOT reboot", i); - goto flash_end; - } - } - - warnx("bootloader update complete"); - -flash_end: - /* unlock the scheduler */ - sched_unlock(); - - free(buf); - exit(0); -} - -static void -setopt(void) -{ - volatile uint32_t *optcr = (volatile uint32_t *)0x40023c14; - - const uint16_t opt_mask = (3 << 2); /* BOR_LEV bitmask */ - const uint16_t opt_bits = (0 << 2); /* BOR = 0, setting for 2.7-3.6V operation */ - - if ((*optcr & opt_mask) == opt_bits) - errx(0, "option bits are already set as required"); - - /* unlock the control register */ - volatile uint32_t *optkeyr = (volatile uint32_t *)0x40023c08; - *optkeyr = 0x08192a3bU; - *optkeyr = 0x4c5d6e7fU; - - if (*optcr & 1) - errx(1, "option control register unlock failed"); - - /* program the new option value */ - *optcr = (*optcr & ~opt_mask) | opt_bits | (1 << 1); - - usleep(1000); - - if ((*optcr & opt_mask) == opt_bits) - errx(0, "option bits set"); - - errx(1, "option bits setting failed; readback 0x%04x", *optcr); - -} \ No newline at end of file diff --git a/apps/systemcmds/boardinfo/.context b/apps/systemcmds/boardinfo/.context deleted file mode 100644 index e69de29bb..000000000 diff --git a/apps/systemcmds/boardinfo/Makefile b/apps/systemcmds/boardinfo/Makefile deleted file mode 100644 index 6f1be149c..000000000 --- a/apps/systemcmds/boardinfo/Makefile +++ /dev/null @@ -1,44 +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. -# -############################################################################ - -# -# Build the boardinfo tool. -# - -APPNAME = boardinfo -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -include $(APPDIR)/mk/app.mk - -MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/boardinfo/boardinfo.c b/apps/systemcmds/boardinfo/boardinfo.c deleted file mode 100644 index fb8b07ef4..000000000 --- a/apps/systemcmds/boardinfo/boardinfo.c +++ /dev/null @@ -1,409 +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 boardinfo.c - * autopilot and carrier board information app - */ - -#include -#include -#include -#include -#include - -#include - -#include -#include -#include - -__EXPORT int boardinfo_main(int argc, char *argv[]); - -struct eeprom_info_s -{ - unsigned bus; - unsigned address; - unsigned page_size; - unsigned page_count; - unsigned page_write_delay; -}; - -/* XXX currently code below only supports 8-bit addressing */ -const struct eeprom_info_s eeprom_info[] = { - {3, 0x57, 8, 16, 3300}, -}; - -struct board_parameter_s { - const char *name; - bson_type_t type; -}; - -const struct board_parameter_s board_parameters[] = { - {"name", BSON_STRING}, /* ascii board name */ - {"vers", BSON_INT32}, /* board version (major << 8) | minor */ - {"date", BSON_INT32}, /* manufacture date */ - {"build", BSON_INT32} /* build code (fab << 8) | tester */ -}; - -const unsigned num_parameters = sizeof(board_parameters) / sizeof(board_parameters[0]); - -static int -eeprom_write(const struct eeprom_info_s *eeprom, uint8_t *buf, unsigned size) -{ - int result = -1; - - struct i2c_dev_s *dev = up_i2cinitialize(eeprom->bus); - if (dev == NULL) { - warnx("failed to init bus %d for EEPROM", eeprom->bus); - goto out; - } - I2C_SETFREQUENCY(dev, 400000); - - /* loop until all data has been transferred */ - for (unsigned address = 0; address < size; ) { - - uint8_t pagebuf[eeprom->page_size + 1]; - - /* how many bytes available to transfer? */ - unsigned count = size - address; - - /* constrain writes to the page size */ - if (count > eeprom->page_size) - count = eeprom->page_size; - - pagebuf[0] = address & 0xff; - memcpy(pagebuf + 1, buf + address, count); - - struct i2c_msg_s msgv[1] = { - { - .addr = eeprom->address, - .flags = 0, - .buffer = pagebuf, - .length = count + 1 - } - }; - - result = I2C_TRANSFER(dev, msgv, 1); - if (result != OK) { - warnx("EEPROM write failed: %d", result); - goto out; - } - usleep(eeprom->page_write_delay); - address += count; - } - -out: - if (dev != NULL) - up_i2cuninitialize(dev); - return result; -} - -static int -eeprom_read(const struct eeprom_info_s *eeprom, uint8_t *buf, unsigned size) -{ - int result = -1; - - struct i2c_dev_s *dev = up_i2cinitialize(eeprom->bus); - if (dev == NULL) { - warnx("failed to init bus %d for EEPROM", eeprom->bus); - goto out; - } - I2C_SETFREQUENCY(dev, 400000); - - /* loop until all data has been transferred */ - for (unsigned address = 0; address < size; ) { - - /* how many bytes available to transfer? */ - unsigned count = size - address; - - /* constrain transfers to the page size (bus anti-hog) */ - if (count > eeprom->page_size) - count = eeprom->page_size; - - uint8_t addr = address; - struct i2c_msg_s msgv[2] = { - { - .addr = eeprom->address, - .flags = 0, - .buffer = &addr, - .length = 1 - }, - { - .addr = eeprom->address, - .flags = I2C_M_READ, - .buffer = buf + address, - .length = count - } - }; - - result = I2C_TRANSFER(dev, msgv, 2); - if (result != OK) { - warnx("EEPROM read failed: %d", result); - goto out; - } - address += count; - } - -out: - if (dev != NULL) - up_i2cuninitialize(dev); - return result; -} - -static void * -idrom_read(const struct eeprom_info_s *eeprom) -{ - uint32_t size = 0xffffffff; - int result; - void *buf = NULL; - - result = eeprom_read(eeprom, (uint8_t *)&size, sizeof(size)); - if (result != 0) { - warnx("failed reading ID ROM length"); - goto fail; - } - if (size > (eeprom->page_size * eeprom->page_count)) { - warnx("ID ROM not programmed"); - goto fail; - } - - buf = malloc(size); - if (buf == NULL) { - warnx("could not allocate %d bytes for ID ROM", size); - goto fail; - } - result = eeprom_read(eeprom, buf, size); - if (result != 0) { - warnx("failed reading ID ROM"); - goto fail; - } - return buf; - -fail: - if (buf != NULL) - free(buf); - return NULL; -} - -static void -boardinfo_set(const struct eeprom_info_s *eeprom, char *spec) -{ - struct bson_encoder_s encoder; - int result = 1; - char *state, *token; - unsigned i; - - /* create the encoder and make a writable copy of the spec */ - bson_encoder_init_buf(&encoder, NULL, 0); - - for (i = 0, token = strtok_r(spec, ",", &state); - token && (i < num_parameters); - i++, token = strtok_r(NULL, ",", &state)) { - - switch (board_parameters[i].type) { - case BSON_STRING: - result = bson_encoder_append_string(&encoder, board_parameters[i].name, token); - break; - case BSON_INT32: - result = bson_encoder_append_int(&encoder, board_parameters[i].name, strtoul(token, NULL, 0)); - break; - default: - result = 1; - } - if (result) { - warnx("bson append failed for %s<%s>", board_parameters[i].name, token); - goto out; - } - } - bson_encoder_fini(&encoder); - if (i != num_parameters) { - warnx("incorrect parameter list, expected: \",,\""); - result = 1; - goto out; - } - if (bson_encoder_buf_size(&encoder) > (int)(eeprom->page_size * eeprom->page_count)) { - warnx("data too large for EEPROM"); - result = 1; - goto out; - } - if ((int)*(uint32_t *)bson_encoder_buf_data(&encoder) != bson_encoder_buf_size(&encoder)) { - warnx("buffer length mismatch"); - result = 1; - goto out; - } - warnx("writing %p/%u", bson_encoder_buf_data(&encoder), bson_encoder_buf_size(&encoder)); - - result = eeprom_write(eeprom, (uint8_t *)bson_encoder_buf_data(&encoder), bson_encoder_buf_size(&encoder)); - if (result < 0) { - warnc(-result, "error writing EEPROM"); - result = 1; - } else { - result = 0; - } - -out: - free(bson_encoder_buf_data(&encoder)); - - exit(result); -} - -static int -boardinfo_print(bson_decoder_t decoder, void *private, bson_node_t node) -{ - switch (node->type) { - case BSON_INT32: - printf("%s: %d / 0x%08x\n", node->name, (int)node->i, (unsigned)node->i); - break; - case BSON_STRING: { - char buf[bson_decoder_data_pending(decoder)]; - bson_decoder_copy_data(decoder, buf); - printf("%s: %s\n", node->name, buf); - break; - } - case BSON_EOO: - break; - default: - warnx("unexpected node type %d", node->type); - break; - } - return 1; -} - -static void -boardinfo_show(const struct eeprom_info_s *eeprom) -{ - struct bson_decoder_s decoder; - void *buf; - - buf = idrom_read(eeprom); - if (buf == NULL) - errx(1, "ID ROM read failed"); - - if (bson_decoder_init_buf(&decoder, buf, 0, boardinfo_print, NULL) == 0) { - while (bson_decoder_next(&decoder) > 0) - ; - } else { - warnx("failed to init decoder"); - } - free(buf); - exit(0); -} - -struct { - const char *property; - const char *value; -} test_args; - -static int -boardinfo_test_callback(bson_decoder_t decoder, void *private, bson_node_t node) -{ - /* reject nodes with non-matching names */ - if (strcmp(node->name, test_args.property)) - return 1; - - /* compare node values to check for a match */ - switch (node->type) { - case BSON_STRING: { - char buf[bson_decoder_data_pending(decoder)]; - bson_decoder_copy_data(decoder, buf); - - /* check for a match */ - if (!strcmp(test_args.value, buf)) { - return 2; - } - break; - } - - case BSON_INT32: { - int32_t val = strtol(test_args.value, NULL, 0); - - /* check for a match */ - if (node->i == val) { - return 2; - } - break; - } - - default: - break; - } - - return 1; -} - -static void -boardinfo_test(const struct eeprom_info_s *eeprom, const char *property, const char *value) -{ - struct bson_decoder_s decoder; - void *buf; - int result = -1; - - if ((property == NULL) || (strlen(property) == 0) || - (value == NULL) || (strlen(value) == 0)) - errx(1, "missing property name or value"); - - test_args.property = property; - test_args.value = value; - - buf = idrom_read(eeprom); - if (buf == NULL) - errx(1, "ID ROM read failed"); - - if (bson_decoder_init_buf(&decoder, buf, 0, boardinfo_test_callback, NULL) == 0) { - do { - result = bson_decoder_next(&decoder); - } while (result == 1); - } else { - warnx("failed to init decoder"); - } - free(buf); - - /* if we matched, we exit with zero success */ - exit((result == 2) ? 0 : 1); -} - -int -boardinfo_main(int argc, char *argv[]) -{ - if (!strcmp(argv[1], "set")) - boardinfo_set(&eeprom_info[0], argv[2]); - - if (!strcmp(argv[1], "show")) - boardinfo_show(&eeprom_info[0]); - - if (!strcmp(argv[1], "test")) - boardinfo_test(&eeprom_info[0], argv[2], argv[3]); - - errx(1, "missing/unrecognised command, try one of 'set', 'show', 'test'"); -} diff --git a/apps/systemcmds/calibration/Makefile b/apps/systemcmds/calibration/Makefile deleted file mode 100644 index a1735962e..000000000 --- a/apps/systemcmds/calibration/Makefile +++ /dev/null @@ -1,44 +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. -# -############################################################################ - -# -# Makefile to build the calibration tool -# - -APPNAME = calibration -PRIORITY = SCHED_PRIORITY_MAX - 1 -STACKSIZE = 4096 - -include $(APPDIR)/mk/app.mk - -MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/calibration/calibration.c b/apps/systemcmds/calibration/calibration.c deleted file mode 100755 index 7f8b9240f..000000000 --- a/apps/systemcmds/calibration/calibration.c +++ /dev/null @@ -1,147 +0,0 @@ -/**************************************************************************** - * calibration.c - * - * Copyright (C) 2012 Ivan Ovinnikov. All rights reserved. - * Authors: Nils Wenzler - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name NuttX nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/**************************************************************************** - * Included Files - ****************************************************************************/ -#include -#include -#include -#include "calibration.h" - -/**************************************************************************** - * Private Function Prototypes - ****************************************************************************/ - -static int calibrate_help(int argc, char *argv[]); -static int calibrate_all(int argc, char *argv[]); - -__EXPORT int calibration_main(int argc, char *argv[]); - -/**************************************************************************** - * Private Data - ****************************************************************************/ - -struct { - const char *name; - int (* fn)(int argc, char *argv[]); - unsigned options; -#define OPT_NOHELP (1<<0) -#define OPT_NOALLTEST (1<<1) -} calibrates[] = { - {"range", range_cal, 0}, - {"servo", servo_cal, 0}, - {"all", calibrate_all, OPT_NOALLTEST}, - {"help", calibrate_help, OPT_NOALLTEST | OPT_NOHELP}, - {NULL, NULL} -}; - -/**************************************************************************** - * Public Data - ****************************************************************************/ - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - -static int -calibrate_help(int argc, char *argv[]) -{ - unsigned i; - - printf("Available calibration routines:\n"); - - for (i = 0; calibrates[i].name; i++) - printf(" %s\n", calibrates[i].name); - - return 0; -} - -static int -calibrate_all(int argc, char *argv[]) -{ - unsigned i; - char *args[2] = {"all", NULL}; - - printf("Running all calibration routines...\n\n"); - - for (i = 0; calibrates[i].name; i++) { - printf(" %s:\n", calibrates[i].name); - - if (calibrates[i].fn(1, args)) { - printf(" FAIL\n"); - - } else { - printf(" DONE\n"); - } - } - - return 0; -} - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -void press_enter(void) -{ - int c; - printf("Press CTRL+ENTER to continue... \n"); - fflush(stdout); - - do c = getchar(); while ((c != '\n') && (c != EOF)); -} - -/**************************************************************************** - * Name: calibrate_main - ****************************************************************************/ - -int calibration_main(int argc, char *argv[]) -{ - unsigned i; - - if (argc < 2) { - printf("calibration: missing name - 'calibration help' for a list of routines\n"); - return 1; - } - - for (i = 0; calibrates[i].name; i++) { - if (!strcmp(calibrates[i].name, argv[1])) - return calibrates[i].fn(argc - 1, argv + 1); - } - - printf("calibrate: no routines called '%s' - 'calibration help' for a list of routines\n", argv[1]); - return 1; -} diff --git a/apps/systemcmds/calibration/calibration.h b/apps/systemcmds/calibration/calibration.h deleted file mode 100644 index 028853ec8..000000000 --- a/apps/systemcmds/calibration/calibration.h +++ /dev/null @@ -1,60 +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. - * - ****************************************************************************/ - -#ifndef _CALIBRATION_H -#define _CALIBRATION_H - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -/**************************************************************************** - * Definitions - ****************************************************************************/ - -/**************************************************************************** - * Public Types - ****************************************************************************/ - -/**************************************************************************** - * Public Variables - ****************************************************************************/ - -/**************************************************************************** - * Public Function Prototypes - ****************************************************************************/ - -extern int range_cal(int argc, char *argv[]); -extern int servo_cal(int argc, char *argv[]); - -#endif diff --git a/apps/systemcmds/calibration/channels_cal.c b/apps/systemcmds/calibration/channels_cal.c deleted file mode 100755 index 575138b12..000000000 --- a/apps/systemcmds/calibration/channels_cal.c +++ /dev/null @@ -1,196 +0,0 @@ -/**************************************************************************** - * channels_cal.c - * - * Copyright (C) 2012 Nils Wenzler. All rights reserved. - * Authors: Nils Wenzler - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name NuttX nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/**************************************************************************** - * Included Files - ****************************************************************************/ -#include -#include -#include -#include "calibration.h" - -/**************************************************************************** - * Defines - ****************************************************************************/ -#define ABS(a) (((a) < 0) ? -(a) : (a)) -#define MIN(a,b) (((a)<(b))?(a):(b)) -#define MAX(a,b) (((a)>(b))?(a):(b)) - -/**************************************************************************** - * Private Data - ****************************************************************************/ -//Store the values here before writing them to global_data_rc_channels -uint16_t old_values[NR_CHANNELS]; -uint16_t cur_values[NR_CHANNELS]; -uint8_t function_map[NR_CHANNELS]; -char names[12][9]; - - - -/**************************************************************************** - * Private Functions - ****************************************************************************/ -void press_enter_2(void) -{ - int c; - printf("Press CTRL+ENTER to continue... \n"); - fflush(stdout); - - do c = getchar(); while ((c != '\n') && (c != EOF)); -} - -/**This gets all current values and writes them to min or max - */ -uint8_t get_val(uint16_t *buffer) -{ - if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) { - uint8_t i; - - for (i = 0; i < NR_CHANNELS; i++) { - printf("Channel: %i\t RAW Value: %i: \n", i, global_data_rc_channels->chan[i].raw); - buffer[i] = global_data_rc_channels->chan[i].raw; - } - - global_data_unlock(&global_data_rc_channels->access_conf); - return 0; - - } else - return -1; -} - -void set_channel(void) -{ - static uint8_t j = 0; - uint8_t i; - - for (i = 0; i < NR_CHANNELS; i++) { - if (ABS(old_values - cur_values) > 50) { - function_map[j] = i; - strcpy(names[i], global_data_rc_channels->function_names[j]); - j++; - } - } -} - -void write_dat(void) -{ - global_data_lock(&global_data_rc_channels->access_conf); - uint8_t i; - - for (i = 0; i < NR_CHANNELS; i++) { - global_data_rc_channels->function[i] = function_map[i]; - strcpy(global_data_rc_channels->chan[i].name, names[i]); - - printf("Channel %i\t Function %s\n", i, - global_data_rc_channels->chan[i].name); - } - - global_data_unlock(&global_data_rc_channels->access_conf); -} - - -/**************************************************************************** - * Public Functions - ****************************************************************************/ -int channels_cal(int argc, char *argv[]) -{ - - printf("This routine maps the input functions on the remote control\n"); - printf("to the corresponding function (Throttle, Roll,..)\n"); - printf("Always move the stick all the way\n"); - press_enter_2(); - - printf("Pull the THROTTLE stick down\n"); - press_enter_2(); - - while (get_val(old_values)); - - printf("Move the THROTTLE stick up\n "); - press_enter_2(); - - while (get_val(cur_values)); - - set_channel(); - - printf("Pull the PITCH stick down\n"); - press_enter_2(); - - while (get_val(old_values)); - - printf("Move the PITCH stick up\n "); - press_enter_2(); - - while (get_val(cur_values)); - - set_channel(); - - printf("Put the ROLL stick to the left\n"); - press_enter_2(); - - while (get_val(old_values)); - - printf("Put the ROLL stick to the right\n "); - press_enter_2(); - - while (get_val(cur_values)); - - set_channel(); - - printf("Put the YAW stick to the left\n"); - press_enter_2(); - - while (get_val(old_values)); - - printf("Put the YAW stick to the right\n "); - press_enter_2(); - - while (get_val(cur_values)); - - set_channel(); - - uint8_t k; - - for (k = 5; k < NR_CHANNELS; k++) { - function_map[k] = k; - strcpy(names[k], global_data_rc_channels->function_names[k]); - } - -//write the values to global_data_rc_channels - write_dat(); - - return 0; - -} - diff --git a/apps/systemcmds/calibration/range_cal.c b/apps/systemcmds/calibration/range_cal.c deleted file mode 100755 index 159a0d06b..000000000 --- a/apps/systemcmds/calibration/range_cal.c +++ /dev/null @@ -1,224 +0,0 @@ -/**************************************************************************** - * range_cal.c - * - * Copyright (C) 2012 Nils Wenzler. All rights reserved. - * Authors: Nils Wenzler - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name NuttX nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/**************************************************************************** - * Included Files - ****************************************************************************/ -#include -#include -#include "calibration.h" - -/**************************************************************************** - * Defines - ****************************************************************************/ - -/**************************************************************************** - * Private Data - ****************************************************************************/ -//Store the values here before writing them to global_data_rc_channels -uint16_t max_values[NR_CHANNELS]; -uint16_t min_values[NR_CHANNELS]; -uint16_t mid_values[NR_CHANNELS]; - - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - -/**This sets the middle values - */ -uint8_t set_mid(void) -{ - if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) { - uint8_t i; - - for (i = 0; i < NR_CHANNELS; i++) { - if (i == global_data_rc_channels->function[ROLL] || - i == global_data_rc_channels->function[YAW] || - i == global_data_rc_channels->function[PITCH]) { - - mid_values[i] = global_data_rc_channels->chan[i].raw; - - } else { - mid_values[i] = (max_values[i] + min_values[i]) / 2; - } - - } - - global_data_unlock(&global_data_rc_channels->access_conf); - return 0; - - } else - return -1; -} - -/**This gets all current values and writes them to min or max - */ -uint8_t get_value(void) -{ - if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) { - uint8_t i; - - for (i = 0; i < NR_CHANNELS; i++) { - //see if the value is bigger or smaller than 1500 (roughly neutral) - //and write to the appropriate array - if (global_data_rc_channels->chan[i].raw != 0 && - global_data_rc_channels->chan[i].raw < 1500) { - min_values[i] = global_data_rc_channels->chan[i].raw; - - } else if (global_data_rc_channels->chan[i].raw != 0 && - global_data_rc_channels->chan[i].raw > 1500) { - max_values[i] = global_data_rc_channels->chan[i].raw; - - } else { - max_values[i] = 0; - min_values[i] = 0; - } - } - - global_data_unlock(&global_data_rc_channels->access_conf); - return 0; - - } else - return -1; -} - - -void write_data(void) -{ - // global_data_lock(&global_data_rc_channels->access_conf); - // uint8_t i; - // for(i=0; i < NR_CHANNELS; i++){ - // //Write the data to global_data_rc_channels (if not 0) - // if(mid_values[i]!=0 && min_values[i]!=0 && max_values[i]!=0){ - // global_data_rc_channels->chan[i].scaling_factor = - // 10000/((max_values[i] - min_values[i])/2); - // - // global_data_rc_channels->chan[i].mid = mid_values[i]; - // } - // - // printf("Channel %i\t Function %s \t\t Min %i\t\t Max %i\t\t Scaling Factor: %i\t Middle Value %i\n", - // i, - // global_data_rc_channels->function_name[global_data_rc_channels->function[i]], - // min_values[i], max_values[i], - // global_data_rc_channels->chan[i].scaling_factor, - // global_data_rc_channels->chan[i].mid); - // } - // global_data_unlock(&global_data_rc_channels->access_conf); - - //Write to the Parameter storage - - global_data_parameter_storage->pm.param_values[PARAM_RC1_MIN] = min_values[0]; - global_data_parameter_storage->pm.param_values[PARAM_RC2_MIN] = min_values[1]; - global_data_parameter_storage->pm.param_values[PARAM_RC3_MIN] = min_values[2]; - global_data_parameter_storage->pm.param_values[PARAM_RC4_MIN] = min_values[3]; - global_data_parameter_storage->pm.param_values[PARAM_RC5_MIN] = min_values[4]; - global_data_parameter_storage->pm.param_values[PARAM_RC6_MIN] = min_values[5]; - global_data_parameter_storage->pm.param_values[PARAM_RC7_MIN] = min_values[6]; - global_data_parameter_storage->pm.param_values[PARAM_RC8_MIN] = min_values[7]; - - - global_data_parameter_storage->pm.param_values[PARAM_RC1_MAX] = max_values[0]; - global_data_parameter_storage->pm.param_values[PARAM_RC2_MAX] = max_values[1]; - global_data_parameter_storage->pm.param_values[PARAM_RC3_MAX] = max_values[2]; - global_data_parameter_storage->pm.param_values[PARAM_RC4_MAX] = max_values[3]; - global_data_parameter_storage->pm.param_values[PARAM_RC5_MAX] = max_values[4]; - global_data_parameter_storage->pm.param_values[PARAM_RC6_MAX] = max_values[5]; - global_data_parameter_storage->pm.param_values[PARAM_RC7_MAX] = max_values[6]; - global_data_parameter_storage->pm.param_values[PARAM_RC8_MAX] = max_values[7]; - - - global_data_parameter_storage->pm.param_values[PARAM_RC1_TRIM] = mid_values[0]; - global_data_parameter_storage->pm.param_values[PARAM_RC2_TRIM] = mid_values[1]; - global_data_parameter_storage->pm.param_values[PARAM_RC3_TRIM] = mid_values[2]; - global_data_parameter_storage->pm.param_values[PARAM_RC4_TRIM] = mid_values[3]; - global_data_parameter_storage->pm.param_values[PARAM_RC5_TRIM] = mid_values[4]; - global_data_parameter_storage->pm.param_values[PARAM_RC6_TRIM] = mid_values[5]; - global_data_parameter_storage->pm.param_values[PARAM_RC7_TRIM] = mid_values[6]; - global_data_parameter_storage->pm.param_values[PARAM_RC8_TRIM] = mid_values[7]; - - usleep(3e6); - uint8_t i; - - for (i = 0; i < NR_CHANNELS; i++) { - printf("Channel %i:\t\t Min %i\t\t Max %i\t\t Scaling Factor: %i\t Middle Value %i\n", - i, - min_values[i], max_values[i], - global_data_rc_channels->chan[i].scaling_factor, - global_data_rc_channels->chan[i].mid); - } - - -} - -/**************************************************************************** - * Public Functions - ****************************************************************************/ -int range_cal(int argc, char *argv[]) -{ - - printf("The range calibration routine assumes you already did the channel\n"); - printf("assignment\n"); - printf("This routine chooses the minimum, maximum and middle\n"); - printf("value for each channel separatly. The ROLL, PITCH and YAW function\n"); - printf("get their middle value from the RC direct, for the rest it is\n"); - printf("calculated out of the min and max values.\n"); - press_enter(); - - printf("Hold both sticks in lower left corner and continue\n "); - press_enter(); - usleep(500000); - - while (get_value()); - - printf("Hold both sticks in upper right corner and continue\n"); - press_enter(); - usleep(500000); - - while (get_value()); - - printf("Set the trim to 0 and leave the sticks in the neutral position\n"); - press_enter(); - - //Loop until successfull - while (set_mid()); - - //write the values to global_data_rc_channels - write_data(); - - return 0; - -} - diff --git a/apps/systemcmds/calibration/servo_cal.c b/apps/systemcmds/calibration/servo_cal.c deleted file mode 100644 index 96b3045a9..000000000 --- a/apps/systemcmds/calibration/servo_cal.c +++ /dev/null @@ -1,264 +0,0 @@ -/**************************************************************************** - * servo_cal.c - * - * Copyright (C) 2012 Nils Wenzler. All rights reserved. - * Authors: Nils Wenzler - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name NuttX nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/**************************************************************************** - * Included Files - ****************************************************************************/ -#include -#include -#include -#include -#include "calibration.h" - -/**************************************************************************** - * Defines - ****************************************************************************/ - -/**************************************************************************** - * Private Data - ****************************************************************************/ -//Store the values here before writing them to global_data_rc_channels -uint16_t max_values_servo[PWM_SERVO_MAX_CHANNELS]; -uint16_t min_values_servo[PWM_SERVO_MAX_CHANNELS]; -uint16_t mid_values_servo[PWM_SERVO_MAX_CHANNELS]; - -// Servo loop thread - -pthread_t servo_calib_thread; - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - -/**This sets the middle values - */ -uint8_t set_mid_s(void) -{ - if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) { - uint8_t i; - - for (i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) { - if (i == global_data_rc_channels->function[ROLL] || - i == global_data_rc_channels->function[YAW] || - i == global_data_rc_channels->function[PITCH]) { - - mid_values_servo[i] = global_data_rc_channels->chan[i].raw; - - } else { - mid_values_servo[i] = (max_values_servo[i] + min_values_servo[i]) / 2; - } - - } - - global_data_unlock(&global_data_rc_channels->access_conf); - return 0; - - } else - return -1; -} - -/**This gets all current values and writes them to min or max - */ -uint8_t get_value_s(void) -{ - if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) { - uint8_t i; - - for (i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) { - //see if the value is bigger or smaller than 1500 (roughly neutral) - //and write to the appropriate array - if (global_data_rc_channels->chan[i].raw != 0 && - global_data_rc_channels->chan[i].raw < 1500) { - min_values_servo[i] = global_data_rc_channels->chan[i].raw; - - } else if (global_data_rc_channels->chan[i].raw != 0 && - global_data_rc_channels->chan[i].raw > 1500) { - max_values_servo[i] = global_data_rc_channels->chan[i].raw; - - } else { - max_values_servo[i] = 0; - min_values_servo[i] = 0; - } - } - - global_data_unlock(&global_data_rc_channels->access_conf); - return 0; - - } else - return -1; -} - - -void write_data_s(void) -{ - // global_data_lock(&global_data_rc_channels->access_conf); - // uint8_t i; - // for(i=0; i < NR_CHANNELS; i++){ - // //Write the data to global_data_rc_channels (if not 0) - // if(mid_values_servo[i]!=0 && min_values_servo[i]!=0 && max_values_servo[i]!=0){ - // global_data_rc_channels->chan[i].scaling_factor = - // 10000/((max_values_servo[i] - min_values_servo[i])/2); - // - // global_data_rc_channels->chan[i].mid = mid_values_servo[i]; - // } - // - // printf("Channel %i\t Function %s \t\t Min %i\t\t Max %i\t\t Scaling Factor: %i\t Middle Value %i\n", - // i, - // global_data_rc_channels->function_name[global_data_rc_channels->function[i]], - // min_values_servo[i], max_values_servo[i], - // global_data_rc_channels->chan[i].scaling_factor, - // global_data_rc_channels->chan[i].mid); - // } - // global_data_unlock(&global_data_rc_channels->access_conf); - - //Write to the Parameter storage - - - - global_data_lock(&global_data_parameter_storage->access_conf); - - global_data_parameter_storage->pm.param_values[PARAM_SERVO1_MIN] = min_values_servo[0]; - global_data_parameter_storage->pm.param_values[PARAM_SERVO2_MIN] = min_values_servo[1]; - global_data_parameter_storage->pm.param_values[PARAM_SERVO3_MIN] = min_values_servo[2]; - global_data_parameter_storage->pm.param_values[PARAM_SERVO4_MIN] = min_values_servo[3]; - - global_data_parameter_storage->pm.param_values[PARAM_SERVO1_MAX] = max_values_servo[0]; - global_data_parameter_storage->pm.param_values[PARAM_SERVO2_MAX] = max_values_servo[1]; - global_data_parameter_storage->pm.param_values[PARAM_SERVO3_MAX] = max_values_servo[2]; - global_data_parameter_storage->pm.param_values[PARAM_SERVO4_MAX] = max_values_servo[3]; - - global_data_parameter_storage->pm.param_values[PARAM_SERVO1_TRIM] = mid_values_servo[0]; - global_data_parameter_storage->pm.param_values[PARAM_SERVO2_TRIM] = mid_values_servo[1]; - global_data_parameter_storage->pm.param_values[PARAM_SERVO3_TRIM] = mid_values_servo[2]; - global_data_parameter_storage->pm.param_values[PARAM_SERVO4_TRIM] = mid_values_servo[3]; - - global_data_unlock(&global_data_parameter_storage->access_conf); - - usleep(3e6); - uint8_t i; - - for (i = 0; i < NR_CHANNELS; i++) { - printf("Channel %i:\t\t Min %i\t\t Max %i\t\t Scaling Factor: %i\t Middle Value %i\n", - i, - min_values_servo[i], max_values_servo[i], - global_data_rc_channels->chan[i].scaling_factor, - global_data_rc_channels->chan[i].mid); - } - -} - -static void *servo_loop(void *arg) -{ - - // Set thread name - prctl(1, "calibration servo", getpid()); - - // initialize servos - int fd; - servo_position_t data[PWM_SERVO_MAX_CHANNELS]; - - fd = open("/dev/pwm_servo", O_RDWR); - - if (fd < 0) { - printf("failed opening /dev/pwm_servo\n"); - } - - ioctl(fd, PWM_SERVO_ARM, 0); - - while (1) { - int i; - - for (i = 0; i < 4; i++) { - data[i] = global_data_rc_channels->chan[global_data_rc_channels->function[THROTTLE]].raw; - } - - int result = write(fd, &data, sizeof(data)); - - if (result != sizeof(data)) { - printf("failed bulk-reading channel values\n"); - } - - // 5Hz loop - usleep(200000); - } - - return NULL; -} - - -/**************************************************************************** - * Public Functions - ****************************************************************************/ -int servo_cal(int argc, char *argv[]) -{ - // pthread_attr_t servo_loop_attr; - // pthread_attr_init(&servo_loop_attr); - // pthread_attr_setstacksize(&servo_loop_attr, 1024); - pthread_create(&servo_calib_thread, NULL, servo_loop, NULL); - pthread_join(servo_calib_thread, NULL); - - printf("The servo calibration routine assumes you already did the channel\n"); - printf("assignment with 'calibration channels'\n"); - printf("This routine chooses the minimum, maximum and middle\n"); - printf("value for each channel separately. The ROLL, PITCH and YAW function\n"); - printf("get their middle value from the RC direct, for the rest it is\n"); - printf("calculated out of the min and max values.\n"); - press_enter(); - - printf("Hold both sticks in lower left corner and continue\n "); - press_enter(); - usleep(500000); - - while (get_value_s()); - - printf("Hold both sticks in upper right corner and continue\n"); - press_enter(); - usleep(500000); - - while (get_value_s()); - - printf("Set the trim to 0 and leave the sticks in the neutral position\n"); - press_enter(); - - //Loop until successfull - while (set_mid_s()); - - //write the values to global_data_rc_channels - write_data_s(); - - return 0; - -} - diff --git a/apps/systemcmds/i2c/Makefile b/apps/systemcmds/i2c/Makefile deleted file mode 100644 index 046e74757..000000000 --- a/apps/systemcmds/i2c/Makefile +++ /dev/null @@ -1,42 +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. -# -############################################################################ - -# -# 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 deleted file mode 100644 index e1babdc12..000000000 --- a/apps/systemcmds/i2c/i2c.c +++ /dev/null @@ -1,140 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * 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 - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include - -#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/Makefile b/apps/systemcmds/mixer/Makefile deleted file mode 100644 index 3d8ac38cb..000000000 --- a/apps/systemcmds/mixer/Makefile +++ /dev/null @@ -1,44 +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. -# -############################################################################ - -# -# Build the mixer tool. -# - -APPNAME = mixer -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 4096 - -include $(APPDIR)/mk/app.mk - -MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/mixer/mixer.c b/apps/systemcmds/mixer/mixer.c deleted file mode 100644 index 55c4f0836..000000000 --- a/apps/systemcmds/mixer/mixer.c +++ /dev/null @@ -1,152 +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 mixer.c - * - * Mixer utility. - */ - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -__EXPORT int mixer_main(int argc, char *argv[]); - -static void usage(const char *reason) noreturn_function; -static void load(const char *devname, const char *fname) noreturn_function; - -int -mixer_main(int argc, char *argv[]) -{ - if (argc < 2) - usage("missing command"); - - if (!strcmp(argv[1], "load")) { - if (argc < 4) - usage("missing device or filename"); - - load(argv[2], argv[3]); - } - - usage("unrecognised command"); -} - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - fprintf(stderr, "usage:\n"); - fprintf(stderr, " mixer load \n"); - /* XXX other useful commands? */ - exit(1); -} - -static void -load(const char *devname, const char *fname) -{ - int dev; - FILE *fp; - char line[80]; - char buf[512]; - - /* open the device */ - if ((dev = open(devname, 0)) < 0) - err(1, "can't open %s\n", devname); - - /* reset mixers on the device */ - if (ioctl(dev, MIXERIOCRESET, 0)) - err(1, "can't reset mixers on %s", devname); - - /* open the mixer definition file */ - fp = fopen(fname, "r"); - if (fp == NULL) - err(1, "can't open %s", fname); - - /* read valid lines from the file into a buffer */ - buf[0] = '\0'; - for (;;) { - - /* get a line, bail on error/EOF */ - line[0] = '\0'; - if (fgets(line, sizeof(line), fp) == NULL) - break; - - /* if the line doesn't look like a mixer definition line, skip it */ - if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) - continue; - - /* compact whitespace in the buffer */ - char *t, *f; - for (f = buf; *f != '\0'; f++) { - /* scan for space characters */ - if (*f == ' ') { - /* look for additional spaces */ - t = f + 1; - while (*t == ' ') - t++; - if (*t == '\0') { - /* strip trailing whitespace */ - *f = '\0'; - } else if (t > (f + 1)) { - memmove(f + 1, t, strlen(t) + 1); - } - } - } - - /* if the line is too long to fit in the buffer, bail */ - if ((strlen(line) + strlen(buf) + 1) >= sizeof(buf)) - break; - - /* add the line to the buffer */ - strcat(buf, line); - } - - /* XXX pass the buffer to the device */ - int ret = ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf); - - if (ret < 0) - err(1, "error loading mixers from %s", fname); - exit(0); -} diff --git a/apps/systemcmds/param/Makefile b/apps/systemcmds/param/Makefile deleted file mode 100644 index f19cadbb6..000000000 --- a/apps/systemcmds/param/Makefile +++ /dev/null @@ -1,44 +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. -# -############################################################################ - -# -# Build the parameters tool. -# - -APPNAME = param -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 4096 - -include $(APPDIR)/mk/app.mk - -MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/param/param.c b/apps/systemcmds/param/param.c deleted file mode 100644 index 56f5317e3..000000000 --- a/apps/systemcmds/param/param.c +++ /dev/null @@ -1,297 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * 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 param.c - * - * Parameter tool. - */ - -#include - -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "systemlib/systemlib.h" -#include "systemlib/param/param.h" -#include "systemlib/err.h" - -__EXPORT int param_main(int argc, char *argv[]); - -static void do_save(const char* param_file_name); -static void do_load(const char* param_file_name); -static void do_import(const char* param_file_name); -static void do_show(const char* search_string); -static void do_show_print(void *arg, param_t param); -static void do_set(const char* name, const char* val); - -int -param_main(int argc, char *argv[]) -{ - if (argc >= 2) { - if (!strcmp(argv[1], "save")) { - if (argc >= 3) { - do_save(argv[2]); - } else { - do_save(param_get_default_file()); - } - } - - if (!strcmp(argv[1], "load")) { - if (argc >= 3) { - do_load(argv[2]); - } else { - do_load(param_get_default_file()); - } - } - - if (!strcmp(argv[1], "import")) { - if (argc >= 3) { - do_import(argv[2]); - } else { - do_import(param_get_default_file()); - } - } - - if (!strcmp(argv[1], "select")) { - if (argc >= 3) { - param_set_default_file(argv[2]); - } else { - param_set_default_file(NULL); - } - warnx("selected parameter default file %s", param_get_default_file()); - exit(0); - } - - if (!strcmp(argv[1], "show")) { - if (argc >= 3) { - do_show(argv[2]); - } else { - do_show(NULL); - } - } - - if (!strcmp(argv[1], "set")) { - if (argc >= 4) { - do_set(argv[2], argv[3]); - } else { - errx(1, "not enough arguments.\nTry 'param set PARAM_NAME 3'"); - } - } - } - - errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'select' or 'save'"); -} - -static void -do_save(const char* param_file_name) -{ - /* delete the parameter file in case it exists */ - unlink(param_file_name); - - /* create the file */ - int fd = open(param_file_name, O_WRONLY | O_CREAT | O_EXCL); - - if (fd < 0) - err(1, "opening '%s' failed", param_file_name); - - int result = param_export(fd, false); - close(fd); - - if (result < 0) { - unlink(param_file_name); - errx(1, "error exporting to '%s'", param_file_name); - } - - exit(0); -} - -static void -do_load(const char* param_file_name) -{ - int fd = open(param_file_name, O_RDONLY); - - if (fd < 0) - err(1, "open '%s'", param_file_name); - - int result = param_load(fd); - close(fd); - - if (result < 0) { - errx(1, "error importing from '%s'", param_file_name); - } - - exit(0); -} - -static void -do_import(const char* param_file_name) -{ - int fd = open(param_file_name, O_RDONLY); - - if (fd < 0) - err(1, "open '%s'", param_file_name); - - int result = param_import(fd); - close(fd); - - if (result < 0) - errx(1, "error importing from '%s'", param_file_name); - - exit(0); -} - -static void -do_show(const char* search_string) -{ - printf(" + = saved, * = unsaved\n"); - param_foreach(do_show_print, search_string, false); - - exit(0); -} - -static void -do_show_print(void *arg, param_t param) -{ - int32_t i; - float f; - const char *search_string = (const char*)arg; - - /* print nothing if search string is invalid and not matching */ - if (!(arg == NULL || (!strcmp(search_string, param_name(param))))) { - /* param not found */ - return; - } - - printf("%c %s: ", - param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), - param_name(param)); - - /* - * This case can be expanded to handle printing common structure types. - */ - - switch (param_type(param)) { - case PARAM_TYPE_INT32: - if (!param_get(param, &i)) { - printf("%d\n", i); - return; - } - - break; - - case PARAM_TYPE_FLOAT: - if (!param_get(param, &f)) { - printf("%4.4f\n", (double)f); - return; - } - - break; - - case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: - printf("\n", 0 + param_type(param), param_size(param)); - return; - - default: - printf("\n", 0 + param_type(param)); - return; - } - - printf("\n", param); -} - -static void -do_set(const char* name, const char* val) -{ - int32_t i; - float f; - param_t param = param_find(name); - - /* set nothing if parameter cannot be found */ - if (param == PARAM_INVALID) { - /* param not found */ - errx(1, "Error: Parameter %s not found.", name); - } - - printf("%c %s: ", - param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), - param_name(param)); - - /* - * Set parameter if type is known and conversion from string to value turns out fine - */ - - switch (param_type(param)) { - case PARAM_TYPE_INT32: - if (!param_get(param, &i)) { - printf("old: %d", i); - - /* convert string */ - char* end; - i = strtol(val,&end,10); - param_set(param, &i); - printf(" -> new: %d\n", i); - - } - - break; - - case PARAM_TYPE_FLOAT: - if (!param_get(param, &f)) { - printf("float values are not yet supported."); - // printf("old: %4.4f", (double)f); - - // /* convert string */ - // char* end; - // f = strtof(val,&end); - // param_set(param, &f); - // printf(" -> new: %4.4f\n", f); - - } - - break; - - default: - errx(1, "\n", 0 + param_type(param)); - } - - exit(0); -} diff --git a/apps/systemcmds/perf/.context b/apps/systemcmds/perf/.context deleted file mode 100644 index e69de29bb..000000000 diff --git a/apps/systemcmds/perf/Makefile b/apps/systemcmds/perf/Makefile deleted file mode 100644 index f8bab41b6..000000000 --- a/apps/systemcmds/perf/Makefile +++ /dev/null @@ -1,44 +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. -# -############################################################################ - -# -# perf_counter reporting tool -# - -APPNAME = perf -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -include $(APPDIR)/mk/app.mk - -MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/perf/perf.c b/apps/systemcmds/perf/perf.c deleted file mode 100644 index 64443d019..000000000 --- a/apps/systemcmds/perf/perf.c +++ /dev/null @@ -1,81 +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. - * - ****************************************************************************/ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - - -#include -#include -#include -#include - -#include "systemlib/perf_counter.h" - - -/**************************************************************************** - * Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Data - ****************************************************************************/ - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -__EXPORT int perf_main(int argc, char *argv[]); - -/**************************************************************************** - * user_start - ****************************************************************************/ - -int perf_main(int argc, char *argv[]) -{ - if (argc > 1) { - if (strcmp(argv[1], "reset") == 0) { - perf_reset_all(); - return 0; - } - printf("Usage: perf \n"); - return -1; - } - - perf_print_all(); - fflush(stdout); - return 0; -} - - diff --git a/apps/systemcmds/preflight_check/Makefile b/apps/systemcmds/preflight_check/Makefile deleted file mode 100644 index 98aadaa86..000000000 --- a/apps/systemcmds/preflight_check/Makefile +++ /dev/null @@ -1,44 +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. -# -############################################################################ - -# -# Reboot command. -# - -APPNAME = preflight_check -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -include $(APPDIR)/mk/app.mk - -MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/preflight_check/preflight_check.c b/apps/systemcmds/preflight_check/preflight_check.c deleted file mode 100644 index 9ac6f0b5e..000000000 --- a/apps/systemcmds/preflight_check/preflight_check.c +++ /dev/null @@ -1,206 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file reboot.c - * Tool similar to UNIX reboot command - */ - -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include - -#include - -__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); -static int led_off(int leds, int led); - -int preflight_check_main(int argc, char *argv[]) -{ - bool fail_on_error = false; - - if (argc > 1 && !strcmp(argv[1], "--help")) { - warnx("usage: preflight_check [--fail-on-error]\n\tif fail on error is enabled, will return 1 on error"); - exit(1); - } - - if (argc > 1 && !strcmp(argv[1], "--fail-on-error")) { - fail_on_error = true; - } - - 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 */ - usleep(150000); - - - /* ---- MAG ---- */ - close(fd); - 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; - } - ret = ioctl(fd, MAGIOCSELFTEST, 0); - - 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; - } - - /* ---- ACCEL ---- */ - - close(fd); - fd = open(ACCEL_DEVICE_PATH, 0); - ret = ioctl(fd, ACCELIOCSELFTEST, 0); - - if (ret != OK) { - warnx("accel self test failed"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK"); - system_ok = false; - goto system_eval; - } - - /* ---- GYRO ---- */ - - close(fd); - fd = open(GYRO_DEVICE_PATH, 0); - ret = ioctl(fd, GYROIOCSELFTEST, 0); - - if (ret != OK) { - warnx("gyro self test failed"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK"); - system_ok = false; - goto system_eval; - } - - /* ---- BARO ---- */ - - close(fd); - fd = open(BARO_DEVICE_PATH, 0); - - - -system_eval: - - if (system_ok) { - /* all good, exit silently */ - exit(0); - } else { - fflush(stdout); - - int buzzer = open("/dev/tone_alarm", O_WRONLY); - int leds = open(LED_DEVICE_PATH, 0); - - /* flip blue led into alternating amber */ - led_off(leds, LED_BLUE); - led_off(leds, LED_AMBER); - led_toggle(leds, LED_BLUE); - - /* display and sound error */ - for (int i = 0; i < 150; i++) - { - led_toggle(leds, LED_BLUE); - led_toggle(leds, LED_AMBER); - - if (i % 10 == 0) { - ioctl(buzzer, TONE_SET_ALARM, 4); - } else if (i % 5 == 0) { - ioctl(buzzer, TONE_SET_ALARM, 2); - } - usleep(100000); - } - - /* stop alarm */ - ioctl(buzzer, TONE_SET_ALARM, 0); - - /* switch on leds */ - led_on(leds, LED_BLUE); - led_on(leds, LED_AMBER); - - if (fail_on_error) { - /* exit with error message */ - exit(1); - } else { - /* do not emit an error code to make sure system still boots */ - exit(0); - } - } -} - -static int led_toggle(int leds, int led) -{ - static int last_blue = LED_ON; - static int last_amber = LED_ON; - - if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON; - - if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON; - - return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led); -} - -static int led_off(int leds, int led) -{ - return ioctl(leds, LED_OFF, led); -} - -static int led_on(int leds, int led) -{ - return ioctl(leds, LED_ON, led); -} \ No newline at end of file diff --git a/apps/systemcmds/pwm/Makefile b/apps/systemcmds/pwm/Makefile deleted file mode 100644 index 5ab105ed1..000000000 --- a/apps/systemcmds/pwm/Makefile +++ /dev/null @@ -1,42 +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. -# -############################################################################ - -# -# 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 deleted file mode 100644 index de7a53199..000000000 --- a/apps/systemcmds/pwm/pwm.c +++ /dev/null @@ -1,233 +0,0 @@ -/**************************************************************************** - * - * 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 - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -#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 ] [-u ] [-c ] [arm|disarm] [ ...]\n" - " -v Print information about the PWM device\n" - " PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" - " PWM update rate for channels in \n" - " Channel group that should update at the alternate rate (may be specified more than once)\n" - " arm | disarm Arm or disarm the ouptut\n" - " ... 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/systemcmds/reboot/.context b/apps/systemcmds/reboot/.context deleted file mode 100644 index e69de29bb..000000000 diff --git a/apps/systemcmds/reboot/Makefile b/apps/systemcmds/reboot/Makefile deleted file mode 100644 index 15dd19982..000000000 --- a/apps/systemcmds/reboot/Makefile +++ /dev/null @@ -1,44 +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. -# -############################################################################ - -# -# Reboot command. -# - -APPNAME = reboot -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -include $(APPDIR)/mk/app.mk - -MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/reboot/reboot.c b/apps/systemcmds/reboot/reboot.c deleted file mode 100644 index 47d3cd948..000000000 --- a/apps/systemcmds/reboot/reboot.c +++ /dev/null @@ -1,53 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file reboot.c - * Tool similar to UNIX reboot command - */ - -#include -#include -#include - -#include - -__EXPORT int reboot_main(int argc, char *argv[]); - -int reboot_main(int argc, char *argv[]) -{ - up_systemreset(); -} - - diff --git a/apps/systemcmds/top/.context b/apps/systemcmds/top/.context deleted file mode 100644 index e69de29bb..000000000 diff --git a/apps/systemcmds/top/Makefile b/apps/systemcmds/top/Makefile deleted file mode 100644 index f58f9212e..000000000 --- a/apps/systemcmds/top/Makefile +++ /dev/null @@ -1,44 +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. -# -############################################################################ - -# -# realtime system performance display -# - -APPNAME = top -PRIORITY = SCHED_PRIORITY_DEFAULT - 10 -STACKSIZE = 3000 - -include $(APPDIR)/mk/app.mk - -MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/top/top.c b/apps/systemcmds/top/top.c deleted file mode 100644 index 59d2bc8f1..000000000 --- a/apps/systemcmds/top/top.c +++ /dev/null @@ -1,253 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file top.c - * Tool similar to UNIX top command - * @see http://en.wikipedia.org/wiki/Top_unix - */ - -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -/** - * Start the top application. - */ -__EXPORT int top_main(int argc, char *argv[]); - -extern struct system_load_s system_load; - -bool top_sigusr1_rcvd = false; - -int top_main(int argc, char *argv[]) -{ - int t; - - uint64_t total_user_time = 0; - - int running_count = 0; - int blocked_count = 0; - - uint64_t new_time = hrt_absolute_time(); - uint64_t interval_start_time = new_time; - - uint64_t last_times[CONFIG_MAX_TASKS]; - float curr_loads[CONFIG_MAX_TASKS]; - - for (t = 0; t < CONFIG_MAX_TASKS; t++) - last_times[t] = 0; - - float interval_time_ms_inv = 0.f; - - /* Open console directly to grab CTRL-C signal */ - int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); - - while (true) -// for (t = 0; t < 10; t++) - { - int i; - - uint64_t curr_time_ms = (hrt_absolute_time() / 1000LLU); - unsigned int curr_time_s = curr_time_ms / 1000LLU; - - uint64_t idle_time_total_ms = (system_load.tasks[0].total_runtime / 1000LLU); - unsigned int idle_time_total_s = idle_time_total_ms / 1000LLU; - - if (new_time > interval_start_time) - interval_time_ms_inv = 1.f / ((float)((new_time - interval_start_time) / 1000)); - - running_count = 0; - blocked_count = 0; - total_user_time = 0; - - for (i = 0; i < CONFIG_MAX_TASKS; i++) { - uint64_t interval_runtime = (system_load.tasks[i].valid && last_times[i] > 0 && system_load.tasks[i].total_runtime > last_times[i]) ? (system_load.tasks[i].total_runtime - last_times[i]) / 1000 : 0; - - last_times[i] = system_load.tasks[i].total_runtime; - - if (system_load.tasks[i].valid && (new_time > interval_start_time)) { - curr_loads[i] = interval_runtime * interval_time_ms_inv; - - if (i > 0) - total_user_time += interval_runtime; - - } else - curr_loads[i] = 0; - } - - for (i = 0; i < CONFIG_MAX_TASKS; i++) { - if (system_load.tasks[i].valid && (new_time > interval_start_time)) { - if (system_load.tasks[i].tcb->pid == 0) { - float idle = curr_loads[0]; - float task_load = (float)(total_user_time) * interval_time_ms_inv; - - if (task_load > (1.f - idle)) task_load = (1.f - idle); /* this can happen if one tasks total runtime was not computed correctly by the scheduler instrumentation TODO */ - - float sched_load = 1.f - idle - task_load; - - /* print system information */ - printf("\033[H"); /* cursor home */ - printf("\033[KProcesses: %d total, %d running, %d sleeping\n", system_load.total_count, running_count, blocked_count); - printf("\033[KCPU usage: %d.%02d%% tasks, %d.%02d%% sched, %d.%02d%% idle\n", (int)(task_load * 100), (int)((task_load * 10000.0f) - (int)(task_load * 100.0f) * 100), (int)(sched_load * 100), (int)((sched_load * 10000.0f) - (int)(sched_load * 100.0f) * 100), (int)(idle * 100), (int)((idle * 10000.0f) - ((int)(idle * 100)) * 100)); - printf("\033[KUptime: %u.%03u s total, %d.%03d s idle\n\033[K\n", curr_time_s, (unsigned int)(curr_time_ms - curr_time_s * 1000LLU), idle_time_total_s, (int)(idle_time_total_ms - idle_time_total_s * 1000)); - - /* 34 chars command name length (32 chars plus two spaces) */ - char header_spaces[CONFIG_TASK_NAME_SIZE + 1]; - memset(header_spaces, ' ', CONFIG_TASK_NAME_SIZE); - header_spaces[CONFIG_TASK_NAME_SIZE] = '\0'; -#if CONFIG_RR_INTERVAL > 0 - printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tSTACK USE\tCURR (BASE) PRIO\tRR SLICE\n", header_spaces); -#else - printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tSTACK USE\tCURR (BASE) PRIO\n", header_spaces); -#endif - - } else { - enum tstate_e task_state = (enum tstate_e)system_load.tasks[i].tcb->task_state; - - if (task_state == TSTATE_TASK_PENDING || - task_state == TSTATE_TASK_READYTORUN || - task_state == TSTATE_TASK_RUNNING) { - running_count++; - } - - if (task_state == TSTATE_TASK_INACTIVE || /* BLOCKED - Initialized but not yet activated */ - task_state == TSTATE_WAIT_SEM /* BLOCKED - Waiting for a semaphore */ -#ifndef CONFIG_DISABLE_SIGNALS - || task_state == TSTATE_WAIT_SIG /* BLOCKED - Waiting for a signal */ -#endif -#ifndef CONFIG_DISABLE_MQUEUE - || task_state == TSTATE_WAIT_MQNOTEMPTY /* BLOCKED - Waiting for a MQ to become not empty. */ - || task_state == TSTATE_WAIT_MQNOTFULL /* BLOCKED - Waiting for a MQ to become not full. */ -#endif -#ifdef CONFIG_PAGING - || task_state == TSTATE_WAIT_PAGEFILL /* BLOCKED - Waiting for page fill */ -#endif - ) { - blocked_count++; - } - - char spaces[CONFIG_TASK_NAME_SIZE + 2]; - - /* count name len */ - int namelen = 0; - - while (namelen < CONFIG_TASK_NAME_SIZE) { - if (system_load.tasks[i].tcb->name[namelen] == '\0') break; - - namelen++; - } - - int s = 0; - - for (s = 0; s < CONFIG_TASK_NAME_SIZE + 2 - namelen; s++) { - spaces[s] = ' '; - } - - spaces[s] = '\0'; - - char *runtime_spaces = " "; - - if ((system_load.tasks[i].total_runtime / 1000) < 99) { - runtime_spaces = ""; - } - - unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr - - (uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr; - unsigned stack_free = 0; - uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr; - - while (stack_free < stack_size) { - if (*stack_sweeper++ != 0xff) - break; - - stack_free++; - } - - printf("\033[K % 2d\t%s%s % 8lld ms%s \t % 2d.%03d \t % 4u / % 4u", - (int)system_load.tasks[i].tcb->pid, - system_load.tasks[i].tcb->name, - spaces, - (system_load.tasks[i].total_runtime / 1000), - runtime_spaces, - (int)(curr_loads[i] * 100), - (int)(curr_loads[i] * 100000.0f - (int)(curr_loads[i] * 1000.0f) * 100), - stack_size - stack_free, - stack_size); - /* Print scheduling info with RR time slice */ -#if CONFIG_RR_INTERVAL > 0 - printf("\t%d\t(%d)\t\t%d\n", (int)system_load.tasks[i].tcb->sched_priority, (int)system_load.tasks[i].tcb->base_priority, (int)system_load.tasks[i].tcb->timeslice); -#else - /* Print scheduling info without time slice*/ - printf("\t%d (%d)\n", (int)system_load.tasks[i].tcb->sched_priority, (int)system_load.tasks[i].tcb->base_priority); -#endif - } - } - } - - printf("\033[K[ Hit Ctrl-C to quit. ]\n\033[J"); - fflush(stdout); - - interval_start_time = new_time; - - char c; - - /* Sleep 200 ms waiting for user input four times */ - /* XXX use poll ... */ - for (int k = 0; k < 4; k++) { - if (read(console, &c, 1) == 1) { - if (c == 0x03 || c == 0x63) { - printf("Abort\n"); - close(console); - return OK; - } - } - - usleep(200000); - } - - new_time = hrt_absolute_time(); - } - - close(console); - - return OK; -} diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index 44e35bdf9..b3ebe4553 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -16,9 +16,19 @@ MODULES += drivers/l3gd20 MODULES += drivers/ardrone_interface # -# System utilities +# System commands # MODULES += systemcmds/eeprom +MODULES += systemcmds/bl_update +MODULES += systemcmds/boardinfo +MODULES += systemcmds/i2c +MODULES += systemcmds/mixer +MODULES += systemcmds/param +MODULES += systemcmds/perf +MODULES += systemcmds/preflight_check +MODULES += systemcmds/pwm +MODULES += systemcmds/reboot +MODULES += systemcmds/top MODULES += systemcmds/tests # @@ -48,12 +58,9 @@ endef # command priority stack entrypoint BUILTIN_COMMANDS := \ $(call _B, adc, , 2048, adc_main ) \ - $(call _B, bl_update, , 4096, bl_update_main ) \ $(call _B, blinkm, , 2048, blinkm_main ) \ $(call _B, bma180, , 2048, bma180_main ) \ - $(call _B, boardinfo, , 2048, boardinfo_main ) \ $(call _B, control_demo, , 2048, control_demo_main ) \ - $(call _B, delay_test, , 2048, delay_test_main ) \ $(call _B, fixedwing_att_control, SCHED_PRIORITY_MAX-30, 2048, fixedwing_att_control_main ) \ $(call _B, fixedwing_pos_control, SCHED_PRIORITY_MAX-30, 2048, fixedwing_pos_control_main ) \ $(call _B, gps, , 2048, gps_main ) \ @@ -62,22 +69,15 @@ BUILTIN_COMMANDS := \ $(call _B, hott_telemetry, , 2048, hott_telemetry_main ) \ $(call _B, kalman_demo, SCHED_PRIORITY_MAX-30, 2048, kalman_demo_main ) \ $(call _B, math_demo, , 8192, math_demo_main ) \ - $(call _B, mixer, , 4096, mixer_main ) \ $(call _B, mpu6000, , 4096, mpu6000_main ) \ $(call _B, ms5611, , 2048, ms5611_main ) \ $(call _B, multirotor_att_control, SCHED_PRIORITY_MAX-15, 2048, multirotor_att_control_main) \ $(call _B, multirotor_pos_control, SCHED_PRIORITY_MAX-25, 2048, multirotor_pos_control_main) \ - $(call _B, param, , 4096, param_main ) \ - $(call _B, perf, , 2048, perf_main ) \ $(call _B, position_estimator, , 4096, position_estimator_main ) \ - $(call _B, preflight_check, , 2048, preflight_check_main ) \ $(call _B, px4io, , 2048, px4io_main ) \ - $(call _B, reboot, , 2048, reboot_main ) \ $(call _B, sdlog, SCHED_PRIORITY_MAX-30, 2048, sdlog_main ) \ $(call _B, sensors, SCHED_PRIORITY_MAX-5, 4096, sensors_main ) \ $(call _B, sercon, , 2048, sercon_main ) \ $(call _B, serdis, , 2048, serdis_main ) \ $(call _B, tone_alarm, , 2048, tone_alarm_main ) \ - $(call _B, top, SCHED_PRIORITY_DEFAULT-10, 3000, top_main ) \ - $(call _B, param, SCHED_PRIORITY_DEFAULT-10, 2048, param_main ) \ $(call _B, uorb, , 4096, uorb_main ) diff --git a/src/modules/mavlink_onboard/module.mk b/src/modules/mavlink_onboard/module.mk index c40fa042e..a7a4980fa 100644 --- a/src/modules/mavlink_onboard/module.mk +++ b/src/modules/mavlink_onboard/module.mk @@ -37,6 +37,6 @@ MODULE_COMMAND = mavlink_onboard SRCS = mavlink.c \ - mavlink_receiver.c + mavlink_receiver.c INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/systemcmds/bl_update/bl_update.c b/src/systemcmds/bl_update/bl_update.c new file mode 100644 index 000000000..0569d89f5 --- /dev/null +++ b/src/systemcmds/bl_update/bl_update.c @@ -0,0 +1,215 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file bl_update.c + * + * STM32F4 bootloader update tool. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "systemlib/systemlib.h" +#include "systemlib/err.h" + +__EXPORT int bl_update_main(int argc, char *argv[]); + +static void setopt(void); + +int +bl_update_main(int argc, char *argv[]) +{ + if (argc != 2) + errx(1, "missing firmware filename or command"); + + if (!strcmp(argv[1], "setopt")) + setopt(); + + int fd = open(argv[1], O_RDONLY); + + if (fd < 0) + err(1, "open %s", argv[1]); + + struct stat s; + + if (stat(argv[1], &s) < 0) + err(1, "stat %s", argv[1]); + + /* sanity-check file size */ + if (s.st_size > 16384) + errx(1, "%s: file too large", argv[1]); + + uint8_t *buf = malloc(s.st_size); + + if (buf == NULL) + errx(1, "failed to allocate %u bytes for firmware buffer", s.st_size); + + if (read(fd, buf, s.st_size) != s.st_size) + err(1, "firmware read error"); + + close(fd); + + uint32_t *hdr = (uint32_t *)buf; + + if ((hdr[0] < 0x20000000) || /* stack not below RAM */ + (hdr[0] > (0x20000000 + (128 * 1024))) || /* stack not above RAM */ + (hdr[1] < 0x08000000) || /* entrypoint not below flash */ + ((hdr[1] - 0x08000000) > 16384)) { /* entrypoint not outside bootloader */ + free(buf); + errx(1, "not a bootloader image"); + } + + warnx("image validated, erasing bootloader..."); + usleep(10000); + + /* prevent other tasks from running while we do this */ + sched_lock(); + + /* unlock the control register */ + volatile uint32_t *keyr = (volatile uint32_t *)0x40023c04; + *keyr = 0x45670123U; + *keyr = 0xcdef89abU; + + volatile uint32_t *sr = (volatile uint32_t *)0x40023c0c; + volatile uint32_t *cr = (volatile uint32_t *)0x40023c10; + volatile uint8_t *base = (volatile uint8_t *)0x08000000; + + /* check the control register */ + if (*cr & 0x80000000) { + warnx("WARNING: flash unlock failed, flash aborted"); + goto flash_end; + } + + /* erase the bootloader sector */ + *cr = 0x2; + *cr = 0x10002; + + /* wait for the operation to complete */ + while (*sr & 0x1000) { + } + + if (*sr & 0xf2) { + warnx("WARNING: erase error 0x%02x", *sr); + goto flash_end; + } + + /* verify the erase */ + for (int i = 0; i < s.st_size; i++) { + if (base[i] != 0xff) { + warnx("WARNING: erase failed at %d - retry update, DO NOT reboot", i); + goto flash_end; + } + } + + warnx("flashing..."); + + /* now program the bootloader - speed is not critical so use x8 mode */ + for (int i = 0; i < s.st_size; i++) { + + /* program a byte */ + *cr = 1; + base[i] = buf[i]; + + /* wait for the operation to complete */ + while (*sr & 0x1000) { + } + + if (*sr & 0xf2) { + warnx("WARNING: program error 0x%02x", *sr); + goto flash_end; + } + } + + /* re-lock the flash control register */ + *cr = 0x80000000; + + warnx("verifying..."); + + /* now run a verify pass */ + for (int i = 0; i < s.st_size; i++) { + if (base[i] != buf[i]) { + warnx("WARNING: verify failed at %u - retry update, DO NOT reboot", i); + goto flash_end; + } + } + + warnx("bootloader update complete"); + +flash_end: + /* unlock the scheduler */ + sched_unlock(); + + free(buf); + exit(0); +} + +static void +setopt(void) +{ + volatile uint32_t *optcr = (volatile uint32_t *)0x40023c14; + + const uint16_t opt_mask = (3 << 2); /* BOR_LEV bitmask */ + const uint16_t opt_bits = (0 << 2); /* BOR = 0, setting for 2.7-3.6V operation */ + + if ((*optcr & opt_mask) == opt_bits) + errx(0, "option bits are already set as required"); + + /* unlock the control register */ + volatile uint32_t *optkeyr = (volatile uint32_t *)0x40023c08; + *optkeyr = 0x08192a3bU; + *optkeyr = 0x4c5d6e7fU; + + if (*optcr & 1) + errx(1, "option control register unlock failed"); + + /* program the new option value */ + *optcr = (*optcr & ~opt_mask) | opt_bits | (1 << 1); + + usleep(1000); + + if ((*optcr & opt_mask) == opt_bits) + errx(0, "option bits set"); + + errx(1, "option bits setting failed; readback 0x%04x", *optcr); + +} \ No newline at end of file diff --git a/src/systemcmds/bl_update/module.mk b/src/systemcmds/bl_update/module.mk new file mode 100644 index 000000000..e8eea045e --- /dev/null +++ b/src/systemcmds/bl_update/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Bootloader updater (flashes the FMU USB bootloader software) +# + +MODULE_COMMAND = bl_update +SRCS = bl_update.c + +MODULE_STACKSIZE = 4096 + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/boardinfo/boardinfo.c b/src/systemcmds/boardinfo/boardinfo.c new file mode 100644 index 000000000..3ff5a066c --- /dev/null +++ b/src/systemcmds/boardinfo/boardinfo.c @@ -0,0 +1,409 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file boardinfo.c + * autopilot and carrier board information app + */ + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +__EXPORT int boardinfo_main(int argc, char *argv[]); + +struct eeprom_info_s +{ + unsigned bus; + unsigned address; + unsigned page_size; + unsigned page_count; + unsigned page_write_delay; +}; + +/* XXX currently code below only supports 8-bit addressing */ +const struct eeprom_info_s eeprom_info[] = { + {3, 0x57, 8, 16, 3300}, +}; + +struct board_parameter_s { + const char *name; + bson_type_t type; +}; + +const struct board_parameter_s board_parameters[] = { + {"name", BSON_STRING}, /* ascii board name */ + {"vers", BSON_INT32}, /* board version (major << 8) | minor */ + {"date", BSON_INT32}, /* manufacture date */ + {"build", BSON_INT32} /* build code (fab << 8) | tester */ +}; + +const unsigned num_parameters = sizeof(board_parameters) / sizeof(board_parameters[0]); + +static int +eeprom_write(const struct eeprom_info_s *eeprom, uint8_t *buf, unsigned size) +{ + int result = -1; + + struct i2c_dev_s *dev = up_i2cinitialize(eeprom->bus); + if (dev == NULL) { + warnx("failed to init bus %d for EEPROM", eeprom->bus); + goto out; + } + I2C_SETFREQUENCY(dev, 400000); + + /* loop until all data has been transferred */ + for (unsigned address = 0; address < size; ) { + + uint8_t pagebuf[eeprom->page_size + 1]; + + /* how many bytes available to transfer? */ + unsigned count = size - address; + + /* constrain writes to the page size */ + if (count > eeprom->page_size) + count = eeprom->page_size; + + pagebuf[0] = address & 0xff; + memcpy(pagebuf + 1, buf + address, count); + + struct i2c_msg_s msgv[1] = { + { + .addr = eeprom->address, + .flags = 0, + .buffer = pagebuf, + .length = count + 1 + } + }; + + result = I2C_TRANSFER(dev, msgv, 1); + if (result != OK) { + warnx("EEPROM write failed: %d", result); + goto out; + } + usleep(eeprom->page_write_delay); + address += count; + } + +out: + if (dev != NULL) + up_i2cuninitialize(dev); + return result; +} + +static int +eeprom_read(const struct eeprom_info_s *eeprom, uint8_t *buf, unsigned size) +{ + int result = -1; + + struct i2c_dev_s *dev = up_i2cinitialize(eeprom->bus); + if (dev == NULL) { + warnx("failed to init bus %d for EEPROM", eeprom->bus); + goto out; + } + I2C_SETFREQUENCY(dev, 400000); + + /* loop until all data has been transferred */ + for (unsigned address = 0; address < size; ) { + + /* how many bytes available to transfer? */ + unsigned count = size - address; + + /* constrain transfers to the page size (bus anti-hog) */ + if (count > eeprom->page_size) + count = eeprom->page_size; + + uint8_t addr = address; + struct i2c_msg_s msgv[2] = { + { + .addr = eeprom->address, + .flags = 0, + .buffer = &addr, + .length = 1 + }, + { + .addr = eeprom->address, + .flags = I2C_M_READ, + .buffer = buf + address, + .length = count + } + }; + + result = I2C_TRANSFER(dev, msgv, 2); + if (result != OK) { + warnx("EEPROM read failed: %d", result); + goto out; + } + address += count; + } + +out: + if (dev != NULL) + up_i2cuninitialize(dev); + return result; +} + +static void * +idrom_read(const struct eeprom_info_s *eeprom) +{ + uint32_t size = 0xffffffff; + int result; + void *buf = NULL; + + result = eeprom_read(eeprom, (uint8_t *)&size, sizeof(size)); + if (result != 0) { + warnx("failed reading ID ROM length"); + goto fail; + } + if (size > (eeprom->page_size * eeprom->page_count)) { + warnx("ID ROM not programmed"); + goto fail; + } + + buf = malloc(size); + if (buf == NULL) { + warnx("could not allocate %d bytes for ID ROM", size); + goto fail; + } + result = eeprom_read(eeprom, buf, size); + if (result != 0) { + warnx("failed reading ID ROM"); + goto fail; + } + return buf; + +fail: + if (buf != NULL) + free(buf); + return NULL; +} + +static void +boardinfo_set(const struct eeprom_info_s *eeprom, char *spec) +{ + struct bson_encoder_s encoder; + int result = 1; + char *state, *token; + unsigned i; + + /* create the encoder and make a writable copy of the spec */ + bson_encoder_init_buf(&encoder, NULL, 0); + + for (i = 0, token = strtok_r(spec, ",", &state); + token && (i < num_parameters); + i++, token = strtok_r(NULL, ",", &state)) { + + switch (board_parameters[i].type) { + case BSON_STRING: + result = bson_encoder_append_string(&encoder, board_parameters[i].name, token); + break; + case BSON_INT32: + result = bson_encoder_append_int(&encoder, board_parameters[i].name, strtoul(token, NULL, 0)); + break; + default: + result = 1; + } + if (result) { + warnx("bson append failed for %s<%s>", board_parameters[i].name, token); + goto out; + } + } + bson_encoder_fini(&encoder); + if (i != num_parameters) { + warnx("incorrect parameter list, expected: \",,\""); + result = 1; + goto out; + } + if (bson_encoder_buf_size(&encoder) > (int)(eeprom->page_size * eeprom->page_count)) { + warnx("data too large for EEPROM"); + result = 1; + goto out; + } + if ((int)*(uint32_t *)bson_encoder_buf_data(&encoder) != bson_encoder_buf_size(&encoder)) { + warnx("buffer length mismatch"); + result = 1; + goto out; + } + warnx("writing %p/%u", bson_encoder_buf_data(&encoder), bson_encoder_buf_size(&encoder)); + + result = eeprom_write(eeprom, (uint8_t *)bson_encoder_buf_data(&encoder), bson_encoder_buf_size(&encoder)); + if (result < 0) { + warnc(-result, "error writing EEPROM"); + result = 1; + } else { + result = 0; + } + +out: + free(bson_encoder_buf_data(&encoder)); + + exit(result); +} + +static int +boardinfo_print(bson_decoder_t decoder, void *private, bson_node_t node) +{ + switch (node->type) { + case BSON_INT32: + printf("%s: %d / 0x%08x\n", node->name, (int)node->i, (unsigned)node->i); + break; + case BSON_STRING: { + char buf[bson_decoder_data_pending(decoder)]; + bson_decoder_copy_data(decoder, buf); + printf("%s: %s\n", node->name, buf); + break; + } + case BSON_EOO: + break; + default: + warnx("unexpected node type %d", node->type); + break; + } + return 1; +} + +static void +boardinfo_show(const struct eeprom_info_s *eeprom) +{ + struct bson_decoder_s decoder; + void *buf; + + buf = idrom_read(eeprom); + if (buf == NULL) + errx(1, "ID ROM read failed"); + + if (bson_decoder_init_buf(&decoder, buf, 0, boardinfo_print, NULL) == 0) { + while (bson_decoder_next(&decoder) > 0) + ; + } else { + warnx("failed to init decoder"); + } + free(buf); + exit(0); +} + +struct { + const char *property; + const char *value; +} test_args; + +static int +boardinfo_test_callback(bson_decoder_t decoder, void *private, bson_node_t node) +{ + /* reject nodes with non-matching names */ + if (strcmp(node->name, test_args.property)) + return 1; + + /* compare node values to check for a match */ + switch (node->type) { + case BSON_STRING: { + char buf[bson_decoder_data_pending(decoder)]; + bson_decoder_copy_data(decoder, buf); + + /* check for a match */ + if (!strcmp(test_args.value, buf)) { + return 2; + } + break; + } + + case BSON_INT32: { + int32_t val = strtol(test_args.value, NULL, 0); + + /* check for a match */ + if (node->i == val) { + return 2; + } + break; + } + + default: + break; + } + + return 1; +} + +static void +boardinfo_test(const struct eeprom_info_s *eeprom, const char *property, const char *value) +{ + struct bson_decoder_s decoder; + void *buf; + int result = -1; + + if ((property == NULL) || (strlen(property) == 0) || + (value == NULL) || (strlen(value) == 0)) + errx(1, "missing property name or value"); + + test_args.property = property; + test_args.value = value; + + buf = idrom_read(eeprom); + if (buf == NULL) + errx(1, "ID ROM read failed"); + + if (bson_decoder_init_buf(&decoder, buf, 0, boardinfo_test_callback, NULL) == 0) { + do { + result = bson_decoder_next(&decoder); + } while (result == 1); + } else { + warnx("failed to init decoder"); + } + free(buf); + + /* if we matched, we exit with zero success */ + exit((result == 2) ? 0 : 1); +} + +int +boardinfo_main(int argc, char *argv[]) +{ + if (!strcmp(argv[1], "set")) + boardinfo_set(&eeprom_info[0], argv[2]); + + if (!strcmp(argv[1], "show")) + boardinfo_show(&eeprom_info[0]); + + if (!strcmp(argv[1], "test")) + boardinfo_test(&eeprom_info[0], argv[2], argv[3]); + + errx(1, "missing/unrecognised command, try one of 'set', 'show', 'test'"); +} diff --git a/src/systemcmds/boardinfo/module.mk b/src/systemcmds/boardinfo/module.mk new file mode 100644 index 000000000..6851d229b --- /dev/null +++ b/src/systemcmds/boardinfo/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Information about FMU and IO boards connected +# + +MODULE_COMMAND = boardinfo +SRCS = boardinfo.c + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/eeprom/module.mk b/src/systemcmds/eeprom/module.mk index 3b4fc0479..07f3945be 100644 --- a/src/systemcmds/eeprom/module.mk +++ b/src/systemcmds/eeprom/module.mk @@ -1,3 +1,36 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + # # EEPROM file system driver # diff --git a/src/systemcmds/i2c/i2c.c b/src/systemcmds/i2c/i2c.c new file mode 100644 index 000000000..4da238039 --- /dev/null +++ b/src/systemcmds/i2c/i2c.c @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * 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 + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#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/src/systemcmds/i2c/module.mk b/src/systemcmds/i2c/module.mk new file mode 100644 index 000000000..ab2357c7d --- /dev/null +++ b/src/systemcmds/i2c/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Build the i2c test tool. +# + +MODULE_COMMAND = i2c +SRCS = i2c.c + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/mixer/mixer.c b/src/systemcmds/mixer/mixer.c new file mode 100644 index 000000000..55c4f0836 --- /dev/null +++ b/src/systemcmds/mixer/mixer.c @@ -0,0 +1,152 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mixer.c + * + * Mixer utility. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +__EXPORT int mixer_main(int argc, char *argv[]); + +static void usage(const char *reason) noreturn_function; +static void load(const char *devname, const char *fname) noreturn_function; + +int +mixer_main(int argc, char *argv[]) +{ + if (argc < 2) + usage("missing command"); + + if (!strcmp(argv[1], "load")) { + if (argc < 4) + usage("missing device or filename"); + + load(argv[2], argv[3]); + } + + usage("unrecognised command"); +} + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + + fprintf(stderr, "usage:\n"); + fprintf(stderr, " mixer load \n"); + /* XXX other useful commands? */ + exit(1); +} + +static void +load(const char *devname, const char *fname) +{ + int dev; + FILE *fp; + char line[80]; + char buf[512]; + + /* open the device */ + if ((dev = open(devname, 0)) < 0) + err(1, "can't open %s\n", devname); + + /* reset mixers on the device */ + if (ioctl(dev, MIXERIOCRESET, 0)) + err(1, "can't reset mixers on %s", devname); + + /* open the mixer definition file */ + fp = fopen(fname, "r"); + if (fp == NULL) + err(1, "can't open %s", fname); + + /* read valid lines from the file into a buffer */ + buf[0] = '\0'; + for (;;) { + + /* get a line, bail on error/EOF */ + line[0] = '\0'; + if (fgets(line, sizeof(line), fp) == NULL) + break; + + /* if the line doesn't look like a mixer definition line, skip it */ + if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) + continue; + + /* compact whitespace in the buffer */ + char *t, *f; + for (f = buf; *f != '\0'; f++) { + /* scan for space characters */ + if (*f == ' ') { + /* look for additional spaces */ + t = f + 1; + while (*t == ' ') + t++; + if (*t == '\0') { + /* strip trailing whitespace */ + *f = '\0'; + } else if (t > (f + 1)) { + memmove(f + 1, t, strlen(t) + 1); + } + } + } + + /* if the line is too long to fit in the buffer, bail */ + if ((strlen(line) + strlen(buf) + 1) >= sizeof(buf)) + break; + + /* add the line to the buffer */ + strcat(buf, line); + } + + /* XXX pass the buffer to the device */ + int ret = ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf); + + if (ret < 0) + err(1, "error loading mixers from %s", fname); + exit(0); +} diff --git a/src/systemcmds/mixer/module.mk b/src/systemcmds/mixer/module.mk new file mode 100644 index 000000000..d5a3f9f77 --- /dev/null +++ b/src/systemcmds/mixer/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Build the mixer tool. +# + +MODULE_COMMAND = mixer +SRCS = mixer.c + +MODULE_STACKSIZE = 4096 + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/param/module.mk b/src/systemcmds/param/module.mk new file mode 100644 index 000000000..63f15ad28 --- /dev/null +++ b/src/systemcmds/param/module.mk @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Build the parameters tool. +# + +MODULE_COMMAND = param +SRCS = param.c + +MODULE_STACKSIZE = 4096 + +MAXOPTIMIZATION = -Os + diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c new file mode 100644 index 000000000..56f5317e3 --- /dev/null +++ b/src/systemcmds/param/param.c @@ -0,0 +1,297 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * 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 param.c + * + * Parameter tool. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "systemlib/systemlib.h" +#include "systemlib/param/param.h" +#include "systemlib/err.h" + +__EXPORT int param_main(int argc, char *argv[]); + +static void do_save(const char* param_file_name); +static void do_load(const char* param_file_name); +static void do_import(const char* param_file_name); +static void do_show(const char* search_string); +static void do_show_print(void *arg, param_t param); +static void do_set(const char* name, const char* val); + +int +param_main(int argc, char *argv[]) +{ + if (argc >= 2) { + if (!strcmp(argv[1], "save")) { + if (argc >= 3) { + do_save(argv[2]); + } else { + do_save(param_get_default_file()); + } + } + + if (!strcmp(argv[1], "load")) { + if (argc >= 3) { + do_load(argv[2]); + } else { + do_load(param_get_default_file()); + } + } + + if (!strcmp(argv[1], "import")) { + if (argc >= 3) { + do_import(argv[2]); + } else { + do_import(param_get_default_file()); + } + } + + if (!strcmp(argv[1], "select")) { + if (argc >= 3) { + param_set_default_file(argv[2]); + } else { + param_set_default_file(NULL); + } + warnx("selected parameter default file %s", param_get_default_file()); + exit(0); + } + + if (!strcmp(argv[1], "show")) { + if (argc >= 3) { + do_show(argv[2]); + } else { + do_show(NULL); + } + } + + if (!strcmp(argv[1], "set")) { + if (argc >= 4) { + do_set(argv[2], argv[3]); + } else { + errx(1, "not enough arguments.\nTry 'param set PARAM_NAME 3'"); + } + } + } + + errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'select' or 'save'"); +} + +static void +do_save(const char* param_file_name) +{ + /* delete the parameter file in case it exists */ + unlink(param_file_name); + + /* create the file */ + int fd = open(param_file_name, O_WRONLY | O_CREAT | O_EXCL); + + if (fd < 0) + err(1, "opening '%s' failed", param_file_name); + + int result = param_export(fd, false); + close(fd); + + if (result < 0) { + unlink(param_file_name); + errx(1, "error exporting to '%s'", param_file_name); + } + + exit(0); +} + +static void +do_load(const char* param_file_name) +{ + int fd = open(param_file_name, O_RDONLY); + + if (fd < 0) + err(1, "open '%s'", param_file_name); + + int result = param_load(fd); + close(fd); + + if (result < 0) { + errx(1, "error importing from '%s'", param_file_name); + } + + exit(0); +} + +static void +do_import(const char* param_file_name) +{ + int fd = open(param_file_name, O_RDONLY); + + if (fd < 0) + err(1, "open '%s'", param_file_name); + + int result = param_import(fd); + close(fd); + + if (result < 0) + errx(1, "error importing from '%s'", param_file_name); + + exit(0); +} + +static void +do_show(const char* search_string) +{ + printf(" + = saved, * = unsaved\n"); + param_foreach(do_show_print, search_string, false); + + exit(0); +} + +static void +do_show_print(void *arg, param_t param) +{ + int32_t i; + float f; + const char *search_string = (const char*)arg; + + /* print nothing if search string is invalid and not matching */ + if (!(arg == NULL || (!strcmp(search_string, param_name(param))))) { + /* param not found */ + return; + } + + printf("%c %s: ", + param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), + param_name(param)); + + /* + * This case can be expanded to handle printing common structure types. + */ + + switch (param_type(param)) { + case PARAM_TYPE_INT32: + if (!param_get(param, &i)) { + printf("%d\n", i); + return; + } + + break; + + case PARAM_TYPE_FLOAT: + if (!param_get(param, &f)) { + printf("%4.4f\n", (double)f); + return; + } + + break; + + case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: + printf("\n", 0 + param_type(param), param_size(param)); + return; + + default: + printf("\n", 0 + param_type(param)); + return; + } + + printf("\n", param); +} + +static void +do_set(const char* name, const char* val) +{ + int32_t i; + float f; + param_t param = param_find(name); + + /* set nothing if parameter cannot be found */ + if (param == PARAM_INVALID) { + /* param not found */ + errx(1, "Error: Parameter %s not found.", name); + } + + printf("%c %s: ", + param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), + param_name(param)); + + /* + * Set parameter if type is known and conversion from string to value turns out fine + */ + + switch (param_type(param)) { + case PARAM_TYPE_INT32: + if (!param_get(param, &i)) { + printf("old: %d", i); + + /* convert string */ + char* end; + i = strtol(val,&end,10); + param_set(param, &i); + printf(" -> new: %d\n", i); + + } + + break; + + case PARAM_TYPE_FLOAT: + if (!param_get(param, &f)) { + printf("float values are not yet supported."); + // printf("old: %4.4f", (double)f); + + // /* convert string */ + // char* end; + // f = strtof(val,&end); + // param_set(param, &f); + // printf(" -> new: %4.4f\n", f); + + } + + break; + + default: + errx(1, "\n", 0 + param_type(param)); + } + + exit(0); +} diff --git a/src/systemcmds/perf/module.mk b/src/systemcmds/perf/module.mk new file mode 100644 index 000000000..77952842b --- /dev/null +++ b/src/systemcmds/perf/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# perf_counter reporting tool +# + +MODULE_COMMAND = perf +SRCS = perf.c + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/perf/perf.c b/src/systemcmds/perf/perf.c new file mode 100644 index 000000000..b69ea597b --- /dev/null +++ b/src/systemcmds/perf/perf.c @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + + +#include +#include +#include +#include + +#include "systemlib/perf_counter.h" + + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +__EXPORT int perf_main(int argc, char *argv[]); + +/**************************************************************************** + * user_start + ****************************************************************************/ + +int perf_main(int argc, char *argv[]) +{ + if (argc > 1) { + if (strcmp(argv[1], "reset") == 0) { + perf_reset_all(); + return 0; + } + printf("Usage: perf \n"); + return -1; + } + + perf_print_all(); + fflush(stdout); + return 0; +} + + diff --git a/src/systemcmds/preflight_check/module.mk b/src/systemcmds/preflight_check/module.mk new file mode 100644 index 000000000..7c3c88783 --- /dev/null +++ b/src/systemcmds/preflight_check/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Pre-flight check. Locks down system for a few systems with blinking leds +# and buzzer if the sensors do not report an OK status. +# + +MODULE_COMMAND = preflight_check +SRCS = preflight_check.c + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c new file mode 100644 index 000000000..42256be61 --- /dev/null +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -0,0 +1,206 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file reboot.c + * Tool similar to UNIX reboot command + */ + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +__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); +static int led_off(int leds, int led); + +int preflight_check_main(int argc, char *argv[]) +{ + bool fail_on_error = false; + + if (argc > 1 && !strcmp(argv[1], "--help")) { + warnx("usage: preflight_check [--fail-on-error]\n\tif fail on error is enabled, will return 1 on error"); + exit(1); + } + + if (argc > 1 && !strcmp(argv[1], "--fail-on-error")) { + fail_on_error = true; + } + + 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 */ + usleep(150000); + + + /* ---- MAG ---- */ + close(fd); + 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; + } + ret = ioctl(fd, MAGIOCSELFTEST, 0); + + 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; + } + + /* ---- ACCEL ---- */ + + close(fd); + fd = open(ACCEL_DEVICE_PATH, 0); + ret = ioctl(fd, ACCELIOCSELFTEST, 0); + + if (ret != OK) { + warnx("accel self test failed"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK"); + system_ok = false; + goto system_eval; + } + + /* ---- GYRO ---- */ + + close(fd); + fd = open(GYRO_DEVICE_PATH, 0); + ret = ioctl(fd, GYROIOCSELFTEST, 0); + + if (ret != OK) { + warnx("gyro self test failed"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK"); + system_ok = false; + goto system_eval; + } + + /* ---- BARO ---- */ + + close(fd); + fd = open(BARO_DEVICE_PATH, 0); + + + +system_eval: + + if (system_ok) { + /* all good, exit silently */ + exit(0); + } else { + fflush(stdout); + + int buzzer = open("/dev/tone_alarm", O_WRONLY); + int leds = open(LED_DEVICE_PATH, 0); + + /* flip blue led into alternating amber */ + led_off(leds, LED_BLUE); + led_off(leds, LED_AMBER); + led_toggle(leds, LED_BLUE); + + /* display and sound error */ + for (int i = 0; i < 150; i++) + { + led_toggle(leds, LED_BLUE); + led_toggle(leds, LED_AMBER); + + if (i % 10 == 0) { + ioctl(buzzer, TONE_SET_ALARM, 4); + } else if (i % 5 == 0) { + ioctl(buzzer, TONE_SET_ALARM, 2); + } + usleep(100000); + } + + /* stop alarm */ + ioctl(buzzer, TONE_SET_ALARM, 0); + + /* switch on leds */ + led_on(leds, LED_BLUE); + led_on(leds, LED_AMBER); + + if (fail_on_error) { + /* exit with error message */ + exit(1); + } else { + /* do not emit an error code to make sure system still boots */ + exit(0); + } + } +} + +static int led_toggle(int leds, int led) +{ + static int last_blue = LED_ON; + static int last_amber = LED_ON; + + if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON; + + if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON; + + return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led); +} + +static int led_off(int leds, int led) +{ + return ioctl(leds, LED_OFF, led); +} + +static int led_on(int leds, int led) +{ + return ioctl(leds, LED_ON, led); +} \ No newline at end of file diff --git a/src/systemcmds/pwm/module.mk b/src/systemcmds/pwm/module.mk new file mode 100644 index 000000000..4a23bba90 --- /dev/null +++ b/src/systemcmds/pwm/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Build the pwm tool. +# + +MODULE_COMMAND = pwm +SRCS = pwm.c + +MODULE_STACKSIZE = 4096 diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c new file mode 100644 index 000000000..de7a53199 --- /dev/null +++ b/src/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 + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#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 ] [-u ] [-c ] [arm|disarm] [ ...]\n" + " -v Print information about the PWM device\n" + " PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" + " PWM update rate for channels in \n" + " Channel group that should update at the alternate rate (may be specified more than once)\n" + " arm | disarm Arm or disarm the ouptut\n" + " ... 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/src/systemcmds/reboot/module.mk b/src/systemcmds/reboot/module.mk new file mode 100644 index 000000000..19f64af54 --- /dev/null +++ b/src/systemcmds/reboot/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# reboot command. +# + +MODULE_COMMAND = reboot +SRCS = reboot.c + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/reboot/reboot.c b/src/systemcmds/reboot/reboot.c new file mode 100644 index 000000000..47d3cd948 --- /dev/null +++ b/src/systemcmds/reboot/reboot.c @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file reboot.c + * Tool similar to UNIX reboot command + */ + +#include +#include +#include + +#include + +__EXPORT int reboot_main(int argc, char *argv[]); + +int reboot_main(int argc, char *argv[]) +{ + up_systemreset(); +} + + diff --git a/src/systemcmds/top/module.mk b/src/systemcmds/top/module.mk new file mode 100644 index 000000000..9239aafc3 --- /dev/null +++ b/src/systemcmds/top/module.mk @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Build top memory / cpu status tool. +# + +MODULE_COMMAND = top +SRCS = top.c + +MODULE_STACKSIZE = 3000 + +MAXOPTIMIZATION = -Os + diff --git a/src/systemcmds/top/top.c b/src/systemcmds/top/top.c new file mode 100644 index 000000000..59d2bc8f1 --- /dev/null +++ b/src/systemcmds/top/top.c @@ -0,0 +1,253 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file top.c + * Tool similar to UNIX top command + * @see http://en.wikipedia.org/wiki/Top_unix + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +/** + * Start the top application. + */ +__EXPORT int top_main(int argc, char *argv[]); + +extern struct system_load_s system_load; + +bool top_sigusr1_rcvd = false; + +int top_main(int argc, char *argv[]) +{ + int t; + + uint64_t total_user_time = 0; + + int running_count = 0; + int blocked_count = 0; + + uint64_t new_time = hrt_absolute_time(); + uint64_t interval_start_time = new_time; + + uint64_t last_times[CONFIG_MAX_TASKS]; + float curr_loads[CONFIG_MAX_TASKS]; + + for (t = 0; t < CONFIG_MAX_TASKS; t++) + last_times[t] = 0; + + float interval_time_ms_inv = 0.f; + + /* Open console directly to grab CTRL-C signal */ + int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); + + while (true) +// for (t = 0; t < 10; t++) + { + int i; + + uint64_t curr_time_ms = (hrt_absolute_time() / 1000LLU); + unsigned int curr_time_s = curr_time_ms / 1000LLU; + + uint64_t idle_time_total_ms = (system_load.tasks[0].total_runtime / 1000LLU); + unsigned int idle_time_total_s = idle_time_total_ms / 1000LLU; + + if (new_time > interval_start_time) + interval_time_ms_inv = 1.f / ((float)((new_time - interval_start_time) / 1000)); + + running_count = 0; + blocked_count = 0; + total_user_time = 0; + + for (i = 0; i < CONFIG_MAX_TASKS; i++) { + uint64_t interval_runtime = (system_load.tasks[i].valid && last_times[i] > 0 && system_load.tasks[i].total_runtime > last_times[i]) ? (system_load.tasks[i].total_runtime - last_times[i]) / 1000 : 0; + + last_times[i] = system_load.tasks[i].total_runtime; + + if (system_load.tasks[i].valid && (new_time > interval_start_time)) { + curr_loads[i] = interval_runtime * interval_time_ms_inv; + + if (i > 0) + total_user_time += interval_runtime; + + } else + curr_loads[i] = 0; + } + + for (i = 0; i < CONFIG_MAX_TASKS; i++) { + if (system_load.tasks[i].valid && (new_time > interval_start_time)) { + if (system_load.tasks[i].tcb->pid == 0) { + float idle = curr_loads[0]; + float task_load = (float)(total_user_time) * interval_time_ms_inv; + + if (task_load > (1.f - idle)) task_load = (1.f - idle); /* this can happen if one tasks total runtime was not computed correctly by the scheduler instrumentation TODO */ + + float sched_load = 1.f - idle - task_load; + + /* print system information */ + printf("\033[H"); /* cursor home */ + printf("\033[KProcesses: %d total, %d running, %d sleeping\n", system_load.total_count, running_count, blocked_count); + printf("\033[KCPU usage: %d.%02d%% tasks, %d.%02d%% sched, %d.%02d%% idle\n", (int)(task_load * 100), (int)((task_load * 10000.0f) - (int)(task_load * 100.0f) * 100), (int)(sched_load * 100), (int)((sched_load * 10000.0f) - (int)(sched_load * 100.0f) * 100), (int)(idle * 100), (int)((idle * 10000.0f) - ((int)(idle * 100)) * 100)); + printf("\033[KUptime: %u.%03u s total, %d.%03d s idle\n\033[K\n", curr_time_s, (unsigned int)(curr_time_ms - curr_time_s * 1000LLU), idle_time_total_s, (int)(idle_time_total_ms - idle_time_total_s * 1000)); + + /* 34 chars command name length (32 chars plus two spaces) */ + char header_spaces[CONFIG_TASK_NAME_SIZE + 1]; + memset(header_spaces, ' ', CONFIG_TASK_NAME_SIZE); + header_spaces[CONFIG_TASK_NAME_SIZE] = '\0'; +#if CONFIG_RR_INTERVAL > 0 + printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tSTACK USE\tCURR (BASE) PRIO\tRR SLICE\n", header_spaces); +#else + printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tSTACK USE\tCURR (BASE) PRIO\n", header_spaces); +#endif + + } else { + enum tstate_e task_state = (enum tstate_e)system_load.tasks[i].tcb->task_state; + + if (task_state == TSTATE_TASK_PENDING || + task_state == TSTATE_TASK_READYTORUN || + task_state == TSTATE_TASK_RUNNING) { + running_count++; + } + + if (task_state == TSTATE_TASK_INACTIVE || /* BLOCKED - Initialized but not yet activated */ + task_state == TSTATE_WAIT_SEM /* BLOCKED - Waiting for a semaphore */ +#ifndef CONFIG_DISABLE_SIGNALS + || task_state == TSTATE_WAIT_SIG /* BLOCKED - Waiting for a signal */ +#endif +#ifndef CONFIG_DISABLE_MQUEUE + || task_state == TSTATE_WAIT_MQNOTEMPTY /* BLOCKED - Waiting for a MQ to become not empty. */ + || task_state == TSTATE_WAIT_MQNOTFULL /* BLOCKED - Waiting for a MQ to become not full. */ +#endif +#ifdef CONFIG_PAGING + || task_state == TSTATE_WAIT_PAGEFILL /* BLOCKED - Waiting for page fill */ +#endif + ) { + blocked_count++; + } + + char spaces[CONFIG_TASK_NAME_SIZE + 2]; + + /* count name len */ + int namelen = 0; + + while (namelen < CONFIG_TASK_NAME_SIZE) { + if (system_load.tasks[i].tcb->name[namelen] == '\0') break; + + namelen++; + } + + int s = 0; + + for (s = 0; s < CONFIG_TASK_NAME_SIZE + 2 - namelen; s++) { + spaces[s] = ' '; + } + + spaces[s] = '\0'; + + char *runtime_spaces = " "; + + if ((system_load.tasks[i].total_runtime / 1000) < 99) { + runtime_spaces = ""; + } + + unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr - + (uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr; + unsigned stack_free = 0; + uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr; + + while (stack_free < stack_size) { + if (*stack_sweeper++ != 0xff) + break; + + stack_free++; + } + + printf("\033[K % 2d\t%s%s % 8lld ms%s \t % 2d.%03d \t % 4u / % 4u", + (int)system_load.tasks[i].tcb->pid, + system_load.tasks[i].tcb->name, + spaces, + (system_load.tasks[i].total_runtime / 1000), + runtime_spaces, + (int)(curr_loads[i] * 100), + (int)(curr_loads[i] * 100000.0f - (int)(curr_loads[i] * 1000.0f) * 100), + stack_size - stack_free, + stack_size); + /* Print scheduling info with RR time slice */ +#if CONFIG_RR_INTERVAL > 0 + printf("\t%d\t(%d)\t\t%d\n", (int)system_load.tasks[i].tcb->sched_priority, (int)system_load.tasks[i].tcb->base_priority, (int)system_load.tasks[i].tcb->timeslice); +#else + /* Print scheduling info without time slice*/ + printf("\t%d (%d)\n", (int)system_load.tasks[i].tcb->sched_priority, (int)system_load.tasks[i].tcb->base_priority); +#endif + } + } + } + + printf("\033[K[ Hit Ctrl-C to quit. ]\n\033[J"); + fflush(stdout); + + interval_start_time = new_time; + + char c; + + /* Sleep 200 ms waiting for user input four times */ + /* XXX use poll ... */ + for (int k = 0; k < 4; k++) { + if (read(console, &c, 1) == 1) { + if (c == 0x03 || c == 0x63) { + printf("Abort\n"); + close(console); + return OK; + } + } + + usleep(200000); + } + + new_time = hrt_absolute_time(); + } + + close(console); + + return OK; +} -- cgit v1.2.3